― 38 ―倒立 2 輪ロボットの安定化制御
佐藤光＊・木澤悟

Stabilization of the Wheeled Inverted Pendulum with Optimal Robust Servo SystemHikaru SATO ＊ and Satoru KIZAWA（平成22年11月26日受理）
In recent years, Wheeled Inverted Pendulum like Segway has been developed and studied
actively. So in this study, we challenge to realize the Wheeled Inverted Pendulum by applying
linear control theory and modeling the robot and making it from the beginning.
Wheeled Inverted Pendulum has unstable system, and its modeling is realized by motion
equation of nonlinear. So, because it has many element of uncertainty like frictional properties,
deriving strict physics parameter is a challenging problem. And, in this paper, we describedabout a stabilization controller for balancing mode by using Optimal Robust Servo System which
has a good robustness property. The eﬀectiveness of the proposed control law was shown byexperiments.
1.

緒言

近年，セグウェイ等に代表されるような倒立 2 輪ロボットの開発，研究が盛んに行われている。そこで本研究では，実際に一から倒立 2 輪ロボットを製作し，そして製作したロボットをモデル化することにより，線形制御理論を応用して，倒立 2 輪ロボットの倒立の実現を目指した。倒立 2 輪ロボットは，不安定なシステムかつ，モデル化においても，非線形な運動方程式で実現される。つまり，摩擦係数等の不確定要素も多く，物理パラメータの同定は困難な課題である，また車輪の角度や本体の姿勢等もリアルタイムで計測しながら制御する必要がある。
本研究では，システムのモデル化誤差を考慮し，倒立したまま車輪の速度を一定に保つことができるように，最適ロバストサーボ制御法をコントローラに用いて，倒立 2 輪ロボットの倒立制御を試み，提案した制御手法がこの倒立問題に有効であることを報告する。
2.

倒立 2 輪ロボットの概要

2.1 倒立 2 輪ロボットの設計
Fig. 1 に設計した倒立 2 輪ロボットの設計図とそ＊

秋田高専専攻科学生

Fig. 1

倒立 2 輪ロボットの設計図及び外観の外観を示し，以下に設計方針を示す。
・上部と中央部に梁を取り付けることで本体の剛性を上げた。
・モータを下部に取り付けることで重心を低くした。
・モータは一軸とする。
2.2 倒立 2 輪ロボットのシステムの概要
Fig. 2 に 倒 立 2 輪 ロ ボ ッ ト の シ ス テ ム を 示 す。
Fig. 3 にこのシステムの模式図を示す。実験システ
ムは PC，ディジタルコントロールボード，ターミナルボード，電源ユニット，モータアンプから構成される。2 輪ロボットの車輪角度はエンコーダ付き
DC モータにより，また本体の姿勢角度はエンコーダに取り付けられた棒状の部品から随時検出され平成23年２月

― 39 ―
倒立 2 輪ロボットの安定化制御

Fig. 2

倒立 2 輪ロボットシステム
Fig. 4

倒立 2 輪ロボットモデル

Table. 1

Fig. 3システムの模式図る。そしてその角度情報がターミナルボードを介して，ディジタルコントロールボードに送られ，その情報をもとに MATLAB/Simulink を利用して PC 内で演算され，制御に必要なモータトルクの指令信号がディジタルコントロールボード上の D/A コンバータを介してモータアンプに送られ，DC モータをコントロールする。
3.

本体の質量m［kg］

0.281

車輪（タイヤ、ギヤ、シャフト）の質量M［kg］

0.102

車軸から測った本体の重心の距離

L［m］

0.037

車輪の半径

r［m］0.029

本体の慣性モーメントI

2［kg・m ］ 1.639E-04

車輪の慣性モーメントJ［kg・m2］ 2.357E-05

車輪の粘性摩擦係数

Dφ［kg・m2/s］ 2.00E-07

本体の粘性摩擦係数Dθ［kg・m2/s］ 1.00E-04

重力加速度

g

［m/s2］

9.8

3.1 モデルの運動方程式
1）車輪の回転運動エネルギについて
Twr＝運動方程式の導出

