VIO/LIO入門 — IMUとカメラ・LiDARを組み合わせて自己位置を求める技術

Visual-SLAMもLiDAR-SLAMも、単体では特定の環境で破綻する。カメラは暗所や高速運動に弱く、LiDARは特徴の乏しい環境やコストに課題を抱える。この弱点を補う鍵になるのが、加速度とジャイロを高頻度で測定できるIMU(慣性計測装置)である。カメラ・LiDARとIMUを組み合わせたVIO(Visual-Inertial Odometry)・LIO(LiDAR-Inertial Odometry)は、現在の多くの自律移動ロボット・ドローン・AR/VRデバイスで事実上の標準構成になっている。この記事では、なぜIMU単体では位置が求まらないのかという原理から出発し、センサー融合の発想、Filter-basedとOptimization-basedという2つの設計思想、そしてROVIO・OKVIS・VINS-Mono・OpenVINS・LIO-SAM・FAST-LIO2といった代表アルゴリズムまでを体系的に見ていく。

IMU/INSの実例

LiDARの実例

画像: Xsens MTi-G(Kallap85, CC BY-SA 4.0) / Livox Mid-40, Horizon, and Tele-15(Dllu, CC BY-SA 4.0)、Wikimedia Commons。VIO/LIOの入力センサを具体化する代表例であり、本文の構成を限定しない。

0. この記事で分かること

• IMUが何を測っていて、なぜIMU単体では位置を求め続けられないのか
• Camera・LiDARそれぞれの弱点と、IMUとの組み合わせがなぜ有効なのか
• VIOとLIOがそれぞれ何を指すのか、両者の違い
• Filter-based(EKF/MSCKF)とOptimization-based(Factor Graph)という2つの設計思想の違い
• ROVIO/OKVIS/VINS-Mono/VINS-Fusion/OpenVINS(VIO)、LIO-SAM/FAST-LIO/FAST-LIO2(LIO)がそれぞれ何を工夫しているのか
• 実際にVIOとLIOのどちらを選ぶべきか、用途別の判断軸

1. IMU・VIO・LIOとは何か

一言でまとめると次のようになる。IMU(Inertial Measurement Unit、慣性計測装置)とは、加速度計とジャイロスコープを組み合わせ、外部の目印を一切参照せずに自分自身の加速度と角速度を高頻度で測定するセンサーである。VIO(Visual-Inertial Odometry)は、このIMUとカメラを組み合わせて自己位置と姿勢を推定する技術であり、LIO(LiDAR-Inertial Odometry)は、IMUとLiDARを組み合わせて同じことを行う技術である。

IMUは「今この瞬間、自分がどれだけの力で加速し、どれだけの速さで回転しているか」を高頻度(数百Hz〜1kHz程度)で教えてくれる一方、その積分値である位置は時間とともに急速にズレていく(3節で詳しく見る)。カメラやLiDARは、周囲の環境と照合することで環境に対する相対的な位置拘束を低頻度(数十Hz程度)で与えてくれるが、暗所や特徴に乏しい環境、高速運動といった状況で機能を失う瞬間がある。VIO・LIOの核心は、この「高頻度だがズレていくIMU」と「低頻度だが環境依存の弱点を持つCamera/LiDAR」を組み合わせることで、互いの弱点を補い合う点にある。

2. IMUは何を測っている?

IMUの内部には、大きく分けて2種類のセンサーが入っている。

Accelerometer（加速度計）は比力をセンサー座標で測る。理想化すると \mathbf{f}_b=R_{wb}^{T}(\mathbf{a}_w-\mathbf{g}_w) であり、R_{wb} はセンサー座標から世界座標への回転、\mathbf{a}_w は世界座標での加速度、\mathbf{g}_w は重力加速度である。静止時には約1 g、理想自由落下ではほぼゼロを示す。位置を積分する前に、バイアス補正後の比力を世界座標へ回して重力を加える。単に生の3軸値を2回積分しても位置にはならない。OpenVINSのIMUモデルも座標・重力・バイアスを分けている。

Gyroscope(ジャイロスコープ)は、センサー自身の回転の速さ、すなわち角速度を測定する。こちらも3軸分を組み合わせることで、ロール・ピッチ・ヨーの3方向の回転速度を同時に測定できる。

IMUはこの2種類のセンサーを組み合わせ、6自由度(並進3自由度+回転3自由度)の運動情報を、外部の目印(ランドマークや電波)を一切参照せずに、センサー単体の内部状態だけから高頻度で出力できる。この「外部を一切参照しない」という性質が、IMUの最大の長所であり、同時に最大の弱点にもなる——長所は暗闇でもガラス張りの部屋でも変わらず動作する頑健性、弱点は3節で見る誤差の蓄積である。

