⚡ electrical engineering

FusionCore: A 23-State Unscented Kalman Filter for IMU, Wheel Encoder, GPS, and Visual SLAM Fusion in ROS 2

FusionCore は、IMU、車輪エンコーダ、GPS、および Visual SLAM のデータを高周波のオドメトリストリームに強固に融合させるために 23 状態の Unscented カルマンフィルタを採用するオープンソースの ROS 2 パッケージであり、多様な NCLT データセットシーケンスにおいて標準的な robot_localization パッケージよりも著しく低い軌道誤差とより大きな数値的安定性を達成します。

原著者： Manan Kharwar (Independent Researcher, Hamilton, ON, Canada)

公開日 2026-05-26

📖 1 分で読めます☕ さくっと読める

CC BY 4.0

原著者： Manan Kharwar (Independent Researcher, Hamilton, ON, Canada)

原論文は CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/) でライセンスされています。 ✨ これは以下の論文のAI生成解説です。著者が執筆または承認したものではありません。技術的な正確性については原論文を参照してください。免責事項の全文を読む

ロボットを街中をナビゲートさせようとしていると想像してください。しかし、あなたは目隠しをしており、耳も塞がれていて、車輪が回るのを感じることでしか自分の位置を推測できません。これが、ロボットが自分の位置を把握しようとする際に直面する本質的な課題です。ロボットは、自分がどこにいるのかを明確に把握するために、異なる「感覚」からのデータを組み合わせなければなりません。

本論文は、ROS 2 システムを使用するロボット向けに、これらの感覚を統合する「脳」として機能する新しいソフトウェアツール「FusionCore」を紹介します。その仕組みを、簡単な比喩を用いて説明します。

課題：「ノイズの多い」チーム

ロボットが自分の位置を推測する主な方法は通常 3 つあります。

車輪エンコーダ（歩数計）： 車輪が何回転したかを数えます。短距離では優れていますが、車輪が氷や泥で滑ると混乱してしまいます。
GPS（衛星地図）： 地球上での正確な位置を教えてくれます。しかし、遅く、時には消えてしまいます（橋の下を運転しているときのように）、また、別の都市にいると誤って示す「グリッチ」を時折発生させることがあります。
IMU（内耳）： 加速度や回転を感じるセンサーです。非常に高速ですが、時間が経つと「ドリフト」（ゆっくりと傾く回転するコマのように）し始め、静止しているにもかかわらず移動しているとロボットに誤認させます。

既存のソフトウェア（robot_localization）はこれらの信号を混合しようとしますが、いくつかの欠点があります。

センサーの「ドリフト」を、時間とともに変化するものではなく、固定されたエラーとして扱います。
GPS 信号が地図の境界をまたいでジャンプすると混乱します。
実際の GPS 更新と、異常なグリッチとの区別がつきません。

解決策：FusionCore の「賢い脳」

FusionCore は、これらのすべての信号を、1 秒間に 100 回という高速で滑らかな位置データの流れに統合する新しい「脳」（数学的フィルタ）です。これは、他のシステムが無視しているものも含めてすべてを追跡するための 23 次元の「状態」を使用します。

以下に、比喩を用いて説明する、FusionCore が使用する主要なテクニックを示します。

1. 「ドリフト探偵」（オンラインバイアス推定）

少し足を引きずる友人と歩いていると想像してください。その引きずりを認識しなければ、自分の位置の地図はゆっくりと誤ったものになっていきます。

従来の方法： ソフトウェアは引きずりが固定されており、それを無視すると仮定します。
FusionCore の方法： 内部に「ドリフト探偵」を持っています。センサーを常に見守り、「車輪は滑っているか？ジャイロスコープは疲れてきているか？」と問いかけます。リアルタイムで「引きずり」（バイアス）の正確な量を計算し、それを差し引きます。これにより、GPS が一時的に失われても、ロボットが迷子になるのを防ぎます。

2. 「グローバル住所」（ECEF ネイティブ GPS）

