壁トレース制御


マイクロマウスロボットでは,機体の進行方向に対して斜め向けに光センサーが配置されることが多い.そして,マウスを迷路の真ん中に置いたときのセンサー値を一定に保つようにP制御をかけることで,迷路の真ん中を走るという制御がされている.

光センサーという計測器の物理的なダイナミクスは非線形なものであり(対数近似できることは知られてはいますが),ブラックボックス状態である.なぜ,ブラックボックスな計測器を使って,迷路の真ん中を走ることが出来ているのかを解析する.

物理モデル

以下のロボットを考える.

  • 斜めにセンサーを配置している;20度センサー,45度センサーなど,2種類のセンサーを持っている.
  • 直進速度一定

すると,

  • センサーは局所的に単調増加な計測器とみなせる

これを分かりやすいようにそのうち書く.

ロボットは速度入力,直進速度は等速の場合

定式化

以下のようにモデル化されるX:=\mathbb{R}\times S^1\displaystyle上のシステムを考える.

(1)    \begin{align*} \begin{bmatrix} \dot{y}\\ \dot{\theta} \end{bmatrix} = \begin{bmatrix} v \sin\theta\\ \omega \end{bmatrix} \end{align*}

ただし,(y,\theta )\in X\displaystyleはそれぞれ位置,姿勢角であり,v,\omega \displaystyleは制御入力である直進速度,角速度である.v>0\displaystyleは定数とする.このシステムは以下のような条件を満たす計測システムm:\mathbb{R}^2 \to \mathbb{R} \displaystyleを持っているとする.

  • m\displaystyleC^1\displaystyle級でy,\theta\displaystyleに関して単調増加.

制御則の構築

前節で取り上げたシステムを考える.また,状態フィードバック則として,

(2)    \begin{align*} \omega = -k(m(y,\theta)-m(0,0)),k>0 \end{align*}

を考える.この時,線形近似したシステムにおいて,原点は安定である.

証明

原点周りの安定性を,線形近似システムで議論する.制御則込みのシステムは,

(3)    \begin{align*} \begin{bmatrix} \dot{y}\\ \dot{\theta} \end{bmatrix} = \begin{bmatrix} v\sin\theta\\ -k(m(y,\theta)-m(0,0)) \end{bmatrix} \end{align*}

であるから,これを線形近似して,

(4)    \begin{align*} \begin{bmatrix} \dot{y}\\ \dot{\theta} \end{bmatrix} &= \begin{bmatrix} 0& v\\ a_{21}& a_{22} \end{bmatrix} \begin{bmatrix} y\\ \theta \end{bmatrix}\\ a_{21}&:=-k\frac{\partial m(y,\theta)}{\partial y}|_{y=\theta=0}\\ a_{22}&:=-k\frac{\partial m(y,\theta)}{\partial \theta}|_{y=\theta=0} \end{align*}

ただし,記号の置き換えとして,定数a_{21},a_{22}\displaystyleを置いた.

固有多項式は,

(5)   \begin{align*}\lambda^2-\lambda a_{22}-va_{21}=0\end{align*}

であるから,これを解いて,

(6)   \begin{align*} \lambda=\frac{a_{22}\pm \sqrt{a_{22}^2+4va_{21}}}{2} \end{align*}

m\displaystyley,\theta\displaystyleに関して単調増加であるため,a_{21}<0,a_{22}<0\displaystyle.仮定よりv>0\displaystyle.ゆえに,|a_{22}|> \sqrt{a_{22}^2+4va_{21}}\displaystyleであり,極の実部は負である.すなわち,原点まわりで線形化したシステムは安定である.

考察

 k>\frac{4v\frac{\partial m(y,\theta)}{\partial y}|_{y=\theta=0}}{(\frac{\partial m(y,\theta)}{\partial \theta}|_{y=\theta=0})^2}

であるとき,すべての極の虚部は0\displaystyleとなる.サンプリングタイムの関係上,高速度域においてはゲインk\displaystyleを落とすことがよいとされている.しかしながら,ゲインk\displaystyleを落としすぎると,虚部の絶対値が大きくなり,高周波で振動現象を起こしながら収束することがわかる.

収束を早くするためには,極の実部をより大きなマイナスにすればよい.そのため,収束性をはやめたければ,\frac{\partial m(y,\theta)}{\partial \theta}|_{y=\theta=0}\displaystyleを大きくすればよい.大きくするためには,\theta\displaystyleに関して敏感なセンサーを用いればよい.例えば,センサーの取り付け角度を急にするとよいことがわかる.もちろん,k\displaystyleを上げても収束性は早くなる.

