EKF ベースのランドマーク SLAM

この例では、ekfSLAM オブジェクトを使用して、データの関連付けに拡張カルマン フィルター (EKF) アルゴリズムと最尤アルゴリズムを使用したランドマークの自己位置推定と環境地図作成の同時実行 (SLAM) を確実に実装する方法を説明します。この例では、ビークルの周辺のランドマーク マップを作成し、同時にビークルのパスを追跡します。ノイズを含む制御コマンドを使用してビークルを移動することで軌跡を生成し、パスに沿って検出したランドマークを使用してマップを作成します。ランドマークを再度観測して、ビークルの軌跡とランドマークの推定を補正します。

データ セットの読み込み

コントローラー入力、測定値、GPS の緯度と経度、およびコントローラー入力と運動モデルを使用して生成されたデッド レコニングを含むヴィクトリア パークのデータ セットの変更されたバージョンを読み込みます。

load("victoriaParkDataset.mat","controllerInput", ...
     "measurements","gpsLatLong","deadReckoning");

パラメーターの設定

ビークルの初期状態と状態の共分散を指定します。

initialState = [gpsLatLong(1,2) gpsLatLong(1,1) deg2rad(37)]';
initialCovar = eps*eye(3);

速度コマンドとステアリング コマンドでプロセス ノイズの共分散を指定します。

sigmaVelocity = 2;      % [m/s]
sigmaSteer= deg2rad(6); % [rad]
processNoise = [sigmaVelocity^2 0; 0 sigmaSteer^2];

範囲と方向で測定共分散を指定します。

sigmaRange = 1;            % [m]
sigmaBearing = deg2rad(3); % [rad]
measCovar = [sigmaRange^2 sigmaBearing^2];

関連付けについてランドマークをチェックする最大範囲を指定します。

maxSensorRange = 30; % [m]

ビークルが移動するタイム ステップ サイズを指定します。

timeStep = 0.025; % [sec]

データ関連付け関数 nav.algs.associateMaxLikelihood のしきい値を指定します。ランドマーク棄却しきい値は、カイ二乗 ($${\chi }^2$$) 分布表における 95% の正しい関連付けを表す値です。

landmarkRejectionThres = 5.991;  % maximum distance for association
landmarkAugmentationThres = 200; % minimum distance for creation of new landmark
validationGate = [landmarkRejectionThres landmarkAugmentationThres];

実行中にマップをプロットするかどうかを判定するフラグを設定します。

plotOnTheRun = false;

データ セットのサンプルの数を取得します。

numSamples = size(controllerInput,1);

Figure または進行状況バーの初期化

ライブ プロット オプションが有効になっている場合は、新しい Figure を作成し、さまざまなコンポーネントをプロットするためのハンドルを設定します。それ以外の場合は、実行されたデータ セットの比率で更新されるウェイト バーを作成します。[キャンセル] をクリックすると進行中の計算が中止されるようにウェイト バーを設定します。ウェイト バーを閉じると実行が中止されます。

if plotOnTheRun
    [robotHandle,covarianceHandle,sensorHandle, ...
     observationHandle,landmarkHandle, ...
     deadRecHandle,estTrajHandle] = ...
        exampleHelperInitializeVisualizationEKFSLAM(initialState,gpsLatLong);
else
    waitBarHandle = waitbar(0,'Example initializing...', ...
        'Name','ekfSLAM example', ...
        'CreateCancelBtn','setappdata(gcbf,''canceling'',1)');
    setappdata(waitBarHandle,'canceling',0);
end

ekfSLAM の設定

ビークルの初期状態、初期状態の共分散、プロセス ノイズの共分散、およびビークルの運動モデルを使用してekfSLAMオブジェクトを構成します。

ekfSlamObj = ekfSLAM('State',initialState, ...
    'StateCovariance',initialCovar, ...
    'StateTransitionFcn',@exampleHelperVictoriaParkStateTransition);
ekfSlamObj.ProcessNoise = processNoise;
ekfSlamObj.MaxAssociationRange = maxSensorRange;

メイン ループ

メイン ループは次の主要な処理で構成されます。

• 予測 — 制御コマンドと現在の状態に基づき、次の状態を予測します。

