SLAM定式化

変数の定義

x：自己位置姿勢

m \equiv (q_{1}, q_{2} \ldots ) ：地図データ（ランドマーク q_{i}から構成される）

z^{i}：i番目のランドマークの観測結果

a：オドメトリ

c：計測データ zを地図データのランドマーク q_{i}に対応づけするための変数:

x_{i:j} \equiv (x_{i}, x_{i+1}, \ldots x_{j})

ベイズの定理

\begin{eqnarray} (1)\ P(A|B, C) &=& \frac{P(A, B, C)}{P(B, C)} \\\ &=& \frac{P(C|A, B) \times P(A, B)}{P(C|B) \times P(B)} \\\ &=& \frac{P(C|A, B) \times P(A| B)}{P(C|B)} \\\ (2)\ P(A,B|C) &=& \frac{P(A, B, C)}{P(C)} \\\ &=& \frac{P(A|B, C) \times P(B, C)}{P(C)} \\\ &=& P(A|B,C) \times P(B|C) \end{eqnarray}

完全SLAM問題

背景

z_{1:t}, a_{1:t}, c_{1:t}が与えられた状況で x_{1:t}, mを求めること、すなわち SL_{t} \equiv P(x_{1:t}, m | z_{1:t}, a_{1:t}, c_{1:t})を計算することを考える。

\begin{eqnarray} SL_{t} &\equiv& P(x_{1:t}, m | z_{1:t}, a_{1:t}, c_{1:t}) \\\ &=& \frac{P(z_{t} | x_{1:t}, m, z_{1:t-1}, a_{1:t}, c_{1:t}) P(x_{1:t}, m | z_{1:t-1}, a_{1:t}, c_{1:t}) }{P(z_{t} | z_{1:t-1}, a_{1:t}, c_{1:t})} \ \ (ベイズの定理(1))\\\ &\propto& P(z_{t} | x_{1:t}, m, z_{1:t-1}, a_{1:t}, c_{1:t}) P(x_{1:t}, m | z_{1:t-1}, a_{1:t}, c_{1:t}) \ \ (興味あるのはx_{t}, mの項のみ) \\\ &=& P(z_{t} | x_{t}, m, c_{t}) P(x_{1:t}, m | z_{1:t-1}, a_{1:t}, c_{1:t}) \ \ (z_{t}の算出に不要な項を落とす) \\\ &=& P(z_{t} | x_{t}, m, c_{t}) P(x_{t} | x_{1:t-1}, m, z_{1:t-1}, a_{1:t}, c_{1:t}) P(x_{1:t-1}, m | z_{1:t-1}, a_{1:t}, c_{1:t}) \ \ (ベイズの定理(2))\\\ &=& P(z_{t} | x_{t}, m, c_{t}) P(x_{t} | x_{t-1}, a_{t}) P(x_{1:t-1}, m | z_{1:t-1}, a_{1:t-1}, c_{1:t-1}) \ \ (x_{t}の算出に不要な項を落とす) \\\ &=& P(z_{t} | x_{t}, m, c_{t}) P(x_{t} | x_{t-1}, a_{t}) SL_{t-1} \end{eqnarray}

となり SL_{t}に関する漸化式が導かれることがわかった。したがって

\begin{eqnarray} SL_{t} &\propto & P(x_{1}) \prod_{t} P(z_{t} | x_{t}, m, c_{t}) P(x_{t} | x_{t-1}, a_{t}) \\\ &=& P(x_{1}) \prod_{t} P(x_{t} | x_{t-1}, a_{t}) \prod_{i} P(z_{t}^{i} | x_{t}, m, c_{t}) \end{eqnarray}

となる。 P(x_{t} | x_{t-1}, a_{t})は前の周期の位置とオドメトリから次の周期の位置を予測するオドメトリ項、 P(z_{t}^{i} | x_{t}, m, c_{t})は位置と地図が与えられた時にランドマークがどのように観測されるかを表す観測項である。 SL_{t}を最大化する x_{1:t}, mが完全SLAM問題の解である。

軌跡推定項と地図生成項の分離

一般に完全SLAM問題は計算量が多く大変なので、先に移動軌跡 x_{1:t}を全て決め切ってしまってから地図 mを推定するということがよく行われる。具体的には

\begin{equation} SL_{t} = P(x_{1:t} | z_{1:t}, a_{1:t}, c_{1:t}) P (m | x_{1:t}, z_{1:t}, a_{1:t}, c_{1:t}) \end{equation}

と変形する（導出はこちらが詳しい）。第一項は mによらないのでこの項が最大になるように x_{1:t}を決定し、その後に第二項を最大にするように m=(q_{1}, q_{2}, \ldots)を決めるという方針で進める。この項が最適解である保証は当然ない。

