SLAMにおけるセンサフュージョン

※書き途中です．

本記事は「SLAM」入門の省略された部分を補足する備忘録です．
主にSLAMで用いられるレーザースキャナとオドメトリのセンサフュージョン（センサ融合）のやり方に関する記事です．

参考文献は以下の通り

1. SLAM入門 -- ロボットの自己位置推定と地図構築の技術、友納正裕、オーム社、2018年
2. 確率ロボティクス, Sebastian Thrun, Wolfram Burgard, Dieter Fox

センサ融合とは

特性の異なるセンサを用いてそれらのセンサ値を融合させることにより，信頼性の高い推定値を得ることを言います．
本記事ではレーザースキャナとオドメトリを使用したSLAMに焦点をあて，そのなかで起こる退化といわれる問題に対してセンサ融合を使用することを想定します．

レーザースキャナにおいて，角があるような特徴的な地形の場合にはいい感じにマッチングすることができますが，平坦な通路などでは特徴が出にくく，どこにでもマッチングしてしまう状況が発生してしまいます．これは退化と呼ばれ，その対処法としてオドメトリの情報とセンサ融合することにより正しい位置を推定をする手法が挙げられます．

どのようにセンサ融合するのか

そもそもセンサ融合とはどのようにやるの？という方も多いと思われます．とても簡単にいうと用いるセンサの不確定性を表せる式（運動方程式とか）をたてて，それらの平均と分散値を求めて正規分布をつくり，その正規分布を掛けあわせることでセンサ融合ができます．感覚的には片方のセンサの信頼性が落ちると分散値が大きくなり，もう片方のセンサの値が優先的に用いられるということです．

求めたいのは時刻$t$のロボット位置$x_t$です．スキャンマッチングで推定したロボットの位置を$x_{s,t}$，オドメトリで推定したロボットの位置を$x_{o,t}$とします．このとき，融合したロボット位置$x_{f,t}$は以下のようになります．

x_{f,t} = W_{s,t}x_{s,t} + W_{o,t}x_{o,t}

$W_{s,t}$スキャンマッチング，$W_{o,t}$はオドメトリの推定値の信頼度を表す重み行列です．なお，これは重み付き平均であり$W_{s,t} + W_{o,t} = 1$のようになります．
時刻$t$を省略して以下のように表します．

x_f = W_s x_s + W_o x_o

今回この重み$W$に用いるのが共分散（行列）$ \Sigma$です．上記で「正規分布を掛け算する」と書きましたが，それに使われる共分散を求めていきます．

それでは実際に計算してみましょう．
本記事ではレーザースキャナによるスキャンマッチング，そしてオドメトリの2つのデータを融合させて位置推定の精度を向上させます．

共分散（行列）の導出

基本的には$x_{t-1}$から$x_t$へ移り変わるときの式を用います．

本記事では求める平均値を

\begin{array}{}
x_f &= W_s x_s + W_o x_o \\
    &= \Sigma_s x_s + \Sigma_o x_o
\end{array}

として，それぞれの共分散行列$\Sigma_s$と$\Sigma_o$を求めます．

レーザースキャナ（スキャンマッチング）

レーザースキャナでは得た点群のスキャンマッチングにより$x_{t-1}$から$x_t$へ遷移させます．そのため，本記事ではスキャンマッチングにおけるデータ対応付けのコスト関数を使用します．

ラプラス近似に基づく方法を用いてスキャンマッチングの共分散を求めます．ラプラス近似は確率密度関数の最大値付近の形状を正規分布で近似する方法です．
ただし，今期はICPのコスト関数（確率密度の負の対数に相当）に適応するため，最大値を極小値に置き換えます．

本記事ではスキャンマッチングにICPアルゴリズムを用います．このような繰り返し処理において，$k$回目のロボット位置$x_t^k$が得られたとして，その位置での現在スキャン$s_t$の書くスキャン点$p_i^k$は次のように計算されます（添え字のtは省略）．$p_i$はセンサ座標系でのスキャン点の位置です（共分散を正確に求めるためスキャン点と参照点の点間距離はユークリッド距離ではなく垂直距離を使います）．

G(x) = \frac{1}{N}\sum_{i=1}^N||n_{j_i} \cdot (R \cdot p_i + t - q_{j_i})||^2

ここで$R$と$t$は$x_t$における回転行列，と並進ベクトルであり，$R \cdot p_i + t$でセンサ座標系からグローバル座標系へ変換しています．
時刻$t-1$におけるスキャン点を$q_j$とし，時刻$t$におけるスキャン点を$p_i$としています．$q_{j_i}$は$p_i$が参照する$q_j$の点です．
$n_{j_i} $は参照スキャン点$q_{j_i}$の法線ベクトルです．
上記の式を以下のように変形します．

G(x) = \frac{1}{N}|| h(x) - z||^2

このときの$h(x)$と$z$は以下のように表します．

h(x) = 
\left(
\begin{array}{c}
h_1(x)\\
h_2(x)\\
\vdots\\
h_N(x)
\end{array}
\right)
=
\left(
\begin{array}{c}
n_{j_1} \cdot (R \cdot p_1 + t)\\
n_{j_2} \cdot (R \cdot p_2 + t)\\
\vdots\\
n_{j_N} \cdot (R \cdot p_N + t)
\end{array}
\right)

