目次
背景
- 差動二輪ロボットの駆動系とAMCL・SLAM・Nav2で、ホイールオドメトリはホイールの回転量だけから積分した値のため、スリップやエンコーダのノイズでドリフトしていく、という前提を扱った
- このドリフトを、LiDARと地図の照合（AMCL・SLAM）とは別のレイヤーで、IMU（慣性計測装置）と組み合わせることで補う手法として、EKF（Extended Kalman Filter）・UKF（Unscented Kalman Filter）によるセンサーフュージョンがある。今回はその仕組みを整理する
センサーフュージョンとは
- センサーフュージョンとは、複数の異なるセンサーから得られる情報を組み合わせることで、単一のセンサーだけでは得られない、より正確で頑健な推定を得る技術
- 組み合わせ方は、大きく以下の3種類に分類されることが多い
| 種類 | 内容 |
|---|---|
| 相補的（Complementary）融合 | 異なる種類の情報を持つセンサー同士を組み合わせ、互いの弱点を補い合う |
| 競合的（Competitive）融合 | 同じ対象を測る複数のセンサーを組み合わせ、冗長性や精度の向上を図る |
| 協調的（Cooperative）融合 | 単体のセンサーでは得られない新しい情報を、複数センサーの組み合わせから導き出す（例: ステレオカメラによる距離推定） |
- 今回扱うホイールオドメトリとIMUの組み合わせは、「並進方向の推定には強いがスリップに弱いセンサー」と「短時間の姿勢変化には強いが長期的にドリフトするセンサー」という、異なる誤差特性を補い合う相補的融合にあたる
なぜセンサーフュージョンが必要か
ホイールオドメトリの弱点
- ホイールオドメトリは、ホイールの回転量（エンコーダ値）から並進・回転を積分して求める
- ホイールのスリップ（空転・横滑り）が起きると、実際には動いていない・向きが変わっていないのに、エンコーダ上は回転したことになってしまい、推定位置が大きくズレる
- ホイールオドメトリ単体では、このスリップが起きたこと自体を検出できない
IMUの特性と弱点
- IMUは、角速度を測るジャイロスコープと、加速度（＋重力）を測る加速度計を組み合わせたセンサー
- ジャイロスコープは、短時間の角速度の変化には敏感で、ホイールの回転とは独立に姿勢の変化を検出できる。ただし、角速度を積分して姿勢を求めるため、センサー自体のバイアス誤差が蓄積し、長時間では姿勢（特にヨー角）がドリフトする
- 加速度計は、重力方向を基準にしたロール・ピッチの推定には使えるが、ヨー角の基準にはならない。また、加速度を2回積分して位置を求めようとすると、ノイズが急激に拡大するため、位置推定にはほとんど使えない
組み合わせる意義
- ホイールオドメトリは並進方向の推定に強いがスリップに弱く、IMUは短時間の姿勢変化の検出に強いが長時間では姿勢がドリフトする、という互いに異なる誤差特性を持つ
- この2つを組み合わせることで、例えば「ホイールオドメトリ上は直進しているはずなのに、IMUのジャイロが大きな角速度を検出している」というような食い違いから、スリップの発生を間接的に検出し、推定値を補正できる
カルマンフィルタの基本
状態空間モデルと予測・更新の2ステップ
- カルマンフィルタは、ロボットの位置・速度などの状態を、予測ステップと更新ステップの繰り返しで推定する手法
- 予測ステップでは、前回の状態推定値と、運動モデル（今回であれば、速度・角速度からどれだけ移動するかという式）を使って、次の時刻の状態を予測する
- 更新ステップでは、実際のセンサー観測値と、予測値から期待される観測値との差（残差）を使って、予測値を補正する
- 線形カルマンフィルタは、運動モデル・観測モデルがどちらも線形である場合に最適な推定を与えるが、ロボットの姿勢（回転）を含む運動モデルは、三角関数を含む非線形な式になるため、そのままでは適用できない
具体例: 1次元のカルマンフィルタ
- 直感をつかむために、簡単な1次元の例で予測・更新を計算してみる
- ロボットが毎秒$v=1.0$m/sで直進していると仮定し、現在の位置推定を$x=0$、その不確かさ（分散）を$P=1.0$とする。プロセスノイズ（運動モデルの不確かさ）を$Q=0.1$とする
- 予測ステップでは、以下のように位置と不確かさを更新する
- ここに、ノイズを含むセンサー（測定誤差の分散$R=0.5$）が、位置$z=1.2$という観測値を返したとする
- 更新ステップでは、まずカルマンゲイン$K$を計算する
- この$K$を使って、予測値と観測値を重み付けして合成し、新しい推定値と不確かさを求める
- カルマンゲイン$K$は、予測の不確かさ$P_{\text{pred}}$とセンサーの不確かさ$R$の相対的な大きさで決まる。センサーの方が信頼できる（$R$が小さい）ほど$K$は1に近づき、観測値を重視した更新になる。逆に予測の方が信頼できる（$P_{\text{pred}}$が小さい）ほど$K$は0に近づき、予測値をそのまま維持する
- 更新後の不確かさ$P$は、更新前の$P_{\text{pred}}$より必ず小さくなる。新しい観測値を取り込むことで、推定の不確かさが必ず減ることを意味する
非線形系への対応: EKFとUKF
EKF: ヤコビアンによる線形化
- EKF（Extended Kalman Filter）は、非線形な運動モデル・観測モデルを、現在の推定値のまわりでヤコビアン（偏微分からなる行列）を使って1次近似（線形化）し、その線形化されたモデルに通常のカルマンフィルタの式を適用する手法
- 予測ステップでは、非線形な運動モデル$f$で状態を予測しつつ、共分散（推定の不確かさ）の伝播には、$f$のヤコビアン$F$を使う
- 更新ステップでは、観測値と予測観測値の残差を、観測モデルのヤコビアン$H$とカルマンゲインで重み付けして、状態を補正する
- 非線形性が強い場面では、1次近似による誤差が無視できなくなる
UKF: シグマポイントによるUnscented変換
- UKF（Unscented Kalman Filter）は、ヤコビアンによる線形化を行わず、Unscented変換と呼ばれる手法で非線形性を扱う
- 現在の推定値の平均・共分散から、シグマポイントと呼ばれる複数の代表点（状態の次元を$n$とすると、$2n+1$個）を決定論的に選び出す
- 各シグマポイントを、非線形な運動モデル・観測モデルにそのまま（線形化せず）通し、変換後の点群の平均・共分散を計算することで、変換後の分布を近似する
- ヤコビアンの導出・計算が不要になり、微分可能でない関数にも適用できる。非線形性が強い系でも、EKFより正確な推定が得られる傾向がある
EKFとUKFの比較
| 観点 | EKF | UKF |
|---|---|---|
| 非線形性への対応 | 現在の推定値まわりでヤコビアンを計算し、1次近似（線形化）する | シグマ点を使い、非線形関数をそのまま適用するUnscented変換を使う |
| 必要な計算 | ヤコビアン（偏微分）の導出・計算が必要 | ヤコビアンは不要。微分可能でない関数にも適用できる |
| 精度 | 強い非線形性があると近似誤差が大きくなる | 非線形性が強い系でも、より正確な平均・共分散を推定できる傾向 |
| 計算コスト | 比較的軽い | シグマ点（$2n+1$個）分の伝播が必要で、やや重い |
ROS2での実装: robot_localizationパッケージ
15次元の状態ベクトル
- ROS2では、EKF・UKFによる複数センサーのフュージョンを行うパッケージとして、
robot_localizationがよく使われる - 実際にソースを確認したところ、状態ベクトルは15次元で、以下のように定義されている
| インデックス | 内容 |
|---|---|
| 0〜2 | 位置（$x, y, z$） |
| 3〜5 | 姿勢（roll, pitch, yaw） |
| 6〜8 | 並進速度（$v_x, v_y, v_z$） |
| 9〜11 | 角速度（roll, pitch, yawそれぞれの速度） |
| 12〜14 | 並進加速度（$a_x, a_y, a_z$） |
ekf_node・ukf_nodeは、この15次元の状態に対し、ホイールオドメトリ・IMU・GPSなど、複数のセンサーソースのうちどの成分（位置・速度・加速度など）をどのセンサーから取り込むかを、設定ファイルで細かく指定できる
dual EKFパターン: odom->base_linkとmap->odomを分ける
robot_localizationは、world_frameというパラメータを持ち、odomまたはmapを指定できる- 代表的な使い方として、2つのEKF（またはUKF）ノードのインスタンスを同時に動かす、いわゆるdual EKFパターンがよく使われる
- 1つ目:
world_frameをodomにし、ホイールオドメトリ・IMUのような、連続的でドリフトはするが滑らかなセンサーだけを融合し、odom->base_linkを配信する - 2つ目:
world_frameをmapにし、GPSのような、絶対的だが不連続な位置情報も追加で融合し、map->odomを配信する
- 1つ目:
- この構成は、差動二輪ロボットの駆動系とAMCL・SLAM・Nav2で扱ったAMCL・SLAMの役割分担（
odom->base_linkは駆動系側、map->odomは自己位置推定側）と同じ構造を持っており、AMCL・SLAMの代わりに（あるいは併用して）robot_localizationがmap->odomを担うこともできる
まとめ
- ホイールオドメトリはスリップに弱く、IMUは短時間の姿勢変化には強いが長時間ではドリフトするという、互いに異なる誤差特性を持つため、組み合わせることで単独より頑健な推定ができる
- カルマンフィルタは予測・更新の2ステップで状態を推定するが、回転を含むロボットの運動モデルは非線形なため、線形カルマンフィルタをそのまま適用できない
- EKFはヤコビアンによる1次近似で非線形性を扱い、UKFはシグマポイントによるUnscented変換で、線形化せずに非線形性を扱う。UKFの方が精度面で有利な傾向があるが、計算コストはやや高い
- ROS2の
robot_localizationパッケージは、15次元（位置・姿勢・並進速度・角速度・並進加速度）の状態ベクトルに対し、複数センサーを融合するEKF・UKFノードを提供する world_frameをodomにしたインスタンスでodom->base_link、mapにしたインスタンスでmap->odomを配信するdual EKFパターンが代表的な構成で、AMCL・SLAMと同じTFの役割分担に沿っている