またIMUの出力には、必ずBias(バイアス)とNoise(ノイズ)が含まれる。バイアスは、真の値がゼロであっても出力がゼロにならない、センサー固有のオフセット誤差であり、温度変化などによってゆっくり時間変化する(Bias Instability)。ノイズは、観測のたびにランダムに乗る細かい揺らぎである。低コストなMEMS(微小電気機械システム)IMUほどこのバイアス・ノイズが大きく、後述するDriftの主要な原因になる。

3. IMUだけで位置を求める(積分とDrift)

IMUの出力から位置を求めるには、加速度を2回積分する必要がある。速度は加速度の積分、位置は速度の積分だからである。

ここでp_0は初期位置、v_0は初期速度、a(s)は時刻sでの加速度である。この式自体は単純だが、実際に使われる加速度a(s)には、2節で見たバイアスbが常に乗っている。バイアスを含む加速度a(s) + bをそのまま2回積分すると、バイアス由来の誤差項は

という形で、時間の2乗に比例して発散していく。さらにジャイロスコープのバイアスは姿勢(向き)の推定を徐々にズラし、その姿勢の誤差が加速度をどの方向に積分するかという計算に混入するため、位置誤差全体としてはO(t^2)からO(t^3)のオーダーで急速に成長する。この「時間とともに誤差が際限なく蓄積していく現象」をDrift(ドリフト)と呼ぶ。

具体的な感覚をつかむために数値例を見てみよう。安価なMEMS IMUの加速度計バイアスが仮に0.01\,\mathrm{m/s^2}程度あったとすると、10秒後の位置誤差はおよそ\frac{1}{2} \times 0.01 \times 10^2 = 0.5\,\mathrm{m}に達する。1分後には18\,\mathrm{m}になる計算である。IMU単体では、数秒から数十秒というごく短い時間スケールでしか信頼できる位置推定を維持できない——これがIMU単体を「自己位置推定システム」として使えない、根本的な理由である。裏を返せば、IMUは「短時間・高頻度の変化」には強く、「長時間・絶対的な位置」には弱いという、Camera/LiDARとちょうど正反対の得意・不得意を持っている。

4. Camera/LiDARにも弱点がある

IMUだけでは位置が求まらないなら、Camera(「Visual-SLAM入門」参照)やLiDAR(「LiDAR-SLAMの技術動向」参照)を使えばよいと思うかもしれない。実際その通りだが、Camera・LiDARにもそれぞれ固有の弱点がある。

Cameraの弱点は、暗所での画質劣化(特徴点やDirect法の輝度勾配が計算できなくなる)、低テクスチャ環境(白い壁や単色の床では対応点が見つからない)、そして高速運動によるMotion Blur(露光時間中に画像がブレて特徴点の記述子が不安定になる)である。これらはいずれも「カメラが十分な視覚的手がかりを得られない」という共通点を持つ。

LiDARの弱点は、特徴の乏しい環境(長いトンネルや広い平面が続く空間では、点群同士の位置合わせ(ICP/NDT)が一意に定まらない、いわゆる幾何学的な縮退が起きる)と、センサー自体のコストである。LiDARは能動的にレーザーを照射して距離を測るため暗所や逆光には強い一方、環境の幾何形状そのものに乏しい場所ではCameraと同じように手がかりを失う。

IMUの弱点は3節で見た通り、時間とともに際限なく蓄積するDriftであり、そもそも絶対位置を単体で求める手段を持たない。

重要なのは、これら3種類のセンサーの弱点が生じる状況が、互いにほぼ重ならないという点である。Cameraが弱い暗所でもIMUは平然と動作し、Camera/LiDARが特徴を失う瞬間でもIMUは直前までの動きの情報を高頻度で供給し続けられる。Camera/LiDARは外界の幾何でドリフトを抑える。ただし、既知地図・GNSS・ループ閉じ込み等の拘束なしに長期の絶対位置精度が保証されるわけではない。この相補性こそが、次節で見るセンサー融合の出発点になる。

5. センサー融合という発想

Sensor Fusion(センサー融合)とは、複数のセンサーが持つそれぞれ異なる誤差特性・得意分野を組み合わせ、単体のセンサーよりも高精度・高頻度・高頑健性な推定を得る枠組みである(基礎的な融合の考え方全般は「Sensor Fusion入門」で扱っている)。VIO・LIOに共通する骨格は、次のようなPredict(予測)–Update(補正)のループとして整理できる。

図1 · ボタンで表示を切り替え

