$$\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}} \def\mOmega{\boldsymbol{\Omega}} $$

はじめに

前回までの記事でKalman filterに始まり、EKF, PF, EKF-SLAMなどについて説明してきましたが、これらの手法はLidarのように観測値の次元が大きい場合には直接使うには適していません。この記事では、Graph-based SLAMというLidarを用いたSLAMなどでよく使われているであろう手法について説明していきます。

参考文献

[1] A Tutorial on Graph-Based SLAM, G. Grisetti et al., IEEE Intelligent Transportation Systems Magazine, 2010.
[2] SLAM入門
[3] 詳解確率ロボティクス

数式表記

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

Graph-based SLAM

Graph-based SLAMはEKF-SLAMなどのような逐次更新ではなく、ある程度観測値が溜まったときに、その観測値全てを使ってロボットの経路 $\vx_{1:t}$ と地図を推定します。状態空間モデルではなく、ノード間の制約条件から成るグラフを考えることによって、状態空間モデルでは扱うのが困難であったループを持つような場合に対応することができます。この記事では、[1,2]を参考に、観測をロボット姿勢間の制約条件に置き換えて、ロボット姿勢のみをノードに持つ以下の図のようなポーズグラフを扱う場合について説明します。

この記事では観測はLidarによる点群であると仮定します。次回以降で説明していくスキャンマッチング手法、例えば Iterative Closest Point (ICP) や、その拡張であるGICP, NDTなどをLidarの観測に適用することで、時刻 $t-1$ から$t$までの間のロボット姿勢の変化を推定することができます。

大まかにICPを説明すると、Lidarで得られる点群はLidarの位置を原点としたLidar座標系上での値となっており、これをロボットの姿勢の推定値に基づいてGlobal座標系に変換することを考えます。ロボットの姿勢が正しく推定できていれば、変換した点群はGlobal座標系上に登録された過去の時刻の点群と（部分的に）一致するはずです。この考え方に基づいて、現在の点群が過去の点群となるべく一致するようなロボットの姿勢を求めます。これによって、ノード$\vx_{t-1}$とノード$\vx_t$の間の制約が与えられます。

$\vx_{t-1}$と$\vx_t$のように隣り合うノード間の制約しかない場合には、単純に$\vx_{t-1}$に対して制約が満たされるように$\vx_{t}$を決めていく、というのを繰り返せばよいですが、例えばロボットがコースを一周して前いた場所に戻ってきた場合には時間的に遠く離れたノード間の制約（＝ループ）が発生します。観測値から計算された制約は当然ノイズが入っているので、隣り合うノード間の制約と遠く離れたノード間の制約は矛盾してしまう可能性があります。この時にGraph-based SLAMを用いることによって、なるべく制約が満たされるように姿勢を求めます。

隣り合うノード間の制約でも離れているノード間の制約でも同じように扱うことができるので、あるノード $\vx_i$ と $\vx_j$の間の制約について考えます。Lidarで得られた点群に対してスキャンマッチング手法を用いることで姿勢の変化量 $\hat{\vz}_{ij}$ と精度行列（分散共分散行列の逆行列）$\mOmega_{ij}$ が得られたと仮定します。精度行列の計算についてはラプラス近似を用いた手法などが使われるようで、詳しくは次回の記事で説明します。ここで重要となるのは、得られる姿勢の変化量というのは、$\vx_i$ を基準としたときの変化量であるということです。$\vx_i - \vx_j$ で計算されるのはグローバル座標系で見たときの変化量であり、異なるものです。例えば、２次元平面上で考えている場合に、$\va=(a_x, a_y, a_\theta)$ という変化量がスキャンマッチングで得られたとすると、$\vx_j$は以下のように計算できます。

\begin{align} 
\vx_j = \vx_i + 
\left( \begin{matrix}
 \cos x_{i,\theta} & -\sin x_{i,\theta} & 0 \\ 
 \sin x_{i,\theta} & \cos x_{i,\theta} & 0 \\ 
 0 & 0 & 1 
\end{matrix} \right)
\va
\end{align}

ここで、$x_{i,\theta}$ は$\vx_i$のうちの向きを表しています。この式は、compounding演算子$\oplus$ を用いて、$\vx_j = \vx_i \oplus \va$ とも表記されます。もし$\vx_i$ と $\vx_j$ が与えられているとするならば、その変化量 $\va$ は以下のように計算できます。

\begin{align} 
\va &=  
\left( \begin{matrix}
 \cos x_{i,\theta} & \sin x_{i,\theta} & 0 \\ 
 -\sin x_{i,\theta} & \cos x_{i,\theta} & 0 \\ 
 0 & 0 & 1 
\end{matrix} \right)
(\vx_j - \vx_i) \\
&= \vz(\vx_i, \vx_j)
\end{align}