GPS 座標は通常、局所的な地図（UTM 帯域など）に変換されます。帯域の境界を越えて運転すると、地図がグリッチを起こしてクラッシュする可能性があります。

FusionCore の方法： 局所的な住所を使用するのではなく、地球全体を話す言語（ECEF）を使用します。惑星を 1 つの巨大な球体として扱うため、地図の境界によって混乱することはありません。10 マイルごとに新しい都市の地図に切り替えるのではなく、全世界で緯度経度を使用するようなものです。

3. 「嘘発見器」（適応的アウライヤーゲーティング）

ロボットが明らかに道路上にあるのに、GPS が「今は海の中にいます！」というメッセージを送ることがあります。

従来の方法： ソフトウェアは厳格なルールを使用します。「エラーが 5 メートルより大きければ、無視する」。しかし、GPS が実際には少しノイズがあるだけの場合、良いデータを無視してしまう可能性があります。
FusionCore の方法： 学習する「嘘発見器」を使用します。エラーの履歴を確認します。GPS が通常は正確だが、たまたま調子が悪い日であれば、検出器は握力を緩めます。GPS が狂ったように振る舞っている場合は、扉を強く閉めます。グリッチに騙されたり、良いデータを拒絶したりしないよう、感度を自動的に調整します。

4. 「タイムトラベラー」（遅延補償）

GPS データは、衛星が信号を送る前に計算を行う必要があるため、しばしばわずかに遅れて到着します。

FusionCore の方法： 最後の 1 秒間の IMU データの「メモリバッファ（リングバッファ）」を保持します。遅れて到着した GPS メッセージがようやく届いたとき、ソフトウェアはそれを単に「現在」の瞬間に適用するのではなく、GPS 信号が取得された正確な瞬間に「巻き戻し」、補正を適用してから、再び「早送り」して現在に戻します。これにより、遅いセンサーであっても、ロボットの位置が完全に同期されていることを保証します。

5. 「視覚的記憶」（VSLAM 融合）

ロボットが GPS を失った場合（トンネルに運転して入るなど）、カメラ（Visual SLAM）を使用して部屋の特徴を追跡できます。

FusionCore の方法： カメラの視点にシームレスに切り替えることができます。カメラが道に迷って地図を「再開始」しなければならない場合、FusionCore はロボットがパニックになったりクラッシュしたりすることなく、ロボットの位置を現実に戻す方法を知っています。

結果：大テスト

著者は、公開データセットからの 12 の長距離走行シーケンス（それぞれ最大 92 分）を使用して、FusionCore を標準ソフトウェア（robot_localization）と比較テストしました。

勝者： FusionCore は 12 のレースのうち 10 で勝利しました。
差：場合によっては、標準ソフトウェアよりも 22 倍 正確でした。
敗者： 標準ソフトウェアの高度なバージョン（UKF）は、すべての 12 のシーケンスでクラッシュ（発散）し、ロボットの位置の追跡を完全に失いました。
FusionCore が 2 ラースで負けた理由：
1. 長期のブラックアウト： あるレースでは、GPS 信号がほぼ 8 分間失われました。どんな優れた探偵でも、その間地図なしでは完璧に推測できず、ロボットはわずかにドリフトしました。
2. 「敵対的」なグリッチ： もう一つのレースでは、GPS がフィルタを欺くほど説得力のある嘘のクラスターを送信しました。標準ソフトウェアは、そのルールが厳しすぎたため偶然それらを拒絶しましたが、FusionCore はあまりにも協力的になろうとして騙されました。著者は、これらは特定かつ修正可能な問題であると指摘しています。

まとめ

FusionCore は、ロボットが自分の位置を知るのを助ける、より賢く、適応性の高い方法です。センサーのエラーを時間とともに変化する生きているものとして扱い、嘘を無視することを学び、過去を記憶して現在を補正します。完璧ではありませんが、ほとんどの実世界シナリオにおいて、現在の業界標準を大幅に上回る性能を発揮します。

技術概要：FusionCore

問題定義