このように,センサーの取り付け角度を調整することによってある程度自由に極配置が出来る.しかし,ゲインの調整だけでは極配置は自由度が足りず,強い制約が残ることがわかる.(調整する空間は複素平面だが,調整できるゲインは一次元しかない)

センサーを増やしたらどうなるか?

計測システムを2つ使うことを考える.すなわち,C^1\displaystyle級でy,\theta\displaystyleに関して単調増加な計測器m_1,m_2:\mathbb{R}^2 \to \mathbb{R} \displaystyleを考える.

この時,以下の制御則を解析しよう.

(7)    \begin{align*} \omega = -k_1(m_1(y,\theta)-m(0,0))-k_2(m_2(y,\theta)-m(0,0)),k_1>0,k_2>0 \end{align*}

極は,

(8)    \begin{align*} \lambda&=\frac{b_{22}\pm \sqrt{b_{22}^2+4vb_{21}}}{2}\\ b_{21}&:=-k_1\frac{\partial m_1(y,\theta)}{\partial y}|_{y=\theta=0}-k_2\frac{\partial m_2(y,\theta)}{\partial y}|_{y=\theta=0}\\ b_{22}&:=-k_1\frac{\partial m_1(y,\theta)}{\partial \theta}|_{y=\theta=0}-k_2\frac{\partial m_2(y,\theta)}{\partial \theta}|_{y=\theta=0} \end{align*}

そのうち2つの場合に任意の極配置ができることを解析

センサー1つ,ロボットは加速度入力,直進速度は等速の場合

定式化

以下のようにモデル化されるX:=\mathbb{R}\times S^1\times \mathbb{R}\displaystyle上のシステムを考える.

(9)    \begin{align*} \begin{bmatrix} \dot{y}\\ \dot{\theta}\\ \dot{\omega} \end{bmatrix} = \begin{bmatrix} v \sin\theta\\ \omega\\ u \end{bmatrix} \end{align*}

ただし,(y,\theta,\omega )\in X\displaystyleはそれぞれ位置,姿勢角,角速度であり,v,u \displaystyleは制御入力である直進速度,角加速度である.v>0\displaystyleは定数とする.このシステムは以下のような条件を満たす計測システムm:\mathbb{R}^2 \to \mathbb{R} \displaystyleを持っているとする.

  • m\displaystyleC^1\displaystyle級でy,\theta\displaystyleに関して単調増加.

制御則の構築

前節で取り上げたシステムを考える.また,状態フィードバック則として,

(10)    \begin{align*} u = -k(m(y,\theta)-m(0,0)),k>0 \end{align*}

を考える.この時,線形近似したシステムにおいて,原点は安定である.

証明

原点周りの安定性を,線形近似システムで議論する.制御則込みのシステムは,

(11)    \begin{align*} \begin{bmatrix} \dot{y}\\ \dot{\theta}\\ \dot{\omega} \end{bmatrix} = \begin{bmatrix} v\sin\theta\\ \omega \\ -k(m(y,\theta)-m(0,0)) \end{bmatrix} \end{align*}

であるから,これを線形近似して,

(12)    \begin{align*} \begin{bmatrix} \dot{y}\\ \dot{\theta}\\ \dot{\omega} \end{bmatrix} = \begin{bmatrix} 0& v & 0\\ 0&0&1\\ -k\frac{\partial m(y,\theta)}{\partial y}|_{y=\theta=0}& -k\frac{\partial m(y,\theta)}{\partial \theta}|_{y=\theta=0}&0 \end{bmatrix} \begin{bmatrix} y\\ \theta \\ \omega \end{bmatrix} \end{align*}

固有多項式は,

(13)   \begin{align*}\lambda^3+\lambda k\frac{\partial m(y,\theta)}{\partial \theta}|_{y=\theta=0}+vk\frac{\partial m(y,\theta)}{\partial y}|_{y=\theta=0}=0\end{align*}

である.3次の多項式を解くのは大変なので,フルビッツの安定性判別法を使う.全ての係数が正であるので,フルビッツの安定性判別法より,原点まわりで線形化したシステムは安定である.

考察

角加速度に対してセンサーの値をフィードバックをかけるだけでも,安定性が保証できることがわかった.

距離はかんがえず、角度だけ考慮した制御

壁までの距離は(いつかやってくる)曲がり角で,まがった後に直進量を調整することによって修正する制御法が考えられる.この制御法は,制御目標が

  • 壁までの距離を一定化する
  • 壁と姿勢角を並行にする

の2点から,後者の1点のみに緩和されている.そのため,より早い時間で収束する制御を構築しやすい利点がある.