この式はinverse compounding演算子 $\ominus$ を用いて、$\va = \vx_j \ominus \vx_i$ とも表記されます。姿勢 $\vx_i, \vx_j$ から変化量を計算する関数を $\vz(\vx_i, \vx_j)$ と定義することにします。

スキャンマッチングで得られた変化量 $\hat{\vz}_{ij}$ が実際の姿勢の変化量 $\vz(\vx_i, \vx_j)$ を平均とするガウス分布から生成されたと考えると、以下の式が成り立ちます。

\begin{align} 
p(\hat{\vz}_{ij} | \vx_i, \vx_j) &= \mathcal{N}( \hat{\vz}_{ij} | \vz(\vx_i, \vx_j), \mOmega_{ij}^{-1}) \\
&\propto \exp \left( - \frac{1}{2} (\hat{\vz}_{ij} - \vz(\vx_i, \vx_j))^T \mOmega (\hat{\vz}_{ij} - \vz(\vx_i, \vx_j)) \right)
\end{align}

したがって、エッジの集合を$E$とすると、$\{\hat{\vz}_{ij}\}_{(i,j) \in E}$ の負の対数尤度は以下のように計算することができます。

\begin{align} 
-\log p(\{\hat{\vz}_{ij}\}_{(i,j) \in E} | \vx_{1:t}) &= -\log \prod_{(i,j) \in E} \mathcal{N}( \hat{\vz}_{ij} | \vz(\vx_i, \vx_j), \mOmega_{ij}^{-1}) \\
&=  \frac{1}{2} \sum_{(i,j) \in E} (\hat{\vz}_{ij} - \vz(\vx_i, \vx_j))^T \mOmega_{ij} (\hat{\vz}_{ij} - \vz(\vx_i, \vx_j)) + \text{const}  \\
&=   \frac{1}{2} \sum_{(i,j) \in E} \vd_{ij}^T \mOmega_{ij} \vd_{ij} + \text{const} \\
 &= \sum_{(i,j) \in E} L_{ij}  + \text{const} 
\end{align}

ここで、$\vd_{ij} = \hat{\vz}_{ij} - \vz(\vx_i, \vx_j), \ L_{ij} = \vd_{ij}^T \mOmega_{ij} \vd_{ij} / 2$ と置きました。この値が小さくなるような $\vx_{1:t}$ を推定すればよいのですが、今のままだと $\vx_{1:t}$ 全体を平行移動・回転させても負の対数尤度の値が変わらないため無数の解が存在することになってしまいます。そのため $\vx_1$に以下の分布を仮定して制約を与えます。

\begin{align} 
p(\vx_1) = \mathcal{N}(\vx_1 | \mathbf{0}, \mOmega_0^{-1})
\end{align}

ここで、$\mOmega_0$ は精度行列であり、通常は $\mI$ を単位行列として、$\mOmega_0^{-1} = \epsilon \mI$などが用いられます。分散が小さいほど$\vx_1$は原点に強く固定されますが、[1] では $\epsilon=1$ が用いられており、小さい値にしなくても大丈夫なのかもしれません。この項を先程の対数尤度関数に加えると以下のようになります。

\begin{align} 
-\log \left( p(\vx_1) p(\{\hat{\vz}_{ij}\}_{(i,j) \in E} | \vx_{1:t}) \right) &= -\log p(\vx_1) - \log \prod_{(i,j) \in E} \mathcal{N}( \hat{\vz}_{ij} | \vz(\vx_i, \vx_j), \mOmega_{ij}^{-1}) \\

&=   \frac{1}{2} \left( \vx_1^T \mOmega_0 \vx_1 + \sum_{(i,j) \in E} \vd_{ij}^T \mOmega_{ij} \vd_{ij} \right)  + \text{const} \\
 &= \vx_1^T \mOmega_0' \vx_1 + \sum_{(i,j) \in E} L_{ij} + \text{const} \\
&= L
\end{align}

これは $\vx_{1:t}$に関する非線形最小２乗問題となっているので、ガウス・ニュートン法などのニュートン法を拡張した手法で推定していきます。ニュートン法は大雑把に説明すると、最小化したいコスト関数 $L$ をテイラー展開により２次までで近似（または、微分を１次までで近似）して微分=0となるようにパラメータを求める手法で、元の関数の勾配ベクトル $\nabla L$ とヘッセ行列 $\mH$ を用いて、$\vx_{1:t} \leftarrow \vx_{1:t} - \mH^{-1} \nabla L$ のように更新します。ヘッセ行列の計算は一般的に複雑となるため、ヘッセ行列に近似を用いたガウス・ニュートン法が用いられることがあります。しかし、この近似は解の周辺でしか成り立たないため、良い初期値が得られないような場合には、レーベンバーグ・マーカート法という最急降下法とガウス・ニュートン法を組み合わせた手法が使われることもあるようです。