図2 · ボタンで表示を切り替え

図1 — IMU(高頻度)はPropagation(積分)によって状態を予測し続け、Camera/LiDAR(低頻度)から得られる特徴・点群マッチングの観測がUpdateで予測を補正する。補正時に推定されたIMUバイアスは、次のPropagationへフィードバックされ、Driftの蓄積を継続的に抑える。

このループの要点は2つある。第一に、IMUによるPropagationは高頻度で常に動き続け、Camera/LiDARの観測が得られない瞬間(暗所・低テクスチャ・特徴の乏しい空間・高速運動)でも姿勢の予測を止めない。第二に、Camera/LiDARの観測が得られたときにだけ発火するUpdateが、IMU単体では際限なく蓄積するDriftを抑え、同時にIMUのバイアス自体の推定値も更新してPropagationにフィードバックする。この「常に動き続ける高頻度の予測」と「観測が拘束できる方向の誤差を減らす補正」の組み合わせが、VIO・LIOに共通する設計思想である。

6. VIO(Visual-Inertial Odometry)

VIO(Visual-Inertial Odometry)は、この枠組みでCameraをUpdate側のセンサーに使う構成である。IMUの高頻度な予測とCameraの特徴点観測を組み合わせることで、単眼カメラのVisual Odometryが抱えるスケール不定性(4節で触れた、単眼画像だけでは実寸の距離が分からない問題)を、IMUの実寸の加速度情報を使って解消できる。またCameraが苦手とする高速運動下でも、IMUがその間の姿勢変化を高頻度で補い続けるため、追跡が完全に破綻しにくくなる。

VIOはさらに、CameraとIMUの結び付け方によってLoosely-Coupled(疎結合)とTightly-Coupled(密結合)に分けられる。疎結合は、Camera側の姿勢推定とIMU側の姿勢推定をそれぞれ独立に計算し、得られた2つの推定値を後から統合する構成であり、実装は単純だが、Camera側の推定に含まれる誤差の内部構造(どの特徴点の観測がどれだけ不確かか)をIMU側が活用できないため精度で劣る。密結合は、IMUの生データと画像の特徴点観測を、単一の状態推定問題として同時に扱う構成であり、実装は複雑になるが、両方のセンサーの情報を余すことなく活用できるため高精度になる。現代の代表的なVIO実装(8・9節、10節で見るOKVIS・VINS-Mono・OpenVINSなど)は、いずれも密結合を採用している。

7. LIO(LiDAR-Inertial Odometry)

LIO(LiDAR-Inertial Odometry)は、同じ枠組みでLiDARをUpdate側のセンサーに使う構成である。LiDARの点群は環境の幾何形状を直接・高精度に捉えられる一方、1回のスキャンには数十〜数百ミリ秒の走査時間がかかり、その間にセンサー自身が動くとスキャン内の点群が歪む(Motion Distortion)という問題を抱える。IMUの高頻度な姿勢情報があれば、この走査中の動きを補正し、歪みのない点群を再構成できる。

また6節のVIOと同様、LIOが苦手とする幾何学的な縮退環境(長いトンネル・広い平面)でも、IMUが短時間の姿勢予測を補い続けることで、点群同士の位置合わせ(ICP/NDTのような手法)が発散しにくくなる。LIOも実装上はTightly-Coupledな構成が主流であり、10節で見るLIO-SAM・FAST-LIO/FAST-LIO2はいずれもこの密結合設計を採用している。

8. Filter-based(Kalman Filter/EKF/MSCKF)

VIO・LIOの「予測と観測を統合する」という核心部分は、大きく分けてFilter-based(フィルタベース)と、次節で見るOptimization-based(最適化ベース)という2つの設計思想のどちらかで実装される。

Filter-basedの基本となるのがKalman Filter(カルマンフィルタ)であり、状態が線形なシステムに対して、予測(Prediction)と観測による補正(Update)を交互に繰り返すことで、最適な状態推定を逐次的に求める枠組みである。VIO・LIOのようにIMUの積分(姿勢の合成)やCameraの投影が本質的に非線形な問題では、その拡張であるEKF(Extended Kalman Filter、拡張カルマンフィルタ)が使われる。EKFは非線形な関数をその時点の推定値のまわりで線形近似(1次のテイラー展開)した上で、通常のカルマンフィルタと同じ更新式を適用する。

