ヨー角を考慮した車輪型倒立振子の姿勢制御
2009SE070 稲葉隼人 2009SE162 三枝俊輝
指導教員：陳幹表 1 物理定数

はじめに

1

本研究の目的は Vstone 株式会社から販売されている

2 車輪型の倒立振子型ロボット BeautoBalancerDuo(以下BBD) の倒立および旋回制御を目的とする. 倒立振子は重心が上にあるため不安定なシステムであり, 全体の角度を調整し倒れないように制御することが必要である. 倒立振子の姿勢制御はモデル化が容易なので, フィードバック制御の基本的な実験としてしばしば行われる. 今日では倒立振子を制御する技術の応用として, セグウェイなどの移動装置にその技術が使われている. 本研究では, 制御対象である BBD を空間的にモデリングし, 状態空間表現を求め,最適レギュレータ理論を用いて制御器を設計する. シミュレーション結果からおよび実験結果から本研究の評価をする.

記号

Rm
Kb
Kt
fm
fw
g
Rw
Mw
Mm
Mb
Jw
Jm
JbL

制御対象とモデリング

2

実験に用いる BBD は二つの車輪の上部にそれぞれモータが搭載されており, モータの回転を摩擦によって車輪にlGr

名前
モータの電気抵抗
モータの逆起電力定数
モータのトルク定数
モータと車体の間にある摩擦の摩擦係数
床と車体の間にある摩擦の摩擦係数
重力加速度
車輪の半径
車輪の質量
モータの質量
車体の質量
車輪の慣性モーメント
モータの慣性モーメント
車体の慣性モーメント
車輪の中心から車体の重心までの長さ
車輪の中心からモータ軸までの距離
モータと車輪とのギア比

値

単位0.6818[Ω]
0.0014[Vs/rad]0.0012[Nm/A]
4.9825×10−7 [ ]0[ ]9.8100[m/s2 ]
0.0207[m]
0.0053
[kg]
0.018
[kg]
0.1453[kg]
2.3373×10−6 [kgm2 ]9.375×10−7 [kgm2 ]2.4218×10−4 [kgm2 ]0.0520[m]
0.0217[m]

20.8[ ]伝達する仕組みを持っている. このロボットはロータリーエンコーダにより左右の車輪の回転角を, ジャイロセンサにより車体の角速度を得て, 倒立制御を実現している [2].
また BBD は 2 本の単三電池を主電源としており, 実験では 1.2[V] の電池を用いる. 右の図 1, 図 2 にその実験機のモデルを示す. また, Vstone 株式会社からオプションとして販売されているライントレース用基盤を実験機前方に接続する.

2.1

物理定数

BeautoBalancerDuo に関する物理定数 [3]. を下の表１にまとめた.

図 2 BeautoBalancerDuo の平面図

ir,l [mA] を DC モータの電流, vr,l [mV] を DC モータの電圧, Lm [H] を DC モータのインダクタンス, θ[rad] を左右車輪の平均回転角度, ψ[rad] を車体の傾斜角度 (ピッチ角度), ϕ[rad] を車体の平面回転角度 (ヨー角度) とする.
平均車輪回転角度θ, および平均車体回転角度ϕは以下のように表す.

図 1 BeautoBalancerDuo の側面図
図 1 および図 2 において, 座標に用いられる変数は以下

R
1(θ, ϕ) = ( (θr + θl ), (θr − θl ))
2
W

(1)

車軸中心の座標は以下のように表す.のように定義する.

∫

進行方向に垂直方向に x 軸, x 軸に垂直方向に y 軸 鉛直方向に z 軸をとる.

(xc , yc , zc ) = (∫ẋc dt,ẏc dt, Rw )(2)

車輪の座標は左右それぞれ以下のように表す.

