Main Content

このページの内容は最新ではありません。最新版の英語を参照するには、ここをクリックします。

粒子フィルターを使用した自動車型ロボットの追跡

粒子フィルターは、サンプリングベースの再帰ベイズ推定アルゴリズムであり、stateEstimatorPF オブジェクトに実装されています。この例では、リモート制御の自動車型ロボットを屋外環境で追跡します。ロボットの姿勢の測定値はオンボードの GPS によって提供され、ノイズを含みます。ロボットに送信される既知の運動コマンドがありますが、機械の緩みやモデルの精度の低さにより、ロボットは指示された動きを正確に実行しません。この例では、stateEstimatorPFを使用して測定データにおけるノイズの影響を低減し、ロボットの姿勢をより正確に推定する方法を説明します。自動車型ロボットの運動学モデルは、以下の非線形システムによって説明されます。粒子フィルターは、本質的な非線形性を処理できるため、そのようなシステムの状態推定に最適です。

x˙=vcos(θ)y˙=vsin(θ)θ˙=vLtanϕϕ˙=ω

シナリオ: 自動車型ロボットが駆動し、速度とステアリング角度を連続的に変更します。ロボットの姿勢は、GPS や Vicon システムなど、ノイズを含む外部システムによって測定されます。パスの途中には屋根付きの場所を走行する領域があり、そこでは測定は実施できません。

入力:

  • ロボットの部分的姿勢に関するノイズを含む測定値 (xyθ)。メモ: これは完全な状態の測定ではありません。前輪の向き (ϕ) およびすべての速度 (x˙y˙θ˙ϕ˙) に関する測定値はありません。

  • ロボットに送信される線形速度および角速度のコマンド (vcωc)。メモ: 指示された運動とロボットの実際の運動にはある程度差異があります。

目的: 自動車型ロボットの部分的姿勢 (xyθ) を推定します。メモ: ここでも、車輪の向き (ϕ) は推定に含まれません。オブザーバーの視点で、自動車の完全な状態は [x, y, θ, x˙, y˙, θ˙] のみです。

方法: stateEstimatorPFを使用してノイズを含む 2 つの入力 (いずれの入力も単独では信頼性が低い) を処理し、現在の (部分的) 姿勢について最良の推定を行います。

  • 予測段階では、以下のような簡易的な一輪車型のロボット モデルで粒子の状態を更新します。状態の推定に使用するシステム モデルは、実際のシステムを正確に表現したものではないことに注意してください。モデルの差異がシステム ノイズの中で十分に取得される場合は、これは許容されます (粒子群で表現)。詳細については、predictを参照してください。

x˙=vcos(θ)y˙=vsin(θ)θ˙=ω

  • 補正段階では、粒子の重要度の重み (尤度) が、現在の測定値の誤差ノルム ((Δx)2+(Δy)2+(Δθ)2) によって決定されます。これは、これら 3 つの成分についてのみ測定値があるためです。詳細については、correctを参照してください。

自動車型のロボットの初期化

rng('default'); % for repeatable result
dt = 0.05; % time step
initialPose = [0  0  0  0]';
carbot = ExampleHelperCarBot(initialPose, dt);

Figure CarBot contains an axes object. The axes object with title t = 0, xlabel x (m), ylabel y (m) contains 11 objects of type rectangle, scatter, line, text. One or more of the lines displays its values using only markers These objects represent actual pose, estimated pose.

粒子フィルターの設定

この節では、5000 個の粒子を使用して粒子フィルターを設定します。最初は、初期状態の平均値と単位共分散を使用して、すべての粒子が正規分布からランダムに抽出されます。各粒子には、6 つの状態変数 (xyθx˙y˙θ˙) が含まれています。3 番目の変数は Circular としてマークされていますが、これは、自動車の向きであるためです。2 つのコールバック関数 StateTransitionFcn および MeasurementLikelihoodFcn を指定することも非常に重要です。これら 2 つの関数は、粒子フィルターのパフォーマンスを直接決定します。これら 2 つの関数の詳細については、この例の最後の 2 つの節を参照してください。

pf = stateEstimatorPF;

