$$\def\mA{\mathbf{A}} \def\mB{\mathbf{B}} \def\mC{\mathbf{C}} \def\mD{\mathbf{D}} \def\mE{\mathbf{E}} \def\mF{\mathbf{F}} \def\mG{\mathbf{G}} \def\mH{\mathbf{H}} \def\mI{\mathbf{I}} \def\mJ{\mathbf{J}} \def\mK{\mathbf{K}} \def\mL{\mathbf{L}} \def\mM{\mathbf{M}} \def\mN{\mathbf{N}} \def\mO{\mathbf{O}} \def\mP{\mathbf{P}} \def\mQ{\mathbf{Q}} \def\mR{\mathbf{R}} \def\mS{\mathbf{S}} \def\mT{\mathbf{T}} \def\mU{\mathbf{U}} \def\mV{\mathbf{V}} \def\mW{\mathbf{W}} \def\mX{\mathbf{X}} \def\mY{\mathbf{Y}} \def\mZ{\mathbf{Z}} \def\va{\mathbf{a}} \def\vb{\mathbf{b}} \def\vc{\mathbf{c}} \def\vd{\mathbf{d}} \def\ve{\mathbf{e}} \def\vf{\mathbf{f}} \def\vg{\mathbf{g}} \def\vh{\mathbf{h}} \def\vi{\mathbf{i}} \def\vj{\mathbf{j}} \def\vk{\mathbf{k}} \def\vl{\mathbf{l}} \def\vm{\mathbf{m}} \def\vn{\mathbf{n}} \def\vo{\mathbf{o}} \def\vp{\mathbf{p}} \def\vq{\mathbf{q}} \def\vr{\mathbf{r}} \def\vs{\mathbf{s}} \def\vt{\mathbf{t}} \def\vu{\mathbf{u}} \def\vv{\mathbf{v}} \def\vw{\mathbf{w}} \def\vx{\mathbf{x}} \def\vy{\mathbf{y}} \def\vz{\mathbf{z}} \def\veps{\boldsymbol{\epsilon}} \def\vnu{\boldsymbol{\nu}} \def \vmu{\boldsymbol{\mu}} \def\mSigma{\boldsymbol{\Sigma}} $$

はじめに

この記事では、前回説明したExtended Kalman filterをロボット位置と周囲の地図を同時に推定するSimultaneous Localization and Mapping (SLAM)問題に適用したEKF-SLAMについて説明します。ここで言う地図とは周辺にあるランドマークの位置です。深度センサやカメラなどでランドマークまでの距離と方向が観測値として得られると仮定して、新しい観測値が得られる毎に、その時のロボットの位置とランドマークの位置を逐次的に推定していきます。この記事では、ロボットが異なる時刻で同じランドマークを観測した時に、それらが同じであると判定できると仮定して説明していきます。例えばランドマークの特徴量などを利用することでランドマークと観測の対応関係が推定できる場合があります。そうでない場合には対応関係も同時に推定することが必要となり、かなり複雑になります。

※ Lidarを用いる場合のように観測の数が多いとEKF-SLAMなどは計算量が莫大になってしまうため、次の記事で説明するGraph-based SLAMなどが用いられます。

参考文献

[1] Probabilistic robotics
[2] 詳解確率ロボティクス

数式表記

• $x$ : 小文字＋イタリック $\rightarrow$ スカラー
• $ \vx$ : 小文字+太字 $\rightarrow$ 列ベクトル
• $\mA$ : 大文字＋太字 $\rightarrow$ 行列
• $\approx$ : 近似
• $\vx \sim p(\vx)$ : 確率変数 $\vx$ が分布 $p(\vx)$ に従う

EKF-SLAM

準備

$\vy_t = [\vx_t^T, \vm^T]^T = [\vx_t^T, \vm_1^T, \cdots, \vm_N^T]$ をロボットの状態と$N$個のランドマーク位置を表すベクトルとすると、EKF-SLAMは単純に、EKFで推定される状態$\vx_t$を$\vy_t$に置き換えるだけです。説明の簡単化のために2次元平面上での位置推定だとすると、$\vy_t = [x_t, y_t, \theta_t, m_{1x}, m_{1y}, \cdots, m_{Nx}, m_{Ny}]\in \mathbb{R}^{3+2N}$ になります。ロボット姿勢には時刻のインデックス$t$が付いていますが、ランドマーク位置は$m_{nx}, m_{ny}$のように$t$が付いていません。これはランドマークが静止していることを仮定しているためです。

