センサーを増やすほど自己位置推定が安定するのか、環境条件から考え直せる点がこのテーマの面白さです。
導入
ロボットの自己位置推定で、カメラ・LiDAR・IMUを常にすべて使えばよいとは限りません。選択の基本は、IMUを短時間の運動伝播に使い、カメラは視覚特徴、LiDARは距離と幾何形状の拘束に使うことです。明るく特徴の多い環境ではカメラ+IMU、暗所や照明変動下で幾何が豊富な環境ではLiDAR+IMUが有力です。視覚と幾何のどちらか一方が失敗しやすく、冗長性も必要なら3センサー構成を検討します。
ただし、センサー数の多さだけで精度が決まるわけではありません。時刻同期、外部パラメータ、座標系、IMUバイアスを整合でき、追加される計算量と実装負荷を許容できることが前提です。
なぜ組み合わせの判断が必要か
自己位置推定は、各センサーの観測を同じ状態推定問題へ取り込む処理です。センサーには得意な時間スケールと失敗条件があります。IMUは高レートですが、バイアスやノイズを積分するため、単独では時間とともにドリフトします。カメラは再観測による相対的な拘束を与えられますが、照明やテクスチャに左右されます。LiDARは照明そのものではなく形状を観測できますが、幾何的に特徴の少ない場所や点群が疎になる条件では位置合わせが退化します。
したがって設計の問いは「どのセンサーが最も高性能か」ではなく、「対象環境でどの観測が残り、どの失敗を別の観測で補えるか」です。
各センサーが提供する情報
IMU：高レートの短期伝播
IMUは角速度と比力を取得し、姿勢・速度・位置の短時間の伝播を支えます。カメラやLiDARの観測間をつなぎ、高速運動時にも状態を更新できる点が役割です。一方、慣性積分だけでは誤差が蓄積するため、カメラやLiDARからの拘束でバイアスと状態を補正します。
カメラ：特徴と外観による相対拘束
カメラは画像特徴の追跡と、外観情報の再観測を利用します。十分な照明、視覚テクスチャ、適度なモーションがある環境では、IMUと組み合わせたVisual-Inertial Odometry（VIO）が機能します。画像特徴を一時的に追跡できない場合でも、IMUが短時間の運動拘束を提供しますが、視覚観測が長く失われれば慣性ドリフトを抑えにくくなります。
LiDAR：距離と幾何による拘束
LiDARは点群の距離・幾何形状を使い、スキャン間またはスキャンと地図の位置合わせを行います。暗所や照明変動の影響を受けにくい環境では、IMUと組み合わせたLiDAR-Inertial Odometry（LIO）が候補になります。IMUは短時間の運動推定だけでなく、点群のモーションデスクューにも利用できます。ただし、広く平坦な場所など幾何的な手掛かりが少ない環境では、LiDARの拘束が弱くなる可能性があります。
flowchart LR I["IMU\n角速度・比力"] --> P["高レートの短時間伝播\n姿勢・速度・位置"] C["カメラ\n画像特徴・再観測"] --> V["視覚拘束"] L["LiDAR\n距離・点群・幾何形状"] --> G["幾何拘束\nスキャン間または地図との位置合わせ"] I --> D["点群のモーションデスクュー"] D --> G P --> E["状態推定器"] V --> E G --> E E --> S["状態・IMUバイアスの補正"] E -.-> Q["品質指標の監視\n残差・イノベーションなど"] E --> M["推定状態"] K["EKF系または因子グラフ・平滑化系"] --> E
代表的な構成の比較
| 構成 | 主な観測 | 向く条件 | 主な失敗条件・負荷 |
|---|---|---|---|
| IMU単独 | 角速度・比力の積分 | 高レートの短期伝播 | バイアスとノイズによるドリフト |
| カメラ+IMU（VIO） | 画像特徴、再観測、慣性 | 明るく視覚特徴が豊富な屋内外 | 暗所、照明変動、テクスチャ不足、視覚追跡喪失 |
| LiDAR+IMU（LIO） | 点群の距離・幾何、慣性 | 暗所、照明変動下、幾何が豊富な場所 | 幾何的特徴の不足、点群が疎な条件 |
| カメラ+LiDAR+IMU | 視覚、幾何、慣性 | 視覚と幾何の一方が失敗しやすい環境、冗長性が必要な長時間運用 | 計算量、実装・校正負荷、同期や座標整合の複雑化 |
3センサー構成は、カメラとLiDARの観測が相互補完になるときに意味があります。たとえば照明条件が不安定でも形状が豊富ならLiDARが支え、幾何が一時的に弱くても視覚特徴が残っていればカメラが補います。ただし、3センサーの優位性をあらゆる条件で示す一般的な比較結果があるわけではなく、採用判断は環境、実装、計算資源に依存します。
EKFか因子グラフか
融合方式は、センサーの種類ではなく推定の時間構造で選びます。EKF系は状態を限定して逐次更新するため、低遅延かつ計算量を管理しやすいオンライン推定に向きます。代表的な視覚慣性推定では、複数時刻のカメラ姿勢を状態として保持し、特徴の幾何拘束を加えます。
因子グラフ・平滑化系は、複数時刻の状態と観測因子をまとめて扱います。遅延した拘束、ループ閉じ込み、履歴の再線形化、追加の地図・GPS因子などを扱いたい場合に適します。一方、履歴やウィンドウを管理するため、計算量やメモリ使用量は実装、ウィンドウサイズ、特徴管理、ハードウェアに依存します。LiDAR+IMUでも、因子グラフを使う方式と反復誤差状態カルマンフィルタを使う方式があり、同じセンサー構成でも計算特性は同一ではありません。
| 判断軸 | EKF系 | 因子グラフ・平滑化系 |
|---|---|---|
| 低遅延の逐次推定 | 向く | 実装次第で調整が必要 |
| 過去観測の再利用 | 限定的 | 向く |
| 遅延した拘束 | 扱いに工夫が必要 | 扱いやすい |
| ループ閉じ込み | 拡張が必要 | 組み込みやすい |
| 計算量の管理 | 状態を限定しやすい | 履歴・ウィンドウに依存 |
flowchart LR
A["要求と時間構造を確認"] --> B{"重視する要件"}
B -->|"低遅延・逐次推定\n計算量を管理"| C["EKF系"]
B -->|"過去観測の再利用\n遅延した拘束\nループ閉じ込み"| D["因子グラフ・平滑化系"]
C --> C1["限定した状態を\n逐次更新"]
C1 --> C2["過去観測の再利用や\nループ閉じ込みは拡張が必要"]
D --> D1["複数時刻の状態と\n観測因子を扱う"]
D1 --> D2["履歴・ウィンドウ・特徴管理に\n計算量とメモリが依存"]
C2 --> E["実装条件とハードウェアで検証"]
D2 --> E実装時に先に確認する条件
第一に、センサー間の外部パラメータと時刻を整合させます。カメラとIMU、LiDARとIMUの相対姿勢や時間オフセットがずれると、観測が同じ運動を示さなくなります。特にLiDARでは、スキャン中の運動を補正するデスクューにもIMUが関係します。
第二に、座標系の責務を明確にします。推定器の状態、センサーのフレーム、ロボット本体のフレーム、地図との関係を整理し、map、odom、base_linkなどの連続性とグローバル整合性を混同しないようにします。
第三に、初期化とバイアス推定を設計します。IMUバイアスを状態として扱い、カメラまたはLiDARの観測で補正できる構成にします。初期姿勢や初期運動が安定しない条件では、単一の観測に依存しないことが重要です。
制約と失敗しやすい点
照度、画像特徴点数、LiDAR点密度、速度、角速度などについて、構成を切り替える普遍的なしきい値は確認できません。カメラ仕様、LiDARの視野・測距距離・線数、IMUのノイズ特性、CPU/GPU性能によって適切な推定周期も変わります。
また、雨、霧、ガラス、粉じん、強い反射、完全な暗所、動的物体が多い環境での性能差は、構成だけから一般化できません。センサー追加後に必ず性能が上がるとも限らず、誤った同期やキャリブレーションは観測を増やすどころか推定を不安定にします。
実運用では、視覚特徴の追跡状況、LiDARの幾何拘束の強さ、IMUの飽和やバイアス状態、残差やイノベーションなどを品質指標として監視し、観測の重み付けや縮退処理を設計します。ただし、自動切り替えのしきい値や動的物体除去の方法は、対象システムでの検証が必要です。
採用判断の手順
- 環境に視覚特徴と照明が安定してあるなら、まずカメラ+IMUを候補にする。
- 暗所・照明変動が支配的で、壁や構造物など幾何が豊富なら、LiDAR+IMUを候補にする。
- 視覚と幾何のどちらも不安定になり得る、または長時間運用で冗長性が必要なら、3センサー構成を検討する。
- 低遅延と計算量の上限が厳しければEKF系、ループ閉じ込みや履歴の再利用が必要なら因子グラフ・平滑化系を検討する。
- 最後に、同期、外部パラメータ、座標系、バイアス推定、品質指標を実装条件として確認する。
この手順で重要なのは、センサー構成を先に固定せず、失敗モードと要求する時間遅延、運用時間、計算資源を同時に評価することです。
まとめ
IMUは高レートの短期伝播、カメラは視覚特徴と再観測、LiDARは距離と幾何拘束を担います。明るく特徴豊富ならVIO、暗所や照明変動下で幾何が豊富ならLIOが基本の候補です。3センサー融合は相互補完と冗長性が必要な場合に有効ですが、同期・キャリブレーション・座標系・計算量を満たす場合に限って採用します。EKFと因子グラフの選択はセンサー数ではなく、低遅延の逐次推定を優先するか、履歴再利用やループ閉じ込みを必要とするかで決めるのが妥当です。
参考情報
- On-Manifold Preintegration for Real-Time Visual-Inertial Odometry
- VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator
- ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial, and Multi-Map SLAM
- LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping
- FAST-LIO2: Fast Direct LiDAR-Inertial Odometry
- A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation
- REP-105: Coordinate Frames for Mobile Platforms
実際の構成では、センサーの数よりも失敗モードと観測品質を測れる設計が判断を左右します。
AI利用に関する注記
本サイトの記事は、複数のAIエージェントによる調査・執筆・レビューを経て公開しています。正確性と品質の向上に努めていますが、内容については必ず一次情報をご確認ください。
