この記事はマイクロマウス Advent Calendar 2021の20日目の記事です．昨日の記事はそらさんの全日本学生マイクロマウス大会2021(リアル大会)の感想でした．リアル大会は色々な方と交流できていいですよね．私も三月の全日本大会が楽しみです．
今回は，ロボトレースのショートカット走行に必要な要素の一つである自己位置推定について，私が実装している方法を紹介します．
追記：今のところスマホ版では数式が正しく表示されません．PC版を見るか，脳内で数式をレンダリングしてください．たぶん解消されました．数式の表示にはMathJaxを使用しています．
ショートカット走行と自己位置推定
ショートカット走行は
ロボトレース競技における上級者向けの技術課題の一つです．ロボトレースはラインを辿る速さを競う競技ですが，機体の一部がラインにかかってさえいればラインを忠実にたどる必要はありません．ラインとは別個に最速経路を生成し，走行することができれば大幅にタイムを短縮することができます．このようなショートカット走行の実現に向けた取り組みは
少なくとも約10年前から存在し，近年では様々なコース形状についてショートカット経路を生成・走行しようとする競技者もちらほらと現れています．しかし，直線中のシケインのような単純なケースを除けば，実際の大会でショートカット走行を成功させている競技者の数はまだ多くありません．
ショートカット走行の難しい要素の一つとしてショートカット走行中の自己位置推定が挙げられます．どんなに素晴らしい目標経路が生成できていたとしても，自己位置推定が誤っていればロボットは目標経路と全く異なる場所を走り，コースアウトします．そのためショートカット走行で完走するためには正確な自己位置推定が必要不可欠なのですが，外部のカメラなどから情報を取得することを許されていないロボトレースにおいてこれを実現することは容易ではありません．自立したロボットの自己位置推定方法として，ロータリーエンコーダやジャイロセンサなどの計測値を数値積分して自己位置を求めるデッドレコニングがよく知られています．しかし，数値積分は計測誤差が蓄積するため，長いコースをデッドレコニングだけを頼りにして走りきることは(少なくとも上位勢を意識した速度域では)不可能です．累積誤差を補正する方法として，コーナーマーカーやライン形状のパターンを特徴点(ランドマーク)として利用する方法が考えられます．もし，ショートカット走行中に検出された特徴点と一走目に記録したマップの中の特徴点の対応付けを行うことができれば，対応付けられた特徴点同士の位置ずれをなくすように位置推定結果を補正することができます．しかし，ロボトレースで広く用いられている一次元的なラインセンサはラインもマーカーもただの白い点として検出するため，特徴点の検出・対応付けを行うことは容易ではありません．そのため，自己位置推定はロボトレース競技者がショートカット走行にチャレンジする上での大きな障壁になっていると言えます．
この記事では，ラインセンサのみを用いて，比較的単純な方法でデッドレコニングの累積誤差を(主に横方向とヨー角について)補正する方法を紹介します．正直なところ，本当にそれでいいの？という部分もあるのですが，完走実績のある方法ではあるので，ショートカット走行中の自己位置推定で悩んでいる方のヒントになればと考え，公開します．
ショートカット走行中の自己位置推定
 |
| ロボットの姿勢とラインの検出位置 |
 |
