前壁を使った位置決め


多くのマイクロマウスは前方に向けられたセンサーを二つ持っている.本稿では,前方センサーを使った位置決め制御を紹介する.

対象とするマウス

本稿では,以下にモデル化される速度入力型の独立型二輪車量を対象とする.

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

ただし,(x, y, \theta)\in \mathbb{R}^2\times S^1 \displaystyleはロボットの位置・姿勢角であり,(v,\omega)\in \mathbb{R}^2は速度入力・角速度入力である.

Mae2座標系は左図のように取る.

Mae1ロボットの前には壁が配置されている.ロボットはLEDとフォトトランジスタの対で構成されたセンサーを持っており,壁に向かって放った光の反射光の量を測定することができる.

測定値をm:\mathbb{R}^2\times S^1\to \mathbb{R};(x,y,\theta)\mapsto m(x,y,\theta)で表記する.

前壁を使った位置決め制御

もっとも簡単な前壁を使った制御.シンプルながらもそこそこの性能.

制御則

本稿では,ロボットが壁に向かって真っすぐ向いている状況を考える.すなわち,\theta=0の状況を考える.この時,以下のような制御則を使えば,\lim_{t\to \infty}x(t)=0\displaystyleが達成される.

(2)    \begin{align*} v(t)     &=-k_1(m(0,0,0)-m(x(t),y(t),\theta(t)))\\ \omega(t)&=0 \end{align*}

ただし,k_1>0\displaystyleは定数,m(0,0,0)\displaystyleはあらかじめ計測しておく基準値である.

考察

この制御を使えば,x\displaystyle軸方向のずれをほぼすべてキャンセルできる.厳密に\theta=0である必要はなく,マウスが壁に向いてさえいればそこそこ位置補正ができる.

2010年度北信越大会では,この制御と壁に対するP制御だけで完走した.超信地旋回の直前・直後にこの制御をかけるのがポイント.極めて遅くても必ず完走するモードをマウスに積むことができる.その上でスラローム探索を実装すれば,安定して記録を残すことができる.

センサーフュージョンを使った位置決め制御

オドメトリと光センサーによる計測のセンサーフュージョンを使った制御.前項の制御より強力.

ただし,理論の説明は整備中.結果のみ示す.

大雑把に位置情報を計測

本項で述べる制御則を実行するためには,大雑把でも良いので大域的な位置情報が必要である.

ロボットが壁に「正確に」真っすぐ向いている状況を考えよう.この場合,あらかじめロボットを(x,\theta)=(-10,0),(-9,0),(-8,0),,,(0,0),(1,0),,,(10,0)\displaystyleにおいたときのセンサーの値を覚えておけば,ロボットから壁までの距離を計測できる.

右前センサーで得られた壁までの距離をm_r(t)\displaystyle,左前センサーで得られた壁までの距離をm_l(t)\displaystyleとする.ただし,x=0の場合に,m_r(t)=m_l(t)=0\displaystyleとする.

ロボットが壁に「ほぼ」真っすぐ向いている状況でも,この計測に影響は「ほぼ」ない.すなわち,\theta \sim 0\displaystyleの状況でも,壁までの距離が求まることに注意する.(実際には大きな影響があるが,ここではそう思ってしまうことにする)

すると,時刻t\displaystyleにおけるx,\theta\displaystyleの計測値x_g(t)\theta_g(t)\displaystyleは,以下のように求まる.

(3)    \begin{align*} x_g(t)     &= -(m_r(t)+m_l(t))/2\\ \theta_g(t)&= -(m_r(t)-m_l(t))/W \end{align*}

ただし,W\displaystyleは車幅(右タイヤから左タイヤまでの距離)である.

この計測法で得られた計測値は,「本当にこれを計測と言っていいのか?」と思うほど,荒い計測しかできない.大変大きな誤差が乗っており,そのままではまず制御に使うことはできない信頼できない計測である.

ところが,1回の計測では信頼できなくても,10回,100回と多数の計測を行い,それらの計測結果を統合すると,信頼できる計測データが得られる.それでは,次節で,どのようにしてこの計測値を統合するか見てみ行くことにしよう.

計測結果の統合(センサーフュージョン)

本稿では,ロボットが壁に向かって「ほぼ」真っすぐ向いている状況を考える.すなわち,\theta\sim 0\displaystyle の状況を考える.

時刻t\displaystyleにおけるロボットの位置の推定値\hat{x}(t),\hat{\theta}(t)\displaystyleを求めよう.

まず,オドメトリにより,ロボットの局所的な移動量を求めておく.すなわち,以下の更新式を十分短い時間間隔\Delta t\displaystylesec毎に行い,ロボットの局所的な移動量を積算する.(\delta\displaystyleは0.01秒から0.001秒程度に取るとよい.)

(4)    \begin{align*} \hat{x}     (0)&=x_g(0)\\ \hat{\theta}(0)&=\theta_g(0)\\ \hat{x}     (t+\Delta t)&=\hat{x}     (t) + \Delta t v     (t)\cos\theta(t)\\ \hat{y}     (t+\Delta t)&=\hat{y}     (t) + \Delta t v     (t)\sin\theta(t)\\ \hat{\theta}(t+\Delta t)&=\hat{\theta}(t) + \Delta t \omega(t) \end{align*}

ただし,v(t),\omega(t)\displaystyleは時刻t\displaystyleにおけるロボットの直進速度,回転速度であり,タイヤの回転速度から計算される.

このオドメトリという位置推定は,

  • 時刻0で計測した位置が絶対に正しいと仮定する.
  • 相対的な移動量は正確に求まると仮定する.
  • ゆえに,初期位置に移動量を足せば,現在位置が推定できる

という発想で作られている.ところが,実際は,時刻0で計測した位置は正しいとは限らない.大雑把な計測しかしていないからである.そこで,光センサーを使って位置情報が測定できるたびに,以下の更新式で補正を行う.

(5)    \begin{align*} \hat{x}     (t)&=\hat{x}     (t) + k_1(x_g(t)     -\hat{x}     (t))\\ \hat{\theta}(t)&=\hat{\theta}(t) + k_2(\theta_g(t)-\hat{\theta}(t)) \end{align*}

ただし,k_1,k_2\in [0,1)\displaystyleは正の定数ゲインであり,0.1\displaystyle程度の値を採用すると良い.

この更新式を,10回,100回と,何度も何度も実行することで,信頼できる位置情報\hat{x},\hat{\theta}\displaystyleを得ることができる.

制御則

位置情報が計測できれば,制御は非常に簡単である.例えば,以下のような制御則を使えば,\lim_{t\to \infty}x(t)=0, \lim_{t\to \infty}\theta(t)=0\displaystyleが達成される.

(6)    \begin{align*} v(t)     &=-k_x\hat{x}(t)\\ \omega(t)&=-k_\theta\hat{\theta}(t) \end{align*}

ただし,k_x,k_\theta\in [0,\infty)\displaystyleは正の定数ゲインである.

考察

このセンサーフュージョンはカルマンフィルターの一種になっている.

光センサーを使った位置計測は,非常に大雑把な値しか計測できない.単体には強い外乱が乗っており,そのままでは制御に使用できない.これに対し,多数の計測結果を統合することで,信頼できるデータを得ることができる.

この計測・制御を導入するだけで,ほぼ無調整でスラローム走行ができるようになる.壁ぎれ制御も導入しなくても簡単な迷路なら走り切ってしまう.大変強力.(難しい迷路になると壁きれ制御と本項で述べた制御を併用する必要がある)