ここで\hat{x}_k^{-}はPropagationによる予測状態、z_kは実際の観測値、h(\cdot)は状態から観測を予測する観測モデル、K_kはKalman Gain(カルマンゲイン)と呼ばれる、予測と観測のどちらをどれだけ信頼するかを決める重みである。P_k^{-}は予測状態の共分散(不確かさ)、H_kは観測モデルを線形近似したヤコビアン、Rは観測ノイズの共分散を表す。直感的には、予測の不確かさP_k^{-}が観測ノイズRに比べて大きいほどKalman GainK_kは大きくなり、観測z_kの情報がより強く状態の補正に反映される。

EKFベースのVIOをそのまま素朴に実装すると、画像から得られる特徴点1つ1つを状態ベクトルに含める必要があり、特徴点の数が増えるほど計算量が急速に増大するという問題がある。この問題を解決するのがMSCKF(Multi-State Constraint Kalman Filter)であり、Mourikis & Roumeliotisが2007年に提案した。MSCKFは、特徴点そのものをフィルタの状態には含めず、代わりに複数のカメラ姿勢(スライディングウィンドウ)を状態に保持し、同じ特徴点が複数の姿勢から観測されたという幾何学的な拘束だけを使って状態を更新する。特徴点を状態から除外することで、特徴点数に対する計算量の増大を避けつつ、複数フレームにまたがる観測情報を活用できる、効率的なFilter-based VIOの設計として広く使われている。

9. Optimization-based(Bundle Adjustment/Factor Graph)

Filter-basedは逐次更新を基本とするが、MSCKFのように過去姿勢のcloneを一時的に保持する方式もある。Optimization-based(最適化ベース)は、複数のキーフレームにまたがる状態・観測をwindowやグラフに保持し、それら全体の整合性を非線形最適化する設計である。Visual-SLAMのBundle Adjustment(「Visual-SLAM入門」10節参照)と同じ発想を、IMUの情報も含めて拡張したものと理解すると分かりやすい。

IMUの生の観測(数百Hzで得られる加速度・角速度)は既知データで、最適化変数ではない。各IMU時刻の状態追加や、再線形化ごとの再積分を避ける手法がIMU Preintegration(IMU事前積分)である。Forsterらが2015年に提案した(arXiv:1512.02363)。キーフレーム間の計測をまとめ、バイアス補正可能な相対運動拘束(Factor)として扱う。状態には姿勢だけでなく速度・バイアス、方式によってランドマークも含まれるため、計算量はwindow設計と再線形化頻度で決まる。

こうして得られたIMU Preintegration Factorと、Camera/LiDARの観測から得られる再投影誤差やスキャンマッチング誤差のFactorを、共通のFactor Graph(因子グラフ)の上に配置し、まとめて最尤推定(あるいは最大事後確率推定)を解くのがOptimization-basedの基本形である。

\mathcal{X}は推定対象の姿勢・速度・バイアス(必要に応じて地図点)の集合、r_{\mathrm{IMU}}はIMU Preintegration Factorから得られる残差である。Filter-basedは逐次更新と限定した履歴で計算量を制御し、Optimization-basedはwindowを再線形化できる。精度と計算量はwindow長、初期値、再線形化頻度、モデル、計算資源に依存する。

図3 · ボタンで表示を切り替え

図4 · ボタンで表示を切り替え

i Keyframe i{+}1 高頻度のIMU計測(数百Hz) 区間内でまとめて積分 → 1つの相対運動Factorへ

図2 — Keyframe iとi{+}1の間に得られる大量のIMU計測(数百Hz)を、Preintegrationによって区間内でまとめて積分し、Factor Graph上の1つの相対運動Factorとして扱う。これにより最適化変数の数はIMUのサンプリング周波数に依存せず、キーフレーム数だけで済む。

10. 主要アルゴリズム

VIOの代表アルゴリズム

ROVIO(Robust Visual Inertial Odometry)は、BloeschらがIROS 2015で発表した、EKFベースのVIOである。特徴点の記述子照合ではなく、画像パッチの輝度情報を直接EKFの観測モデルに組み込むDirect法的なアプローチを採用しており、Filter-based×Direct法という組み合わせの代表例になっている。

OKVIS(Open Keyframe-based Visual-Inertial SLAM)は、LeuteneggerらがIJRR 2015で発表した、キーフレームベースの非線形最適化によるVIOである。9節で見たIMU PreintegrationとBundle Adjustment的な再投影誤差の最小化を組み合わせた、Optimization-based VIOの初期の代表的な実装として知られる。

VINS-Monoは、Qin・Li・Shen(香港科技大学)が発表した単眼カメラ向けのVIOであり、9節のIMU Preintegrationに基づく非線形最適化に加え、Loop Closure(ループクロージャ)による大域的な補正、そして各コンポーネントを疎結合にも密結合にも切り替えられる柔軟な設計を特徴とする。VINS-Fusionは、このVINS-Monoをステレオカメラ・GPSなど複数のセンサー構成に対応するよう拡張した後継実装であり、より多様なハードウェア構成での実用性を重視している。