正確な姿勢推定は自律型ロボットナビゲーションの基盤であるが、センサーフュージョンシステムの実際の展開には依然として課題が残っている。既存のソリューションは、いくつかの決定的な限界に直面している：

バイアスドリフト: IMU ジャイロスコープおよび加速度計のバイアスは継続的にドリフトし、車輪エンコーダのヨーレートバイアス（差動駆動における系統的オフセット）は、ほとんどがフィルタ状態として扱われない。その結果、GPS 不通時に観測不能なHeading誤差の蓄積が生じる。
アーキテクチャの脆弱性: robot_localization などの支配的な ROS パッケージは、GPS 座標変換（UTM）のために別ノードに依存しており、ゾーン境界での失敗を引き起こす。また、測定次元性を考慮しない固定ノイズ共分散とスカラー外れ値拒否閾値を使用している。
運用上のギャップ: 現在のシステムは、GPS 拒否環境下での動作、マップ再初期化からの回復、および Visual SLAM（VSLAM）やレーダーなど多様なセンサーの複雑な調整なしでの統合において困難に直面している。

手法

FusionCore は、IMU、車輪エンコーダオドメトリ、GPS、および Visual SLAM 姿勢を単一の 100 Hz オドメトリストリームに融合する、**23 状態の Unscented Kalman Filter（UKF）**を実装するオープンソースの ROS 2 パッケージである。

コアアーキテクチャ

状態ベクトル: フィルタは、位置（ $p$ ）、姿勢（ $q$ ）、線形/角速度（ $v, \omega$ ）、線形加速度（ $a$ ）、ジャイロスコープバイアス（ $b_g$ ）、加速度計バイアス（ $b_a$ ）、そして重要な**車輪エンコーダヨーレートバイアス（ $b_{ewz}$ ）**を含む 23 次元の状態ベクトルを維持する。
バイアス推定: 従来のアプローチとは異なり、ジャイロスコープ、加速度計、およびエンコーダのヨーバイアスは、専用のプロセスノイズを持つ第一級のフィルタ状態として扱われる。これにより、フィルタは個別の較正フェーズなしに、オンラインでセンサー固有のバイアス値に収束することを可能にする。
GPS フュージョン: システムは、**Earth-Centered, Earth-Fixed（ECEF）**フレームで GPS データをネイティブに融合し、フィルタ状態に対してのみ ENU へ変換する。これにより、navsat_transform ノードの必要性を排除し、UTM ゾーン境界での失敗を回避する。
測定モデル:
- IMU: 重力とバイアスを考慮し、6-DOF の生データおよび 2/3-DOF の姿勢更新を処理する。
- 車輪エンコーダ: 積分前に報告された角速度から推定された $b_{ewz}$ を減算する。適応ノイズを用いて非ホロノミック制約（垂直方向の速度および加速度がゼロ）を強制する。
- VSLAM: 任意の視覚オドメトリ/SLAM システムからの 6-DOF 姿勢測定を受け入れ、連続する外れ値拒否後のトランスフォームの再アンカーリングによる、マップ再初期化からの自動回復を実現する。
- 速度: スリップ検出のために、GPS ドップラー速度およびレーダードップラー自己速度をサポートする。

フィルタ設計の革新

センサーごとのカイ二乗ゲーティング: 各センサーパスの自由度（DOF）（例：3-DOF の GPS 対 1-DOF のHeading）に対して較正された閾値を用いたマハラノビス距離による外れ値拒否を実装し、単一のスカラー閾値を使用するのではなく、センサーごとに個別に対応する。
適応ノイズ共分散: 革新シーケンスの移動窓と指数移動平均を用いて、リアルタイムでセンサーノイズ共分散（ $R$ ）を自動的に調整し、環境変化による手動の再調整を不要にする。
遅延補償: 1 秒間の IMU ラインバッファを活用して遡及予測を行い、遅延した GPS または VSLAM 測定値を再伝播する前に、正しい過去の状態に適用する。
ゼロ速度更新（ZUPT）: 静止期間を検出してバイアスドリフトを抑制する。
コーストモード: GPS ブラックアウト中、エンコーダデータから推定されたエンコーダヨーバイアスを減算し、Heading ドリフトを低減する。