物理パラメータ12
J φ＋θ）（2●（2）●である。ここで，回転角速 度 φだけではなく本体の姿勢角速度 θ が加算されているのは傾斜することによっても車輪が回転するためである。
2）傾斜方向の回転運動エネルギについて●

倒立 2 輪ロボットの安定化制御を行うためには，倒立 2 輪ロボットの入出力関係を表した動的システム表現が必要となる。ここで Fig. 4 に倒立 2 輪ロボットのモデル図を示す。ここで用いるパラメータを Table. 1 に示す。
運動方程式を導出するために以下のラグランジュの運動方程式を用いた。
d ∂T
∂T∂U
∂F－＋＋＝ τidt ∂q̇i∂qi∂qi∂q̇i（1）

ここで，T : 運動エネルギ，U : 位置エネルギ，F : 散逸エネルギである。次節に各運動エネルギの導出を示す。
秋田高専研究紀要第46号

●TBr＝

12Iθ
2
●（3）となる。
3）車輪の並進運動エネルギについて
車輪 2 つの質量がｍなので重心が車輪の中心にあると考える。車輪の動く速さを v とすると
v＝（r φ＋θ）●●であり，これより車輪の並進運動エネルギは（4）

― 40 ―佐藤

Twt＝

11222
mv2＝ mr（θ＋2θφ＋φ ）22●●●●

光・木澤（5）る。するとラグランジュの式の各項は以下となる。

2●（12）●

q1＝θとしてラグランジュの式を利用す
q2＝φ●

θ ＋rLcosθ）＋2φ（r｝
●

●

ここで

122
M φ r 2＋θ（r 2＋2rLcosθ＋L2）
2 ｛

＝

1
122
Dφφ ＋ Dθ θ22となる。
8）ラグランジュの式の各項の導出

1
1MV 2＝ M（Vx 2＋Vy 2）
22●

ばよいので，F＝である。
4）本体の並進運動エネルギについて
TBt＝

悟（6）

∂T
φ＝
（J＋mr 2＋Mr 2＋MrLcosθ）∂θ＋（J＋I＋mr 2＋Mr 2＋ML2＋2MrLcosθ）θ

（13）

∂T
φ＝
（J＋mr 2＋Mr 2）∂φ＋（J＋mr 2＋Mr 2＋MrLcosθ）θ

（14）

●

である . ここで，ロボットの台車の位置にあるタイヤの回転中心（xc, yc）はφ）
rxc＝（θ＋yc＝0（7）

●

●

●

●

●となる。振子となる倒立 2 輪ロボットの本体の重心（xp, yp）は
xp＝ xc＋Lsinθ
yp＝ yc＋Lcosθ

（8）

ddt

∂Tφ＝
｛ J＋（m＋M）r 2＋MrLcosθ ｝∂θ－MrLsinθ・φ
●●

●

●

θ＋
｛ J＋I（m＋M）r 2＋ML2＋2MrLcosθ ｝●●

である。また，本体の重心速度は－2MrLθ sinθ・θ●

dxp＝r（φ＋θ）Lθ cosθdt
dVy＝ yp＝Lθ sinθdt●

Vx＝●（15）●●（9）●ddt∂Tφ＝｛ J＋（m＋M）r 2｝∂φθ＋（m＋M）r 2＋MrLcosθ ｝｛ J＋●●●●●－mrLθ sinθ・θ
●である。
5）全体の運動エネルギについて
1）～4）より，全体の運動エネルギは

（16）

●

∂T
M
2＝（－θ 2rLsinθ －2φθ rLsinθ ）∂θ
22＝－MrLθ sinθ－φθ MrLsinθ
●●●

T＝ Twr＋TBr＋Twt＋TBt

●

●

●

（17）

＝

1
1
122222J φ＋θ）＋ I θ ＋ mr（θ（＋2θφ＋φ ）222∂T∂U∂U＝0（18）＝－MgLsinθ（19）＝0 （20）∂φ∂θ∂φ＋122M｛φ r 2＋θ（r 2＋2rLcosθ＋L2）2∂F＝ Dθ θ （21）
∂θ●

