拡張カルマンフィルタのシミュレーション結果：カルマンフィルタでロボットの位置推定をしてみよう（８）

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

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

前回の記事では、拡張カルマンフィルタによる自己位置推定を行うために、非線形関数のヤコビ行列を算出する方法を紹介しました。

拡張カルマンフィルタとヤコビ行列：カルマンフィルタでロボットの位置推定をしてみよう（7） 今回のシリーズでは、ロボットや自動運転車を制御する際に重要な、ロボットや車の正確な位置を得る方法として、カルマンフィルタを用いた...

今回の記事では、実際に非線形動的システムの移動ロボットについて拡張カルマンフィルタを用いた自己位置推定をシミュレーション上で行いたいと思います。

シミュレーション条件

今回行うシミュレーションの条件について説明します。

対向２輪型の移動ロボットは位置[0,0]を始点として、X軸方向から徐々に円を描くように左に曲がる軌跡（黒線）を目標として動作します。

しかし、入力uに対する雑音w（ノイズ、誤差）の影響により、実際の移動ロボットの軌跡（青線）は目標軌跡（黒線）に対してズレが発生します。

そこで、センサを使って地図上に存在するマーカー[10,5]との距離と角度を求めることで、現在の位置を修正します。

しかし、このセンサの出力信号zにも雑音v（ノイズ、誤差）が含まれているため、正確な距離や角度を得られず、ロボットの正確な位置を知ることができません。

この様に、非線形システムで表されるロボットへの入力信号uやセンサでの測定データzにノイズ（wやv）が入っている場合について、拡張カルマンフィルタを用いて雑音（wやv）を加味した上でのロボットの自己位置推定を行います。

アルゴリズムの実装

今回の拡張カルマンフィルタによる非線形システムで与えられた移動ロボットの自己位置推定問題のアルゴリズムは、

1. シミュレーションの初期条件を設定
2. カルマンフィルタの予測ステップ
3. カルマンフィルタの更新ステップ
4. 以降２と３を繰り返す

という流れになります。

基本的なアルゴリズムの流れは、カルマンフィルタによる自己位置推定を行った際と同じです。

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

初期条件

拡張カルマンフィルタによる自己位置推定のシミュレーションを行うために、まず必要となる様々な初期条件を定義します。

今回のシミュレーションで取り扱う非線形動的システムの状態空間モデル

$$ \begin{eqnarray} \boldsymbol{x}_t &=& f \left( \boldsymbol{x}_{t-1}, \boldsymbol{u}_t , \boldsymbol{w}_t \right) \\ \boldsymbol{z}_t &=& h \left( \boldsymbol{x}_{t} \right) + \boldsymbol{v}_t  \end{eqnarray} $$

について、それぞれの式を関数（function）として用意します。

この様に関数を用意しておくことで、何度も計算することになる状態方程式と出力方程式の取り扱いが容易になります。

実際にMATLABで用意した関数は以下の通りです。

function x = StateEquation(x,u,w)
    global Ts;
    global d;
    
    uR = u(1)+normrnd(0,w(1));
    uL = u(2)+normrnd(0,w(2));

    v = (uR+uL)/2;
    omega = (uR-uL)/(2*d);
    
    x(1) = x(1) + v*Ts*cos(x(3)+omega*Ts/2);
    x(2) = x(2) + v*Ts*sin(x(3)+omega*Ts/2);
    x(3) = x(3) + omega*Ts;
end

function z = OutputEquation(x,v,LM)
    z(1) = ((LM(1)-x(1))^2+(LM(2)-x(2))^2)^0.5 + normrnd(0,v(1));
    z(2) = atan2(LM(2)-x(2),LM(1)-x(1)) - x(3) + normrnd(0,v(2));
end

また、入出力に対する雑音wとvの分散を

