エネルギー制御による Pendubot の振り上げと安定化—同定実験による現実的なシミュレーションモデルの構成—
2010SE041 林雅大，2010SE042 日比野正嗣，2010SE090 加藤任真
指導教員：陳幹
表 1 物理パラメータ

はじめに

1

Pendubot とは Pendulum と Robot の合成語であり，モータに繋がれたリンク１と，そのリンク１の先に駆動機を持たない関節により繋がれたリンク２からなる１入力２

リンク１全体の質量

出力系の倒立振子の一種である．本研究の目的は，この不リンク２全体の質量

安定かつ非線形な特性を持つ Pendubot を，重力の影響の

リンク１の長さみを受けている状態 (down resting position と呼ぶ) から

リンク２の長さ

振り上げ，両リンクが水平面に対し垂直に倒立している状

リンク１の支点から態 (top position と呼ぶ) にて安定化させることである．

重心までの長さ

2

モデリング

2.1

制御対象と物理パラメータ

図 1 に Pendubot の構造を示す．また，モデリングに用いる各物理パラメータは右の表 1 の通りである．

g(=9.81)
m1
m2
l1
l2
lc1

[m/s2 ]
[kg]
[kg]
[m]
[m]
[m]

lc2

[m]

q1

[rad]

q2

[rad]

τ1

[Nm]

τ2

[Nm]

I1
I2
Kt
(=0.0049431)
Kt2(=0.0041695)
C1(=0.0002657)
C2(=0.00003896)[kg·m2 ][kg·m2 ]重力加速度

リンク２の支点から重心までの長さ
リンク１の角度(一般化座標１)
リンク２の角度(一般化座標２)
リンク１に加わるトルク(一般化力１)
リンク２に加わるトルク(一般化力２)
リンク１の慣性モーメント
リンク２の慣性モーメント
仕様書に記されていたモータのトルク定数
劣化した現在のモータのトルク定数
モータ・リンク１間の摩擦係数
リンク１・リンク２間の摩擦係数[Nm/V][Nm/V]

図 1 Pendubot の略図

2.2
なお，Kt2 ，C1 ，C2 については，現実的なモデルの構成のために独自に定義したものであり，仕様書では与えられていないため，適切な同定実験を行うことでその値を得た．(詳細な実験内容についてはここでは省くが，理論的な導出方針を簡単に記しておく．)

ラグランジュの運動方程式

まずは [4] を参考に，ラグランジュの方法を用いて，制御対象の振舞を表現する数学モデルを導出する．リンク１，
リンク２の重心の位置をそれぞれ (x1 , y1 )，(x2 , y2 ) とおくと，それらは次の式 (1)〜(4) のように表せる．

劣化したモータのトルク定数 Kt2 については，モータ

x1 = lc1 cos q1(1)に一定の入力電圧を与え続け，重力と釣り合って静止した

y1 = lc1 sin q1(2)

x2 = l1 cos q1 + lc2 cos(q1 + q2 )(3)

y2 = l1 sin q1 + lc2 sin(q1 + q2 )(4)

位置から，実際にモータから出力されているトルクを推定し，それを基に導出した．一方，各関節の摩擦係数 C1 ，C2については，外力が与えられていない状態での実験機の運動を観察し，その際のエネルギー減少率を基にそれぞれの値を導出した．

この (x1 , y1 )，(x2 , y2 ) を用いて，Pendubot 全体の運動
エネルギー T ，ポテンシャル U ，摩擦による損失エネル

ギー D をそれぞれ求めると式 (5)〜(7) のようになる．

これらの５つの定数パラメータを用いて式 (10)，(11) を簡単化するとそれぞれ次の式 (12)，(13) のようになる．

1
1
T = m1 (ẋ21 + ẏ12 ) + m2 (ẋ22 + ẏ22 )
2
2
1 2 1
+ I1 q̇1 + I2 (q̇1 + q̇2 )2
2
2

vin =
(5)

U = m1 gy1 + m2 gy2

