LoginSignup
8
8

More than 1 year has passed since last update.

@ReoNagai

SLAMにおけるセンサ融合

Last updated at Posted at 2021-11-23

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

本記事ではレーザースキャナによるスキャンマッチング，そしてオドメトリの2つのデータを融合させて位置推定の精度を向上させることが目的です．

センサ融合とは

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

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

退化.png

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

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

とてもシンプルに考えると，スキャンマッチングで推定したロボットの位置を$x_s$，オドメトリで推定したロボットの位置を$x_o$，スキャンマッチングとオドメトリの推定値の信頼度を表す重みを$W_s$と$W_o$としたとき，融合したロボット位置$x_f$は以下のようになります．
$$
x_f = W_s x_s + W_o x_o
$$

そのときの簡略図はこのようになっています．スキャンマッチングの分布は縦には信頼度が高いですが，横方向はどこにいるかわかりません．それをオドメトリの分布と合わせて正しい場所を推定しています．

センサ融合の様子.png

この式はあくまでとてもシンプルに考えた場合なので，実際の式は後で示します．

センサ融合の平均と共分散の式

実際のセンサ融合による平均$x_f$と共分散$\Sigma_f$は以下の式で求められます．

$x_s$と$\Sigma_s$はスキャンマッチングの平均と分散，$x_o$と$\Sigma_o$はオドメトリの平均と共分散です．
$$
\begin{array}{}
\Sigma_f
&= (\Sigma_s^{-1} + \Sigma_o^{-1})^{-1}
\end{array}
\tag{1.1}
$$

$$
\begin{array}{}
x_f
&= \Sigma_f(\Sigma_s^{-1}x_s + \Sigma_o^{-1}x_o)
\end{array}
\tag{1.2}
$$

次はこの式を求めるためにスキャンマッチングとオドメトリの共分散を求めます．

共分散（行列）の導出

スキャンマッチング（ICP）の共分散

スキャンマッチングにはICPアルゴリズムを用います．その際スキャンマッチングにおける平均と共分散はラプラス近似（関数を正規分布で近似する方法）を用いて以下で近似します．

  • 平均値$x_s$：ICPのコスト関数の極小値（ICPの推定値）

  • 共分散$\Sigma_s$：極小値におけるヘッセ行列$H$の逆行列の定数倍

で表します．ラプラス近似の平均値は一般的に最大値を用いますが，今回はICPのコスト関数（確率密度の負の対数に相当）の関係上，極小値を用います．

ヘッセ行列$H$は多変数関数の2次微分であるため計算が複雑です．そこでヘッセ行列$H$をヤコビ行列$J$の積（$J^TJ$）で表せるガウス－ニュートン近似を用います．これは極値の近傍で成り立ちます．

つまり共分散$\Sigma_s$は定数$K$を用いて
$$
\begin{equation}
\begin{array}{}
\Sigma_s
&= KH^{-1} \
&= K(J^TJ)^{-1} \
\end{array}
\end{equation}
\tag{2.1}
$$
と表します．それではヤコビ行列$J$を求めて，この共分散を求めてみましょう．

以下にICPのコスト関数$G(x)$を示します．ICPのコスト関数で注意する点として，ユークリッド距離による点間距離のコスト関数では点の対応付けを決めてしまえばたとえそれが間違っていても共分散は小さくなってしまいます．その対処として点間距離はユークリッド距離ではなく垂直距離を用います．

$$
G(x) = \frac{1}{N}\sum_{i=1}^N||n_{j_i} \cdot (R \cdot p_i + t - q_{j_i})||^2
$$
ここで$i$はレーザースキャナのスキャン点の番号，$j$はスキャンマッチングで参照する点の番号，Nはスキャン点の数です．$R$と$t$は回転行列と並進ベクトルであり，取得したスキャン点の座標$p_i$をセンサ座標系からグローバル座標系へ変換しています．$q_{j_i}$はマッチング時に$p_i$が参照する点です．$n_{j_i} $は$p_i$と$q_{j_i}$の法線ベクトルです．

求めたいのは，ロボット位置$x$がずれたときの変化量に関するヤコビ行列$J$です．

すなはち，上式の$R$と$t$の関数$h$を作り，それを偏微分することでヤコビ行列$J$を作成します．

上記の式を以下のように変形します．
$$
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$はベクトル値関数です．$h(x)$を偏微分してヤコビ行列$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)
\tag{2.2}

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

以上より，式(2.2)により求めたヤコビ行列$J$を式(2.1)に代入すればスキャンマッチングの共分散$\Sigma_s$が求められます．

オドメトリの共分散

ロータリーエンコーダ等から速度・角速度が取得できることを想定し，速度運動モデルを使用します．

速度$v_t$と角速度$\omega_t$により速度運動モデルを使用して，微小時間$\Delta_t$の平均値$x_o$（推定値）は以下の式で表されます．

\begin{array}{}

x_o =

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

\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}
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)

\end{array}

ここで上式から，平均値$x_o$の共分散$\Sigma_o$の漸化式を求めます．

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

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

オドメトリの共分散$\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)
\tag{3.2}

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)
\tag{3.3}

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)
\tag{3.4}

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

以上より，式(3.2)(3.3)(3.4)により求めたヤコビ行列と共分散を以下のように式(3.1)に当てはめればセンサ融合が可能です．

\Sigma_o = \Sigma_{t+1} = 
\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)

\Sigma_t
\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)^T
+
\left(
\begin{array}{c}
\Delta_t cos \theta_t & 0 \\
\Delta_t sin \theta_t & 0 \\
0                     & \Delta_t \\
\end{array}
\right)

\left(
\begin{array}{cc}
a_1 v_t^2 & 0 \\
0         & a_2 \omega_t^2 \\
\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)^T

参考文献

参考文献は以下の通りです．

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

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
8
8