軌跡推定項

\begin{eqnarray} X_{t} &\equiv& P(x_{1:t} | z_{1:t}, a_{1:t}, c_{1:t}) \\\ &=& \frac{P(z_{t} | x_{1:t}, z_{1:t-1}, a_{1:t}, c_{1:t}) P( x_{1:t}, z_{1:t-1}, a_{1:t}, c_{1:t} )}{P(z_{t} | z_{1:t-1}, a_{1:t}, c_{1:t})} \ \ (ベイズの定理(1) ) \\\ &\propto & P(z_{t} | x_{1:t}, z_{1:t-1}, a_{1:t}, c_{1:t}) P( x_{1:t}, z_{1:t-1}, a_{1:t}, c_{1:t} ) \ \ (x_{t}に無関係な項を落とす) \\\ &=& P(z_{t} | x_{1:t}, z_{1:t-1}, a_{1:t}, c_{1:t}) P( x_{t} | x_{1:t-1}, z_{1:t-1}, a_{1:t}, c_{1:t} ) P(x_{1:t-1}, z_{1:t-1}, a_{1:t}, c_{1:t}) \ \ (ベイズの定理(2) ) \\\ &=& P(z_{t} | x_{t}, z_{1:t-1}, c_{1:t}) P( x_{t} | x_{t-1}, a_{t}) P(x_{1:t-1}, z_{1:t-1}, a_{1:t-1}, c_{1:t-1}) \ \ (不要な項の依存性を落とす) \\\ &=& P(z_{t} | x_{t}, z_{1:t-1}, c_{1:t}) P( x_{t} | x_{t-1}, a_{t}) X_{t-1} \end{eqnarray}

という漸化式が導かれる。第一項については

\begin{eqnarray} P(z_{t} | x_{t}, z_{1:t-1}, c_{1:t}) &=& \int P(z_{t}, m_{t-1} | x_{t}, z_{1:t-1}, c_{1:t}) dm_{t-1} \ \ (t-1の時点で生成した地図m_{t-1}で周辺積分を実施) \\\ &=& \int P(z_{t} | m_{t-1}, x_{t}, z_{1:t-1}, c_{1:t}) P(m_{t-1} | x_{t}, z_{1:t-1}, c_{1:t}) dm_{t-1} \ \ ( ベイズの定理(2) ) \\\ &=& \int P(z_{t} | m_{t-1}, x_{t}, c_{t}) P(m_{t-1} | x_{t}, z_{1:t-1}, c_{1:t}) dm_{t-1} \ \ (z_{1:t-1}, c_{1:t-1}の情報はm_{t-1}に含まれているとして不要な依存性を削除) \\\ &=& \int P(z_{t} | m_{t-1}, x_{t}, c_{t}) P(m_{t-1} | z_{1:t-1}, c_{1:t-1}) dm_{t-1} \ \ (m_{t-1}を生成するのにtの情報は必要ない) \\\ &\sim& \int P(z_{t} | m_{t-1}, x_{t}, c_{t}) \delta(m_{t-1}-\overline{m}_{t-1}) dm_{t-1} \ \ (センサー精度が良ければ地図はほぼ一意にきまるのでデルタ関数で近似) \\\ &=& P(z_{t} | \overline{m}_{t-1}, x_{t}, c_{t}) \end{eqnarray}

これらを組み合わせると

\begin{eqnarray} X_{t} &=& P(z_{t} | \overline{m}_{t-1}, x_{t}, c_{t}) P( x_{t} | x_{t-1}, a_{t}) X_{t-1} \\\ &=& P(z_{t} | \overline{m}_{t-1}, x_{t}, c_{t}) P( x_{t} | x_{t-1}, a_{t}) P(z_{t-1} | \overline{m}_{t-2}, x_{t-1}, c_{t-1}) P( x_{t-1} | x_{t-2}, a_{t-1}) X_{t-2} \\\ && \ldots \\\ &=& P(x_{1}) \prod_{t} P(z_{t} | \overline{m}_{t-1}, x_{t}, c_{t}) P( x_{t} | x_{t-1}, a_{t}) \end{eqnarray}

P(z_{t} | \overline{m}_{t-1}, x_{t}, c_{t})も P( x_{t} | x_{t-1}, a_{t})も x_{t}に対して正規分布すると仮定すると x_{1:t}の最適値を x_{1}から順次決めていくことができる。

地図生成項

x_{1:t}が決まれば、地図生成項である

