自動運転車

拡張カルマンフィルタとヤコビ行列:カルマンフィルタでロボットの位置推定をしてみよう(7)

今回のシリーズでは、ロボットや自動運転車を制御する際に重要な、ロボットや車の正確な位置を得る方法として、カルマンフィルタを用いたロボットの位置推定の方法を紹介しています。

カルマンフィルタでロボットの位置推定をしてみよう! ロボットや自動運転車を制御するときに、ロボットや車の正確な位置を知ることはとても重要です。 しかし、ロボットを制御...

 

前回の記事では、拡張カルマンフィルタによる自己位置推定を行うために、非線形動的システムを状態空間モデルで表す方法を紹介しました。

非線形動的システムの状態空間モデル:カルマンフィルタでロボットの位置推定をしてみよう(6) 今回のシリーズでは、ロボットや自動運転車を制御する際に重要な、ロボットや車の正確な位置を得る方法として、カルマンフィルタを用いた...

 

今回の記事では、実際に拡張カルマンフィルタを用いて移動ロボットの自己位置推定を行うために必要となる、拡張カルマンフィルタの基本式に含まれるヤコビ行列の算出について紹介していきます。

 

拡張カルマンフィルタの基本式

拡張カルマンフィルタは、通常のカルマンフィルタと同じく予測ステップ観測ステップの2つのステップで構成されています。

 

拡張カルマンフィルタの予測ステップにおける基本式

