Graph-Based SLAMを用いた軌跡推定シミュレーション

# 1 はじめに
『確率ロボティクス』[^1]によると、SLAMは大きく分けて<font color="Blue">__オンラインSLAM__</font>と<font color="Red">__オフラインSLAM__</font>２つの手法に分類される。
<font color="Blue">__オンラインSLAM__</font>とは、各時刻において地図と姿勢の事後確率が計算される。

```math
\begin{align}
    p(\ x_t,\ m\ |\ z_{1:t},\ u_{1:t}\ ) \tag{1.1}\\
\end{align}
```
式$(1.1)$を見ていただいてもわかるように、<font color="Blue">オンラインSLAM</font>では過去の観測、制御に対し、最新の姿勢$x_t$__だけ__を計算している。逐次最新の姿勢を計算するため、<font color="Blue">オンラインSLAM</font>は__リアルタイム・アルゴリズム__である。
私が以前に作成した拡張カルマンフィルタ[^2]やパーティクルフィルタ[^3]による自己位置推定アルゴリズムはこの<font color="Blue">オンラインSLAM</font>に該当する。

一方、<font color="Red">__オフラインSLAM__</font>（__完全SLAM__と言ったりもする）では、最新の姿勢$x_t$だけでなく、ロボットの軌跡全体$x_{1:t}$に対して事後確率が計算される。

```math
\begin{align}
    p(\ x_{1:t},\ m\ |\ z_{1:t},\ u_{1:t}\ ) \tag{1.2}\\
\end{align}
```
<font color="Red">オフラインSLAM</font>は特定のタイミングで計算が実行されるため、__バッチ・アルゴリズム__となる。

# 2 Graph-Based SLAM
最近のSLAMの主流はグラフを用いたオフラインSLAMであり、グラフを用いたオフラインSLAMのことを一般的に__Graph-Based SLAM__という[^4]。
今回はこのGraph-Based SLAMをPythonで実装し、その動作を確認していきたいと思う。
なお、実装の際に参考にした論文は"A Tutorial on Graph-based SLAM[^5]"と"graph-based SLAMの解説[^6]"となります。

# 3 ロボットの姿勢と動作モデル
ロボットの姿勢と動作モデルは書籍『確率ロボティクス』[^1]の「5章 ロボットの動作」を適用するので、詳細は書籍を参考にしてほしい。
よって、ここでは簡単に説明をする。
まず、ある離散的な時刻$t$におけるロボットの姿勢$\boldsymbol{x}$を以下のように定義する。

```math
\begin{align}
    \boldsymbol{x}_{t} =
       \begin{bmatrix}
        x_{t} \\
        y_{t} \\
        \theta_{t} \\
       \end{bmatrix} \tag{3.1}\\
\end{align}
```
時刻$t$において、ロボットは__並進速度$v_{t}$__と__回転速度$\omega_{t}$__を与えられ、時刻$t'$に姿勢$\boldsymbol x_{t'}$が得られるものとする。しかし、実環境では__並進速度$v_{t}$__と__回転速度$\omega_{t}$__には雑音が乗るため、以下のような式で表すことができる。

```math
\begin{align}
    \begin{bmatrix}
        x_{t'} \\
        y_{t'} \\
        \theta_{t'} \\
    \end{bmatrix}
    = 
    \begin{bmatrix}
        x_{t} \\
        y_{t} \\
        \theta_{t} \\
    \end{bmatrix}
    +
    \begin{bmatrix}
        -\frac{v_{t}}{\omega_{t}}sin\theta_{t}
           + \frac{v_{t}}{\omega_{t}}sin(\theta_{t}+\omega_{t}\Delta t) \\
        \frac{v_{t}}{\omega_{t}}cos\theta_{t}
           - \frac{v_{t}}{\omega_{t}}cos(\theta_{t}+\omega_{t}\Delta t) \\        
        \omega_{t}\Delta t + \gamma_{t}\Delta t \\
    \end{bmatrix}

    \tag{3.2}\\
\end{align}
```

但し、

```math
\begin{align}
    \begin{bmatrix}
        v_{t} \\
        \omega_{t} \\
        \gamma_{t} \\
    \end{bmatrix}
    = 
    \begin{bmatrix}
        v^\ast_{t} \\
        \omega^\ast_{t} \\
        0 \\
    \end{bmatrix}
    +
    \begin{bmatrix}
        \varepsilon_{\alpha_1 {v_{t}^\ast}^2 + {\alpha_2 \omega_{t}^\ast}^2} \\
        \varepsilon_{\alpha_3 {v_{t}^\ast}^2 + {\alpha_4 \omega_{t}^\ast}^2} \\
        \varepsilon_{\alpha_5 {v_{t}^\ast}^2 + {\alpha_6 \omega_{t}^\ast}^2} \\
    \end{bmatrix}

    \tag{3.3}\\
\end{align}
```