●

●

●

●

●

●●

θ 2＋rLcosθ）＋2φ（r｝
●

●

●

●

●（10）となる。
6）ポテンシャルエネルギについて重心の位置エネルギを考えればよいのでつまり，
U＝MgLcosθ（11）となる。
7）散逸エネルギについて車輪の摩擦および，車軸まわりの摩擦を考慮すれ∂F＝ Dφφ（22）∂φ●●以上の 式をまとめると
ddt

∂T
∂T
∂U
∂F－＋＋＝0∂θ
∂θ∂θ
∂θ

ddt

∂T
∂T
∂U
∂F－＋＋＝τ∂φ∂φ ∂φ ∂φ

●

●

●（23）

●となる。また，これより平成23年２月

― 41 ―
倒立 2 輪ロボットの安定化制御

θ｛θ＋（m＋M）r 2＋MrLcosθ＋J ｝｛（m＋M）r 2＋J ｝●●

2●●

JmφA＝τA－●●－θ mrLsinθ ＋Dφφ＝τ●

●（24）

●●

θ＋（m＋M）r 2＋2MrLcosθ＋ML2＋J＋I ｝｛●●

2aφ＋（a＋b）θ＋Dφφ
JmφA＝τA－（n ≥ 1）n●式の見通しを良くするために

●

●●

●●

●●

φAとなる。また， ＝nφ－θ MrLsinθ －MgLsinθ＋Dθ θ ＝0●（28）である。また，これに eq.
（27）を代入すると

θ｛（m＋M）r ＋MrLcosθ＋J ｝
2τnより

aφ＋（a＋b）θ＋Dφφ＋Jmn2φ＝nτA●●

2a＝（m＋M）r ＋J
b＝MrL
c＝ML2＋I
d＝MgL

（25）

●●（29）●●●（30）となる。次にタイヤに与えるトルク τ とモータ端子電圧 VE の関係を導出。Fig. 6 に直流モータ回路を示す。

とおく，eq.（24）は2aφ＋（a＋bcosθ）θ－θ mrLsinθ＋Dφφ＝τ
●●

●●

●

●φ＋φ（a＋bcosθ）｛ a＋2bcosθ＋c｝
●●

●●

2（26）

－θ bsinθ －dsinθ＋Dθ θ ＝0●●となる。ここで，倒立状態にあるとき，θ およびθ は小さな値をとると考えられ，θ が小さい間はeq.（26）は線形化できると考えられる。つまり●≈ 1,cosθ ＝

≈ θ,
sinθ＝

Ra : 電機子抵抗，eb : 逆起電力，VE : モータ端子電圧である。インダクタンスを無視すれば次の関係が得られる。とおけば，線形化した運動方程式はaφ＋（a＋b）θ＋Dφφ＝τ
●●●●●φ＋θ－dθ＋Dθ θ ＝0（a＋b）｛ a＋2b＋c ｝●●

Fig. 6 直流モータ回路

2≈0
θ ＝●

●●

●（27）となる。

VE＝Rai＋eb（31）

逆起電力 eb はモータの回転速度に比例するのでeb＝KEφA●

3.2 モータの動特性を考慮した運動方程式の導出
モータの動特性を考慮してタイヤに与えられるトルク τ とモータ端子電圧 V との関係を考える。また，
Fig. 5 に DC モータのモデルを示す。
モータの慣性モーメント Jm，モータ発生トルク
τA，負荷トルク τ とすると角運動方程式は（32）である。ただし KE は誘起電圧定数である。また，
eq.（31），（32）より
VE－eb VE－KEφA VE－KE nφi＝＝＝RaRa
Ra●●（33）となる。またモータ発生トルク τA は電機子の電流に比例するので次式がいえる。
τA＝KT i
ただし，KT はトルク定数。また，eq.（33）に eq.（32）を代入するとVE－KE nφ
τA＝KT i＝KTRa●

Fig. 5   DC モータ
秋田高専研究紀要第46号（34）