(6)

1
1
C1 q̇12 + C2 q̇22
2
2

(7)

D=

1
{(J1 + J2 + 2J3 cos q2 )q̈1 + (J2 + J3 cos q2 )q̈2
Kt2
− (J3 q̇2 sin q2 + C1 )q̇1 − (J3 q̇1 sin q2 + J3 q̇2 sin q2 )q̇2
+ (N1 g cos q1 + N2 g cos(q1 + q2 ))}(12)
0 =(J2 + J3 cos q2 )q̈1 + J2 q̈2+ (J3 q̇1 sin q2 )q̇1 + C2 q̇2+ N2 g cos(q1 + q2 )(13)

２つの一般化座標 q1 ，q2 についてのラグランジュの運動
以上で得られた式 (12)，(13) を，行列表現を利用して方程式は以下の式 (8)，(9) のように与えられる．

[

τ1 =

d ∂T
∂U
∂D
∂T
+
+
−
dt ∂ q̇1
∂q1
∂q1
∂ q̇1
[

τ2 =

[

]

(8)

q̈1
q̈2

d(q, q̇)][+ e(q, q̇)

q̇1
q̇2

]

[
+ f (q, q̇) =

vin
0

(14)

という形にまとめた結果，行列 d(q, q̇)，e(q, q̇)，f (q, q̇) は

]

d ∂T
∂T
∂U
∂D
−
+
+
dt ∂ q̇2
∂q2
∂q2
∂ q̇2

(9)

ここで，モータへの入力電圧を vin とおくと，τ1 は同定

以下の式 (15)〜(35) のようになった．

[
d=1
Kt2 (J1 + J2 + 2J3 cos q2 )J2 + J3 cos q2

1
Kt2 (J2 + J3 cos q2 )J2

Kt2 × vin と表せる．以上を踏まえた上で，式 (5)〜(7) を用いて式 (8) を計算すると次の式 (10) のようになる．

[e=− K1t2 (J3 q̇2 sin q2 − C1 )J3 q̇1 sin q2

22{m1 lc1+ m2 (l12 + lc2)+ 2m2 l1 lc2 cos q2 + I1 + I2 }q̈1[2
+ m2 l1 lc2 cos q2 + I2 )q̈2
+ (m2 lc2

(10)

− K1t2 (J3 q̇1 sin q2 + J3 q̇2 sin q2 )
C2
(16)

1
Kt2 (N1 g cos q1 + N2 g cos(q1 + q2 ))N2 g cos(q1 + q2 )f=

− 2m2 l1 lc2 q̇1 q̇2 sin q2 − m2 l1 lc2 q̇22 sin q2+ m1 glc1 cos q1 + m2 gl1 cos q1+ m2 glc2 cos(q1 + q2 ) + C1 q̇1

]

(15)

実験で得た，劣化したモータのトルク定数 Kt2 を用いて，Kt2 vin =

]

]
(17)

状態方程式

2.3

次に，以上で得られた d，e，f を用いて，Pendubot のまた，リンク２には駆動機が接続されていないため常に

τ2 =0 である．これを考慮した上で，式 (5)〜(7) を用いて式 (9) を計算すると次の式 (11) のようになる．
2+ m2 l1 lc2 cos q2 + I2 )q̈10 = (m2 lc2
2+ (m2 lc2
+ I2 )q̈2 + m2 l1 lc2 q̇12 sin q2+ m2 glc2 cos(q1 + q2 ) + C2 q̇2状態方程式を求める．まずは式 (14) を変形することで以下の式 (18) を得る．

[

q̈1
q̈2

]

= d−1

[

vin
0

]

− d−1 e

[

q̇1
q̇2

]

− d−1 f(18)

式 (15)〜(35) を利用して実際にこの式の計算を行うこ(11)

ここで，以降の記述を簡単にするため，数式の整理用とで，Pendubot の状態方程式 (19) を得る．

[に５つのパラメータ J1 〜J3 および N1 ，N2 を表 2 のよ

q̈1
q̈2

]
=

1
· h(q, q̇)J1 J2 − J32 cos2 q2(19)

うにおく．(なお，これらの値は実験機に付属していた，

MATLAB の m ファイル「id pendubot」内で数値的に与えられている．)
表 2 数式整理用パラメータ2
J1 = m1 lc1+ m2 l12 + I1
2
J2 = m2 lc2+ I2
J3 = m2 l1 lc2
N1 = m1 lc1 + m2 l1
N2 = m2 lc2[kg·m2 ][kg·m2 ][kg·m2 ][kg·m][kg·m]

J2 Kt2 vin + J2 J3 (q̇1 + q̇2 )2 sin q2+J32 q̇12 sin q2 cos q2 − J2 N1 g cos q1+J3 N2 g cos(q1 + q2 ) cos q2−J2 q̇1 C1 + (J2 + J3 cos q2 )q̇2 C2

















 −(J2 + J3 cos q2 )Kt2 vin − J2 J3 (q̇1 + q̇2 )2 sin q2 h(q, q̇) = −J32 (q̇1 + q̇2 )2 sin q2 cos q2 −(J1 + J3 cos q2 )q̇12 J3 sin q2 + J2 N1 g cos q1 +J3 N1 g cos q1 cos q2 − J1 N2 g cos(q1 + q2 )
−J3 N2 g cos(q1 + q2 ) cos q2+(J2 + J3 cos q2 )q̇1 C1−(J1 + J2 + 2J3 cos q2 )q̇2 C2]