\begin{equation} P (m | x_{1:t}, z_{1:t}, a_{1:t}, c_{1:t}) = \prod_{i} P(q_{i} | x_{1:t}, z_{1:t}, a_{1:t}, c_{1:t}) \end{equation}

を最大化するように地図 m=(q_{1}, q_{2}, \ldots)を決めていけばSLAM問題が解けたことになる。

オンラインSLAM問題

オンラインSLAM問題では P(x_{t}, m | z_{1:t}, a_{1:t}, c_{1:t})を求めることを考える。

\begin{eqnarray} OL_{t} &\equiv&P(x_{t}, m | z_{1:t}, a_{1:t}, c_{1:t}) \\\ &=& \frac{\displaystyle P(z_{t} | x_{t}, m, z_{1:t-1}, a_{1:t}, c_{1:t}) P( x_{t}, m | z_{1:t-1}, a_{1:t}, c_{1:t})}{\displaystyle P(z_{t} | z_{1:t-1}, a_{1:t}, c_{1:t})} \ \ (ベイズの定理(1)) \\\ &\propto & P(z_{t} | x_{t}, m, z_{1:t-1}, a_{1:t}, c_{1:t}) P( x_{t}, m | z_{1:t-1}, a_{1:t}, c_{1:t}) \ \ (x_{1:t}, mに無関係な項を落とした) \\\ &=& P(z_{t} | x_{t}, m, c_{t}) P(x_{t}, m | z_{1:t-1}, a_{1:t-1}, c_{1:t-1}) \ \ (不要な項の依存性を削除) \\\ &=& P(z_{t} | x_{t}, m, c_{t}) \int P(x_{t}, x_{t-1}, m | z_{1:t-1}, a_{1:t-1}, c_{1:t-1}) dx_{t-1} \ \ (x_{t-1}で周辺積分化) \\\ &=& P(z_{t} | x_{t}, m, c_{t}) \int P(x_{t} | x_{t-1}, m, z_{1:t-1}, a_{1:t-1}, c_{1:t-1}) P (x_{t-1}, m | z_{1:t-1}, a_{1:t-1}, c_{1:t-1}) dx_{t-1} \ \ (ベイズの定理(2)) \\\ &=& P(z_{t} | x_{t}, m, c_{t}) \int P(x_{t} | x_{t-1}, a_{t}) P (x_{t-1}, m | z_{1:t-1}, a_{1:t-1}, c_{1:t-1}) dx_{t-1} \ \ (不要な項の依存性を削除) \\\ &=& P(z_{t} | x_{t}, m, c_{t}) \int P(x_{t} | x_{t-1}, a_{t}) OL_{t-1} dx_{t-1} \end{eqnarray}

したがって OL_{t}を逐次的に求めることができることがわかった。積分の中がオドメトリによる予測項、 P(z_{t} | x_{t}, m, c_{t})が観測項を表す。

拡張カルマンフィルター を使ったアルゴリズム

オンラインSLAM問題を解くアルゴリズムとして拡張カルマンフィルター を使った方法を示す。ここではカルマンフィルターの状態にロボットの位置姿勢情報だけでなく地図特徴点の情報も含める。そのため計算サイズは行列サイズとして地図特徴点数の2乗で聞いてくるためあまり大きなサイズの特徴点を扱えないという欠点がある。

例として2次元平面上で移動する物体があり、障害物までの相対距離と相対方位角が測定できるとする。カルマンフィルター の状態は

\begin{equation} X = (x, y, \theta, m^{1}_{x}, m^{2}_{y}, s^{1}, m^{2}_{x}, m^{2}_{y}, s^{2}, \dots, m^{N}_{x}, m^{N}_{y}, s^{N}) \end{equation}

という 3N+3次元で表現される。ここで m^{i}_{x}, m^{i}_{y}は i番目の特徴点の地図座標系での位置を表し、 s^{i}は特徴ベクトルを表す。 Nは特徴点の個数である。

伝搬ステップ

2次元平面上で移動する物体の絶対速度と角速度が測定できる場合、伝搬式は

\begin{eqnarray} x(t+1) &=& x(t) + v\Delta t \cos \theta(t) \\\ y(t+1) &=& y(t) + v\Delta t \sin \theta(t) \\\ \theta(t+1) &=& \theta(t) + \omega \Delta t \end{eqnarray}

偏微分は

\begin{eqnarray} \frac{\partial x(t+1)}{\partial x(t)} &=& 1 \\\ \frac{\partial y(t+1)}{\partial y(t)} &=& 1 \\\ \frac{\partial \theta(t+1)}{\partial \theta(t)} &=& 1 \\\ \frac{\partial x(t+1)}{\partial \theta(t)} &=& -v\Delta t \sin \theta(t) \\\ \frac{\partial y(t+1)}{\partial \theta(t)} &=& v\Delta t \cos \theta(t) \\\ \end{eqnarray}