z = 
\left(
\begin{array}{c}
z_1\\
z_2\\
\vdots\\
z_N
\end{array}
\right)
=
\left(
\begin{array}{c}
n_{j_1} \cdot q_{j_1}\\
n_{j_2} \cdot q_{j_2}\\
\vdots\\
n_{j_N} \cdot q_{j_N}
\end{array}
\right)

$h(x)$と$z$はベクトル値関数です．

ICPのコスト関数の極小値におけるヘッセ行列を求める必要がありますが，ヘッセ行列は多変数関数の2次微分であるために計算が複雑で面倒です．そのため，ガウス―ニュートン近似を使うことで1次の偏微分だけで済ませるようにします．ガウス―ニュートン近似はヘッセ行列をヤコビ行列の積で表すものです．
本記事ではICPのコスト関数の極小値における共分散を求めるので，ガウス―ニュートン近似が成り立ちます．

そのため，共分散行列$\Sigma_s$はヤコビ行列$J$を用いて$(J^T W_z J)^{-1}$となりますが，$n_{j_1} \cdot q_{j_i}$の誤差は一様で独立だとすると$W_z\propto I$とすることができ，共分散行列は

\Sigma_s = (J^TJ)^{^1}

の定数倍となります．

ヤコビ行列$J$は以下のように表されます．

J = 
\left(
\begin{array}{ccc}
\frac{\partial h_1}{\partial x} & \frac{\partial h_1}{\partial y} & \frac{\partial h_1}{\partial \theta}\\
\frac{\partial h_2}{\partial x} & \frac{\partial h_2}{\partial y} & \frac{\partial h_2}{\partial \theta}\\

 & \vdots & \\
\frac{\partial h_N}{\partial x} & \frac{\partial h_N}{\partial y} & \frac{\partial h_N}{\partial \theta}\\
\end{array}
\right)

$J$は$h$の$x$に関するヤコビ行列で，ロボット位置$x$がずれたときに，$h$がどれだけ変化するかの程度を表します．

オドメトリ

ロータリーエンコーダ等から速度・角速度が取得できることを想定し，速度運動モデルを使用します．
速度$v_t$と角速度$\omega_t$により速度運動モデルを使用して微小時間$\Delta_t$の運動を表します．

\left(
\begin{array}{c}
x_{t+1}\\
y_{t+1}\\
\theta_{t+1}
\end{array}
\right)
=
\left(
\begin{array}{ccc}
cos\theta_t & -sin\theta_t & 0 \\
cos\theta_t &  sin\theta_t & 0 \\
0           & 0            & 1
\end{array}
\right)
\left(
\begin{array}{c}
v_t\Delta_t\\
0\\
\omega_t\Delta_t
\end{array}
\right)
+
\left(
\begin{array}{c}
x_t\\
y_t\\
\theta_t
\end{array}
\right)

\\

=
\left(
\begin{array}{c}
f_x \\
f_y \\
f_{\theta} \\
\end{array}
\right)

=
\left(
\begin{array}{c}
v_t\Delta_t cos\theta_t + x_t\\
v_t\Delta_t sin\theta_t + y_t\\
\omega_t\Delta_t + \theta_t \\
\end{array}
\right)

ここで，$x_t = (x_t, y_t, \theta_t)^T$，$u_t = (v_t, \omega_t)^T$と置くと，$x_{t+1}$の共分散$\Sigma_{t+1}$は次のように計算できます．

\Sigma_{t+1} = J_{x_t} \Sigma_t J_{x_t}^T + J_{u_t} \Sigma_u J_{u_t}^T

オドメトリ共分散$\Sigma_u$と，ヤコビ行列$J_{x_t}$と$J_{u_t}$は以下のように表されます．

\Sigma_u =
\left(
\begin{array}{cc}
\sigma_{v_t}^2 & 0 \\
0              & \sigma_{\omega_t}^2 \\
\end{array}
\right)
=
\left(
\begin{array}{cc}
a_1 v_t^2 & 0 \\
0         & a_2 \omega_t^2 \\
\end{array}
\right)

J_{x_t} = 
\left(
\begin{array}{ccc}
\frac{\partial f_x}{\partial x_t}        & \frac{\partial f_x}{\partial y_t}        & \frac{\partial f_x}{\partial \theta_t}\\
\frac{\partial f_y}{\partial x_t}        & \frac{\partial f_y}{\partial y_t}        & \frac{\partial f_y}{\partial \theta_t}\\
\frac{\partial f_{\theta}}{\partial x_t} & \frac{\partial f_{\theta}}{\partial y_t} & \frac{\partial f_{\theta}}{\partial \theta_t}\\
\end{array}
\right)
=
\left(
\begin{array}{ccc}
1 & 0 & -v_t \Delta_t sin \theta_t \\
0 & 1 &  v_t \Delta_t cos \theta_t \\
0 & 0 & 1 \\
\end{array}
\right)

J_{u_t} = 
\left(
\begin{array}{cc}
\frac{\partial f_x}{\partial v_t}        & \frac{\partial f_x}{\partial \omega_t} \\
\frac{\partial f_y}{\partial v_t}        & \frac{\partial f_y}{\partial \omega_t} \\
\frac{\partial f_{\theta}}{\partial v_t} & \frac{\partial f_{\theta}}{\partial \omega_t} \\
\end{array}
\right)
=
\left(
\begin{array}{c}
\Delta_t cos \theta_t & 0 \\
\Delta_t sin \theta_t & 0 \\
0                     & \Delta_t \\
\end{array}
\right)

共分散の$a_1$と$a_2$は適当な係数で，経験的に決めます．