平行二輪型倒立振子の自己位置推定によるライントレース
2008MI050 堀昌太郎

2009SE068 今井祥也

2009SE098 狩谷昇吾

指導教員：陳幹
タイムを競う大会である. コンテストの内容には, モデル

1 はじめに
1.1

ライントレースの概要

本研究の目的は, 光センサを一つ搭載した平行二輪型倒立振子ロボットを用いてライントレースを行うことである. ライントレースとは, 床に描かれたラインを追従しながら走行することである. 光センサを用いたライントレー
スでは, 光センサの輝度を使って路面の色を判断し, 現在走行している場所の色を調べ, 色の濃淡を感知することで位置を検出し, ラインの縁に沿って進むように走行体の向きを決定する. ライントレースの方法について, 図 1 に示すような走行体の中央部に光センサを一つ装着した片側エッジ検出型のライントレースを行う.

図1

部門と競技部門があり, 競技部門でライントレースにより走行タイムを競うベーシックエリアと階段, シーソー,ガレージインなどの障害物を攻略する難所エリアがある.
本研究では, ベーシックエリアの完走を目指し, ライントレースでの安定化と高速化を実現する制御を目指す. 制御
方法は倒立制御は最適レギュレータを使用されており, ライントレースは PID 制御を使用する [1].

2 制御対象本研究では, 図 2 に示すような二輪型倒立振子ロボットを制御対象とする.

片側ライントレース

図2平行二輪型倒立振子ロボット

ラインを境に進行方向左が白地, 右が黒地のとき, 光センサが白地の反応を示した場合, 黒色ラインから離れていハードウェアは, 光センサが 1 個, インタラクティブるため右旋回を行い黒色ラインへと近づく. その結果, 光サーボモータ 3 個, インテリジェントブロック NXT を 1

センサが黒地の反応を示すので倒立振子がライン上に来個使用する. 以降, インタラクティブサーボモータを DCたことが分かり, 次に左旋回を行うことにより白地へと向モータ, インテリジェントブロック NXT を NXT と称す

かう. これらの動作を繰り返し行うことにより, 走行体はる.

左右に振れながらラインに沿って倒立走行をすることができる.

1.2

ET ソフトウェアデザインロボットコンテスト

ET ソフトウェアデザインロボットコンテストとは, 社

3 モデリング
3.1 状態方程式の導出
平行二輪型倒立振子の側面図と平面図を図 1 と図 2 に団法人組込みシステム技術協会の開催している, LEGO示す.

MINDSTORMS NXT で組み上げられた同型のロボット

ここで, ラインに対して垂直方向を x 軸, ｘ 軸に対して垂を使用し, そのロボットに搭載するソフトウェアでコース直方向を y 軸, 鉛直方向を ｚ 軸とする.

また, ir,l [mA] を DC モータの電流, vr,l [mV] を DC
モータの電圧, Lm [H] を DC モータのインダクタンス, θ[deg] を左右車輪の平均回転角度, ψ [deg] を車体の傾斜角度 (ピッチ角度), φ [deg] を車体の平面回転角度 (ヨー角度) とする.

4 システム同定
本研究で使用する平行二輪型倒立振子ロボットは光セ
図3ンサから値を取得する. そしてその値からどの方向へ移動

平行二輪型倒立振子の側面図するかを計算し, モータを回すため NXT の電圧として出力を行う. その後, 電圧を入力として車輪を動かすことによって移動を行う.

1. 光センサの値を受け, 現在ラインからどれくらいの距離にいるのかという光センサ値と距離との関係

2. 位置情報から PID 制御を用いることにより, NXT がどれだけ電圧を出力するかの関係

3. NXT からの電圧によって回転する平行二輪型倒立振子の車輪の回転角度の関係

4. 回転角度から自己位置推定を用いてどの方向へどの図4ように移動するかの関係

平行二輪型倒立振子の平面図

1 から 4 についてシステム同定を行う.
制御対象となる平行二輪型倒立振子のモデリングは以下のようになる. 運動方程式を導出するにあたり物理パラメータを表 1 にまとめた [2].