である。ここで、$v_{t}^\ast$、$\omega_{t}^\ast$はそれぞれ、並進速度、回転速度の真値を表す。また、$\varepsilon$は雑音を表し、$\alpha_1$から$\alpha_6$はロボット固有の雑音パラメータである。ここで、$\varepsilon_b$は平均がゼロで標準偏差が$b$の誤差変数を意味する。

## 3.1 座標系の定義
Graph-Based SLAMを実装するにあたり、以下３つの座標系を定義する。
### __世界座標系__
世界のある1点を基準とした座標系。静止したランドマークは基本的に世界座標系では移動しない。
![world_system.png](https://c1.staticflickr.com/5/4381/36964546561_fa892e1fed.jpg)

### __ロボット座標系__
ロボット上のある1点を基準とした座標系。基準点はロボットと共に移動していく。よって、静止したランドマークはロボットの移動に伴い、移動していくことになる。
また、ランドマークは基本的にこのロボット座標系で観測される。
![robot_system.png](https://c1.staticflickr.com/5/4433/36709794230_408d8711a1.jpg)







# 4 ランドマークの観測モデル
ランドマークの観測モデルは"graph-based SLAMの解説[^6]"の「2.2 観測」を適用するのでそちらを参考にしてほしい。よって、ここでは簡単に説明する。
ある離散的な時刻$t$に、ロボット座標系にてランドマーク$L_c$を観測すると、以下3つのランドマークに関する情報が得られるものとする。

- ランドマーク$L_c$までの距離：$d_{c,t}$
- ランドマーク$L_c$が観測された方角：$\varphi_{c,t}$
- ランドマーク$L_c$自身が向いている方角：$\psi_{c,t}$

![observation.png](https://c1.staticflickr.com/5/4367/36293704333_3ca85bf7a8.jpg)

式で表すと以下のようになる。

```math
\begin{align}
    \begin{bmatrix}
        d_{c,t} \\
        \varphi_{c,t} \\
        \psi_{c,t} \\
    \end{bmatrix}
    = 
    \begin{bmatrix}
        d^\ast_{c,t} \\
        \varphi^\ast_{c,t} \\
        \psi^\ast_{c,t} \\
    \end{bmatrix}
    +
    \begin{bmatrix}
        \varepsilon_{\beta_1 {d^\ast_{c,t}}^2} \\
        \varepsilon_{\beta_2} \\
        \varepsilon_{\beta_3} \\
    \end{bmatrix}

    \tag{4.1}\\
\end{align}
```

$d^\ast_{c,t}$、$\varphi^\ast_{c,t}$、$\psi^\ast_{c,t}$、はそれぞれ真値を表し、$\beta_1$から$\beta_3$はロボット固有の雑音パラメータである。

# 5 Graph-Based SLAM 問題

## 5.1 ロボットの相対姿勢計算
問題を解くために、ノード（頂点）とエッジ（辺）を持つグラフを考える。ノードはロボットの各時刻の姿勢に対応しており、エッジは同一のランドマークを観測した２つのノード間に設定される。

![rel_pose.png](https://farm5.staticflickr.com/4426/36271154944_6d869e98f5.jpg)

このとき、各時刻のロボットの相対姿勢を算出する方法は以下の２通り存在することがわかる。
<font color="Red">
①各時刻でのノード間における、ロボット推定姿勢の差：$ \Delta \boldsymbol x_{t,t'}$

これは単純に姿勢差を計算すればよい。

```math
\begin{align}
    \Delta \boldsymbol{x}_{t,t'} = \boldsymbol{x}_{t'} - \boldsymbol{x}_t
    \tag{5.1}\\
\end{align}
```
<font color="Blue">
②同一ランドマークを観測した各時刻での観測ベクトル：$ \Delta \boldsymbol x^{L}_{c,t,t'}$

ランドマークを観測した際、観測結果を元にランドマークを基準とした座標系に変換した結果を下図に示す。

![robot_system.png](https://farm5.staticflickr.com/4407/36295859853_203859bf6a.jpg)

この図より、ランドマークを基準としたロボットの相対姿勢$\boldsymbol x^L_{c}$は以下の式で表される。

```math
\begin{align}
    \boldsymbol{x}^L_{c}
    = 
    \begin{bmatrix}
        x^L_{c} \\
        y^L_{c} \\
        \theta^L_{c} \\
    \end{bmatrix}
    = 
    \begin{bmatrix}
        d_{c} \, cos \left( \pi + \varphi_{c} - \psi_{c} \right) \\
        d_{c} \, sin \left( \pi + \varphi_{c} - \psi_{c} \right) \\
        \frac{\pi}{2} - \psi_{c} \\
    \end{bmatrix}

    \tag{5.2}\\
\end{align}
```

これを、時刻$t$及び、$t'$について計算し、差を求める。

```math
\begin{align}
    \Delta \boldsymbol{x}^{L}_{c,t,t'} = \boldsymbol{x}^L_{c,t'} - \boldsymbol{x}^L_{c,t}
    \tag{5.3}\\
\end{align}
```

ここで、$\Delta \boldsymbol x_{t,t'}$と$\Delta \boldsymbol x^L_{c,t,t'}$の差を考える。

```math
\begin{align}
    \boldsymbol{e}_{c,t,t'} = \Delta \boldsymbol{x}_{t,t'} - \Delta \boldsymbol{x}^L_{c,t,t'}   \tag{5.4}\\
\end{align}
```

この誤差$\boldsymbol e_{c,t,t'}$がゼロとなるときの$\Delta \boldsymbol x_{t,t'}$つまり、$\boldsymbol x_{t}$と$\boldsymbol x_{t'}$が最適な推定姿勢であると言える。
しかし、各エッジには誤差が含まれていることと、各ノードは他のエッジとも紐付いているため、全ての誤差$\boldsymbol e_{c,t,t'}$がゼロになることはまず有りえない。
よって、全ての誤差$\boldsymbol e_{c,t,t'}$が最適と言える推定姿勢の計算方法を考える必要がある。


## 5.2 情報行列の計算
情報行列の計算モデルも"graph-based SLAMの解説[^6]"の「3.1.2 $\Sigma_{c,t,t'}, \Omega_{c,t,t'}$の計算」を適用するのでそちらを参考にしてほしい。よって、ここでは簡単に説明する。

```math
\begin{align}
    \boldsymbol{\Sigma}_{c,t}
    = 
    \begin{bmatrix}
        \left(\beta_1 d^\ast_{c,t}\right)^2 & 0 & 0 \\
        0 & \left(d^\ast_{c,t} \ sin \beta_2\right)^2 & 0 \\
        0 & 0 & {\beta_2}^2 + {\beta_3}^2 \\
    \end{bmatrix}

    \tag{5.5}\\
\end{align}
```

次に、計測座標系で計算した$\boldsymbol \Sigma_{c,t}$ と$\boldsymbol \Sigma_{c,t'}$を世界座標系に変換する。
変換には以下の回転行列を用いる。

```math
\begin{align}
    \boldsymbol{R}_{c,t}
    = 
    \begin{bmatrix}
        c & -s & 0 \\
        s & c & 0 \\
        0 & 0 & 1 \\
    \end{bmatrix}

    \tag{5.6}\\
\end{align}
```

但し、

```math
\begin{align}
    c = cos(\theta_t + \varphi_t)       \tag{5.7}\\
    s = sin(\theta_t + \varphi_t)       \tag{5.8}\\
\end{align}
```
である。

※参考文献の"graph-based SLAMの解説[^6]"の式$(10)$と"s"の符号が異なっている。私は半時計回りの方向を正としているが、参考文献は時計回りの方向を正としたのだろうか？

最終的に世界座標系における、共分散行列$\boldsymbol \Sigma_{c,t,t'}$と情報行列$\boldsymbol \Omega_{c,t,t'}$は以下となる。

```math
\begin{align}
    \boldsymbol{\Sigma}_{c,t,t'} &= \boldsymbol{R}_{c,t}\,\boldsymbol{\Sigma}_{c,t}\,\boldsymbol{R}_{c,t}^T
                     + \boldsymbol{R}_{c,t'}\,\boldsymbol{\Sigma}_{c,t'}\,\boldsymbol{R}_{c,t'}^T   \tag{5.9}\\
    \boldsymbol{\Omega}_{c,t,t'} &= \boldsymbol{\Sigma}_{c,t,t'}^{-1}    \tag{5.10}\\
\end{align}
```

## 5.3 最適化の手法
いよいよ本題の肝となる最適化問題である。まず、ロボットが各時刻に得られた観測結果のエッジ２つ１組のペアを全通り作成する。この全ペアの集合を$\mathcal{C}$とし、２乗誤差の和を求める評価関数を定義する。

```math
\begin{align}
    \boldsymbol{F}(\boldsymbol{x}) = \displaystyle \sum_{ \boldsymbol{e}_{c,t,t'} 
       \in \mathcal{C} }^{  } \ \underbrace{\boldsymbol{e}_{c,t,t'}^T \, \boldsymbol{\Omega}_{c,t,t'} \, \boldsymbol{e}_{c,t,t'}}_{\boldsymbol{F}_{t,t'}}   \tag{5.11}\\
\end{align}
```


この上式の$\boldsymbol{F}(\boldsymbol{x})$が最小となる$\hat{\boldsymbol{x}}$を求めることが最終的な目的である。

最小値$\hat{\boldsymbol{x}}$を求めるアプローチ方法としてはGauss-Newton法やLevenberg-Marquardt法を用いるのが一般的とのこと。
Gauss-Newton法やLevenberg-Marquardt法については以下のサイトが参考になると思いますので、興味あったら覗いてみてください。

- [CVIM#11 3. 最小化のための数値計算](https://www.slideshare.net/sleepy_yoshi/cvim11-3)

今回は"A Tutorial on Graph-based SLAM[^5]"と同じようにGauss-Newton法で解いていきます。

まず始めに、$\boldsymbol F_{c,t,t'}$と$\boldsymbol x_{t'}$をそれぞれ、$\Delta\boldsymbol x_{t}$、$\Delta\boldsymbol x_{t'}$変化させた場合を考える。
式で表すと、

```math
\begin{align}
    \boldsymbol{e}_{c}(\boldsymbol{x}_{t} + \Delta\boldsymbol{x}_{t},\,\boldsymbol{x}_{t'} + \Delta\boldsymbol{x}_{t'}) 
      \ &=\ \boldsymbol{e}_{c}(\boldsymbol{x}_{t,t'}+\Delta\boldsymbol{x}) \tag{5.12}\\
      \ &\simeq\ \boldsymbol{e}_{c,t,t'} + 
         \left.\frac{\partial \boldsymbol{e}_{c,t,t'}}
                    {\partial \boldsymbol{x}}
         \right|_{\boldsymbol{x}=\boldsymbol{x}_{t,t'}}\Delta\boldsymbol{x}
   \tag{5.13}
\end{align}
```

と表される。但し、ヤコビ行列は$\boldsymbol{J}_c$を使って

```math
\boldsymbol{J}_{c,t,t'} = \left.\frac{\partial \boldsymbol{e}_{c,t,t'}}{\partial\boldsymbol{x}}\right|_{\boldsymbol{x}=\boldsymbol{x}_{t,t'}}   \tag{5.14}
```
と表される。よって、

```math
\begin{align}
    \boldsymbol{F}(\boldsymbol{x}+\Delta\boldsymbol{x}) &= \sum_{ \boldsymbol{e}_{c,t,t'} \in \mathcal{C} } \boldsymbol{F}(\boldsymbol{x}_{t,t'}+\Delta\boldsymbol{x})   \tag{5.15}\\
   &\simeq \sum_{ \boldsymbol{e}_{c,t,t'} \in \mathcal{C} } \ 
      (\boldsymbol{e}_{c,t,t'} + \boldsymbol{J}_{c,t,t'}\Delta{\boldsymbol{x}} )^T
      \, \boldsymbol{\Omega}_{c,t,t'} \,
      (\boldsymbol{e}_{c,t,t'} + \boldsymbol{J}_{c,t,t'}\Delta{\boldsymbol{x}} )   \tag{5.16}\\
   &= \sum_{ \boldsymbol{e}_{c,t,t'} \in \mathcal{C} } \ 
      \underbrace{\boldsymbol{e}_{c,t,t'}^T \boldsymbol{\Omega}_{c,t,t'} \boldsymbol{e}_{c,t,t'}}_{c_{c,t,t'}}
     + 2 \, \underbrace{\boldsymbol{e}_{c,t,t'}^T \boldsymbol{\Omega}_{c,t,t'} \boldsymbol{J}_{c,t,t'}}_{\boldsymbol{b}_{c,t,t'}} \Delta{\boldsymbol{x}} + \Delta{\boldsymbol{x}}^T \underbrace{\boldsymbol{J}_{c,t,t'}^T \boldsymbol{\Omega}_{c,t,t'} \boldsymbol{J}_{c,t,t'}}_{\boldsymbol{H}_{c,t,t'}}\Delta{\boldsymbol{x}}   \tag{5.17}\\

   &= \sum_{ \boldsymbol{e}_{c,t,t'} \in \mathcal{C} } \ 
      c_{c,t,t'} + 2 \, \boldsymbol{b}_{c,t,t'} \Delta{\boldsymbol{x}}
     + \Delta{\boldsymbol{x}}^T \boldsymbol{H}_{c,t,t'}\Delta{\boldsymbol{x}}   \tag{5.18}\\

   &= \underbrace{\sum_{ \boldsymbol{e}_{c,t,t'} \in \mathcal{C} } c_{c,t,t'}}_{\boldsymbol{c}}
     + 2 \underbrace{\left(\sum_{ \boldsymbol{e}_{c,t,t'} \in \mathcal{C} } \, \boldsymbol{b}_{c,t,t'} \right)}_{\boldsymbol{b}} \Delta{\boldsymbol{x}} 
     + \Delta{\boldsymbol{x}}^T \underbrace{\left( \sum_{ \boldsymbol{e}_{c,t,t'} \in \mathcal{C} }  \boldsymbol{H}_{c,t,t'} \right)}_{\boldsymbol{H}} \Delta{\boldsymbol{x}}   \tag{5.19}\\

   &= \boldsymbol{c}
    + 2 \boldsymbol{b} \Delta{\boldsymbol{x}}
    + \Delta{\boldsymbol{x}}^T \boldsymbol{H} \Delta{\boldsymbol{x}}   \tag{5.20}\\
\end{align}
```
式$(5.20)$の$\boldsymbol{c}$は定数となる。さらに、式$(5.20)$の変曲点を計算すると、

```math
\begin{align}
   2 \boldsymbol{b} + 2 \boldsymbol{H} \Delta{\boldsymbol{x}} = 0   \tag{5.21}\\
\end{align}
```
よって、$\Delta \boldsymbol{x}$は

```math
\begin{align}
   \Delta{\boldsymbol{x}} = -\boldsymbol{H}^{-1} \boldsymbol{b}   \tag{5.22}\\
\end{align}
```
となる。
計算された$\Delta \boldsymbol{x}$を使って$\boldsymbol{x}$を更新する。

```math
\begin{align}
   \boldsymbol{x} \leftarrow \boldsymbol{x}＋\Delta \boldsymbol{x}   \tag{5.23}\\
\end{align}
```
更新した$\boldsymbol{x}$を使って式$(5.11)$の$\boldsymbol{F}(\boldsymbol{x})$を計算し、計算結果が十分小さくなるまで式$(5.12)$からの計算を繰り返す。
計算結果が十分小さいときの$\boldsymbol{x}$を最適解$\hat{\boldsymbol{x}}$とする。

# 6 実装
Graph-Based SLAMをPythonで実装してみました。
スクリプトはGitHubで公開しています。
・[GitHub - graph_based_slam.py](https://github.com/takuyani/SLAM-Robot_Simu/blob/master/graph_based_slam.py)

スクリプトを実行した結果は以下のようになります。
（画像をクリックするとYouTubeへジャンプします。）
[![extended_kalman_filter](https://farm5.staticflickr.com/4481/23905088258_b9733d1d0b_b.jpg)](https://www.youtube.com/watch?v=5mWNtEoDbuE)

※詳しい説明は後々追加していこうと思います。

# 7 さいごに
最近流行のGraph-Based SLAMを実装してみましたが、正直実装もこの記事書くのもしんどかったです。
上手く動かないこともありましたが、なんとか意図した通りに動かすことができてよかったです。
まだまだ内容の薄い記事ですが、一旦ここで公開しようと思います。
説明不足の個所も多々あると思いますが、指摘していただければ追記していこうと思います。
また、実装するに当たり、上田さんの解説が大変参考になりました。

- "graph-based SLAMの解説[^6]"

次は実機でSLAMを動かしていきたいと思います♪

# 参考文献
[^1]: ["確率ロボティクス (プレミアムブックス版)", マイナビ出版, 2005.](https://book.mynavi.jp/ec/products/detail/id=37337)
[^2]: [拡張カルマンフィルタによる自己位置推定動作の可視化](http://qiita.com/Crafty_as_a_Fox/items/55448e2ed9ce0f340814)
[^3]: [パーティクルフィルタによる自己位置推定動作の可視化](http://qiita.com/Crafty_as_a_Fox/items/d70864ab555f91fef720)
[^4]: 友納 正裕：移動ロボットの環境認識 —地図構築と自己位置推定
[^5]: G. Grisetti, R. Kummerle, C. Stachniss, and W. Burgard. A Tutorial on Graph-based SLAM. IEEE Trans. on Intelligent Transportation Systems Magazine, 2:31–43, 2010.
[^6]: 上田 隆一：[graph-based SLAMの解説](https://github.com/ryuichiueda/commentary_on_graph-based_slam/blob/master/commentary_on_graph-based_slam.pdf)