$$ \begin{eqnarray} \left\{ \begin{array}{l} \boldsymbol{w}_t \sim N \left( 0,　[0.2^2, 0.2^2] \right) \\ \boldsymbol{v}_t \sim N \left( 0, [0.3^2, 0.2^2] \right) \end{array} \right. \end{eqnarray} $$

と定義し、そして各ステップで用いる共分散QとRを

$$ \begin{eqnarray} \left\{ \begin{array}{l} \boldsymbol{Q} &=&  \begin{bmatrix} 0.2^2 & 0 & 0 \\ 0 & 0.2^2 & 0 \\ 0 & 0 & 0.1^2 \end{bmatrix} \\ \boldsymbol{R} &=&  \begin{bmatrix} 0.4^2 & 0 \\ 0 & 0.2^2 \end{bmatrix} \end{array} \right. \end{eqnarray} $$

と定義します。

ロボットの状態ベクトル（位置情報（X軸とY軸）と進行方向）について、目標となる指令の状態をxDes、雑音による影響を受けた実際の状態をxAct、そして拡張カルマンフィルタにより推定された状態をxEstと定義します。

移動ロボットの初期位置は[0,0]で進行方向は0°（X軸方向）なので、これら３つの位置情報は[0,0,0]で初期化します。

さらに、推定値xEstの確からしさを示す誤差共分散Pについて、時刻t=0での推定値[0,0,0]は確かなので、誤差共分散Pを単位行列Iで初期化します

これらの初期設定をMATLABで記すと以下のようになります。

global Ts; Ts = 0.1;
global d; d = 0.2;

u = [2.05;1.95]; % Input Signal Vector
LM = [10;5];     % LandMark Position

w = [0.04;0.04]; % Input Gaussian noise
v = [0.09;0.04]; % Output Gaussian noise
Q = diag([0.04;0.04;0.01]); % Covariance Matrix
R = diag([0.16;0.04]); % Covariance Matrix

xDes = [0;0;deg2rad(0)]; % Desired Trajectory
xAct = [0;0;deg2rad(0)]; % Actual Trajcectory
xEst = [0;0;deg2rad(0)]; % Estimated Trajectory

P = eye(3); % Initialize Covariance Matrix

予測ステップ

予測ステップでは、1つ前の時刻t-1で推定したロボットの状態xEstと入力信号u_tから、時刻tでのロボットの状態xEstの推定（予測）を行います。

ロボットの状態の予測は先程作成した関数StateEquation()を用いて算出します。

この時、状態方程式の非線形関数に含まれてる入力に対する雑音wの影響は考慮せず、雑音はないものとして予測します。

その代わりとして、共分散Qを用いて、推定した状態xEstの確からしさを示す誤差共分散Pを更新します。

誤差共分散Pを算出するために、非線形関数fのヤコビ行列F_tを用います。

この誤差共分散Pを求める計算は何度も行うため、ヤコビ行列F_tを求める関数も用意しておきます。

実際にMATLABで用意したヤコビ行列F_tを求める関数は以下の通りです。

function Ft = JacobianF(x,u)
    global Ts;
    global d;
    
    uR = u(1);
    uL = u(2);

    v = (uR+uL)/2;
    omega = (uR-uL)/(2*d);
    
    Ft = [  
        1, 0, -v*Ts*sin(x(3)+omega*Ts/2);
        0, 1, v*Ts*cos(x(3)+omega*Ts/2);
        0, 0, 1];
end

予測ステップで用いる状態方程式と誤差共分散の式を表すと

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

のようになります。

これら予測ステップでの計算をMATLABで記すと以下のようになります。

Ft = JacobianF(xEst,u);           % Calculate Jacobian f(x)
xEst = StateEquation(xEst,u,w*0); % Predicted State Estimate
P = Ft*P*Ft' + Q;                 % Predicted Error Covariance

更新ステップ

更新ステップでは、実際の観測値zと予測ステップで推定した状態xEstならば得られるはずの観測値との差を求め、カルマンゲインKを求めます。

そして、求めたカルマンゲインKを用いてロボットの推定状態xEstと誤差共分散Pを更新します。

移動ロボットが予測ステップで推定した状態xEstに存在すると考えた時に、得られるはずの観測値は出力方程式より求められるため、先に作成した関数OutputEquation()を用いて算出します。

先程の予測ステップの場合と同様に、この時に出力方程式に含まれる雑音vによる影響は考慮をしません。

その代わり、観測値の確からしさを示す誤差共分散Rを、推定した位置xEstの確からしさを示す誤差共分散Pと共に用いて、カルマンゲインKを算出します。

この際に必要となる非線形関数hのヤコビ行列H_tを求めるための関数を用意しておきます。

実際にMATLABで用意したヤコビ行列H_tを求める関数は以下の通りです。

function Ht = JacobianH(x,LM)
    Ht = [  
        -(LM(1)-x(1))/((LM(1)-x(1))^2+(LM(2)-x(2))^2)^0.5, -(LM(2)-x(2))/((LM(1)-x(1))^2+(LM(2)-x(2))^2)^0.5, 0;
        -(LM(2)-x(2))/((LM(2)-x(2))^2+(LM(1)-x(1))^2), -(LM(1)-x(1))/((LM(2)-x(2))^2+(LM(1)-x(1))^2), -1];
end

そして、求めたカルマンゲインKと観測値yを基にして、ロボットの推定位置xEstと誤差共分散Pを更新します。

この更新ステップで行う計算式を表すと

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

となります。

また、更新ステップでの計算をMATLABで記すと以下のようになります。

Ht = JacobianH(xEst,LM);            % Calculate Jacobian f(x)
zEst = OutputEquation(xEst,v*0,LM); % Predicted Sensor Output

y = z - zEst;     % Innovation or Measurement Pre-fit Residual
S = Ht*P*Ht' + R; % Innovation or Pre-fit Residual Covariance
    
K = P*Ht'/S; % Optimal Kalman Gain
    
xEst = xEst + K*y';    % Updated State Estimate
P = (eye(3) - K*Ht)*P; % Updated Estimate Covariance

シミュレーション結果

この予測ステップと更新ステップを順に繰り返すことで、拡張カルマンフィルタを用いた移動ロボットの自己位置推定を実現することが出来ます。

今回紹介したアルゴリズムを用いて、実際にシミュレーションを行った結果をX-Y平面のグラフに示します。

シミュレーション結果から、目標の軌跡（黒線）から実際のロボットの軌跡（青線）がずれた場合でも、拡張カルマンフィルタを用いた非線形システムの自己位置推定により移動ロボットの現在位置が上手く推定（赤線）できていることが分かります。

まとめ

今回は、拡張カルマンフィルタを用いて非線形動的システムで表される移動ロボットの自己位置推定のアルゴリズムを実装し、実際にシミュレーションを用いて動作の確認を行いました。

今回のシリーズで紹介した方法を用いることで、非線形動的システムで表される移動ロボットの位置を拡張カルマンフィルタを用いて推定することが出来ます。