• ランドマーク抽出 — 環境内のランドマークを取得します。

• 補正 — 観測されたランドマークを使用して状態と状態の共分散を更新します。

予測

この例では、ビークルは制御入力に基づいて前の状態を基準として移動しますが、ランドマークは静止したままです。したがって、ビークルの状態のみが伝播されます。ビークルの運動モデルを使用して、データ セットを使用することでビークルの状態を次のタイム ステップに伝播します。

predictメソッドは、ekfSlamObj の StateTransitionFcn プロパティで指定された関数を呼び出して、ビークルの状態を予測します。predict メソッドは、制御入力、その他の必要な入力、および現在のビークルの姿勢を StateTransitionFcn. に渡します。

for count = 1:numSamples
    predict(ekfSlamObj,controllerInput(count,:),timeStep);

ランドマーク抽出

この例では、データ セットの観測されたランドマークのリストを使用するため、センサーを使用して環境からランドマークを抽出する必要はありません。ランドマークは、ビークル前方の半円領域から得られます。半円領域の半径は、センサーの最大範囲によって定義されます。

    observedLandmarks = measurements{count};

補正

ekfSLAM オブジェクトは、与えられた測定値に基づいて状態を補正し、一致したランドマークのリストと新しいランドマークのリストを返します。correctメソッドは、ekfSlamObj の DataAssociationFcn プロパティで指定された関数を使用して、観測されたランドマークまたは測定値を既知のランドマークに関連付けます。データ関連付け関数は、関連付けのリストと新しいランドマークのリストを返します。correct メソッドは、関連付けを使用して、観測されたランドマークと既知のランドマークの間の相関を使用することでビークルとマップの確率を更新します。さらに、correct メソッドは、State ベクトルと StateCovariance ベクトル内で新しいランドマークの位置と共分散をそれぞれ拡張します。

ライブ プロット オプションが有効になっている場合は、Figure を現在時間のスキャンで更新します。

  if ~isempty(observedLandmarks)
        correct(ekfSlamObj,observedLandmarks,measCovar,validationGate);
        % Update the corrected position of vehicle in the figure
        if plotOnTheRun
            exampleHelperUpdateScans(ekfSlamObj.State, ...
                              ekfSlamObj.MaxAssociationRange, ...
                              observedLandmarks, ...
                              sensorHandle,observationHandle);
        end
  end

drawnow を使用して、Figure をビークルの現在の位置およびすべての既知のランドマークとそれらの共分散で更新します。

ライブ プロットが無効で、ウェイト バーがアクティブな場合は、実行されたデータ セットの割合を更新します。

    if plotOnTheRun
        addpoints(deadRecHandle,deadReckoning(count,1),deadReckoning(count,2));
        addpoints(estTrajHandle,ekfSlamObj.State(1),ekfSlamObj.State(2));
        exampleHelperUpdateRobotAndLandmarks(ekfSlamObj.State,ekfSlamObj.StateCovariance, ...
            robotHandle,covarianceHandle,landmarkHandle);
        drawnow limitrate
    else
        % update the wait bar
        waitbar(count/numSamples,waitBarHandle, ...
            sprintf("%2.2f%% executed",count/numSamples*100));
        % Check for clicked Cancel button
        if getappdata(waitBarHandle,"canceling")
            break
        end
    end
end

すべての計算が完了したら、関数 delete を使用してウェイト バーを閉じます。

if ~plotOnTheRun
    delete(waitBarHandle);
end

マップの可視化

GPS データのグラウンド トゥルースを使用して地理プロットを作成します。ビークルの軌跡を、検出されたすべてのランドマークおよびそれらに関連する共分散と共に、同じ地理プロット上にプロットします。

% get the corrected and predicted poses
[corrPoses,predPoses] = poseHistory(ekfSlamObj);

% show the map
exampleHelperShowMap(ekfSlamObj.State,ekfSlamObj.StateCovariance, ...
                     gpsLatLong,corrPoses,predPoses);

参考

ekfSLAM

関連するトピック

• Fuse Inertial Sensor Data Using insEKF-Based Flexible Fusion Framework
• Landmark SLAM Using AprilTag Markers
• LIDAR スキャンでの自己位置推定と環境地図作成の同時実行 (SLAM) の実装