モンテカルロ位置推定アルゴリズム

概要

モンテカルロ位置推定 (MCL) アルゴリズムは、ロボットの位置と向きを推定するために使用されます。このアルゴリズムでは、環境の既知のマップ、距離センサーのデータ、オドメトリセンサーのデータを使用します。オブジェクトを作成してこのアルゴリズムを使用する方法については、monteCarloLocalization を参照してください。

ロボットの位置推定を行うために、MCL アルゴリズムでは粒子フィルターを使用して位置を推定します。粒子は、ロボットについて、可能性の高い状態の分布を表します。各粒子はロボットが取り得る状態を表します。ロボットが環境内で動き、距離センサーを使用して環境内のさまざまな部分を検出する中で、粒子は 1 か所の周りに収束します。ロボットの動きは、オドメトリセンサーを使用して検出されます。

粒子は以下のプロセスで更新されます。

粒子は、姿勢の変化と指定した運動モデル MotionModel に基づいて伝播します。
粒子には、各粒子の距離センサーの読み取り値を受信する可能性に基づいて、重みが割り当てられています。この読み取り値は、SensorModel で指定したセンサーモデルに基づきます。
これらの重みに基づいて、ロボットの状態の推定が、粒子の重みに基づいて抽出されます。最も重みの大きい粒子のグループが、ロボットの位置推定に使用されます。
最後に、指定した ResamplingInterval に基づいて粒子がリサンプリングされます。リサンプリングでは、粒子の位置が調整され、使用する粒子数を調整することによってパフォーマンスが向上します。これは、変化に合わせて調整し、粒子とロボットの状態推定との関連性を維持するための重要な機能です。

アルゴリズムにより、推定姿勢と共分散が出力されます。この推定は、最も重みの大きい粒子のクラスターの平均と共分散です。継続的に追跡するには、これらのステップをループで繰り返して粒子を伝播し、粒子の尤度を評価して、最良の状態推定を取得します。

一般的なアプリケーションとしての粒子フィルターの詳細については、粒子フィルターワークフローを参照してください。

状態表現

位置推定アルゴリズムで作業する際の目標は、システムの状態を推定することです。ロボティクスアプリケーションでは、この推定状態は通常、ロボットの姿勢になります。monteCarloLocalization オブジェクトでは、この姿勢を 3 要素ベクトルとして指定します。姿勢は x-y 位置 ([x y]) と、角度方向 (theta) に対応します。

MCL アルゴリズムは、これら 3 つの値を環境のセンサー入力と、システムの特定の運動モデルに基づいて推定します。monteCarloLocalization オブジェクトの使用による出力には pose が含まれます。これは、[x y theta] 値の最適推定状態です。粒子は初期姿勢 InitialPose の周辺に分布するか、グローバル位置推定に基づいて一様にサンプリングされます。これらの粒子が測定値に基づいて補正された後、最も重みの大きい粒子のクラスターの平均値として姿勢が計算されます。

このプロットでは、最も重みの大きいクラスターと最終的なロボットの姿勢を、緑色のサンプル粒子の上に示しています。MCL アルゴリズムと測定補正をさらに反復するうちに、粒子はロボットの真の位置に収束します。しかし、粒子のクラスターは、誤った推定によって重みが大きくなり、誤った位置に収束することがあります。誤った収束が発生した場合、InitialPose を更新して MCL アルゴリズムをリセットし、粒子をリサンプリングしてください。

粒子の初期化

monteCarloLocalization アルゴリズムを最初に作成するときに、ParticleLimits プロパティを使用して粒子の下限と上限を指定します。粒子数を増やすと、粒子が実際の位置で収束する尤度が上がります。ただし、粒子数が少ない方が高速です。粒子数は、粒子のクラスターの重みに基づいて、制限の範囲内で動的に調整されます。この調整は、時間の経過に伴って粒子数を減らすことで、より効率的に位置推定を実行するのに役立ちます。

粒子の分布