となる。行列表記のためは 3次元のベクトルを 3N+3次元に写像する

\begin{equation} F_{x} \equiv \left( \begin{array}{cc} I_{3\times 3} & 0_{3N\times 3N} \end{array} \right) \end{equation}

という行列 F_{x}を用いると便利であり、伝搬式を行列表記で書くと

\begin{eqnarray} \left( \begin{array}{c} x(t+1) \\ y(t+1) \\ \theta (t+1) \end{array} \right) &=& \left( \begin{array}{ccc} 1 & 0 & -v\Delta t \sin \theta(t) \\ 0 & 1 & v\Delta t \cos \theta(t) \\ 0 & 0 & 1 \end{array} \right) \left( \begin{array}{c} x(t) \\ y(t) \\ \theta (t) \end{array} \right) \\ &=& \left[ I_{3} + F^\mathsf{T} \left( \begin{array}{c} -v\Delta t \sin \theta(t) \\ v\Delta t \cos \theta(t) \\ 0 \end{array} \right) \right] \left( \begin{array}{c} x(t) \\ y(t) \\ \theta (t) \end{array} \right) \\ &\equiv& J(x, y, \theta) \left( \begin{array}{c} x(t) \\ y(t) \\ \theta (t) \end{array} \right) \\ \end{eqnarray}

なので、全状態ベクトルに対する伝搬を省略せずに書くと

\begin{eqnarray} X(t+1) &=& \left( \begin{array}{cc} J(x, y, \theta) & 0 \\ 0 & 0 \end{array} \right) X(t) \\\ &=& \left[ \left( \begin{array}{c} I_{3\times 3} \\ 0_{3N\times 3N} \end{array} \right) J(x, y, \theta) \left( \begin{array}{cc} I_{3\times 3} & 0_{3N\times 3N} \end{array} \right) \right] X(t) \\\ &=& \left[ F_{x}^\mathsf{T} J(x, y, \theta) F_{x} \right] X(t) \\\ &\equiv& G(t) X(t) \end{eqnarray}

と表されることがわかった。

観測ステップ

観測ステップは、観測した特徴点と状態量の特徴点の間のインデックスの対応がわかっていないことが多いが、まずは簡単のためわかっている状況を想定する。

観測点と状態ベクトルインデックスの対応が既知のケース

観測点に順に与えられたインデックス iと、状態ベクトルの地図点インデックス jの対応付けがわかっているとする（ j=c_{i}(t)）。観測量は相対距離 r^{i}_{m}(t)と相対方位角 \theta^{i}_{m}(t)と特徴ベクトル s^{i}_{m}であり

\begin{eqnarray} r^{i}_{m}(t) &=& \sqrt{(x(t) - x^{j}(t))^2 + (y(t) - y^{j}(t))^2} \\\ \theta^{i}_{m}(t) &=& \tan^{-1} \left( \frac{y^{j}(t) - y(t)}{x^{j}(t) - x(t)} \right) - \theta(t) \\\ s^{i}_{m} &=& \overline{s}_{j}(t) \end{eqnarray}

iと jの対応付けはわかっているので s^{i}_{m} = s_{j}(t)は前提としている。したがって観測量に関係する部分のみを抜き出したJacobianは

\begin{eqnarray} J_{\mathrm{extract}} &=& \left( \begin{array}{cccccc} \frac{\partial r^{i}_{m}(t)}{\partial x(t)} & \frac{\partial r^{i}_{m}(t)}{\partial y(t)} & \frac{\partial r^{i}_{m}(t)}{\partial \theta(t)} & \frac{\partial r^{i}_{m}(t)}{\partial x^{j}(t)} & \frac{\partial r^{i}_{m}(t)}{\partial y^{j}(t)} & \frac{\partial r^{i}_{m}(t)}{\partial s^{j}} \\ \frac{\partial \theta^{i}_{m}(t)}{\partial x(t)} & \frac{\partial \theta^{i}_{m}(t)}{\partial y(t)} & \frac{\partial \theta^{i}_{m}(t)}{\partial \theta(t)} & \frac{\partial \theta^{i}_{m}(t)}{\partial x^{j}(t)} & \frac{\partial \theta^{i}_{m}(t)}{\partial y^{j}(t)} & \frac{\partial \theta^{i}_{m}(t)}{\partial s^{j}} \\ \frac{\partial s^{i}_{m}(t)}{\partial x(t)} & \frac{\partial s^{i}_{m}(t)}{\partial y(t)} & \frac{\partial s^{i}_{m}(t)}{\partial \theta(t)} & \frac{\partial s^{i}_{m}(t)}{\partial x^{j}(t)} & \frac{\partial s^{i}_{m}(t)}{\partial y^{j}(t)} & \frac{\partial s^{i}_{m}(t)}{\partial s^{j}} \\ \end{array} \right) \\\ &=& \left( \begin{array}{cccccc} - \frac{x(t)^{j} -x(t)}{r^{i}_{m}} & - \frac{y^{j}(t) - y(t)}{r^{i}_{m}} & 0 & \frac{x^{j}(t) - x(t)}{r^{i}_{m}} & -\frac{y^{j}(t) - y(t)}{r^{i}_{m}} & 0 \\ \frac{y^{j}(t) - y(y)}{(r^{i}_{m})^{2}} & - \frac{x^{j}(t) - x(y)}{(r^{i}_{m})^{2}} & -1 & -\frac{y^{j}(t) - y(y)}{(r^{i}_{m})^{2}} & \frac{x^{j}(t) - x(y)}{(r^{i}_{m})^{2}} & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{array} \right) \\\ \end{eqnarray}

