[rogy Advent Calendar 2015] マイクロマウスと制御理論 (1)壁トレース制御

rogy Advent Calendar 2015，20日目はざくろくんの Dynamorphism 〜 Haskellでも動的計画法がしたい！〜でした． DPなのに再帰的な手法によって計算されるって，よくあるメモ化再帰の計算オーダ改善としてDPが出てくる話の流れに馴染みがある私からすれば「あれ，話が逆なんじゃないかな」と最初思ったのですが，後で詳しく読んでみます．今回は毛色が変わってロボットの制御に関する話です．

概要

マイクロマウスで多く用いられるのは，2輪や4輪の台車型の構造をした車両ロボットで，自動車と同様に真横に動くことができないような数学的に特殊な構造を有しています．真横に動けないという構造的な制約によって多くの困難が生じるのですが，ほとんどの競技者はこの困難を経験的に獲得した手法によって解決していることが多いように見受けられます．一方で，このような構造を持つ系に対する制御理論は80年代ごろから盛んに研究されていますが，数学的側面から考察されているものが多数であり，実用面について弱いという指摘がよくなされます．

その中で，今回はマイクロマウスという題材に絞って，どのように制御理論が活用できるかについて実用上の観点から考察したいと思います．分量が多くなりそうなので，今回は前半をAdvent Calendarの一環として書き，後半を12/26に公開する予定です．

目標第1回(12/21)

2輪台車の数学モデルについて紹介する
迷路の区画の真ん中を走るための「壁(トレース)制御」について解析する

第2回(12/29)

自由度と制御できる変数の数，劣駆動性と非ホロノミック拘束について解説する
絶対運動と相対運動のどちらを制御するかによる違いを説明する

結論を先に示すと

壁との距離を制御するという考え方では原理的に安定化できない
相対運動のつなぎ合わせで走行軌跡を考えるオーソドックスな手法は，制御理論の観点からもある意味で合理的である

モデリング

速度・角速度入力の場合

ロボットの状態を $$ q_v = \left[ x\ y\ \theta \right]^T $$ 入力を $$ u_v = \left[ v\ \omega \right]^T $$ とすると， $$ \dot q_v = \begin v\cos\theta\\ v\sin\theta\\ \omega \end = \begin \cos\theta & 0\\ \sin\theta & 0\\ 0 & 1 \end 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 v \cos\theta\\ v \sin\theta\\ \dot \theta\\ a\\ \dot \omega \end = \begin v \cos\theta\\ v \sin\theta\\ \dot\theta\\ 0\\ 0 \end + \begin 0 & 0\\ 0 & 0\\ 0 & 0\\ 1 & 0\\ 0 & 1 \end u_a $$

速度入力の場合とは違って，入力 $u_a$ に依存しない項が出現していることに注意してください．

壁トレース制御

例えば，壁トレース制御 | マイクロマウス委員会関西支部では，壁との距離と機体の角度によって定まるセンサ値を用いて制御することで，原点(ここでいうと$y=\theta=\dot y = \dot\theta=0$)が(少なくともその近傍において)安定となることを示しています．

このように，良い方法が理論的に示されていることはよくあるのですが，やってもほぼ成功しないようなアンチパターンについて解説されている例はあまり見かけません．そこで今回は，壁との距離を制御するという考え方では安定化ができないことと，解決方法として距離の微分や角度を考慮することによって安定化できるということを示します．以下の議論では，機体の直進速度は一定($v = const., a = 0$)として，関係のない $x$ 方向の運動については式中から外して考えます．

比例制御の場合