ガウス・ニュートン法の具体的な計算について説明します。まずは現在の推定値での勾配ベクトルとヘッセ行列を計算していきます。いきなりベクトル形式で微分するとわかりにくいかもしれないので、$\vx_i$の $k$ 番目の要素を $x_{i,k}$ として、$L_{ij}$ を $x_{i,k}$ で微分してみます。

\begin{align} 
\frac{\partial L_{ij}}{\partial x_{i,k}} &= \frac{1}{2} \left( \frac{\partial L_{ij}}{\partial \vd_{ij}} \right)^T \frac{\partial \vd_{ij}}{\partial x_{i,k}} = \vd_{ij}^T \mOmega_{ij} \frac{\partial \vd_{ij}}{\partial x_{i,k}} \\
\frac{\partial^2 L_{ij}}{\partial x_{i,k} \partial x_{i,l}} &= \left( \frac{\partial \vd_{ij}}{\partial x_{i,l}} \right)^T \mOmega_{ij} \frac{\partial \vd_{ij}}{\partial x_{i,k}} + \vd_{ij}^T \mOmega_{ij} \frac{\partial^2 \vd_{ij}}{\partial x_{i,k} \partial x_{i,l}}
\end{align}

ガウス・ニュートン法では、もし $\vx_i, \vx_j$が最適解に近い場合には、$\vd_{ij} = \hat{\vz}_{ij} - \vz(\vx_i, \vx_j)$ は小さい値になっているはずなので、２行目の第二項は無視できると考えます。$\vx_{1:t} = [\vx_1^T, \cdots, \vx_t^T]^T$ 全体で微分した場合には以下のようにベクトル形式で表されます。

\begin{align} 
\nabla L_{ij} &= \frac{\partial L_{ij}}{\partial \vx} = (\vd_{ij}^T \mOmega_{ij} \mJ_{ij} )^T \\
\mH_{ij} &= \frac{\partial^2 L_{ij}}{\partial x_{i,k} \partial x_{i,l}} \approx \mJ_{ij}^T \mOmega_{ij} \mJ_{ij}
\end{align}

ここで、$\mJ_{ij}$ は $\vd_{ij}$ を$\vx_{1:t}$ で微分した行列を表していて、次元は ($\vd_{ij}$ の次元) $\times$ ($\vx_{1:t}$の次元) になっています。$\vd_{ij}$ は $\vx_{i}$ と $\vx_{j}$ のみの関数なので、それ以外の列は全て０になっています。$\vx_i$ と $\vx_j$ に関する列については、２次元平面で考えている場合は、上で示した $\vz(\vx_i, \vx_j)$ の計算式から簡単に計算することができます。$\mH_{ij}$ は ($\vx_{1:t}$の次元) $\times$ ($\vx_{1:t}$の次元) の行列であり、$\vx_i, \vx_j$ に対応する $(i,i), (i,j), (j,i), (j,j)$ ブロックのみに値を持っています。$\nabla L = \sum_{(i,j) \in E} \nabla L_{ij}, \ \mH = \sum_{(i,j) \in E} \mH_{ij} $ （$\vx_1 \mOmega_0 \vx_1$ の項があるため $\mH$ の $(1, 1)$ ブロックは $\mOmega_0$ が足されます）であるため、各エッジについて $\nabla L_{ij}, \ \mH_{ij}$ を計算していき、順に加えていけばよいです。
ニュートン法ではステップ幅を決める必要などはありませんが、コスト関数$L$ の勾配ベクトルとヘッセ行列を計算する$\vx$ の値が重要となるので、「現在の推定値 $\vx_{1:t}$ について$\nabla L$ と $\mH$ を計算し、$\vx_{1:t} \leftarrow \vx_{1:t} - \mH^{-1} \nabla L$ で更新」を繰り返していきます。

まとめ

この記事では、Lidarを用いたときのように観測の次元が大きい場合に、時刻間の関係をポーズグラフで表して、観測をノード間の制約へと変換し、非線形最小２乗問題へと変換するGraph-based SLAMについて説明しました。次回以降の記事では、観測をノード間の制約へと変換するために使われるスキャンマッチング手法について説明していきます。