OpenVINSは、Geneva・Eckenhoff・Lee・Yang・Huangらが発表した、MSCKFベースのオープンソースVIO研究プラットフォームである。8節で見たMSCKFの効率的なFilter-based設計を踏襲しつつ、研究用途での拡張・検証のしやすさを重視した実装として広く利用されている。

LIOの代表アルゴリズム

LIO-SAM(Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping)は、Shan・Englot・Meyers・Wang・Ratti・RusらがIROS 2020で発表した、Factor Graphベースの密結合LIOである。IMU Preintegration Factor、LiDARスキャンマッチングによるOdometry Factor、GPS Factor、Loop Closure Factorという複数種類のFactorを同一のFactor Graph上に統合し、9節で見たOptimization-basedの設計思想をLiDARに適用した代表例になっている。

FAST-LIO(Fast LiDAR-Inertial Odometry)は、香港大学MARSラボのXu・Zhangらが発表した、Tightly-Coupled Iterated Kalman Filter(反復拡張カルマンフィルタ)による高速なLIOである。Optimization-basedのLIO-SAMとは対照的に、Filter-basedでありながら高頻度なLiDAR点群を直接扱える設計を採り、計算資源の限られた小型プラットフォームでの実時間動作を重視している。FAST-LIO2は、この後継として、点群をそのまま逐次的な最近傍探索構造(iKD-Tree)で管理することで地図の再構築コストを大幅に削減し、より高速かつ高精度な密結合LIOを実現した。

11. VIO vs LIO比較表

VIOはセンサーコストの低さと、色・模様に基づく高レベルな環境理解(物体認識などとの親和性)に強みがあり、LIOは幾何学的な精度と暗所・悪天候への頑健性に強みがある、という棲み分けが基本になる。

12. 実用上の選び方

VIOとLIOのどちらを選ぶか、あるいは両方を組み合わせるかは、搭載できるセンサー・予算・想定環境によって変わる。

• 屋内サービスロボット・掃除ロボット: 低コストを優先するならVIO(あるいはカメラ単体+IMUの疎結合構成)、精度と幾何学的な地図の正確さを優先するならLIOが選ばれやすい。廊下や壁の多い定型的な屋内環境ではLiDARのコストに見合う精度向上が得やすい。
• ドローン: 搭載重量・電力に厳しい制約があるため、軽量なカメラ+IMUによるVIOが主流になりやすい。屋外の開けた空間ではGPSとの併用も一般的である。
• AR/VR: ヘッドセット内蔵のカメラとIMUを使うVIO(Visual-Inertial Odometry)が事実上の標準であり、LiDARを追加搭載する一部のハイエンド機種を除けば、コスト・重量の制約からVIOが第一選択になる。
• 自動運転・自律移動ロボット(屋外): 悪天候・逆光・夜間走行での頑健性が要求されるため、LIOを中核に据え、VIOやGNSSと多層的に組み合わせる構成が一般的である(「Sensor Fusion入門」参照)。
• トンネル・長い直線廊下のような幾何学的に縮退した環境: LiDAR単体・LIO単体では位置合わせが不安定になりやすいため、VIOやホイールオドメトリのような別系統の情報源を追加して縮退を補うことが重要になる。

予算とセンサー重量に余裕があり幾何学的な精度を最優先するならLIO、コストと軽量性を優先し色・模様に基づく環境理解も活かしたいならVIO、というのが基本的な判断軸である。実際の製品開発では、この2つを排他的に選ぶのではなく、VIO・LIO・GNSS・ホイールオドメトリを状況に応じて重み付けする多センサー融合(「Sensor Fusion入門」参照)へと発展させていくのが一般的である。

13. まとめ

IMUは高頻度で自己完結した運動情報を得られる一方、積分による位置推定は時間の2乗〜3乗のオーダーで急速にDriftする。Camera(VIO)・LiDAR(LIO)は、この蓄積誤差を定期的に補正する一方、それぞれ暗所・低テクスチャや幾何学的な縮退環境という別の弱点を抱えている。VIO・LIOは、この相補的な弱点をPredict(IMUによる高頻度な予測)–Update(Camera/LiDARによる補正)のループで組み合わせ、Filter-based(EKF/MSCKF)かOptimization-based(IMU Preintegration+Factor Graph)のいずれかの枠組みで実装することで、単体のセンサーでは到達できない頑健性と精度を実現する技術である。