カルマンフィルタでロボットの自己位置推定をするプログラムの紹介

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

ロボットの自己位置推定(Localization)技術についての情報を紹介します。 カルマンフィルタによる自己位置推定 ...

以前の記事では、移動ロボットの自己位置推定を行うために、カルマンフィルタを用いたアルゴリズムをMATALABで実装し、シミュレーションで動作の確認を行いました。

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

今回の記事では、シミュレーションの際に使用したプログラム(MATLAB)について説明したいと思います。

取り扱う自己位置推定問題

今回のシミュレーションでは、ロボットの状態空間モデルが

$$ \begin{eqnarray} \left\{ \begin{array}{l} \begin{bmatrix} x_t \\ y_t \end{bmatrix} &=& \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} x_{t-1} \\ y_{t-1} \end{bmatrix} + \begin{bmatrix} 0.2 \\ 0.1 \end{bmatrix} + \begin{bmatrix} w_1 \\ w_2 \end{bmatrix} \\ \begin{bmatrix} z_{x,t} \\ z_{y,t} \end{bmatrix} &=& \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} x_{t-1} \\ y_{t-1} \end{bmatrix} + \begin{bmatrix} v_1 \\ v_2 \end{bmatrix} \end{array} \right. \end{eqnarray} $$

で表される線形システムについて、自己位置推定を行います。

詳しくは、こちらの記事を参考にしてください。

今回のシリーズでは、ロボットや自動運転車を制御する際にとても重要となる、ロボットや車の正確な位置を知る方法として、カルマンフィルタを...

使用したプログラム

今回の記事で取り扱っている移動ロボットの自己位置推定の問題について、以下のMATALABのプログラム(コード)を用いてシミュレーションを行いました。

clear;
close all;

F = eye(2);     % State Matrix
H = eye(2);     % Output Matrix

u = [0.2;0.1];  % Input Signal Vector

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

xDes = [0;0];   % Desired State
xAct = [0;0];   % Actual State
xEst = [0;0];   % Estimated State

P = eye(2);     % Initialize Covariance Matrix

Out.xDes = xDes;    % Desired Trajectory
Out.xAct = xAct;    % Actual Trajectory
Out.xEst = xEst;    % Estimated Trajectory

for t = 1:50
    xDes = StateEquation(xDes,u,w*0);   % Update Desired State

    xAct = StateEquation(xAct,u,w);     % Update Actual State
    z = OutputEquation(xAct,v);         % Get Observation Value
    
    xEst = F*xEst + u;  % Predicted State Estimate
    P = F*P*F' + Q;     % Predicted Error Covariance
    
    y = z - H*xEst;     % Innovation or Measurement Pre-fit Residual
    S = R + H*P*H';     % Innovation or Pre-fit Residual Covariance
    
    K = P*H'/S;         % Optimal Kalman Gain
    
    xEst = xEst + K*y;      % Updated State Estimate
    P = (eye(2) - K*H)*P;   % Updated Estimate Covariance
   
    Out.xDes = [Out.xDes,xDes];
    Out.xAct = [Out.xAct,xAct];
    Out.xEst = [Out.xEst,xEst];
end

time = 0:1:length(Out.xAct)-1;

figure;
plot(Out.xDes(1,:),Out.xDes(2,:),'k',Out.xAct(1,:),Out.xAct(2,:),'b',Out.xEst(1,:),Out.xEst(2,:),'r','LineWidth',2);
grid on; axis equal; axis([-1 11 -1 6]);
xlabel('X-axis [m]'); ylabel('Y-axis [m]')

return


function x = StateEquation(x,u,w)
    x = x + u + normrnd(0,w);
end

function z = OutputEquation(x,v)
    z = x + normrnd(0,v);
end

プログラム内で使用している理論や数式についての詳細は、こちらの記事を参考にしてください。

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

初期条件

シミュレーションに必要となる初期条件を以下のように設定します。

clear;
close all;

F = eye(2);     % State Matrix
H = eye(2);     % Output Matrix

u = [0.2;0.1];  % Input Signal Vector

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

xDes = [0;0];   % Desired State
xAct = [0;0];   % Actual State
xEst = [0;0];   % Estimated State

P = eye(2);     % Initialize Covariance Matrix

Out.xDes = xDes;    % Desired Trajectory
Out.xAct = xAct;    % Actual Trajectory
Out.xEst = xEst;    % Estimated Trajectory

初期条件を設定した後、for文を使って以下の項目を繰り返します。

ロボットの状態、センサ出力の算出

まず繰り返し部分の初めに、時刻tでのロボットの状態xとセンサによる観測値zを算出します。

xDes = StateEquation(xDes,u,w*0);   % Update Desired State

xAct = StateEquation(xAct,u,w);     % Update Actual State
z = OutputEquation(xAct,v);         % Get Observation Value

今回は、プログラムの便利性を上げるために、それぞれロボットの状態とセンサの観測値を算出する関数(StateEquationOutputEquation)を定義しました。

function x = StateEquation(x,u,w)
    x = x + u + normrnd(0,w);
end

function z = OutputEquation(x,v)
    z = x + normrnd(0,v);
end

目標の状態xDesは雑音wを含めず算出するため、雑音wを0として状態xDesを算出します。

実際の状態xActの算出には雑音wによる影響を考慮するため、雑音wを含めて状態xActを算出します。

そして、実際の状態xActを用いてセンサによる観測値zを算出します。

予想ステップ

次に、1つ前の時刻t-1で推定したロボットの状態xEstと入力信号utから、時刻tでのロボットの状態xEstを推定します。

xEst = F*xEst + u;  % Predicted State Estimate
P = F*P*F' + Q;     % Predicted Error Covariance

更新ステップ

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

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

結果保存~繰り返し

最後に、更新したロボットの推定状態xEstを保存して、次の時刻t+1に移行して繰り返します。

Out.xDes = [Out.xDes,xDes];
Out.xAct = [Out.xAct,xAct];
Out.xEst = [Out.xEst,xEst];

まとめ

今回の記事では、以前に紹介したカルマンフィルタを用いた移動ロボットの自己位置推定について、シミュレーションで使用したプログラムを紹介しました。

紹介したプログラムは洗練されていないと思いますが、カルマンフィルタの基本式を順に計算していく形になっているので、理解はして頂けるかなと思います。

スポンサーリンク
レクタングル(大)広告
レクタングル(大)広告

シェアする

  • このエントリーをはてなブックマークに追加

フォローする

関連コンテンツ