― 42 ―
佐藤光・木澤θ＝a21θ＋a22θ＋a23φ＋b2VEとなる。よって eq.（30），eq.
（34）よりタイヤに与えるトルク τ とモータ端子電圧 VE との関係はaφ＋（a＋b）θ＋Dφφ

●●

●

●

（41）

φ＝a31θ＋a32θ＋a33φ＋b3VE
●●

●

●●

●●

悟

●

●

である。ただし，＝nτA－ Jmn2φ●●

KT KE n φ KT nVE＝Jmn2φ－＋Ra
Ra2

●●

EdEDθ
FG
, a22＝ 2,, a23＝－ 2
a21＝－ 2F －EH
F －EH
F －EH●（35）

FdFDθ
GH, a33＝ 2, a32＝－ 2a31＝ 2F －EH
F －EH
F －EHである。さらに eq.（35）を変形すると
φ＋（a＋Jmn2 ）（a＋b）θ＋ Dφ＋●●●●

KT KE n2K nφ＝ T VERaRa（36）となる。以上より，モータの動特性を考慮した運動方程式は
φ＋（a＋Jmn2 ）（a＋b）θ＋ Dφ＋●●●●2KT KE nKT nVE
φ＝RaRaφ＋（a＋b）（a＋2b＋c）θ－dθ＋Dθ θ ＝0
●●

●●

EKT n
HKT nb2＝, b3＝－22
R（FR（F－EH）－EH）aa●である。eq.（41）の運動方程式を状態方程式に変換すると
θθ0100θ ＝ a21 a22 a23 θ ＋ b2 VE
φa31 a32 a33 φb3●

●

●

●

●●となる。よって連続時間システムの状態方程式は（37）

x＝Ax＋Bu
y＝Cx●となる。式の見通しを良くするために，
E＝a＋Jmn2
F＝a＋bKT KE n2
G＝Dθ＋Ra
H＝a＋2b＋c

●●

●

●●

010
A＝ a21 a22 a23 ,a31 a32 a33

（38）

x＝
［θ θ φ］ ,0
B＝ b2 ,b3

C＝［100］,

（44）

u＝VE

ここで，θ：本体の姿勢角度，θ：本体の姿勢角速度，φ：車輪の角速度とする。
●KT nVE
Ra

●●

T

●

●

●

（39）

Eφ＋Hθ－dθ＋Dθ θ ＝0●●（43）である。ただし，とおくと，eq.（37）は，Eφ＋Fθ＋Gφ＝（42）●となる。
3.3 状態方程式の導出
eq.（39-1）の φを消去すると●●EdEDθFG
FKT nφ＋θ＝－ 2θ＋
θ－ 2VE
2F －EH F 2－EH
F －EH
R（F－EH）
a（40-1）
●●

●

●

●●

ここで，eq.（40）は∞22（45）

0EdEDθGH
HKT nφ＝ 2φ－
θ－ 2θ＋ 2VE
2F －EH
F －EH
F －EH
R（F－EH）
a（40-2）●｛｝dt＝∫｛ y（t）＋ru（t）｝

J＝∫0 xTC TCx＋ru2 dt∞

また，eq.（39-2）の θ を消去すると●●3.4 制御系の設計
2 輪倒立振子の安定化制御に対し，外乱抑制とロバスト性に適した最適ロバストサーボ制御をコントローラに用いた。制御系の設計手順は，線形時不変システム eq.（43）に対し，制御則は最適レギュレータの枠組みで設計するので，次の 2 次形式評価関数を考える。

●

出力 y が t →∞で 0 でない目標値に一致したとすると，入力 u も t →∞で 0 でない一定値になる。したがって eq.（45）の評価関数は無限大となってしまい，このままこの評価関数を用いることができない。こ平成23年２月

― 43 ―
倒立 2 輪ロボットの安定化制御

こで新しい制御入力を v＝u とし拡大制御ベクトルTT
xe＝［x u］を定義すると，拡大システムは●

xe＝Ae xe＋Bev
y＝Ce xe●（46）