[2] に倣い，リアプノフ関数を，任意の正の定数 KE ，
KD ，KP を用いて次のように定める．

3

制御系設計

3.1

振り上げコントローラの設計

V =まずは，Pendubot を down resting position から top

KE 2 KD 2 KP 2
Ẽ +
q̇ +
q̃
2
2 1
2 1

(27)


 Ẽ = E − (N1 + N2 )gposition 近傍まで振り上げるための「振り上げコントロー
ラ」の設計から行う．ここでの Pendubot の振舞いは線形化することができないため，[2] を参考にし，モデルは非線 q̃ = q − π112形のまま，エネルギー保存則に基づいて，非線形なコント
ローラを設計する．
モデリングで求めた T ，U を用いて，Pendubot 全体のエネルギー E(q, q̇) は，次の式 (20) のように表せる．

E(q, q̇) = T + U(20)

なお，Ẽ はその時刻における E と目標地点におけるE との差，q̃1 はその時刻における q1 と目標地点における q1 との差であるが，今回目標地点は top position：(q1 , q̇1 , q2 , q̇2 ) = ( π2 , 0, 0, 0) であるため上記のようになる．
続いて，式 (27) の時間微分をとると次のようになる．

実際に式 (5)，(6) を代入し，さらに表 2 より数式整理用
パラメータを用いて整理し，次の式 (21) を得る．

E=

V̇ = KE Ẽ Ė + KD q̇1 q̈1 + KP q̃1 q̇1
これに式 (26) を代入し，計算すると

1(J1 + J2 + 2J3 cos q2 )q̇12 + (J2 + J3 cos q2 )q̇1 q̇2
2
1+ J2 q̇22 + N1 g sin q1 + N2 g sin(q1 + q2 ) (21)2

これの時間微分をとると次の式 (22) のようになる．

Ė = −J3 q̇12 q̇2 sin q2 + (J1 + J2 + 2J3 cos q2 )q̇1 q̈1−J3 q̇1 q̇22 sin q2 + (J2 + J3 cos q2 )q̈1 q̇2+(J2 + J3 cos q2 )q̇1 q̈2 + J2 q̇2 q̈2+N1 g q̇1 cos q1 + N2 g cos(q1 + q2 )(q̇1 + q̇2 ) (22)
ここで，モデリングで得た式 (12) の両辺に q̇1 をかける
ことで以下の式 (23) を得る．