粒子は指定した分布でサンプリングされなければなりません。状態空間内で粒子を初期化するには、初期姿勢またはグローバル位置推定を使用できます。グローバル位置推定を使用すると、想定される状態空間 (SensorModel オブジェクトの Map プロパティから取得) 全体にわたって粒子を一様に分布させることができます。既定の MCL オブジェクトで、GlobalLocalization プロパティを true に設定します。

mcl = monteCarloLocalization;
mcl.GlobalLocalization = true;

グローバル位置推定では、状態空間全体で粒子を効果的にサンプリングするのに多数の粒子が必要になります。粒子を増やすほど、実際の状態で正しく収束する尤度が上がります。この大規模な分布では、粒子が収束し始めて粒子数を減らせるようになるまで、初期パフォーマンスが大幅に低下します。

既定では、グローバル位置推定は false に設定されています。グローバル位置推定を使用しない場合、粒子の位置推定に役立つ InitialPose および InitialCovariance プロパティを指定しなければなりません。この初期姿勢を使用すると、推定状態の周辺に、粒子同士がより密集してグループ化されるようになります。粒子を密集させてグループ化すると、使用する粒子が少なくてすみ、初期の反復中に追跡の速度と正確性が上がります。

これらのイメージはLocalize TurtleBot Using Monte Carlo Localization Algorithmの例から引用したものです。この例では、既知の環境で MCL アルゴリズムを TurtleBot^® と共に使用する方法を説明しています。

粒子のリサンプリングと姿勢の更新

ロボットの位置推定を継続的に行うには、粒子をリサンプリングしてアルゴリズムを更新しなければなりません。UpdateThreshold および ResamplingInterval プロパティを使用して、推定状態に対するリサンプリングと更新を行うタイミングを制御します。

UpdateThreshold は、更新をトリガーするためのロボットの姿勢の最小変化 [x y theta] を定義する 3 要素ベクトルです。この最小値より大きく変数を変化させると更新がトリガーされ、オブジェクトが新しい状態推定を返します。ロボットの姿勢の変化はオドメトリに基づきます。この値はオブジェクトの関数形式によって指定されます。センサーのプロパティとロボットの動きに合わせて、これらのしきい値を調整してください。ランダムノイズまたはわずかな変動がしきい値より大きいと、不要な更新がトリガーされ、パフォーマンスに影響が生じる場合があります。ResamplingInterval プロパティは、粒子のリサンプリングをトリガーする更新回数を定義します。たとえば、リサンプリング間隔 2 では、更新 1 回おきにリサンプリングが行われます。

粒子のリサンプリングの利点は、最終的な推定に寄与する可能性のある位置を更新することです。リサンプリングは、粒子を重みに基づいて再分布し、運動モデルに基づいて粒子を変化させます。このプロセスでは、重みの小さい粒子が除外されるため、粒子がロボットの真の状態に収束しやすくなります。追跡速度を改善するため、粒子数は動的に変動します。

アルゴリズムの速度は、適切なリサンプリングに依存します。粒子が広く分散していて、ロボットの初期姿勢が不明な場合、アルゴリズムは粒子数を大きな数に維持します。アルゴリズムが真の位置に収束するにしたがって、粒子数が減少し、パフォーマンス速度が向上します。ParticleLimits プロパティを調整して粒子の最小数と最大数を制限し、パフォーマンス向上に役立てることができます。

運動およびセンサーモデル

MCL アルゴリズムでの運動モデルおよびセンサーモデルは、stateEstimatorPF オブジェクトの関数 StateTransitionFcn および MeasurementLikelihoodFcn に似ています。これらの関数については、粒子フィルターのパラメーターで説明しています。MCL アルゴリズムでは、これらのモデルは、ロボットの位置推定にさらに特化しています。オブジェクトを呼び出した後、MotionModel または SensorModel プロパティを変更するには、まずオブジェクトで release を呼び出さなければなりません。

センサーモデル

既定で、monteCarloLocalization は likelihoodFieldSensorModel オブジェクトをセンサーモデルとして使用します。このセンサーモデルには、使用中の距離センサー固有のパラメーター、ロボット環境用の 2 次元マップ情報、および測定ノイズ特性が含まれます。センサーモデルはパラメーターを距離測定で使用して、指定されたロボットの現在位置で測定の尤度を計算します。これらのパラメーターを考慮しないと、測定誤差により状態推定がゆがんだり、無関係な粒子の重みが増加したりすることがあります。