AeTPe＋Pe Ae－re－1 PebebeTPe＋Qe＝0となる。ただしAAe＝00Be＝,1B,
0Ce＝［C制御，（Ae, Ce）が可観測であることが必要十分条件である。以上，ロバストサーボ系の設計問題は最適レギュレータ問題に帰着され，eq.
（50）の評価関数を最小にする制御入力はリカッチ方程式を満たす正定解 Pe > 0を用い

0］

一方，適当なフィードバックによりサーボ系を構成して，出力 y が目標値ｒに追従したとき，t →∞における定常状態での x，u，y，v の定常値をそれ
ぞれ x∞ , u∞ , y∞ , v∞ととすると eq.（46）より
x∞＝Ax∞＋Bu∞＝0
u∞＝v∞＝0y∞＝Cx∞＝r
●

●

（47）

v＝rebeTPeδxe＝Ke（48）●●δxe＝Aeδxe＋Bev
e＝y－r＝Ceδxe

はフィードバックゲインである。しかしながら，状態量が δx と δu のフィードバックで表されているため，このままでは実現できない。そこで eq.（54）にeq.（49）を代入する。
v＝Ke｛｝

Je＝∫0 δx Qeδxe＋rev dt2（49）x∞
A B＝u∞C 0｛｝

Je＝∫0 e（t）＋rev（t）dt22（51）である。つまり偏差と制御入力に関する 2 次形式評価関数となる。すなわち，それは，拡大偏差系
eq.（49）に対して eq.（52）の評価関数を最小化し，δxe → 0 を達成する制御問題は通常の最適レギュレータを用いて実現できる。ここで，サーボシステ
ムが可制御，可観測であるためには（Ae, Be）が可秋田高専研究紀要第46号－10r（57）となる。また eq.
（42）は
xA B＝yC 0xu●（58）となる。これより
xA B＝uC 0（50）ただし，Qe は準正定行列。re は正の定数であり，こT
こで，Qe＝Ce Ce とすると eq.（45）は∞（56）

これよりしたがって，この拡大偏差システムに対して δxe → 0となるような制御を行うと y → r が達成され，偏差e＝y－r は 0 となる。これを実現するために次の評価関数を定義
Teδxx －x∞xx∞（t）（t）＝Ke＝Ke－Ke（55）δuu t）u t）
u∞（－u∞（A B x∞
0＝r
C 0 u∞●∞（54）

一方，eq.
（48）より次のように行列に変形できる。

x－x∞＝A（x－x∞）＋B（u－u∞）u－u∞＝v－v∞＝v
y－y∞＝C（x－x∞）●（53）

Ke＝rebeTPeとおき，eq.（46）から eq.（47）を引き，偏差系の状TT
態ベクトルを δxe＝［δx δu］のように定義すると●δxδuで与えられる。ここでここで定常値からの変動分を
δx（t）＝x（t）－x∞δu（t）＝u（t）－u∞

（52）

－1xy
●

（59）

eq.（55）に eq.（57）と eq.
（59）を代入すれば
v＝Kexx∞A B－Ke＝Keuu∞C 0－1

A B＝KeC 0x－0
A B
＝Key－r
C 0●

ここでKeA B
C 0－1－1＝［K1K2］x
A B－Key
C 0●－1x－e－10r●（60）

― 44 ―
佐藤光・木澤とおくと，もともとu＝v＝Ke●

A B
C 0－1x＝［K1－e●xK2］－e●（61）であるから eq.（61）を0 ～ t まで積分すると
te dt
＝K［x］
10－K2∫（t）0ttx －K1（0）x －K2∫（t）e dt＝K1（t）0tとなった。次に最適ロバストサーボ制御に基づいたコントローラを設計するために eq.
（52）のリカッチ
方程式において重み関数を試行錯誤的に以下のように設計した
00 01×108
80 1.5×10 0 0
re＝100, Qe＝0050 0000 15

u＝K1∫ （t）
x dt－K2∫（t）
e dt
00t ●

