状態推定

概要

ロボットの状態を直接計測できるセンサはありません。エンコーダが計測するのは車輪の回転であり、位置ではありません。慣性計測装置（IMU）が計測するのは角速度と比力であり、姿勢ではありません。カメラが計測するのはピクセルです。状態推定は、これらの計測値とロボットの動きのモデルを組み合わせます。そして位置・速度・姿勢など、コントローラが必要とする量について現時点での最良の推定値を求め、その推定がどれだけ不確かかも併せて示します。

不確かさは推定値と同じくらい重要です。自分の位置を5 cmの精度で把握しているプランナは戸口を通り抜けられますが、不確かさが50 cmあるプランナは通り抜けを試みるべきではありません。このハブは、自己位置推定やフュージョンのスタックを構築・調整するエンジニアと、ロボットの位置姿勢の推定値が跳ぶ、ドリフトする、発散するといった問題を抱えている人に向けたものです。

重要な考え方

ベイズフィルタ

このページで扱う手法はすべて、一つの再帰計算を近似したものです。信念（belief）は状態についての確率分布です。予測ステップでは、運動モデルが信念を時間方向に進め、不確かさが大きくなります。更新ステップでは、計測モデルが、考えられる各状態が新しい計測値をどれだけよく説明するかを評価し、信念が鋭くなります。この再帰は、現在の状態が過去の関連情報をすべて含んでいると仮定します。これがマルコフ仮定です。フィルタがおかしな挙動を示すとき、原因はたいていアルゴリズムではなく、モデルやそのノイズレベルがロボットに合っていないことです。

カルマンフィルタの系統

運動モデルと計測モデルが線形で、ノイズがガウス分布に従うなら、ベイズフィルタには厳密な閉形式解があります。これがカルマンフィルタで、Kalmanの1960年の論文に由来します。信念は平均と共分散で表され、各ステップは数回の行列演算で済みます。

実際のロボットは非線形です。方位角が正弦や余弦を通じてモデルに入るためです。拡張カルマンフィルタ（EKF）は、現在の推定値のまわりでヤコビ行列を使ってモデルを線形化します。計算が軽く、推定値が真値に近くモデルが滑らかなときによく機能します。JulierとUhlmann（2004）が述べたアンセンテッドカルマンフィルタ（UKF）は、代わりに少数のシグマ点を非線形モデルそのものに通して伝播させます。ヤコビ行列が不要で、わずかな追加コストでより強い非線形性に対応できます。姿勢については、多くの実用的なフィルタが/topics/spatial-math/で説明している接空間上の小さな誤差状態を推定します。これにより四元数の正規化が保たれます。

チューニングは、プロセスノイズQと計測ノイズRの設定に集約されます。確信が強すぎるフィルタは新しい計測値を無視し、不確かさが大きすぎるフィルタはノイズを追いかけます。精度だけでなく一貫性も確認してください。イノベーション（計測値から予測値を引いたもの）は、平均がゼロで、フィルタが予測する共分散をもつノイズに見えるはずです。

粒子フィルタ

粒子フィルタは、信念を重み付きの状態サンプルの集合で表します。予測では、各粒子をサンプリングしたノイズとともに運動モデルに通して動かします。更新では計測の尤度で重みを付け直し、リサンプリングで可能性の低い粒子を捨てます。互いに離れた複数の仮説を同時に表せるため、自分がどこにいるかまったくわからない状態から始まるロボットでも自己位置推定ができ、ロボットが手で動かされたときにも回復できます。計算コストは状態の次元とともに急速に増えます。そのため粒子フィルタは、既知の地図上での2次元の自己位置推定ではよく使われますが、6 自由度（DoF）すべての推定に使われることはまれです。Nav2のAMCLはこの種の粒子フィルタです。

センサフュージョンとファクターグラフ

フュージョンは、誤差の性質が互いに補い合うセンサを組み合わせます。車輪のオドメトリは滑らかですがドリフトします。IMUは高速ですが位置がドリフトします。GPSは絶対位置を与えますが、ノイズが大きく途切れがちです。フィルタはこれらを計測値一つずつ融合し、遅れて届く計測値や順序が入れ替わった計測値にも対処しなければなりません。

ファクターグラフは別の方法をとります。未知量（位置姿勢、ランドマーク、センサバイアス）を変数とします。各計測値は、それが依存する変数同士をつなぐファクターになります。ソルバは最新の状態だけを保持するのではなく、すべてのファクターを最もよく満たす全変数の値の割り当てを求めます。これは非線形最小二乗問題です。過去の状態を再線形化するためEKFより精度が高く、疎な構造のおかげで高速さも保てます。iSAM2のようなインクリメンタルなソルバは、新しいファクターが届くたびに解を更新します。標準的な入門書はDellaertとKaess（2017）です。同じ仕組みが、/topics/slam-and-mapping/で扱う同時自己位置推定と地図作成（SLAM）のバックエンドの基盤になっています。

どこから始めるか

/learn/beginner/06-sensors-and-noise/ではフィルタに必要なノイズモデルを作り、/learn/beginner/07-differential-drive/では運動モデルを導きます。フィルタを実装する間は/reference/kalman-filter/を開いておいてください。その後、プロジェクト/projects/diff-drive-astar-kalman/でオドメトリとランドマークの計測値を融合します。

ツールとプラットフォーム

よくある間違い

• 一つのログでプロットの見た目が良くなるまでQとRを調整してしまう。別のログでイノベーションの一貫性を確認してください。
• すでに情報を共有している2つのセンサを融合し、同じ情報を二重に数えてしまう。たとえば、車輪オドメトリと、同じ車輪から計算されたオドメトリトピックの組み合わせです。
• タイムスタンプを無視する。計測値を計測時刻ではなく到着時刻で適用すると、ロボットが動いているときに推定値に偏りが生じます。
• 角度を普通の数値として扱う。方位角の差はマイナスπからπの範囲に折り返してください。
• map から odom ではなく、map から base_link を直接パブリッシュしてしまう。これはREP 105のツリーを壊します。

さらに深く

• Sebastian Thrun、Wolfram Burgard、Dieter Fox、「Probabilistic Robotics」、MIT Press、2005年。
• Timothy D. Barfoot、「State Estimation for Robotics」、第2版、Cambridge University Press、2024年。著者のサイトで草稿のPDFが公開されています。
• R. E. Kalman、A new approach to linear filtering and prediction problems、「Journal of Basic Engineering」82(1)、1960年。
• S. J. Julier、J. K. Uhlmann、Unscented filtering and nonlinear estimation、「Proceedings of the IEEE」92(3)、2004年。
• Frank Dellaert、Michael Kaess、Factor graphs for robot perception、「Foundations and Trends in Robotics」6(1-2)、2017年。
• M. Kaess、H. Johannsson、R. Roberts、V. Ila、J. J. Leonard、F. Dellaert、iSAM2: incremental smoothing and mapping using the Bayes tree、「International Journal of Robotics Research」31(2)、2012年。