$$ \begin{eqnarray} \left\{ \begin{array} \hat{\boldsymbol{x}}_{t|t-1} = f \left( \hat{\boldsymbol{x}}_{t-1|t-1} , \boldsymbol{u}_t \right) \\ \hat{\boldsymbol{P}}_{t|t-1} = \boldsymbol{F}_t \hat{\boldsymbol{P}}_{t-1|t-1} {\boldsymbol{F}_t}^T + \boldsymbol{Q}_t \end{array} \right. \end{eqnarray} $$

の2式です。

 

拡張カルマンフィルタの更新ステップにおける基本式

$$ \begin{eqnarray} \left\{ \begin{array} \tilde{\boldsymbol{y}}_t = \boldsymbol{z}_t – h \left(\hat{\boldsymbol{x}}_{t|t-1} \right) \\ \boldsymbol{S}_t = \boldsymbol{H}_t \boldsymbol{P}_{t|t-1} {\boldsymbol{H}_t}^T + \boldsymbol{R}_t \\ \boldsymbol{K}_t = \boldsymbol{P}_{t|t-1} {\boldsymbol{H}_t}^T {\boldsymbol{S}_t}^{-1} \\ \hat{\boldsymbol{x}}_{t|t} = \hat{\boldsymbol{x}}_{t|t-1} + \boldsymbol{K}_t \tilde{\boldsymbol{y}}_t \\ \hat{\boldsymbol{P}}_{t|t} = \left( \boldsymbol{I} – \boldsymbol{K}_t \boldsymbol{H}_t \right) \boldsymbol{P}_{t|t-1} \end{array} \right. \end{eqnarray} $$

の5式です。

 

この拡張カルマンフィルタの基本式には、状態方程式で用いている非線形関数fのヤコビアンで定義した行列Ftと出力方程式で用いられている非線形関数hのヤコビアンで定義した行列Htの2つのヤコビ行列(ヤコビアン)が含まれています。

 

拡張カルマンフィルタを用いるために、このFtHtの2つのヤコビ行列を求める必要があります。

 

ヤコビ行列とは

まず、ヤコビ行列(ヤコビアン)について説明していきます。

 

入力ベクトルxと入力により定まる出力ベクトルy

$$ \begin{eqnarray} \boldsymbol{x} &=& \left( x_1, x_2, \cdots, x_n \right)^T \\ \boldsymbol{y} &=& \left( y_1, y_2, \cdots, y_m \right)^T \end{eqnarray} $$

と定義します。

 

この入力xi番目であるxiと出力yj番目であるyjについて、yjxiで偏微分したものを

$$ \frac{\partial y_j}{\partial x_i} $$

と表します。

 

この偏微分をすべての組み合わせに対して行い、行列式Jの形でまとめると

$$ \boldsymbol{J} = \begin{bmatrix} \frac{\partial y_1}{\partial x_1} & \frac{\partial y_1}{\partial x_2} & \cdots & \frac{\partial y_1}{\partial x_n} \\ \frac{\partial y_2}{\partial x_1} & \frac{\partial y_2}{\partial x_2} & \cdots & \frac{\partial y_2}{\partial x_n} \\ \vdots & \vdots & \ddots & \vdots \\ \frac{\partial y_m}{\partial x_1} & \frac{\partial y_m}{\partial x_2} & \cdots & \frac{\partial y_m}{\partial x_n} \\ \end{bmatrix} $$

と表すことが出来ます。

 

この行列式Jヤコビ行列式またはヤコビアンと言います。

 

状態モデルのヤコビ行列

予測ステップの状態方程式に含まれる非線形関数f

$$ \begin{eqnarray} x_{t+1} &=& x_t + v \Delta t \cos \left(\theta_t + \omega \Delta t / 2\right) \\ y_{t+1} &=& y_t + v \Delta t \sin \left(\theta_t + \omega \Delta t / 2\right) \\ \theta_{t+1} &=& \theta_t + \omega \Delta t \end{eqnarray} $$

と表されます。

 

この非線形関数fの入力ベクトルは[xt,yt,θt]、出力ベクトルは[xt+1,yt+1,θt+1]となります。

 

ここで非線形関数fの中に含まれる速度vと角速度ωを求める式は

$$ \begin{eqnarray} v &=& (v_R+v_L)/2 \\ \omega &=& (v_R-v_L)/2d \end{eqnarray} $$

であるため、ロボットの状態変数[x,y,θ]は含まれていません。

 

よって、非線形関数fより求められるヤコビ行列式Ft

$$ \boldsymbol{F}_t = \begin{bmatrix} 1 & 0 &  – v \Delta t \sin \left(\theta_t + \omega \Delta t / 2\right) \\ 0 & 1 &  -v \Delta t \cos \left(\theta_t + \omega \Delta t / 2\right) \\ 0 & 0 & 1 \end{bmatrix} $$

と求めることが出来ます。

 

観測モデルのヤコビ行列

更新ステップの観測方程式に含まれる非線形関数h

$$ \begin{eqnarray} z_t (1) &=& \sqrt{({LM}_X-x_t)^2+({LM}_Y-y_t)^2} + v(1) \\ z_t (2) &=& \arctan{\left( \frac{{LM}_Y-y_t}{{LM}_X-x_t} \right)} – \theta_t + v(2) \end{eqnarray} $$

と表されます。

 

ここで、センサ出力zの1番目の成分z(1)は距離lt、2番目の成分z(2)は角度βtを表しています。

 

また、LMXLMYはそれぞれマーカー位置のX軸成分とY軸成分を表しています。

 

この時、非線形関数hの入力ベクトルは[xt,yt,θt]、出力ベクトルは[lt,βt]となります。

 

よって、非線形関数hより求められるヤコビ行列式Ht

$$ \boldsymbol{H}_t = \begin{bmatrix} – \frac{{LM}_X-x_t}{\sqrt{({LM}_X-x_t)^2+({LM}_Y-y_t)^2}} & – \frac{{LM}_Y-y_t}{\sqrt{({LM}_X-x_t)^2+({LM}_Y-y_t)^2}} & 0 \\ – \frac{{LM}_Y-y_t}{\left({LM}_Y-y_t\right)^2+\left({LM}_X-x_t\right)^2} & – \frac{{LM}_X-x_t}{\left({LM}_Y-y_t\right)^2+\left({LM}_X-x_t\right)^2} & -1 \end{bmatrix} $$

と求めることが出来ます。

 

まとめ

今回は、実際に拡張カルマンフィルタを用いて移動ロボットの自己位置推定を行うために必要となる、非線形関数のヤコビ行列式(ヤコビアン)を算出する方法を紹介しました。

 

次回は、算出したヤコビ行列を用いて拡張カルマンフィルタによる移動ロボットの自己位置推定をシミュレーション上にて実際に行いたいと思います。

拡張カルマンフィルタのシミュレーション結果:カルマンフィルタでロボットの位置推定をしてみよう(8) 今回のシリーズでは、ロボットや自動運転車を制御する際に重要な、ロボットや車の正確な位置を得る方法として、カルマンフィルタを用いた...

COMMENT

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です