悟（62）（64）とした。また，本コントローラはサーボ仕様なので，r ＝φ（t）
を 0［rad/sec］と車輪の角速度の目標値（t）した。コントローラのゲインは MATLAB を用いて計算した結果，●となり，比例積分型の制御則が得られる。初期状態
x（0）= 0 のとき制御則のブロック線図は Fig. 7 のようになる。5）－1Ke

Fig. 74.

最適ロバストサーボシステム

倒立 2 輪ロボットの安定化制御実験

ここでは，連続時間システム eq.
（43）に対し，実際に倒立実験を行う。制御手法は第 3 章で示した最適ロバストサーボ制御を用いて行った。はじめにコントローラを設計するためにモデルの物理パラメータを代入した結果，eq.（43）に示した倒立 2 輪ロボットシステムの運動方程式は000		0x＝ 131.91 －1.27×10－4 50.27 x＋ －56.15 u－168.29 1.63×10－4 －136.55
152.52●（63）

Fig. 8A B＝［k1 : k2］＝
［1236.37 23.97 7.12 : 0.788］
C 0（65）となった。次に Fig. 8 に Simulink でプログラミングした最適ロバスト制御に基づいたブロック線図を示す。図中のフィードバックゲイン k1および k2はeq.（65）より得られたものである。また，倒立 2 輪ロボットの姿勢角及び車輪の角度はロータリーエンコーダが検出しているが，姿勢の角速度及び車輪の角速度は，次式のフィルタ付き微分器を設計して情報を検出している。
G（s）
＝50s
s＋50s

フィルタはローパスフィルタであり，ノイズ等の不要な高周波成分を取り除いている。Simulink 上で記述されたプログラムは Realtec 社のディジタルボードとそのソフトウェア RT-CON にて直接，倒立 2 輪ロボットシステムを駆動する。
次 に 設 計 し た 最 適 ロ バ ス ト 制 御 に 基 づ く， 倒倒立二輪ロボットのブロック線図
平成23年２月

― 45 ―
倒立 2 輪ロボットの安定化制御

Fig. 9（a） 倒立 2 輪ロボットの移動応答

Fig. 9（d） 車輪の角速度φの時間応答●が分かる。Fig. 9（c）は本体の姿勢角速度 θ での時間応答である。図より，0［rad/sec］に漸近していることが分かる。Fig. 9（d）は車輪の角速度φの時間応答である。本コントローラの仕様において車輪r ＝φの角速度を（t）（t）＝0［rad/sec］としたが，図においても車輪はほぼ静止していることが分かる。
以 上 の 実 験 の 結 果 よ り， 本 研 究 で 設 計 し た 倒
立 2 輪ロボットシステムに対し，最適ロバストサーボ制御に基づいたコントローラを設計して，適用した結果，十分倒立のための安定性が確保できたことが確認できた。
●●●

Fig. 9（b） 本体の姿勢角度θの時間応答

5.

結言

本研究では，最適ロバストサーボ制御法をコントローラに用いて，倒立 2 輪ロボットの倒立制御を試み，提案した制御手法がこの倒立問題に有効であるかを検討した。その結果，倒立 2 輪ロボットの安定化制御において，最適ロバストサーボ制御は有効であることが確認された。
参考文献●

Fig. 9（c） 本体の姿勢角速度θの時間応答

立 2 輪ロボットの安定化の実験結果について述べる。Fig. 9（a）は，移動距離の応答である。図より
車輪は 2 秒後にほぼ静止していることがわかる。
Fig. 9（b）は本体の姿勢角度 θ の時間応答である。
図より，姿勢角が 0［°］に漸近し安定していること
秋田高専研究紀要第46号

1）天野耀鴻“やさしいシステム制御工学”:，森北出版，（2008）2）青木貴伸，蔵本一峰，森口 肇，吉田郷弘“使:える！ MATLAB”，講談社，（2002）
3）木田 隆 :“フィードバック制御の基礎”，培風館，（2003）
4）小林一行“MATLAB:ハンドブック”，秀和シス
テム，（2008）
5）橋本洋志，他“Scilab:で学ぶシステム制御の基礎”，ホーム社，（2007）