initialize(pf, 5000, [initialPose(1:3)', 0, 0, 0], eye(6), 'CircularVariables',[0 0 1 0 0 0]);
pf.StateEstimationMethod = 'mean';
pf.ResamplingMethod = 'systematic';

% StateTransitionFcn defines how particles evolve without measurement
pf.StateTransitionFcn = @exampleHelperCarBotStateTransition;

% MeasurementLikelihoodFcn defines how measurement affect the our estimation
pf.MeasurementLikelihoodFcn = @exampleHelperCarBotMeasurementLikelihood;

% Last best estimation for x, y and theta
lastBestGuess = [0 0 0];

メイン ループ

この例では、ロボットに対して指示された線形速度と角速度は、任意に抽出された時間依存の関数です。また、ループの固定レートのタイミングは、rateControlによって実現されます。

固定レートのサポートを使用して、20 Hz で 20 秒間ループを実行します。

r = rateControl(1/dt);

固定レート オブジェクトをリセットしてタイマーを再開します。時間依存コードを実行する直前にタイマーをリセットします。

reset(r);

simulationTime = 0; 

while simulationTime < 20 % if time is not up

    % Generate motion command that is to be sent to the robot
    % NOTE there will be some discrepancy between the commanded motion and the
    % motion actually executed by the robot. 
    uCmd(1) = 0.7*abs(sin(simulationTime)) + 0.1;  % linear velocity
    uCmd(2) = 0.08*cos(simulationTime);            % angular velocity
    
    drive(carbot, uCmd);
        
    % Predict the carbot pose based on the motion model
    [statePred, covPred] = predict(pf, dt, uCmd);

    % Get GPS reading
    measurement = exampleHelperCarBotGetGPSReading(carbot);

    % If measurement is available, then call correct, otherwise just use
    % predicted result
    if ~isempty(measurement)
        [stateCorrected, covCorrected] = correct(pf, measurement');
    else
        stateCorrected = statePred;
        covCorrected = covPred;
    end

    lastBestGuess = stateCorrected(1:3);

    % Update plot
    if ~isempty(get(groot,'CurrentFigure')) % if figure is not prematurely killed
        updatePlot(carbot, pf, lastBestGuess, simulationTime);
    else
        break
    end

    waitfor(r);
    
    % Update simulation time
    simulationTime = simulationTime + dt;
end

Figure CarBot contains an axes object. The axes object with title t = 19.95, xlabel x (m), ylabel y (m) contains 11 objects of type rectangle, scatter, line, text. One or more of the lines displays its values using only markers These objects represent actual pose, estimated pose.

結果の Figure の詳細

3 つの Figure は、粒子フィルターの追跡パフォーマンスを示しています。

  • 1 つ目の Figure では、粒子フィルターは、初期姿勢から走り去る自動車を十分に追跡しています。

  • 2 つ目の Figure では、ロボットが屋根付きの領域に入ります。ここでは測定を行うことができず、粒子は予測モデル (オレンジ色のマーク) のみに基づいて変化します。粒子の前部が徐々に馬蹄型になり、推定姿勢が徐々に実際の姿勢から逸脱することを確認できます。

  • 3 つ目の Figure では、ロボットが屋根付きの領域の外に出ています。新しい測定値により、推定姿勢は徐々に実際の姿勢に収束します。

状態遷移関数

サンプリングベースの状態遷移関数は、指定の運動モデルに基づいて粒子を変化させ、粒子が提案分布を表現するようにします。以下は、一輪車型ロボットの速度運動モデルに基づいた状態遷移関数の例です。この運動モデルの詳細については、[1] の第 5 章を参照してください。sd1sd2 および sd3 の値を小さくして、追跡パフォーマンスがどの程度低下するかを確認します。ここで、sd1 は線形速度の不確定性、sd2 は角速度の不確定性を表します。sd3 は向きに関する追加の摂動です。

   function predictParticles = exampleHelperCarBotStateTransition(pf, prevParticles, dT, u)
       thetas = prevParticles(:,3);
       w = u(2);
       v = u(1);
       l = length(prevParticles);
       % Generate velocity samples
       sd1 = 0.3;
       sd2 = 1.5;
       sd3 = 0.02;
       vh = v + (sd1)^2*randn(l,1);  
       wh = w + (sd2)^2*randn(l,1); 
       gamma = (sd3)^2*randn(l,1); 
       % Add a small number to prevent div/0 error
       wh(abs(wh)<1e-19) = 1e-19;
       % Convert velocity samples to pose samples
       predictParticles(:,1) = prevParticles(:,1) - (vh./wh).*sin(thetas) + (vh./wh).*sin(thetas + wh*dT);
       predictParticles(:,2) = prevParticles(:,2) + (vh./wh).*cos(thetas) - (vh./wh).*cos(thetas + wh*dT);
       predictParticles(:,3) = prevParticles(:,3) + wh*dT + gamma*dT;
       predictParticles(:,4) = (- (vh./wh).*sin(thetas) + (vh./wh).*sin(thetas + wh*dT))/dT;
       predictParticles(:,5) = ( (vh./wh).*cos(thetas) - (vh./wh).*cos(thetas + wh*dT))/dT;
       predictParticles(:,6) = wh + gamma;
   end

測定尤度関数

測定尤度関数は、粒子と測定値の間の誤差ノルムに基づいて、各予測粒子の尤度を計算します。計算された尤度に基づいて、各粒子の重要度の重みが割り当てられます。この例で、predictParticles は N 行 6 列の行列 (N は粒子の数)、measurement は 1 行 3 列のベクトルです。

   function  likelihood = exampleHelperCarBotMeasurementLikelihood(pf, predictParticles, measurement)
       % The measurement contains all state variables
       predictMeasurement = predictParticles;
       % Calculate observed error between predicted and actual measurement
       % NOTE in this example, we don't have full state observation, but only
       % the measurement of current pose, therefore the measurementErrorNorm
       % is only based on the pose error.
       measurementError = bsxfun(@minus, predictMeasurement(:,1:3), measurement);
       measurementErrorNorm = sqrt(sum(measurementError.^2, 2));
       % Normal-distributed noise of measurement
       % Assuming measurements on all three pose components have the same error distribution 
       measurementNoise = eye(3);
       % Convert error norms into likelihood measure. 
       % Evaluate the PDF of the multivariate normal distribution 
       likelihood = 1/sqrt((2*pi).^3 * det(measurementNoise)) * exp(-0.5 * measurementErrorNorm);
   end

参考文献

[1] S. Thrun, W. Burgard, D. Fox, Probabilistic Robotics, MIT Press, 2006