SLAMについて

ROS等で自動制御を行うならば、自己位置推定は必須項目である。
SLAMではLiDARやodometryを用いてロボットの自己位置推定と地図構築を行っている。

北陽のLiDAR UST-30LX

OMRONのE6A2-C

また使用するセンサーの種類によって以下のような違いが出てくる

	ロボットの種類	地図	センサ
2D/2D型	2次元3自由度	2次元	2D Lider, オドメトリ, ジャイロ
2D/3D型	2次元3自由度	3次元	2D/3D Lider, カメラ, オドメトリ, ジャイロ
3D/3D型	3次元6自由度	3次元	3D Lider, カメラ, IMU

表のようにセンサーの次元により、構築される地図の次元数やロボットの自由度が変わってくる。

SLAMの基礎

SLAMにおいて大切なキーワードとなるのがランドマークという概念である。
ランドマークとはいわゆる目印のことで、これとの距離と方向を計測することで地図を作製していく。
具体的には以下のようになる。

図の青がロボット、赤がランドマークである。

ロボットのグローバル座標系における推定座標をオドメトリから積分していくとして、やはりそこには誤差が蓄積していく。
ここでランドマーク$q_{i}$と計測データ$z_{i}$から以下の連立方程式が成り立つ。

\begin{equation}
\left\{ \,
 \begin{aligned}
\boldsymbol{q_{1}} &= R_{1}z_{1} + \boldsymbol{t_{1}} \\
\boldsymbol{q_{2}} &= R_{1}z_{2} + \boldsymbol{t_{1}} \\
\boldsymbol{q_{3}} &= R_{1}z_{3} + \boldsymbol{t_{1}} \\
\boldsymbol{q_{1}} &= R_{2}z_{4} + \boldsymbol{t_{2}} \\
\boldsymbol{q_{2}} &= R_{2}z_{5} + \boldsymbol{t_{2}} \\
\boldsymbol{q_{3}} &= R_{2}z_{6} + \boldsymbol{t_{2}}
 \end{aligned}
\right.
\end{equation}

$R_{i}$はロボット座標系からグローバル座標に変換する回転行列である。
これを$\boldsymbol{q_{i}}$について解くことでランドマークからもロボットの推定座標を計算することが出来る。

SLAMのデータ対応付け

ランドマークが明確に区別できる場合なら上記のやり方でいいが、実際問題、ランドマークはもともと存在している実際の地形・風景・物体によく設定される。それらを区別してデータ対応づけを行うため、以下２つの方法がよく用いられる。

・位置制約による対応づけ
・特徴量による対応づけ

SLAMのループ閉じ込め

ここでいうループとは周回路のことである。
SLAMにおいて、自己位置を推定したとしても、ランドマークにも誤差が生じる。
その誤差を含んだランドマークから自己位置を推定することで、さらに誤差が累積していく。
よってロボットがスタート地点に正確に戻ることが出来ず、閉ループにならない。
よってSLAMではループ閉じ込めを行う

ループ検出

開始位置で検出したランドマークを$q_{1},q_{2}$とする。
ロボットが開始位置に戻ったことを検知し、その誤差が含まれた位置で同じランドマーク$\acute{q_{1}},\acute{q_{2}}$を計測する。
しかし、ランドマークは同じなので、$\acute{q_{1}},\acute{q_{2}}$と$q_{1},q_{2}$が同じ位置になるように推定した自己位置を修正する。
そしてこれを$x_{i-1}$の推定位置にも行い、ループ全体の自己位置を補正するのがループ検出である。

参考文献

SLAM入門ロボットの自己位置推定と地図構築の技術
友納正裕著オーム社