$y, \theta$ 方向の運動のみを記述した状態方程式は $$ \dot>_v = \begin v\sin\theta\\ \omega \end $$ となる．これに対して正の定数 $K$ を用いて，状態フィードバック $\omega = -Ky$ を適用すると， $$ \dot>_v = \begin v\sin\theta\\ -Ky \end $$ 原点 $y=\theta=0$ の近傍での安定性を調べる．原点まわりでテイラー展開すると， $$ \dot>_v \approx \begin v\theta\\ -Ky \end = \begin 0 & v\\ -K & 0 \end \bar_v $$ なる線形近似システムが得られる．式中の 2x2 行列の固有値は $$ \lambda_1 = i\sqrt\\ \lambda_2 = -i\sqrt $$ であるため，固有値の実部が負ではないことから，線形近似システムの原点は漸近安定ではない．したがって，もとの非線形システムの原点はその近傍において漸近安定ではない．

この結果から，壁との距離に比例した角速度を入れても安定化ができないことがわかります．また，固有値の実部が0であることから，$K$ の値によらず持続的に振動を続けることが予想できます． $K=v=1$, $y$ の初期状態を 0.1 としてシミュレーションを行うと，つぎの図のようになります．

同様に，加速度入力の場合について考えると， $$ \dot>_a \approx \begin 0 & v & 0 & 0\\ 0 & 0 & 0 & 1\\ 0 & 0 & 0 & 0\\ -K & 0 & 0 & 0 \end \bar_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つは固有値の実部が正になります)．

それでは，どのようにすればうまく安定化ができるのでしょうか？ 1つの方法は，壁トレース制御 | マイクロマウス委員会関西支部のように，壁との距離と機体の角度の2変数によって定まる値を使って制御する方法です．本文を読むとあたかも難しそうに書いていますが，(間接的にではありますが)角度を用いることで比例制御で一応安定化できるというシンプルなお話です．確かにそうなのですが，外乱や急激な変化に対するリジェクションがうまくできずに，安定性が保証されている原点近傍外に飛び出してしまう可能性は否定できません．そこで，少しでもそれを改善できるように，比例§制御ではなくPD補償を行うことでどのような変化が起こるのか調べることにします．

PD補償の場合

PID補償をご存知のかたは多いことと思いますが，よく「D項はほとんどいらない・使わない」と言って回っている人を見かけます．これは正しくありません．例えるなら「サスペンションにはダンパは要らない」と言っているのと同じことです．時と場合によって必要なものは変わります．ライントレースや，今回の話題である壁トレース制御がその好例です．

先ほどの状態方程式に対して正の定数 $K_p, K_d$ を用いて，状態フィードバック $\omega = -K_p y -K_d \dot y$ を適用すると， $$ \dot>_v = \begin v\sin\theta\\ -K_p y - K_d v \sin\theta \end $$ 原点 $y=\theta=0$ の近傍での安定性を調べる．原点まわりでテイラー展開すると， $$ \dot>_v \approx \begin v\theta\\ -Ky \end = \begin 0 & v\\ -K_p & -K_d v \end \bar_v $$ なる線形近似システムが得られる．式中の 2x2 行列の固有値は $$ \lambda_1 = \frac>\\ \lambda_2 = \frac> $$ であるため，固有値の実部が負となり，線形近似システムの原点は指数安定である．したがって，もとの非線形システムの原点はその近傍において指数安定である．

これは一例として挙げたまでですので，律儀に $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.

Ads recent categories tag cloud archives

January 2021 1
December 2020 1
December 2019 1
December 2018 2
November 2018 2
August 2018 6
July 2018 1
June 2018 1
May 2018 1
March 2018 2
January 2018 1
December 2017 2
November 2017 2
October 2017 2
August 2017 2
July 2017 1
June 2017 1
May 2017 2
December 2016 2
November 2016 2
October 2016 1
September 2016 1
August 2016 2
June 2016 1
April 2016 2
February 2016 1
January 2016 1
December 2015 13
November 2015 8
September 2015 1
August 2015 4
May 2015 18

📎📎📎📎📎📎📎📎📎📎

[rogy Advent Calendar 2015] マイクロマウスと制御理論 (1)壁トレース制御

概要

もくじ

モデリング

壁トレース制御

まとめ

参考文献