q̇1 Kt2 vin = (J1 + J2 + 2J3 cos q2 )q̇1 q̈1+ (J2 + J3 cos q2 )q̇1 q̈2− 2J3 q̇12 q̇2 sin q2 − J3 q̇1 q̇22 sin q2+ N1 g q̇1 cos q1 + N2 g q̇1 cos(q1 + q2 )(23)V̇ = q̇1 (KE ẼKt2 vin + KD q̈1 + KP q̃1 )(KE ẼKt2 vin + KD q̈1 + KP q̃1 ) = −q̇1となるように入力電圧 vin を決めてやれば V̇ = −q̇12 とな
り，V が単調減少関数になることが分かる．実際に関係式からこのような vin を求めると次のようになる．
vin =−q̇1 − KD q̈1 − KP q̃1
KE ẼKt2(30)

ここで，以降の記述を簡単にするため，z(q, q̇) を次のように定義する．

z(q, q̇) = J2 J3 (q̇1 + q̇2 )2 sin q2 + J32 q̇12 sin q2 cos q2− J2 N1 g cos q1 + J3 N2 g cos(q1 + q2 ) cos q2− J2 q̇1 C1 + (J2 + J3 cos q2 )q̇2 C2 (31)モデリングで求めた状態方程式 (19) の一行目より，

q̈1 =ことで以下の式 (24) を得る．

1(z(q, q̇) + J2 Kt2 vin )J1 J2 − J32 cos2 q2(32)である．これを式 (30) に代入し，数式を整理すること

0 = (J2 + J3 cos q2 )q̈1 q̇2 + J2 q̇2 q̈2(24)で，Pendubot を top position へと振り上げるための制御
入力（振り上げコントローラ）を次のように得る．

以上で得られた式 (23) と式 (24) の和をとると

vin =q̇1 Kt2 vin = (J1 + J2 + 2J3 cos q2 )q̇1 q̈1+ (J2 + J3 cos q2 )q̇1 q̈2− J3 q̇12 q̇2 sin q2 − J3 q̇1 q̇22 sin q2+ N1 g q̇1 cos q1 + N2 g q̇1 cos(q1 + q2 )+ (J2 + J3 cos q2 )q̈1 q̇2 + J2 q̇2 q̈2+ N2 g q̇2 cos(q1 + q2 )(25)となる．右辺に注目すると，式 (22) の右辺と一致する
ことが分かる．したがって，次の関係式が成り立つ．

Ė = q̇1 Kt2 vin(29)となる．この式 (29) より，さらに，モデリングで得た式 (13) の両辺に q̇2 をかける

+ J3 q̇12 q̇2 sin q2 + N2 g q̇2 cos(q1 + q2 )

(28)

3.2

−KD z(q, q̇) − (J1 J2 − J32 cos2 q2 )(q̇1 + KP q̃1 ){(J1 J2 − J32 cos2 q2 )KE Ẽ + KD J2 }Kt2(33)

安定化コントローラの設計

続いて，振りあがった Pendubot を top position で安定化するための「安定化コントローラ」の設計を行う．top

position 近傍での振舞いは線形化が可能であるため，まずはモデリングで求めた非線形な状態方程式 (19) を線形化
し，状態空間表現を得る．
平衡点 (xr , ur ) 近傍でのテイラー展開による近似式は次(26)ページの式 (34) のように与えられる．([3] 参照)∂f∂f·(x−xr )+·(u−ur )∂x |xr ,ur∂u |xr ,ur(34)T
x = [q1 , q̇1 , q2 , q̇2 ] とし，この式 (34) を用いて top
position 近傍での ẋ のテイラー展開による近似を行う (具体的には，式 (34) において f (x, u), (xr , ur ) をそれぞれ式(35),(36) のようにおく) と式 (37) のようになる．

b1 =f (x, u) = f (xr , ur )+f (x, u) = ẋ
xr =


[π
2

]T
, 0, 0, 0 ,u(t) = −Kx(t) としたとき評価関数
∫ ∞
J=