今回の場合も6次元を 3N+3次元に写像する行列 F^{'}を用意すると便利である

\begin{equation} F^{'} \equiv \left( \begin{array}{cccc} I_{3} & 0_{(3j-3) \times (3j-3)} & 0_{3\times 3} & 0_{(3N-3j) \times (3N-3j)} \\ 0_{3\times 3} & 0_{(3j-3) \times (3j-3)} & I_{3} & 0_{(3N-3j) \times (3N-3j)} \\ \end{array} \right) \end{equation}

を定義すると、求めたいJacobianは

\begin{eqnarray} H^{i}(t) &=& \left( \begin{array}{cccc} \frac{\partial (r^{i}_{m}(t), \theta^{i}_{m}(t), s^{i}_{m}(t))}{ \partial (x(t), y(y), \theta(t))} & 0 & \frac{\partial (r^{i}_{m}(t), \theta^{i}_{m}(t), s^{i}_{m}(t))}{ \partial (x^{j}(t), y^{j}(y), s^{j}(t))} & 0 \end{array} \right) \\\ &=& \left( \begin{array}{cc} \frac{\partial (r^{i}_{m}(t), \theta^{i}_{m}(t), s^{i}_{m}(t))}{ \partial (x(t), y(y), \theta(t))} & \frac{\partial (r^{i}_{m}(t), \theta^{i}_{m}(t), s^{i}_{m}(t))}{ \partial (x^{j}(t), y^{j}(y), s^{j}(t))} \end{array} \right) F^{'} \\\ &=& J_{\mathrm{extract}} F^{'} \end{eqnarray}

となることがわかった。

観測点と状態ベクトルインデックスの対応が不明のケース

観測点 iに対応する状態ベクトルの地図インデックスが不明の場合、状態ベクトルの地図インデックス jに対して全ての組み合わせを試し確率が高いものを採択する。確率が高いものがなければ新規の地図点であると判断し状態ベクトルに新規で追加する。確率が高いものを採択する時、観測位置 z^{i}(t)とある候補 jインデックスの伝搬後位置 z^{j}(t)の距離を z^{i}(t) - z^{j}(t)マハラノビス距離で測定する。まず、伝搬後の位置に対する分散共分散行列は、伝搬後の分散共分散行列 V(\vec{x}^{odom})に H(t)^{i}をかけたものと、観測の誤差 Qを足し合わせたものあので

\begin{equation} \Sigma(t) = H(t)^{i} V(\vec{x}^{odom}) (H(t)^{i})^\mathsf{T} + Q \end{equation}

となるので、マララノビス距離は

\begin{equation} \Psi(t)^{j} = (z^{i}(t) - z^{j}(t))^\mathsf{T} \Sigma^{-1} (z^{i}(t) - z^{j}(t)) \end{equation}

で求められる。各 iに対して、全ての jで \Psi(t)^{j}を導出しこれが最も大きくなった jを iに対応するインデックスとして選択する。ただし、その時の \Psi(t)^{j}がある閾値を超えていない場合はその点は新規の点だと判断して新たに状態ベクトルに追加する。

参考

https://sterngerlach.github.io/doc/slam-formulation.pdf

SLAM入門: ロボットの自己位置推定と地図構築の技術 | 友納 正裕 |本 | 通販 | Amazon

確率ロボティクス (ROBOT books) | Sebastian Thrun, Wolfram Burgard, Dieter Fox, 上田 隆一 |本 | 通販 | Amazon