主要な貢献

本論文は、10 の具体的な貢献を提示する：

ジャイロスコープ、加速度計、および車輪エンコーダヨーレートバイアスの連続的なオンライン推定を伴う 23 状態の UKF 定式化。
UTM 投影への依存性を排除した ECEF ネイティブの GPS フュージョン。
次元性で較正された閾値を備えた、センサーごとのマハラノビスカイ二乗外れ値ゲーティング。
革新シーケンスからの自動ノイズ共分散適応。
静止期間のための自動ゼロ速度更新（ZUPT）。
遅延補償のための IMU ラインバッファ遡及予測。
マップ再初期化からの自動回復を伴う Visual SLAM 姿勢フュージョン。
クロスセンサーのスリップ検出のための GPS 速度フュージョン。
4D 画像レーダープラットフォームのためのレーダードップラー自己速度フュージョン。
12 の完全な NCLT シーケンスに対する robot_localization との定量的評価。

評価と結果

本システムは、多様な季節条件を網羅するNCLT 公開データセットからの12 の完全なシーケンス（それぞれ 55〜92 分）で評価された。評価では、絶対軌道誤差（ATE）の RMSE を用いて、FusionCore と robot_localization の EKF および UKF を比較した。

性能: FusionCore は、12 シーケンス中 10で robot_localization EKF よりも低い ATE を達成した。勝利したシーケンスにおける改善幅は、1.2 倍から 22.2 倍であった。
安定性: robot_localization の UKF は、最初の 30 秒以内にすべての 12 シーケンスで数値的に発散（NaN）し、高レート入力に関する既知の不安定性の問題を確認した。
失敗分析:
- 2012-06-15（FC 敗北）: 7.7 分間の GPS ブラックアウトにより、FusionCore は robot_localization EKF に敗れた（49.2m 対 18.2m）。論文はこれを、絶対Heading参照（例：地磁気計）なしで、そのような長期間にわたるエンコーダベースの航法推測の限界に起因するとしている。
- 2012-08-20（FC 敗北）: ブラックアウト境界において、真値から 720〜840m 離れた場所に 105 個の修正点が現れた「敵対的 GPS クラスター」により、FusionCore は大幅に敗れた（98.3m 対 10.6m）。論文は、robot_localization の較正不十分なゲートが偶然これらの外れ値を拒否したのに対し、FusionCore の回復に必要な緩和されたゲートはそれらを受け入れたと指摘する。著者は、速度一貫性の事前チェックを解決策として特定している。
共分散の不一致: 論文は、robot_localization が固定かつしばしば不正確な GPS 共分散報告に依存することが、有効なデータの拒否と航法推測への崩壊をもたらすと強調する。FusionCore の適応ノイズモデルは、革新誤差が報告された共分散を超えた場合にノイズモデルを膨張させることでこれを緩和する。

意義と主張

本論文は、FusionCore が、外部コーディネータを必要としない単一のライフサイクルノードを提供することで、既存の ROS 2 センサーフュージョンスタックの実際的な限界に対処すると主張する。その意義は以下の点にある：

堅牢性: 既存の UKF 実装が数値的に失敗する長期の現実世界シナリオにおいて、優れた軌道精度を達成すること。
適応性: 適応ノイズと次元性を考慮したゲーティングを通じて、動的なセンサー品質および GPS 共分散の不一致を処理すること。
運用の継続性: GPS 拒否環境下での動作および VSLAM マップ再初期化からのシームレスな回復を可能にすること。
再現性: 標準プラットフォーム（TurtleBot3、Husky）のベンチマークスクリプトおよび設定、および rtabmap_ros との統合を含む、完全なオープンソース実装を提供すること。

著者は、特定の失敗モード（長期のブラックアウト、敵対的 GPS クラスター）が存在するものの、それらは診断可能であり、磁気計の統合や速度の事前チェックなどのアーキテクチャ的修正を将来の課題として特定したと結論づけている。