1章で述べたようにKFやEKFを使うためには状態遷移モデルと観測モデルを設定することが必要となります。まず状態遷移モデルについてですが、ランドマーク位置は時刻により変化しないため、ロボット姿勢のみを考えればよいです。ここでは[1,2]と同様に、操作$\vu_t$として速さ$v_t$と角速度$\omega_t$を与えるとします。時刻$t-1$から$t$までの間が$\Delta t$秒だと仮定すると、ロボットの向きは$\theta_{t-1} \rightarrow \theta_{t-1} + \omega_t \Delta t$へと変化します。位置については、もし$\omega_t=0$で向きが変わらない場合は$x_{t-1} \rightarrow x_{t-1} + v_t \Delta_t \cos \theta_{t-1}$のように単純に計算できますが、そうでない場合には向きの変化も考慮して以下のように計算します。

\begin{align} 
x_t &= x_{t-1} + \int_{0}^{\Delta t} v_t \cos(\theta_{t-1}+\omega_t t) dt \\
&= x_{t-1} + \left[ \frac{v_t}{\omega_t} \sin(\theta_{t-1}+\omega_t t) \right]_{0}^{\Delta t} \\
&= x_{t-1} + \frac{v_t}{\omega_t} ( \sin(\theta_{t-1}+\omega_t \Delta t) - \sin \theta_{t-1}) \\
y_t &= y_{t-1} + \int_{0}^{\Delta t} v_t \sin(\theta_{t-1}+\omega_t t) dt \\
&= y_{t-1} + \left[ - \frac{v_t}{\omega_t} \cos(\theta_{t-1}+\omega_t t) \right]_{0}^{\Delta t} \\
&= y_{t-1} - \frac{v_t}{\omega_t} ( \cos(\theta_{t-1}+\omega_t \Delta t) - \cos \theta_{t-1})
\end{align}

この式を用いると状態遷移モデルは以下のように定義できます。

\begin{align} 
\vy_t &=
\vy_{t-1} +
\left( \begin{matrix} 
  \frac{v_t}{\omega_t} ( \sin(\theta_{t-1}+\omega_t \Delta t) - \sin \theta_{t-1}) \\ 
  - \frac{v_t}{\omega_t} ( \cos(\theta_{t-1}+\omega_t \Delta t) - \cos \theta_{t-1}) \\ 
  \theta_{t-1} + \omega_t \Delta t \\
 0 \\ \vdots \\ 0 
\end{matrix} \right) + 
\left( \begin{matrix} 
  \epsilon_x \\ \epsilon_y \\ \epsilon_\theta \\
 0 \\ \vdots \\ 0 
\end{matrix} \right) \\
&= \vy_{t-1} +
\mH \left( \begin{matrix} 
  \frac{v_t}{\omega_t} ( \sin(\theta_{t-1}+\omega_t \Delta t) - \sin \theta_{t-1}) \\ 
  - \frac{v_t}{\omega_t} ( \cos(\theta_{t-1}+\omega_t \Delta t) - \cos \theta_{t-1}) \\ 
  \theta_{t-1} + \omega_t \Delta t
\end{matrix} \right) + 
\mH \left( \begin{matrix} 
  \epsilon_x \\ \epsilon_y \\ \epsilon_\theta
\end{matrix} \right) \\
&= \vf(\vy_{t-1}, \vu_t) + \mH \veps
\end{align}

ただし、

\begin{align} 
\mH &=
\left( \begin{matrix} 
 1 & 0 & 0 \\
 0 & 1 & 0 \\
 0 & 0 & 1 \\
 0 & 0 & 0 \\
 & \vdots &  \\
 0 & 0 & 0 \\
\end{matrix} \right) \in \mathbb{R}^{(3+2N) \times 3}
\end{align}

であり、$\veps \sim \mathcal{N}(\mathbf{0}, \mQ)$はいつものようにガウス分布に従うノイズを表すのですが、$\mQ$の与え方については色々な方法があるようです。[2]では$v_t, \omega_t$の大きさに比例して分散を大きくするような方法が使われていたりします。

次は観測モデルについて説明します。例えば時刻$t$で$n$番目のランドマークを観測したとして、ロボットから見たランドマークの距離を$l_{tn}$、方向を$\phi_{tn}$とすると、観測モデルは以下のように定義できます。

\begin{align} 
\vz_{t}^{(n)} &= 
\left( \begin{matrix} l_{tn} \\ \phi_{tn} \end{matrix} \right) = 
\left( \begin{matrix} \sqrt{(m_{nx} - x_t)^2 + (m_{ny} - y_t)^2)}  \\ \text{arctan} \left( \frac{m_{ny}-y_t}{m_{nx}-x_t} \right) - \theta_t \end{matrix} \right) + \vnu \\
&= \vg(\vy_t) + \vnu
\end{align}

ここで、$\vz_{t}^{(n)}$ はランドマーク$n$ の観測値を表し,
$\vnu \sim \mathcal{N}(\mathbf{0}, \mR)$はガウス分布に従うノイズを表します。

事後分布の計算