∂ ẋ2
∂x2

∂ ẋ2
∂x3

∂ ẋ3
∂x2

∂ ẋ3
∂x3





∂ ẋ3 
∂x4 

∂ ẋ4
∂x1

∂ ẋ4
∂x2

∂ ẋ4
∂x3

∂ ẋ4
∂x4



∂ ẋ1
∂x4
∂ ẋ2
∂x4



∂x2

1


·
|xr ,ur∂u ∂ ẋ 2  ∂u + ∂ ẋ3  ∂u ∂ ẋ4∂u{x(t)T Qx(t) + u(t)T Ru(t)}dt

(42)

0

 ∂ ẋ2 ∂x1+ ∂ ẋ3
 ∂x1
 ∂ ẋ

(36)(41)

以上で得られた状態空間表現を用いて，最適レギュレー

∂ ẋ1
∂x3

∂x1


q1 − π2 q̇1
x=
 , u = vinq2
q̇2
タ理論によるコントローラ設計を行う．


 ẋ2  ẋ 
 3 
ẋ4 |xr ,ur ∂ ẋ1 ∂ ẋ1ẋ =ur = vintop = 0−Kt2 (J2 + J3 )J1 J2 − J32



(35)



ẋ1

b4 =

J2 Kt2J1 J2 − J32

x1 − xr1





100
 0
Q=
0
0

0
0
1
0
0 10000000 ，R = 10
0 
1

(43)


 x2 − xr2  x −x 
 3
r3 


を最小にするゲイン K を MATLAB 上で lqr コマンドを
x4 − xr4
利用して求めることで，Pendubot を top position で安定化するためのフィードバックゲイン (安定化コントローラ)
を次のように得る．

·(u − ur )(37)|xr ,urK = [ 159.1196 26.6935 155.0244 20.2493 ] (44)3.3

式 (37) の各行列の各要素を全て計算することで，状態空間表現 (38) を得る．

ẋ = Ax + Bu

0 1 0
 a21 0 a23
A=
0 0 0
a41 0 a43

切り替えタイミングの考察

実際に振り上げから安定化までを行う際は，3.1 節で設計した振り上げコントローラにより top position 近傍まで

(38)

0
0 
1 
0

振り上げた後，3.2 節で設計した安定化コントローラに切り替えることで安定化するという流れで制御を行うことになる．この切り替えタイミングの取り方も，制御性能に大(39)きく影響するはずである．
では，実際にどのようなタイミングで切り替えを行えば良いのかといえば，安定化コントローラが確実に機能する

a21 =ような状態で切り替えを行うことが望ましいと言える．

1(J2 N1 g − J3 N2 g)
J1 J2 − J32

安定化コントローラは最適レギュレータ理論を用いて設計した．このコントローラは平衡点近傍にある制御対象

a23 =を，設計の際に設定した各状態変数に対する重み定数に準1(−J3 N2 g)
J1 J2 − J32

じて，各状態変数を平衡点へと収束させる働きをする．
つまり，目標の平衡点を中心とした状態空間において，

a41 =

1
(−J2 N1 g − J3 N1 g + J1 N2 g + J3 N2 g)
J1 J2 − J32
1(J1 N2 g + J3 N2 g)
a43 =J1 J2 − J32


0
 b 
B= 2 0b4

中心からのズレがこの範囲内の点からであれば中心への収束を保証できる，という「安定化コントローラの守備範囲」
が状態空間上に存在し，その形は綺麗な球状ではなく，各重み定数の比率に応じた形の楕円体であると考えられる．
では，その「安定化コントローラの守備範囲」を数値的に捉え，シミュレーションおよび実験で簡単に観測できる
ような方法を考察する．

(40)

安定化コントローラの設計に用いた評価関数 J(式 (42))
は，各状態変数とそれに応じた重み定数を内包しており，

この件に利用できそうである．しかしこの J は積分項を含
むため，実用的でない．そこで，J 内部の被積分要素を以下のように Jint とおき，これを利用することにした．