4.1 光センサ値と距離との関係
実験機を作成し, 光センサの値と距離について実験を行った.

表1実験の手順は以下のとおりである.

物理パラメータ

記号

名前値

gmRMWHL

重力加速度9.81[m/sec2 ]0.03[kg]
0.04[m]0.6[kg]0.14[m]0.144[m]H/2[m]

車輪質量
車輪半径
車体質量
車体幅
車体高さ
車輪中心から車体重心ま1. 黒地と白地との境界を閾値とし, その境界を 0 とする.
2. 黒地と白地の境界を基準に 1.0[mm] ごとに光センサの位置を動かし, 光センサの値を測定する.
結果, 光センサの値とラインからの距離には相関関係があることが分かった. 光センサの値は 530 から 745 の値をとり, 黒地と白地の境界の閾値を測定したところ 644 が得での距離
222Jφ

車体慣性モーメント

M (w + D )/12[kgm ]Jm(ヨー)
DC モータ慣性モーメン

1 × 10−5 [gm2 ]考える. その後, 閾値よりも白地はマイナス表記とし, 閾トRm
KbKtnfm

DC モータ抵抗
DC モータ逆起電力定数
DC モータトルク定数
ギヤレシオ
車体と DC モータ間の6.69[Ω]0.468[V sec/rad]0.317[Nm/A]
10.0022

摩擦係数fw

車輪と路面間の摩擦係数られた. この光センサの値 644 が基準値となり距離 0 と0値よりも黒地はプラス表記として距離をあらわす. 距離はそれぞれ 1.0[mm] 間隔で測定を行った. そのときの光センサと距離との関係を表 2 と図 5 に示す.

v(t) を表現した. 結果, 式 (3) が得られる.

表 2 光センサ値と距離との関係

基準からの光センサ

基準からの光センサ

距離 [mm]値

距離 [mm]値−20534−2613−19534−1628−185340644−17535

1658−165402678−15541

3695−14541

4711−13541

5722−12542

6732−11543

7736

次に, 式 (5) に導出した微分方程式を PID 制御のプログ−105478738

ラムに組み込むため, 状態空間表現に変換する. ここで状−95509739

態量 x, および入力 u は式 (6) を使用する.

−8556

10740

−7561

11740

−6572

12740

状態方程式は式 (7) から式 (11) となる.

−5580

13741ẋ(t) = Ax(t) + Bu(t)

(7)

−4592

14741

−315742y(t) = Cx(t)[]01
A=K30 − Jm Rt m(8)601v(t) = Li̇(t) + Ri(t) + ke θ̇(t)(3)

ここで, L はコイルのインダクタンス, R は抵抗器の抵抗,i(t) はモータを流れる電流, kt はトルク定数である. 次にモータの回転運動に関する微分方程式が式 (4) で表わされる.

Jm θ̈(t) = kt i(t)(4)

以上の式 (3), 式 (4) と L を 0 として考えると電圧 v(t) と車輪の回転角度 θ(t) の微分方程式が導出できる.

v(t) = JmkR2 m θ̈(t) + kt θ̇(t)(5)

tx(t) = [θ, θ̇]T , u = v[B=0(6)(9)]Kt2
Jm R m[]
C= 1 0(10)(11)

4.3 自己位置推定
自己位置推定とはロボットの左右の車輪の回転角と回転角速度を積算することで, 移動距離と方向を求める方法
である.
はじめに, 車輪の移動量から平行二輪型倒立振子の位置を推定するため, 車両の運動を考える. 旋回角速度を ω, 旋

図 5 光センサ値と距離の関係図

回角度を ψ, 回転半径を ρ, 左右の車輪の周速度を Vl,r , 光
結果, グラフの光センサ値が負の時に式 (1) を, グラフの光センサ値が正の時に式 (2) を得た.

4.2

y = 0.10891x(1)

y = 0.07669x(2)

電圧と車輪の回転量の関係

平行二輪型倒立振子に搭載している車輪のモータはNXT からの電圧 v(t) を入力, モータの回転による車輪のセンサ部の移動速度を V とすると, 式 (12) から式 (16) を得る.