距離センサーのプロパティは次のとおりです。

SensorPose – ロボットの位置との相対値である、距離センサーの姿勢。この姿勢は、距離の読み取り値をロボット座標系に変換するために使用します。
SensorLimits – 距離の下限と上限。これらの距離の範囲外の測定値は、尤度計算で考慮されません。
NumBeams – 尤度の計算に使用するビーム数。使用するビーム数を減らすことで、パフォーマンス速度を改善できます。

距離測定では、システムノイズやその他の環境による干渉が原因で、誤った読み取り値が発生することも知られています。センサーの誤差を考慮するには、次のパラメーターを指定します。

MeasurementNoise – 測定ノイズの標準偏差。この偏差は、距離の読み取り値に適用され、センサーの干渉を考慮します。距離センサーからの情報に基づいてこの値を設定してください。
RandomMeasurementWeight — ランダム測定値の確率の重み。ランダム測定値には低い確率を設定します。既定値は 0.05 です。
ExpectedMeasurementWeight — 想定される測定値の確率の重み。想定される測定値には高い確率を設定します。既定値は 0.95 です。

また、センサーモデルは、ロボット環境のマップを占有グリッドとして保存します。binaryOccupancyMap を使用して、マップに占有スペースとフリースペースを指定します。マップ内の不明なスペースはすべてフリー位置として設定します。不明なスペースをフリー位置に設定することで、検出されたオブジェクトをアルゴリズムがマップのこれらの領域に一致させることを防ぎます。

また、障害物の検索領域を制限する MaximumLikelihoodDistance も指定できます。MaximumLikelihoodDistance の値は、最も近い障害物との最大距離で、尤度計算に使用します。

運動モデル

ロボットの位置推定用の運動モデルは、リサンプリングの際に粒子が時間の経過に伴ってどのように変化するかを予測するのに役立ちます。これは、ロボット運動学の表現です。MCL アルゴリズムに既定で含まれる運動モデルは、オドメトリベースの差動駆動型運動モデル (odometryMotionModel) です。運動モデルがなければ、次のステップの予測はさらに難しくなります。位置推定アルゴリズムで粒子分布を計画し、より良好な状態推定を得るために、システムの機能について知っておくことが重要です。ホイールエンコーダー、またはオドメトリの測定に使用されるその他のセンサーからの誤差を必ず考慮してください。システムの誤差は、粒子分布の広がりを定義します。

ロボットの運動に基づく想定誤差を、4 要素ベクトル Noise として指定できます。次の 4 つの要素が標準偏差の重みとして指定されます[1]。

回転運動による回転誤差
並進運動による回転誤差
並進運動による並進誤差
回転運動による並進誤差

差動駆動型ロボットでは、ロボットが開始姿勢から最終姿勢まで動くときに、姿勢の変化を次のように扱うことができます。

最終位置への回転
最終位置への直線的並進
ゴールの向きへの回転

これらのステップを仮定すると、回転時と並進時の誤差の影響を可視化できます。初期回転に誤差があると、可能な位置が最終位置の周りに C 型に広がります。

大きな並進誤差があると、可能な位置が最終位置への直線の周りに広がります。

並進と回転の両方の誤差が大きいと、位置がより広い範囲に広がる場合があります。

さらに、回転誤差は最終的な姿勢の向きに影響します。これらの影響について理解すると、特定のアプリケーションで MotionModel オブジェクトの Noise プロパティのガウスノイズを定義するのに役立ちます。図に示すとおり、各パラメーターは分散を直接制御せず、ロボットのコンフィギュレーションとジオメトリによって変わります。また、ロボットが環境を移動するにつれて、姿勢が複数回変化すると、多くのステップにわたってこれらの誤差の影響が増す場合があります。これらのパラメーターを正確に定義することで、粒子が適切に分布され、ロボットの位置に関する最良の推定を見つけるのに十分な数の仮説が MCL アルゴリズムに与えられます。