Jint = x(t)T Qx(t) + u(t)T Ru(t)(45)

シミュレーションおよび実験の際は，この Jint が特定の値より小さくなった時にコントローラを切り替えるという構造のスイッチを使用することとする．
実験機に付属していたサンプルプログラムは位置のみ

図 3 q̇1 (リンク１の角速度)を評価基準にしており，q1 が π2 ± 0.2[rad]，かつ，q2 が±0.3[rad] となった時に，制御則を切り替えるというスイッチを使用しているが，それに対し，今回我々が考案したス
イッチは，Jint という変数１つの値を評価するだけで，位置，速度，どちらも合わせて総合的にケアできる．

4

シミュレーション

2.3 節で求めた Pendubot の状態方程式 (19) を用いて，Simulink 上に対象のシステムを表すブロック線図を作成
し，非線形シミュレーションにて，振り上げから安定化までの切り替え制御を行った．なお，初期値は down resting

position：(q1 , q̇1 , q2 , q̇2 )=(− π2 , 0, 0, 0) とする．また，実験機の性能を考慮し，vin は ±10[V] で飽和するようにした．

図 4 q2 (リンク２の角度)

しかし，何度シミュレーションを繰り返しても，リンク２が top position まで振り上がらないという予想外の問題が発生した．これは，劣化したモータを反映したモデ
ルでは，エネルギー不足に陥ることを避けられないためだと考えられる．そこで，「入力電圧を，0〜0.3[s] の間は−7.8[V]，0.3〜0.5[s] の間は 0[V] とすることで助走をつける」という初期ステップを設けることで，エネルギーを稼いでから振り上げ制御へ移行するという方法で，この問題を解決することにした．
初期ステップを与え，スイッチの基準を Jint = 800，重み定数 KE ，KD ，KP をそれぞれ KE = 500，KD = 0.05，

KP = 30 とした時のシミュレーション結果を図 2〜6 に示す．

図 2 q1 (リンク１の角度)

図 5 q̇2 (リンク２の角速度)

図 6 vin (入力電圧)5

実装
前ページの図 2〜6 を見て分かるように，シミュレー

ション上では，Pendubot を down resting position から

top position まで振り上げ，安定化することに成功している．
シミュレーション上で望ましい結果が得られたため，実装を行った．実験機のプログラムを解析し，シミュレー
ションと同様のアルゴリズムを実験機に実装するプログラ
ムを作成して実験を行った．
実験結果とシミュレーション結果を比較したグラフを図

図 10 q̇2 (リンク２の角速度)

7〜11 に示す．

図 7 q1 (リンク１の角度)

図 11 vin (入力電圧)

6

おわりに
本研究では，適切な同定実験を行うことで現実との一致度の高いモデルを構成し，より現実的なコントローラの設計を行った．また，２つのコントローラの，より確実な切り替えタイミングについても考察を行った．
図 7〜11 から分かるように，設計した制御アルゴリズム
によって倒立実験を成功させることができた．

7
図 8 q̇1 (リンク１の角速度)

参考文献[1] Mechatronic Systems：Mechatronics Control Kit
Model M-1 User’s Manual(2001)[2] Isabelle Fantoni，Rogelio Lozano，Mark W. Spong：
Energy Based Control of the Pendubot．IEEE
Transactions on Automatic Control，Vol.45，No.4，
pp.725-729(2000)[3] 松浦達郎，鈴木宏和：エネルギー制御とスライディン
グモード制御による倒立振子の振り上げと安定化．南山大学数理情報学部数理科学科卒業論文 (2005)[4] 石川和幾：エネルギー制御による倒立振子の振り上げ−摩擦モデルの導入−．南山大学数理情報学部数理科学科卒業論文 (2008)

図 9 q2 (リンク２の角度)[5] Daniel Jerome Block ： Mechanical Design and Control of the Pendubot．University of Illinois at Urbana - Champaign(1996)