l)
ω = (Vr −V
2(12)

r)
v = (Vl +V2(13)(Vr +Vl )ρ = d (Vr −Vl )(14)ẋ = V cos(ϕ)(15)

ẏ = V sin(ϕ)(16)

回転量 θ を出力としている. モータの電気的特性からモー

次に, 制御周期を ∆t とした運動を考える. ∆t 間の車輪とタを LR 直列回路として近似し, モータに与えられた電圧

光センサの移動を図 6 に示す.

旋回角度について式 (25) であらわされる.

θi+1 = θi + ∆θ(25)また, 光センサの位置について, 式 (26), 式 (27) を計算することで ∆t の間の円弧運動を考えることができる.

xi+1 = xi + ∆L cos(θi + ∆ θ2 )(26)

yi+1 = yi + ∆L sin(θi + ∆ θ2 )(27)以上の計算を時々刻々と重ねることで車輪の回転角度か
図 6 ∆t 間の車輪と光センサの移動

ら平行二輪型倒立振子の位置と旋回角度を得ることができる [3][4].

また, 時間 ∆t で左右の車輪および光センサの移動量をそれぞれ ∆LL , ∆LR , ∆L とする. 結果, 式 (17), 式 (18)を得る.

5 シミュレーション
光センサの値を入力した場合のライントレースのシ∆LL = Vl ∆t(17)∆LR = Vr ∆t(18)この ∆t の間, 車輪および光センサの移動速度は変化しないものとし, 一定の半径 ρ の円弧運動を行うとする. ∆tで旋回中心周りに ∆ϕ だけ回転したとすると, 式 (17), 式(18) より, 式 (19) から式 (24) を得る.

ミュレーション結果を示す. ここで前章で示した自己位置推定は離散系を用いている. そのため, システム同定を行って調べた関係それぞれを離散系で考えシミュレーションを行った. ここで, 光センサの値の入力, 電圧を出力とした PID 制御を使用し NXT の最大出力電圧に収まるようなコントローラの作成を行った. また, ライント
レースに関しては右旋回, 左旋回の両方を, 電圧に関しては右旋回のシミュレーションを行った.

∆LR = (ρ + W
2 )∆ϕ(19)∆LR = (ρ − W2 )∆ϕ(20)∆L = ρ∆ϕ(21)L∆L = ∆LR +∆L2(22)−∆LL∆θ = ∆LRW(23)ρ = ∆L − ∆ϕ(24)5.1 電圧
5.1.1

PID 制御位置情報を取得し, その結果左右のモータがどれだけの電圧を出力するかを決定するために PID 制御のコントローラを作成した. ライントレースにおける P 制御, I 制
御, D 制御はそれぞれ以下のような役割がある.

P 制御 : 光センサ値の目標値との差を操作量として反映

次に, 光センサの移動量を算出する. 平行二輪型倒立振子する.の位置, 旋回角度を i 回目の制御周期のとき, xi , yi , ϕi , 次I 制御 : P 制御による操作量出力の今回値と前回値との差の周期を xi+1 , yi+1 , ϕi+1 と表記する. このときの状態を分を操作量として反映し, オフセット値の補正をする.

図 7 に示す.

D 制御 : 光センサ値の目標値との差の前回値と今回値の差分を操作量として反映する.
以上から電圧の出力制御を行うため, PID 制御のゲインをKp = 0.1, Ki = 0.001, Kd = 0.7 と定めた.
5.1.2 電圧のシミュレーション
左右の車輪のモータにかかる電圧が NXT の最大出力電圧 9[V] 以内におさまっているかを見る. そこで PID 制
御を用いてシミュレーションを行った. その結果を図 8 と図 9 に示す.
図7

座標の移動

値が 544, 569, 594, 619 を表わし, このときのラインからの距離 -1.08, -0.81, -0.54, -0.27 がそれぞれ光センサの初期位置となる.
図 11 はラインの境界の光センサ値 644 から値を 25 ずつ

図8右モータにかかる電圧

図 11

左旋回のライントレース増やした値を走行体の初期位置としている. 左から光セン
サ値が 669, 694, 719, 744, 769 を表わし, このときのラインからの距離 0.19, 0.38, 0.57, 0.76, 0.95 がそれぞれ初期位置となる. 図 10, 図 11 から, どの軌跡も円弧を描き目標値までトレースしていることがわかる.

図9左モータにかかる電圧

6 実験結果実験機の平行二輪型倒立振子にライントレースさせた

図 8, 図 9 より, 右モータと左モータともに 9[V] 以内に収まってることが分かる.

時の実験結果を示す. はじめに, 平行二輪型倒立振子を直線のラインを約 18 秒間トレースさせた時の様子を図 10に示す.

5.2ライントレース

図 10 から, 光センサ値と位置との関係からラインの境界

図 10 と図 11 は二輪型倒立振子ロボットがある距離か値 0[mm] を基準として白地に対して 4.0[mm], 黒地に対ら目標値までライントレースを行う様子を示したシミュ

して 2.0[mm] の範囲でライントレースを行っていることレーション結果である.

がわかる.

図 10 はラインの境界の光センサ値 644 から値を 25 ずつ

図 10 右旋回のライントレース減らした値を走行体の初期位置としている. 左か光センサ

図 12

ライントレースの軌道次にシミュレーションでは図 12 に示すような, ラインの境界 0[mm] に追従するグラフを得ることができなかった. そこで, 図 12 の範囲内でラインからの距離が白地, 黒地それぞれ最大となる位置について着目する. その最大となる位置からラインの境界までについて白地から境界, 黒地から境界, それぞれについてシミュレーションとの比較を行った. その結果を図 13, 図 14 に示す. ここでライントレースについて白地から境界のラインへは右旋回, 黒地から境界のラインへは左旋回となる.

7 おわりに本研究では, 光センサをひとつ搭載した平行二輪型倒立振子ロボットのライントレースについて考えた. 光セン
サの値だけでは直接ライントレースを行うことができなかったため, 光センサ値からライントレースへとつなげるために 4 つのシステム同定を行った. この 4 つのシステ
ム同定を行うことにより, 光センサ値とラインの境界からの距離との関係を導出でき, 離散系を用いて PID 制御によるライントレースのシミュレーションを行うことができた.
以上の結果より, NXT の制限電圧内に収めたうえでラインに追従するライントレースの軌道をシミュレーションすることができたといえる. またその後の実験では, 始点と終点がほぼ同じになるライントレースの軌道を描くことができた. しかし, シミュレーションと実験では楕円的,直線的なライントレースの軌道の違いが生まれてしまった. 今後の課題として, 二輪型倒立振子ロボットへのプログラムの検証, およびデータ取得法の検討があげられる.

参考文献
図 13

右旋回 (白地から境界) の比較[1] ET ロボコン 2012 技術委員会:ET ソフトウェアデザ
インロボットコンテスト 2012 競技規約

[2] NXTway-GS のモデルベース開発〜LEGO Mindstorms NXT を 用 い た 平 行 二
輪 倒 立 振 子 型 ロ ボ ッ ト の 制 御 〜,The MathWorks,Inc(2009)http://www.mathworks.com/matlabcentral/ﬁleexchange/13399[3] D.Soetanto,L.Lapierre,A.Pascoal:Adaptive,NonSinglar

Path-Following

ControlofDynamic

Wheeled Robots.
http://www2.lirmm.fr/home/lapierre/IMG/pdf/CDC
図 14

左旋回 (黒地から境界) の比較[4] 熊谷正朗:車輪移動ロボット,(2011)
http://www.mech.tohokugakuin.ac.jp/rde/contents/course/robotics/

図 13, 図 14 はそれぞれ光センサの値 605, 690 を初期値とし, 右旋回に 0.476[s], 左旋回に 0.256[s] の時間を要している.
ライントレースの軌道はシミュレーションとは異なり, ほぼ直線的に目標値へと向かうが, 目標値での距離の差はシミュレーションと 0.2[mm] 以下の誤差となっている.
よって, シミュレーションのように楕円軌道は描いていな
いが, 直線のラインをトレースすることができ, 大きな支障はないと考えられる.

wheelrobot.html