| デッドレコニングに基づいて計算されたライン検出位置と実際の位置の差 |
図のように，差動二輪の足回りと一次元的にライン位置を検出するセンサを搭載したロボットを仮定します．このロボットの姿勢は旋回中心の座標 $(x, y)$ および進行方向 $\theta$ で表され，これらはデッドレコニングによって$(\hat{x}, \hat{y})$ および $\hat{\theta}$ のように推定されているとします．また，ロボットの旋回中心からみた相対的なラインの位置 $(t_s, n_s)$ がラインセンサによって計測されており，ラインの形状は一走目に正確に記録されていると仮定します．$(t_s, n_s)$ と記録したライン形状のつじつまが合うように $(\hat{x}, \hat{y}, \hat{\theta})$ を補正することがここでの目標になります．
もし，ラインセンサが今検出しているライン上の点と一走目に記録したライン上の点の対応関係がわかっていれば，デッドレコニングに基づいて計算されたライン検出位置
$$\hat{x_s} = \hat{x} + t_s \cos\hat{\theta} - n_s \sin\hat{\theta} \,, $$
$$\hat{y_s} = \hat{y} + t_s \sin\hat{\theta} + n_s \cos\hat{\theta} \,, $$
と実際のライン位置$(x_s, y_s)$ のずれをなくすようにデッドレコニング結果を補正すればよさそうです．しかし，実際には対応関係は未知なので，この方法をそのまま使うことはできません．代わりに，私のロボットでは $(\hat{x_s}, \hat{y_s})$ のライン上における最近傍点 $(x_n, y_n)$ を用いて，最小化するべきずれ $E$ を．
$$E = \frac{1}{2}\left(\hat{x_s} - x_n\right)^2 + \frac{1}{2}\left(\hat{y_s} - y_n\right)^2 \,, $$
のように計算しています．($\frac{1}{2}$は後の式を見やすくするための係数なのであまり気にしないでください．)
ずれ $E$ を定義したので，あとはこれを最小化するような $(\hat{x}, \hat{y}, \hat{\theta})$ を求められればよいのですが，ここでもう一つ問題になるのが$E$を最小にする $(\hat{x}, \hat{y}, \hat{\theta})$ の組み合わせは無数に存在するということです．この解の曖昧さを真面目に解消しようとすると，複数の時刻の対応関係を利用した
ICPのような感じになると思いますが，私のロボットではより簡易的な方法で補正量を決定しています．具体的には，制御周期ごとに $E$ の $(\hat{x}, \hat{y}, \hat{\theta})$ に関する勾配
$$\frac{\partial E}{\partial \hat{x}} = \left(\hat{x_s} - x_n\right)$$
$$\frac{\partial E}{\partial \hat{y}} = \left(\hat{y_s} - y_n\right)$$
$$\frac{\partial E}{\partial \hat{\theta}}=(\hat{x_s}-x_n)(-t_s\sin\hat{\theta} - n_s\cos\hat{\theta}) + (\hat{y_s}-y_n)(t_s\cos\hat{\theta} - n_s\sin\hat{\theta})$$
を計算し，勾配の大きさに基づいて $(\hat{x}, \hat{y}, \hat{\theta})$ を
$$\hat{x} \leftarrow \hat{x} - a \frac{\partial E}{\partial \hat{x}} \,,$$
$$\hat{y} \leftarrow \hat{y} - a \frac{\partial E}{\partial \hat{y}} \,,$$
$$\hat{\theta} \leftarrow \hat{\theta} - b \frac{\partial E}{\partial \hat{\theta}} \,,$$
のように更新しています．ただし， $a$ と $b$ は補正の強さを決める定数です．要するに，勾配降下法のような感じで $(\hat{x}, \hat{y}, \hat{\theta})$ を制御周期毎に少しずつ補正しています．本当にこの方法でいいのか(安定しているのか，真値 $(x, y, \theta)$に収束するのか) は未検証ですが，とりあえず，今のところこの方法で完走できています．
実装
私のロボットでこの処理を実装している部分を以下に示します．2018年の全日本大会以降，私のロボットではずっとこの方法を使用しています．
おわりに
この記事では，ショートカット走行中の自己位置推定について，私のロボットに実装している簡易的な方法を紹介しました．ロボトレースでショートカット走行にチャレンジしている方やマウスでユニークな経路を走ろうとしている方の参考になれば幸いです．
明日の記事はhmsn_stsさんのギヤ発注にお時間を要した話(共有)です．私も最近トレーサの足回りを作り直したいと考えているので，内容がとても気になります．お楽しみに！
