まえがき
rogy Advent Calendar 2015，20日目は ざくろくんの Dynamorphism 〜 Haskellでも動的計画法がしたい！ 〜 でした． DPなのに再帰的な手法によって計算されるって，よくあるメモ化再帰の計算オーダ改善としてDPが出てくる話の流れに馴染みがある私からすれば「あれ，話が逆なんじゃないかな」と最初思ったのですが，後で詳しく読んでみます． 今回は毛色が変わってロボットの制御に関する話です．
概要
マイクロマウスは，小型ロボットが迷路を走行するスピードを競う競技で，35年以上前から続く長寿大会の一つです．
マイクロマウスで多く用いられるのは，2輪や4輪の台車型の構造をした車両ロボットで，自動車と同様に真横に動くことができないような数学的に特殊な構造を有しています． 真横に動けないという構造的な制約によって多くの困難が生じるのですが，ほとんどの競技者はこの困難を経験的に獲得した手法によって解決していることが多いように見受けられます． 一方で，このような構造を持つ系に対する制御理論は80年代ごろから盛んに研究されていますが，数学的側面から考察されているものが多数であり，実用面について弱いという指摘がよくなされます．
その中で，今回はマイクロマウスという題材に絞って，どのように制御理論が活用できるかについて実用上の観点から考察したいと思います． 分量が多くなりそうなので，今回は前半をAdvent Calendarの一環として書き，後半を12/26に公開する予定です．
目標
第1回(12/21)
- 2輪台車の数学モデルについて紹介する
- 迷路の区画の真ん中を走るための「壁(トレース)制御」について解析する
第2回(12/29)
- 自由度と制御できる変数の数，劣駆動性と非ホロノミック拘束について解説する
- 絶対運動と相対運動のどちらを制御するかによる違いを説明する
結論を先に示すと
- 壁との距離を制御するという考え方では原理的に安定化できない
- 相対運動のつなぎ合わせで走行軌跡を考えるオーソドックスな手法は，制御理論の観点からもある意味で合理的である
もくじ
モデリング
まずはじめに，2輪台車の数式によるモデリングを行います． 簡単のため，車体の幅や車輪の径などの物理的定数はすべて1とします．
ロボットの運動を記述する際に，速度に注目するか加速度に注目するかによって制御の方針も変わってくるので，この2パターンに分けて考えることにします．
速度・角速度入力の場合
速度制御や角速度制御は簡単な制御則(PID補償など)で実現できるため，速度・角速度をベースに機体の運動を考えるのが典型的なやり方です．
ロボットの状態を $$ q_v = \left[ x\ y\ \theta \right]^T $$ 入力を $$ u_v = \left[ v\ \omega \right]^T $$ とすると， $$ \dot q_v = \begin{bmatrix} v\cos\theta\\ v\sin\theta\\ \omega \end{bmatrix} = \begin{bmatrix} \cos\theta & 0\\ \sin\theta & 0\\ 0 & 1 \end{bmatrix} u_v $$
加速度・角加速度入力の場合
加速度や角加速度は，センシングや制御周期の関係で制御を厳密に行うことはなかなか難しいのですが，モータのトルクと回転数を制御することで間接的に(近似的に)制御することができます． これをベースに考えるとつぎのようになります (ただし，滑りは全く無いものと仮定しています．それによって少し不自然な形になっています)．
ロボットの状態を $$ q_a = \left[ x\ y\ \theta\ v\ \dot \theta \right]^T $$ 入力を $$ u_a = \left[ a\ \dot \omega \right]^T $$ とすると， $$ \dot q_a = \begin{bmatrix} v \cos\theta\\ v \sin\theta\\ \dot \theta\\ a\\ \dot \omega \end{bmatrix} = \begin{bmatrix} v \cos\theta\\ v \sin\theta\\ \dot\theta\\ 0\\ 0 \end{bmatrix} + \begin{bmatrix} 0 & 0\\ 0 & 0\\ 0 & 0\\ 1 & 0\\ 0 & 1 \end{bmatrix} u_a $$
速度入力の場合とは違って，入力 \(u_a\) に依存しない項が出現していることに注意してください．
壁トレース制御
マイクロマウスにおいて基本であり最も厄介とされるのが壁トレース制御です． 設計が直感的に理解しやすい方法がなかなかないため，どの観点からアプローチするかによって方法ががらっと変わってしまいます．
例えば，壁トレース制御 | マイクロマウス委員会 関西支部 では，壁との距離と機体の角度によって定まるセンサ値を用いて制御することで，原点(ここでいうと\(y=\theta=\dot y = \dot\theta=0\))が(少なくともその近傍において)安定となることを示しています．
このように，良い方法が理論的に示されていることはよくあるのですが，やってもほぼ成功しないようなアンチパターンについて解説されている例はあまり見かけません． そこで今回は，壁との距離を制御するという考え方では安定化ができないことと，解決方法として距離の微分や角度を考慮することによって安定化できるということを示します． 以下の議論では，機体の直進速度は一定(\(v = const., a = 0\))として，関係のない \(x\) 方向の運動については式中から外して考えます．
比例制御の場合
まず，速度入力の場合について考えます．
\(y, \theta\) 方向の運動のみを記述した状態方程式は $$ \dot{\bar{q}}_v = \begin{bmatrix} v\sin\theta\\ \omega \end{bmatrix} $$ となる． これに対して正の定数 \(K\) を用いて，状態フィードバック \(\omega = -Ky\) を適用すると， $$ \dot{\bar{q}}_v = \begin{bmatrix} v\sin\theta\\ -Ky \end{bmatrix} $$ 原点 \(y=\theta=0\) の近傍での安定性を調べる．原点まわりでテイラー展開すると， $$ \dot{\bar{q}}_v \approx \begin{bmatrix} v\theta\\ -Ky \end{bmatrix} = \begin{bmatrix} 0 & v\\ -K & 0 \end{bmatrix} \bar{q}_v $$ なる線形近似システムが得られる．式中の 2x2 行列の固有値は $$ \lambda_1 = i\sqrt{Kv}\\ \lambda_2 = -i\sqrt{Kv} $$ であるため，固有値の実部が負ではないことから，線形近似システムの原点は漸近安定ではない． したがって，もとの非線形システムの原点はその近傍において漸近安定ではない．
この結果から，壁との距離に比例した角速度を入れても安定化ができないことがわかります． また，固有値の実部が0であることから，\(K\) の値によらず持続的に振動を続けることが予想できます． \(K=v=1\), \(y\) の初期状態を 0.1 としてシミュレーションを行うと，つぎの図のようになります．
同様に，加速度入力の場合について考えると， $$ \dot{\bar{q}}_a \approx \begin{bmatrix} 0 & v & 0 & 0\\ 0 & 0 & 0 & 1\\ 0 & 0 & 0 & 0\\ -K & 0 & 0 & 0 \end{bmatrix} \bar{q}_a $$ 例えば \(K=v=1\) のときの固有値は $$ \lambda_1 = 0\\ \lambda_2 = -1\\ \lambda_3 \approx 0.5 - 0.87i\\ \lambda_4 \approx 0.5 + 0.87i $$ となり，固有値の実部が正になるものが表れます(0以外のどんな定数をもってきても必ず1つは固有値の実部が正になります)．
以上から，壁との距離を単純に定数倍して角速度にフィードバックする方法ではうまく制御できないことがわかりました． 実はこの事実については経験的にわかっていることでもあります．以前Twitterでkatoさんが以下のようなツイートをされていました．
マイクロマウスの直進時の制御で「壁との距離を制御する」という考え方は上手くいかない。「壁から遠ざかる方向に少し機体を傾ける」が正しい。
— kato (@robolaboN) 2014, 7月 26それでは，どのようにすればうまく安定化ができるのでしょうか？ 1つの方法は，壁トレース制御 | マイクロマウス委員会 関西支部のように，壁との距離と機体の角度の2変数によって定まる値を使って制御する方法です． 本文を読むとあたかも難しそうに書いていますが，(間接的にではありますが)角度を用いることで比例制御で一応安定化できるというシンプルなお話です． 確かにそうなのですが，外乱や急激な変化に対するリジェクションがうまくできずに，安定性が保証されている原点近傍外に飛び出してしまう可能性は否定できません． そこで，少しでもそれを改善できるように，比例§制御ではなくPD補償を行うことでどのような変化が起こるのか調べることにします．
PD補償の場合
PID補償をご存知のかたは多いことと思いますが，よく「D項はほとんどいらない・使わない」と言って回っている人を見かけます．これは正しくありません． 例えるなら「サスペンションにはダンパは要らない」と言っているのと同じことです．時と場合によって必要なものは変わります． ライントレースや，今回の話題である壁トレース制御がその好例です．
速度入力の場合について考えます．
先ほどの状態方程式に対して正の定数 \(K_p, K_d\) を用いて，状態フィードバック \(\omega = -K_p y -K_d \dot y\) を適用すると， $$ \dot{\bar{q}}_v = \begin{bmatrix} v\sin\theta\\ -K_p y - K_d v \sin\theta \end{bmatrix} $$ 原点 \(y=\theta=0\) の近傍での安定性を調べる．原点まわりでテイラー展開すると， $$ \dot{\bar{q}}_v \approx \begin{bmatrix} v\theta\\ -Ky \end{bmatrix} = \begin{bmatrix} 0 & v\\ -K_p & -K_d v \end{bmatrix} \bar{q}_v $$ なる線形近似システムが得られる．式中の 2x2 行列の固有値は $$ \lambda_1 = \frac{-K_d v + \sqrt{K_d^2v^2-4K_p}}{2}\\ \lambda_2 = \frac{-K_d v - \sqrt{K_d^2v^2-4K_p}}{2} $$ であるため，固有値の実部が負となり，線形近似システムの原点は指数安定である． したがって，もとの非線形システムの原点はその近傍において指数安定である．
\(K_p=K_d=1\) のときのシミュレーション結果を以下に示します．
これは一例として挙げたまでですので，律儀に \(v\) や \(\sin \theta\) を使ってフィードバックしなくても，単純に \(-k \theta\) とかでフィードバックをかけてもうまく行くはずです(関西支部版と違うポイントは直接角度情報を使用することです．ジャイロセンサ等で比較的正確に取れますので)． 加速度入力の場合も基本的に同じようなフィードバック制御則を組んで安定性を示すことができます．スペースの都合上割愛します．
まとめ
- 壁との距離のみを制御するという考え方では安定化できないことを理論的に示した
- 機体の角度や壁に近づく速度(微分に関する項)を使うことで安定化が可能であることを示した
参考文献
- 壁トレース制御 | マイクロマウス委員会 関西支部
- R. Murray, S. Sastry and Z. Li, “A Mathematical Introduction to Robotic Manipulation”. CRC Press, 1994.
- R. Brockett, R. Millman and H. Sussman, "Differential Geometric Control Theory”. Birkhauser, 1983, pp. 181–208.