問題設定

以下のようにモデル化されるX:=\mathbb{R}\times (-\pi/2,\pi/2)\displaystyle上のシステムを考える.

(14)    \begin{align*} \begin{bmatrix} \dot{y}\\ \dot{\theta} \end{bmatrix} = \begin{bmatrix} v \sin\theta\\ \omega \end{bmatrix} \end{align*}

このシステムは以下のような条件を満たす計測システムm:\mathbb{R}^2 \to \mathbb{R} \displaystyle;(y,\theta)\mapsto m(y,\theta)を持っているとする.

  • m\displaystyleC^2\displaystyle級でy,\theta\displaystyleに関して単調増加.
  • m\displaystyleはリプシッツ関数

このとき,t\to \infty\theta\to 0となる静的状態フィードバック則を求めることが問題である.(システムを線形化し,原点近傍の安定性を議論するためにはm\displaystyleC^2\displaystyle級である必要がある.)

制御則

以下のような制御を考える.

(15)    \begin{align*} \omega = k\frac{dm(y,\theta)}{dt},k>1/\frac{\partial m}{\partial \theta} \end{align*}

チェインルール\frac{dm(y,\theta)}{dt}=\frac{\partial m(y,\theta)}{\partial y}\dot{y}+\frac{\partial m(y,\theta)}{\partial \theta}\dot{\theta} \displaystyleに注意し,これをシステムに代入・適切な式変形を行うと,

(16)    \begin{align*} \begin{bmatrix} \dot{y}\\ \dot{\theta} \end{bmatrix} = \begin{bmatrix} v \sin\theta\\ \frac{1}{\frac{1}{k}-\frac{\partial m}{\partial \theta}}\frac{\partial m}{\partial y}v\sin\theta \end{bmatrix} \end{align*}

ただし,式変形の途中で条件k\neq 1/\frac{\partial m}{\partial \theta}\displaystyleを使った.

式より,仮定の下で\theta=0\displaystyleは安定であることを意味している.もう少し厳密な表現をすると,任意の初期点に対し,カラテオドリ解の意味で,集合\{ (y,\theta)||y|\leq K, \theta=0\}\displaystyleが大域的漸近安定となるK> 0\displaystyleが存在する.(目標集合への安定性は目標集合がコンパクトな場合のみ定義されているので,このような表現を行った.)

証明

システムはリプシッツ条件が成立している.よってカラテオドリ解が存在する.

V(y,\theta)=\sin^2\theta\displaystyleを考える.この関数は明らかに正定である.\thetaの定義域が(-\pi/2,\pi/2)\displaystyleであることに注意しよう.\dot{V}=2\frac{1}{\frac{1}{k}-\frac{\partial m}{\partial \theta}}\frac{\partial m}{\partial y}v\sin^2\theta\cos\theta\displaystyleであるため,\dot{V}<0\displaystyleが成立する.これより,\lim_{t\to\infty}V(t)=0\displaystyleが成立する.ゆえに,\lim_{t\to\infty}\theta(t)=0\displaystyleが成立する.

\lim_{t\to\infty}\theta(t)=0より,\lim_{t\to\infty}\dot{y}(t)=0である.y\displaystyleの定義域\mathbb{R}は完備.y(t)\displaystyleの任意の部分列はコーシー列である.ゆえに,\lim_{t\to\infty}y(t)=y_\omegaを満たすy_\omega\displaystyleが存在する.これは,y(t)\displaystyleが有界であることを意味している.

K=\sup y(t)\displaystyleととれば,任意のt\geq 0\displaystyleに対し,|y(t)|\leq K\displaystyleが成立する.これと\lim_{t\to\infty}\theta(t)=0\displaystyleより,題意は成立する.

メリット・デメリット

以下のようなメリットがある.

  • 安定性が保証されている範囲が広いため,外乱に対して非常に強い.
  • 陽に\theta\displaystyleに対して極配置ができる.

以下のようなデメリットがある.

  • y\displaystyleに関して安定性は保証されない.(カラテオドリ解は存在し,有界な範囲を動く.)
  • k\displaystyleの最小大きさが1/\frac{\partial m}{\partial \theta}であり,評価しにくい.
  • k\displaystyleをいくら大きく取っても,極を-v\frac{\partial m}{\partial y}/\frac{\partial m}{\partial \theta}\sin\theta\displaystyleより小さくすることができない.

なお,2番目については単位系を\frac{\partial m}{\partial \theta}>1\displaystyleとなるように工夫し,最小値を1とみなすことが考えられる.

参考文献