EKF-SLAMでは$p(\vy_t | \vu_{1:t-1}, \vz_{1:t-1})$は既知として、$p(\vy_t | \vu_{1:t}, \vz_{1:t})$を計算することが目標となります。基本的には以下のEFKの更新式に従って計算するだけです。

\begin{align} 
\mF_t &= \left.\frac{\partial \vf}{\partial \vy}\right|_{\vy=\vmu_{t-1}} \\
\tilde{\vmu}_{t} &= \vf(\vmu_{t-1}, \vu_t) \\ 
\tilde{\mSigma}_{t} &= \mF_t \mSigma_{t-1} \mF_t^T + \mQ \\ \\ 

\mG_t &=\left.\frac{\partial \vg}{\partial \vy}\right|_{\vy=\tilde{\vmu}_{t}} \\ 
\mK_t &= \tilde{\mSigma}_t \mG_t^T (\mG_t \tilde{\mSigma}_t \mG_t^T + \mR)^{-1} \\
\vmu_t &= \tilde{\vmu}_t + \mK_t(\vz_t - \vg(\tilde{\vmu}_{t} ) ) \\
\mSigma_t &= (\mI - \mK_t \mG_t) \tilde{\mSigma}_t
\end{align}

しかし、以下の2点が異なります。

• 状態遷移時はロボット姿勢のみが更新
• 観測の数が時刻によって異なる

状態遷移については、$\vf(\vy_{t-1}, \vu_t)$は既にロボット姿勢のみが変化することを考慮しているので、$\mQ$について以下のように置き換えるだけです。

\begin{align} 
\mQ \rightarrow
\left( \begin{matrix}
 \mQ & \mathbf{0}_{3 \times 2N} \\
 \mathbf{0}_{2N \times 3} & \mathbf{0}_{2N \times 2N} \\
\end{matrix} \right) 
\end{align}

観測の数が時刻毎に異なる問題については、まず以下の仮定をします。

\begin{align} 
 p(\vz_t | \vx_t, \vm) = \prod_{n \in \vn_t} p(\vz_{t}^{(n)} | \vx_t, \vm_n)
\end{align}

$\vn_t$ は時刻$t$に観測されたランドマークの集合、$\vz_{t}^{(n)}$ はランドマーク$n$ の観測値を表します。この式はロボット姿勢とランドマーク位置が既知ならば各観測は独立に扱えるという自然な仮定です。この仮定を用いて $p(\vy_t | \vu_{1:t}, \vz_{1:t})$ を書き直してみます。

\begin{align} 
p(\vy_t | \vu_{1:t}, \vz_{1:t}) &\approx p(\vz_t | \vy_t, \vu_{1:t}, \vz_{1:t-1}) p(\vy_t | \vu_{1:t}, \vz_{1:t-1}) \\
&= \left( \prod_{n \in \vn_t} p(\vz_{t}^{(n)} | \vx_t, \vm_n) \right) p(\vy_t | \vu_{1:t}, \vz_{1:t-1}) \\
&= \left( \prod_{n \in \vn_t - \{ n_1 \} } p(\vz_{t}^{(n)} | \vx_t, \vm_n) \right) p(\vz_{t}^{(n_1)} | \vx_t, \vm_{n_1}) \mathcal{N}(\tilde{\vmu}_t, \tilde{\mSigma}_t) \\
\end{align}

最後の行の右２つの項については従来のEKFと同様に計算することができ、その結果新たなガウス分布になります。なので、この計算を全ての観測について繰り返せば良いということになります。したがって、EKF-SLAMのアルゴリズムは以下のようになります。

\begin{align} 
\mF_t &= \left.\frac{\partial \vf}{\partial \vy}\right|_{\vy=\vmu_{t-1}} \\
\tilde{\vmu}_{t} &= \vf(\vmu_{t-1}, \vu_t) \\ 
\tilde{\mSigma}_{t} &= \mF_t \mSigma_{t-1} \mF_t^T + \left( \begin{matrix}
 \mQ & \mathbf{0}_{3 \times 2N} \\
 \mathbf{0}_{2N \times 3} & \mathbf{0}_{2N \times 2N} \\
\end{matrix} \right)  \\ \\ 

for &\ \ \vz_t^{(n)} \ in \ \ \vz_t  \\
&\mG_{tn} =\left.\frac{\partial \vg(\vx_t, \vm_n)}{\partial \vy}\right|_{\vy=\tilde{\vmu}_{t}} \\ 
&\mK_{tn} = \tilde{\mSigma}_t \mG_{tn}^T (\mG_{tn} \tilde{\mSigma}_t \mG_{tn}^T + \mR)^{-1} \\
&\tilde{\vmu}_t = \tilde{\vmu}_t + \mK_{tn}(\vz_t^{(n)} - \vg(\tilde{\vmu}_{t} ) ) \\
&\tilde{\mSigma}_t = (\mI - \mK_{tn} \mG_{tn}) \tilde{\mSigma}_t \\
\\
\vmu_t &= \tilde{\vmu}_t \\
\mSigma_t &= \tilde{\mSigma}_t \\
\end{align}