WWsin ϕ, yc +cos ϕ, Rw ) (3)22WW(xr , yr , zr ) = (xc +sin ϕ, yc −cos ϕ, Rw ) (4)22(xl , yl , zl ) = (xc −2Fθ = ( (Mb + 2Mw + 2Mm )Rw+ 2Jw + 2G2r Jm )θ̈+ (Mb Rw L + 2Mm Rw l − 2G2r Jm )ψ̈

車体重心の座標は以下のように表す.

Fψ = ( Mb Rw L + 2Mm Rw l − 2G2r Jm )θ̈+ (Mb L2 + 2Mm l2 + Jψ + 2G2r Jm )ψ̈− (2Mm gl + Mb gL)ψ(xb , yb , zb ) =(5)(xc + L sin ψ cos ϕ, yc + L sin ψ sin ϕ, Rw + L cos ψ
2.2(14)(15)運動方程式の導出

運動方程式の導出には Lagrange の運動方程式を用いる.

Fϕ = ( (Mw + Mm +LagrangianL を次のように定義する.
L = (T1 + T2 ) − U+ Jϕ +(6)このとき, T1 は制御対象の並進運動のエネルギー, T2 は回転運動のエネルギー, U はポテンシャルエネルギーである.
次に一般化力 Fθ , Fψ , Fϕ は Lagrange の運動方程式を用

G2r W 2Jm )ϕ̈
22Rw

モータの電気的振る舞いについては次の式で表される.[1]Lm i̇l,r = vl,r + Kb (ψ̇ − θ̇l,r ) − Rm il,rT1 , T2 , U は以下のように表せる.
1T1 = Mb (ẋ2b + ẏb2 + żb2 )(7)
21+ Mw ((ẋ2l + ẏl2 + żl2 ) + (ẋ2r + ẏr2 + żr2 ))212222+ Mm ((ẋ2ml + ẏml+ żml) + (ẋ2mr + ẏmr+ żmr))2(8)(16)

またこれらの Fθ , Fψ , Fϕ はモータからの外力である.

いて求める.

111
1T2 = Jw θ̇l2 + Jw θ̇r2 + Jϕ ϕ̇2 + Jψ ψ̇ 22222
1 22+ Gr Jm ((θ̇l − ψ̇) + (θ̇r − ψ̇)2 )21J )W 22 w
2Rw(17)コイルのインダクタンス Lm は非常に小さな値とみなすことができるので, Lm = 0 とするとモータに流れる電流 il,rは以下のように表せる.

il,r =vl,r + Kb (ψ̇ − θ̇l,r )
Rm

(18)

(19) 式を (15), (16), (17) 式に代入することにより, 一般化力 Fθ , Fψ , Fϕ を導出する.
Gr Kt Kb
Gr Kt Kb+ fm + fw )θ̇ + 2(+ fm )ψ̇RmRm
Gr Kt+(vl + vr )(19)
RmFθ = − 2(U = Mw g(zr + zl ) + Mm g(zmr + zml ) + (Mb gzb ) (9)ラグラジアン L とラグランジュの運動方程式を用いて一般化力 Fθ , Fψ , Fϕ は次のように表せる.

[Gr Kt Kb
Gr Kt Kb+ fm )θ̇ − 2(+ fm )ψ̇RmRm
Gr Kt−(vl + vr )(20)
RmFψ = 2 (]∂Ld ∂L−dt ∂ θ̇∂θ[]d ∂L∂LFψ =−dt ∂ ψ̇∂ψ[]d ∂L
∂LFϕ =−dt ∂ ϕ̇∂ϕFθ =(10)
(11)
(12)

Fϕ =

W 2 Gr Kt Kb
Gr Kt W(vr − vl ) −(+ fm + fw )ϕ̇
2Rm Rw
2RW 2Rm(21)

2.3

状態空間表現

車体傾斜角度（ピッチ角）が十分小さいとしてψ → 0 の極限をとると, 以下のように近似できる.

sin ψ = ψcos ψ = 1 2ψ̇ = 0

2.2 で導出した式を以下のようにまとめる.
(13)一般化力 Fθ , Fψ , Fϕ の式の中に適用することで次のように表せる

E

[ ]
[ ]
[ ][ ]θ̈θ̇θv+F+G=H lψvr
ψ̈
ψ̇
[ ]v
S ϕ̈ + T ϕ̇ = V lvr

(22)

(23)

E, F, G, H, S, T, V は,[E=

e11e21

e12
e22

[]
[010
A2 =, B2 =0 −S −1 J−S −1 V]0],

S −1 VC2 = I2×2(35)(24)[x1 = θ222
e11 = (Mb Rw+ 2Mw Rw+ 2Mm Rw+ 2Jw + 2G2r Jm )e12 = ((Mb Rw L + 2Mm Rw l) − 2Jm G2r )e21 = ((Mb L + 2Mm l)Rw − 2Jm G2r )e22 = (Mb L2 + 2Mm l2 + Jb + 2Jm G2r )

ψθ̇

ψ̇

[x2 = ϕ]Tϕ̇u = [vlvr ]

]T

(36)

(37)

(25)
[2(α + fw )F =−2αβ=0G=03[βH=−βS = Mw W 2 + Mm W 2 +T =β−β制御器設計
本研究では, 最適レギュレータ理論 [4] を用いてコント

MATLAB を用いて計算した結果, 状態行列 A, 入力行
列 B は以下のようになった.

]
(27)

]
(28)

000 0A1 = 0 18.08800 92.4642

1001 −0.29840.2984 0.0915 −0.0915

W2G2r w2J+J+Jm (29)w
ϕ222Rw2RwW2(α + fw )22Rw(30)

Wβ
RM Rw

(31)

V =(38)

ローラの設計を行う.

Gr Kt
Rm

0−(2Mm gl + Mb gL)T(26)Gr Kt Kb+ fm
Rm

α=[−2α2α]00
B1 = 103.3650−31.7017[01A2 =0 −2.3031 × 10−7
[B2 =と表すことができる.
状態空間表現の行列 A, B, C, x は次のように定義さ00 103.3650 −31.7017

0−0.0801]]
00.0801

シミュレーションで試行錯誤的に重み行列 Q, R を決めゲインを求める. 重み行列は以下の値とした.

れる.

{x˙1 = A1 x1 + B1 u
y1 = Cx1(32)Q1 = diag [1000{x˙2 = A2 x2 + B2 u
y2 = Cx2(33)[Q2 = diag 5 × 104

行列 E, F, G, H を用いて行列 A, B, C は次のように定義される.

[A1 =O2×2−E −1 GC1 = I4×4][]
I2×2O2×2, B2 =,−1−1−E F
E H
(34)

1101] , R1 = diag [1500]
1 , R2 = diag [50001500](39)

5000] (40)

MATAB の lqr コマンドを使用して得られたコントロー
ルゲインが以下である.
[]−0.5774 −12.8239 −0.2122 −1.3028
K1 =−0.5774 −12.8239 −0.2122 −1.3028[]−2.2361 −5.2839
K2 =2.2361
5.2839図 3 倒立に関するブロック線図

図 6 倒立制御時の入力電圧

図 7 回転運動時の入力電圧

図 4 本体傾斜角6

おわりに
本研究では, 実験機の倒立・回転運動を考慮した 3 次元のモデリングを行った. 以上の結果より, 倒立制御は成功している. また得られたゲインを用いて平面回転運動を行うプログラムを実験機に実装することにより車体の旋回制御も同時に行える.
実験機前方につけたライントレース基盤のフォトトランジ
スタからの信号を用いてアルゴリズムを構築し実装を行えば, ライントレース走行等を実現できると考える.
図 5 本体回転角度4

シミュレーションπ
初期条件として x1 = [0 360 0], x2 = [ π6 0] を与えたときのシミュレーションを示す.
シミュレーション時間は 10[s] とした.
シミュレーションに用いたブロック線図は以下である.
これらのシミュレーション結果から, 倒立状態を維持した後に平面回転運動を行う事は制限電圧内で実現可能という事がわかった.

5

実験結果得られた倒立に関するゲインを実験機に実装した結果,1 分間以上の倒立ができた.

参考文献[1] NXTway-GS のモデルベース開発〜LEGO Mindstorms NXT を用いた平行二輪倒立振子型ロボットの制御〜,The Math Works,Inc(2009)http://www.mathworks.com/matlabcentral/\\fileexchange/13399[2] ヴィストン:『H8 マイコンによる組み込みプログラミング入門』, オーム社 (2009)[3] マブチモーター株式会社, 製品名 FA-130RA 製品
情報 http://www.mabuchi-motor.co.jp/cgi-bin/search/j_model.cgi[4] 川田昌克『: MATLAB/Simulink による現代制御入門』,
東京, 森北出版株式会社 (2011)