それでは具体的な計算方法について説明していきます。まず$\mF_t$を計算するために、$\vf(\vy_{t-1}, \vu_t)$を$\vy_{t-1}$で微分します。この微分した行列の$(i,j)$成分は$\vf(\vy_{t-1}, \vu_t)$の $i$ 行目を、$\vy_{t-1}$の $j$ 行目で微分した値を表します。

\begin{align} 
\frac{\partial \vf}{\partial \vy_{t-1}} =
\left( \begin{matrix}
  1 & 0 & v_t \omega_t^{-1} (\cos(\theta_{t-1}+\omega_t \Delta t) - \cos \theta_{t-1}) & 0  & 0 & \cdots & 0 & 0 \\
  0 & 1 & v_t \omega_t^{-1} (\sin(\theta_{t-1}+\omega_t \Delta t) - \sin \theta_{t-1}) & 0 & 0 & \cdots & 0 & 0 \\
  0 & 0 & 1 & 0 & 0 & \cdots & 0 & 0 \\
  0 & 0 & 0 & 1 & 0 & \cdots & 0 & 0 \\
    &   & \vdots \\
  0 & 0 & 0 & 0 & 0 & \cdots & 0 & 1 
\end{matrix} \right) 
\end{align}

全体は$((3+2N)\times (3+2N))$の行列であり、右下の$(2N\times 2N)$の部分は単位行列になっています。この行列の $\vy_{t-1}$に $\vmu_{t-1}$を代入することで $\mF_t \in \mathbb{R}^{(3+2N)\times(3+2N)}$が得られます。
ここまでで、$\tilde{\vmu}_t, \tilde{\mSigma}_t$が計算できました。次は$\mG_{tn}$の計算についてです。

\begin{align} 
\frac{\partial \vg(\vx_t, \vm_n)}{\partial \vy_{t}} =
\left( \begin{matrix}
 \frac{x_t - m_{nx}}{l_{tn}} &  \frac{y_t - m_{ny}}{l_{tn}} & 0 & 0 & \cdots & 0 &  \frac{m_{nx} - x_t}{l_{tn}} & \frac{m_{ny} - y_t}{l_{tn}} & 0 & \cdots &  0 \\
\frac{m_{ny} - y_t}{l_{tn}^2} & \frac{m_{nx} - x_t}{l_{tn}^2} & -1 & 0 & \cdots & 0 & \frac{y_t - m_{ny}}{l_{tn}^2} & \frac{x_t - m_{nx}}{l_{tn}^2} & 0 & \cdots &  0
\end{matrix} \right) 
\end{align}

値を持つのは$\vx_t$に対応する列と$\vm_n$に対応する列のみです。この行列の$\vy_t$に$\tilde{\vmu}_t$ を代入することで$\mG_{tn}$を得ることができます。　

初期値

EKFでは状態遷移モデル・観測モデルを線形化する位置が重要となるので、初期値の設定も非常に重要です。ロボットの初期位置 $\vx_0$ は原点に固定されることが多いので、以下のように分布を設定します。

\begin{align} 
p(\vx_0) = \mathcal{N}(\mathbf{0}, \epsilon \mI)
\end{align}

$\epsilon$ を小さい値に設定することで $\vx_0$ を原点に固定することができます。

ランドマークの位置については、最初何の情報もないことが一般的であるため、一様分布を仮定します。しかし、平均を0としたとすると $\mG_t$ の計算時に実際とかけ離れたところで線形化を行うことになり性能が低下する原因となります。そのため、あるランドマークを初めて観測した際には、その時の観測とロボット位置の推定値から逆にランドマーク位置を計算し、その値を初期値として用いることで、この問題を緩和することができます。

まとめ

この記事では前回説明したEKFをSLAMに適用したEKF-SLAMについて説明しました。状態遷移モデルと観測モデルを設計しさえすればEKFをほとんどそのまま適用できて、異なるのは状態遷移の際にランドマーク位置は固定でロボット姿勢のみが変化するという点と、時刻毎に観測の得られる数が異なるため観測を１つずつ使って更新していくという点のみでした。EKFではなくParticle filterを拡張したFastSLAM（ [1] Chapter 13 参照）や、オフラインである程度データが蓄積してからロボットの軌跡 $\vx_{1:t}$ とランドマーク位置を同時に推定する GraphSLAM （ [1] Chapter 11 参照）などの手法もあります。

これらの手法はLidarを用いた場合のように観測の数が非常に多い場合には計算量が多くなってしまうという問題があります。そのため、Lidarを用いる場合などは、次の記事で説明する Graph-based SLAM が用いられます。