ロボットや自動運転車は、カメラ・LiDAR・IMU・Radar・GNSSといった複数のセンサーを同時に積んでいる。だが、それぞれのセンサーは単体では世界を正しく理解できない——カメラは奥行きが苦手で、LiDARは色や質感が分からず、IMUは時間とともに誤差が蓄積する。Sensor Fusion(センサーフュージョン)は、この個々のセンサーの弱点を、他のセンサーの強みで補い合いながら、1つの矛盾のない「世界モデル」を組み立てる技術である。この記事では、確率論・最適化という2つの数学的な土台から、VIO・LIO・Camera×LiDARという具体的な組み合わせまでを一気に見ていく。
0. この記事で分かること
- Sensor Fusionが何を目的とした技術か、なぜ1つのセンサーだけでは足りないのか
- Fusionの基本パイプライン(センサー→計測→特徴/状態→融合→推定状態)
- Early/Mid/Lateという3つの融合レベルの違い
- Kalman Filter/EKF/UKF/Particle Filterという確率的Fusionの仕組み
- Factor Graph/Pose Graphによる最適化ベースFusionの仕組み
- VIO(Camera×IMU)・LIO(LiDAR×IMU)・Camera×LiDARという代表的な組み合わせがなぜ相性が良いのか
- 自動運転を例にした、Camera×LiDAR×Radar×IMUの4センサー統合
- 時刻同期・キャリブレーション・ノイズなど、Fusionで実際に難しいこと
- 用途に応じたFusion設計の考え方(何を融合するか、Filterか最適化か)
1. まず結論：Sensor Fusionとは何か
一言で言えば、Sensor Fusionとは、複数のセンサーが返す不完全で誤差を含む観測値を数学的に統合し、単独のセンサーよりも正確で信頼できる「状態」の推定値を作り出す技術である。
この定義には3つの要素が含まれている。1つ目は相補性(complementarity)——センサーごとに得意・不得意が異なり、組み合わせることで互いの弱点を打ち消し合えるという性質。2つ目はState Estimation(状態推定)——「今、自分(あるいは対象物)がどこに、どんな速度で、どんな姿勢でいるか」という、直接観測できない量を、観測できる量から推定する統計的な枠組み。3つ目はWorld Model(世界モデル)——個々のセンサーが返す断片的な情報を、時間的にも空間的にも矛盾のない1つの表現へ統合した結果である。ロボットが実際に行動判断を下すのは、生のセンサーデータに対してではなく、この統合済みのWorld Modelに対してである。
2. なぜ1つのセンサーでは足りないのか
各センサーの得意・苦手を並べてみると、Fusionがなぜ必須なのかが明確になる。
| センサー | 得意なこと | 苦手なこと |
|---|---|---|
| Camera(カメラ) | 色・テクスチャ・意味情報(物体の種類)の認識、高い角度分解能 | 奥行きの直接計測が困難、暗所・逆光・悪天候に弱い、スケールが不定(単眼の場合) |
| LiDAR | 高精度な距離・幾何形状の直接計測、照明条件に依存しない | 色・質感が分からない、雨や霧・粉塵に弱い、点群が疎になる遠方は精度低下 |
| IMU(慣性計測装置) | 高頻度(数百Hz〜)でのサンプリング、姿勢変化に敏感、外部環境に依存しない | 積分による誤差蓄積(ドリフト)、絶対位置は分からない |
| Radar(ミリ波レーダー) | 悪天候(雨・霧・粉塵)への耐性、速度(ドップラー効果)の直接計測 | 角度分解能が低く物体形状の把握が粗い、静止物体の識別が苦手な場合がある |
| GNSS(衛星測位) | 絶対的なグローバル座標が得られる、誤差が蓄積しない | 更新頻度が低い(通常1〜10Hz)、屋内・トンネル・都市部の高層ビル街(マルチパス)で精度低下・途絶 |
この表から見えてくるのは、「精度が高いセンサー」と「万能なセンサー」は別物だ、という事実である。LiDARは幾何形状の精度でカメラを上回るが、色や意味は分からない。GNSSは絶対位置を与えてくれるが、更新頻度が低くビル街では信頼できない。IMUは唯一、外部環境にまったく依存しないセンサーだが、積分で状態を求める以上、時間とともにドリフトが避けられない。Sensor Fusionは、この「誰かの弱点は、他の誰かの強みで埋まっている」という相補的な関係を、数学的に厳密な形で活用する技術である。
3. Fusionの基本構造
Sensor Fusionのパイプラインは、扱う対象や融合レベル(次節で説明)によって細部は変わるものの、大枠は次の5段階で共通している。
図1 — 各センサーの生データ(Measurement)から、特徴量または状態量を取り出し、Fusion段階で1つの推定状態へ統合する。
各センサーが返す生の計測値(Measurement)は、そのままでは単位も座標系も更新頻度も異なるため直接比較できない。まず各センサー系統ごとに、特徴量(画像の特徴点、点群のエッジなど)や部分的な状態量(速度、相対姿勢など)へと変換し、その上でFusionの段階に渡す。Fusionの中身が、後述する確率的手法(Kalman Filter等)や最適化ベース手法(Factor Graph等)であり、最終的に位置・速度・姿勢などを含む推定状態(Estimated State)が出力される。
4. Fusionの3レベル
Fusionは「パイプラインのどの段階で複数センサーの情報を合流させるか」によって、大きく3つのレベルに分類できる。
| レベル | 融合するタイミング | 特徴 | 代表例 |
|---|---|---|---|
| Early Fusion(早期融合) | 生データに近い段階で合流 | 情報損失が少なく高精度になりうるが、センサー間の時刻同期・座標系整合が厳密に必要 | LiDAR点群をカメラ画像に投影して結合した入力を作る手法 |
| Mid-level Fusion(特徴量融合) | 各センサーで抽出した特徴量の段階で合流 | 生データ融合ほど厳密な同期を要求されず、実装の柔軟性が高い | BEV空間でカメラ特徴とLiDAR特徴を合成するBEV Fusion系 |
| Late Fusion(後期融合) | 各センサーが個別に出した認識・推定結果(検出結果など)を合流 | センサーごとに独立したパイプラインを維持でき実装が容易、ただし早い段階の情報損失を後から取り戻せない | カメラの物体検出結果とLiDARの物体検出結果を、あとから統合するアンサンブル的な手法 |
どのレベルを選ぶかは、精度と実装コストのトレードオフになる。Early Fusionは理論上もっとも情報を活かせるが、センサー間のミリ秒単位の時刻ズレや、数ミリ単位の取り付け位置誤差がそのまま結果を悪化させるため、要求されるキャリブレーション精度が高い。Late Fusionは各センサーの処理系を独立に開発・デバッグできる実務上の利点が大きい一方、個々のセンサーが誤って「見逃した」情報は、後段でどれだけうまく統合しても復元できない。近年主流になりつつあるMid-level Fusion(特に自動運転のBEV Fusion)は、この両者のちょうど中間で折り合いをつける設計として支持を集めている。
5. 確率的Fusion
複数センサーの観測値を統合する数学的な枠組みの1つ目が、確率論に基づくアプローチである。土台になるのはベイズ推定(Bayesian Estimation)の考え方で、「1時刻前までの推定(事前分布)」と「新しい観測(尤度)」を組み合わせて、「今の推定(事後分布)」を更新していく。
左辺は、時刻1からtまでの全観測z_{1:t}が得られたときの、状態x_tの事後分布である。右辺は、1つ前の時刻の推定p(x_{t-1}\mid z_{1:t-1})を運動モデルp(x_t\mid x_{t-1})で1時刻分予測し(積分の部分)、その予測に新しい観測の尤度p(z_t\mid x_t)を掛け合わせて更新する、という「予測→更新」のサイクルを表している。
Kalman Filterは、この一般的なベイズ更新を、状態も観測もすべて線形かつガウス分布に従うという仮定のもとで、閉じた式で解けるようにしたものである。予測ステップでは状態と誤差共分散を運動モデルで進め、更新ステップでは新しい観測との食い違い(イノベーション)をKalman Gain(カルマンゲイン)で重み付けして状態を補正する。
P_t^{-}は予測段階での誤差共分散、Hは状態から観測への変換行列、Rは観測ノイズの共分散である。Kalman Gain K_tは、「モデルの予測」と「新しい観測」のどちらをどれだけ信頼するかを表す重みであり、観測ノイズRが小さい(センサーが信頼できる)ほど観測を重視し、予測の不確かさP_t^{-}が小さい(モデルが自信を持っている)ほど予測を重視する形に自動的に調整される。
現実のロボットの運動やセンサーモデルは線形ではないことがほとんどのため、実際にはKalman Filterをそのまま使えない場面が多い。EKF(拡張カルマンフィルタ)は、非線形な運動モデル・観測モデルを現在の推定値のまわりでヤコビ行列によって線形近似し、通常のKalman Filterと同じ更新式を適用する。UKF(Unscented Kalman Filter、Julier・Uhlmann、1997年)は、ヤコビ行列による近似を使わず、代表的な少数のサンプル点(シグマ点)を非線形関数にそのまま通して分布の平均・共分散を再構成する方式で、強い非線形性に対してEKFより高い精度を出せる場合がある。Particle Filter(Gordon・Salmond・Smith、1993年)は、分布をガウス分布に限定せず、多数の粒子(サンプル)で確率分布そのものを近似する手法で、複数の仮説が同時に存在するような多峰性の分布も表現できるが、精度を上げるには粒子数を増やす必要があり計算コストが高くなりやすい。
6. Optimization-based Fusion
確率的Fusionが「1時刻ごとに逐次更新する」のに対し、もう1つの数学的な枠組みである最適化ベースFusionは、「一定期間分の観測をまとめて保持し、それら全体との整合性が最大になるように、まとめて解き直す」というアプローチを取る。
その代表がFactor Graph(因子グラフ)による定式化である。推定したい状態(ロボットの各時刻の姿勢や、観測したランドマークの位置)をノードとして、それらの間の拘束(あるセンサー観測が「この2つの状態はこういう関係にあるはずだ」と教えてくれる情報)をFactor(因子)として結ぶグラフを構築し、全体を非線形最小二乗問題として解く。
\mathbf{x}は推定したい全状態、\mathbf{z}_kはk番目のセンサー観測、\mathbf{e}_kはその観測から予測される値と実際の状態の食い違いを表す誤差関数、\Sigma_k^{-1}はその観測の信頼度による重み付けである。カメラ姿勢だけでなくランドマークの3次元位置も同時に最適化する場合、この処理は特にBundle Adjustment(バンドル調整)と呼ばれる。ロボットの軌跡上の姿勢をノードとした場合はPose Graph(姿勢グラフ)最適化と呼ばれ、ループクロージャ(過去に訪れた場所の再認識)によって蓄積誤差を後から補正する際の標準的な枠組みになっている。
確率的Fusion(特にEKF)は計算コストが低く逐次処理に向く一方、一度取り込んだ観測を後から修正できない(過去の推定を再利用して前に進むだけ)という制約がある。最適化ベースFusionは、新しい観測が古い推定と矛盾したときに過去に遡って全体を修正できるため、一般に高精度だが、保持するグラフが大きくなるほど計算コストが増える。Ceres Solver・g2o・GTSAMといった汎用ライブラリが、この種の疎な(1つの観測が関わる変数がごく少数に限られる)非線形最小二乗問題を効率的に解くために広く使われている。
7. Camera×IMU(VIO)
Camera(カメラ)とIMUの組み合わせは、Visual-Inertial Odometry(VIO)と呼ばれ、実用上もっとも普及しているFusionの組み合わせの1つである。この2つが相性が良い理由は、弱点が驚くほどきれいに補い合う関係にあるからである。
カメラは、画像中の特徴点の移動から自分の動きを推定できるが、単眼カメラの場合は絶対的なスケール(実際の距離の単位)が原理的に分からず、また高速な回転運動時には画像がブレて特徴点追跡が破綻しやすい。IMUは逆に、加速度・角速度を数百Hzという高頻度で直接計測できるため高速な動きに強く、加速度の積分によって実スケールでの速度・位置変化を得られるが、この積分ゆえに時間とともに誤差が蓄積(ドリフト)し、絶対位置は分からない。
この式が示す通り、IMUの位置推定は加速度a_tを2回積分して求めるため、加速度計のわずかなバイアス誤差が時間の2乗で位置誤差に効いてくる。VIOは、この積分によるドリフトを、カメラが低頻度(数十Hz)ながら定期的に観測する視覚的な特徴点情報で補正し、同時にカメラだけでは決まらないスケールを、IMUの実寸の加速度情報から与える。MSCKF(Mourikis・Roumeliotis、2007年、EKFベース)、OKVIS(Leutenegger他、2015年、非線形最適化ベース)、VINS-Mono(Qin他、2018年、単眼+IMU)といった実装が、この確率的または最適化ベースの枠組みでVIOを実現している代表例である。
8. LiDAR×IMU(LIO)
LiDARとIMUの組み合わせはLiDAR-Inertial Odometry(LIO)と呼ばれ、屋外ロボットや自動運転で広く使われる。LiDARは点群として周囲の幾何形状を直接・高精度に計測できるが、1回のスキャンには数十〜100ミリ秒程度の時間がかかり、その間にセンサー自体が動くと、1回のスキャン内の点群が歪む(モーションディストーション)という問題を抱える。
IMUはここでも、高頻度なサンプリングという強みを発揮する。スキャン中の細かい姿勢変化をIMUで補間することで、点群の歪みを補正できる。さらに、点群同士のマッチング(ICP等)だけに頼ると、幾何学的な特徴が乏しい環境(長い廊下、開けた平地など)で位置がずれやすいが、IMUの慣性情報がその区間を補って推定を安定させる。
初期のLOAM(Zhang・Singh、2014年)は、点群から角(エッジ)と平面の特徴点を抽出し、それらのマッチングでLiDARオドメトリを実現する先駆的な手法だった。LIO-SAM(Shan他、2020年)は、LiDARとIMUの情報をFactor Graph上でタイトに統合し、GPSやループクロージャの拘束も同じグラフに組み込める設計にした。FAST-LIO/FAST-LIO2(Xu他、2021年/2022年)は、Iterated Kalman Filter(反復カルマンフィルタ)による密結合(tightly-coupled)なLiDAR-IMU統合を採用し、疎な特徴点抽出を経ずに点群を直接扱う(FAST-LIO2)ことで、計算効率と精度を大きく引き上げた。密結合(状態推定の内部でLiDARとIMUの情報を一体的に扱う)は、疎結合(それぞれ独立に推定してから結果だけを混ぜる)よりも一般に精度が高いが、実装の複雑さも増す。
9. Camera×LiDAR
CameraとLiDARの組み合わせは、「意味は分かるが距離は苦手なセンサー」と「距離は分かるが意味は苦手なセンサー」を組み合わせる、もっとも直感的に相補的なFusionである。ただしこの組み合わせには、他の組み合わせにはない固有の難しさがある——2つのセンサーの座標系が根本的に異なる(カメラは2次元の画像平面、LiDARは3次元の点群)ため、両者を対応づけるにはキャリブレーションと投影という手続きが不可欠になる。
キャリブレーションでは、カメラの内部パラメータ(焦点距離・レンズ歪み、内部行列K)と、カメラ・LiDAR間の相対的な位置姿勢(外部パラメータ、回転R・並進t)を求める。これが分かれば、LiDARが計測した3次元点\mathbf{X}を、カメラ画像上のピクセル座標\mathbf{u}へ投影(projection)できる。
\pi(\cdot)は同次座標を画像平面上の2次元座標に変換する透視投影関数であり、[R\mid t]はLiDAR座標系からカメラ座標系への変換である。この誤差\mathbf{e}を最小化するようにR・tを求める処理がキャリブレーションであり、キャリブレーションが済んだ後は、この同じ投影によって「LiDARのどの点が、画像のどのピクセルに対応するか(Point-Pixel Association)」を求められる。これにより、LiDAR点にカメラが持つ色・クラス情報を付与したり、逆にカメラが検出した物体にLiDARの正確な距離を与えたりできる。
近年の自動運転で主流になっているのが、この投影を個々の点ではなく特徴量の段階で行うBEV Fusion(Bird's-Eye View Fusion)である。BEVFusion(Liang他、2022年 / Liu他、2022年)は、カメラ画像の特徴とLiDAR点群の特徴を、それぞれ独立にBEV(真上から見た2次元)空間へ変換したうえで合成する設計を採り、Camera×LiDARのMid-level Fusionを代表する手法として広く参照されている。
10. Camera×LiDAR×Radar×IMU
自動運転車を例にとると、実際には2センサーの組み合わせにとどまらず、Camera・LiDAR・Radar・IMU(さらにGNSS)を同時に統合するのが一般的である。それぞれが担う役割は明確に分かれている。
- Camera: 信号機の色、標識の文字、車線の種類など、幾何情報だけでは分からない「意味」を認識する
- LiDAR: 周囲の物体の正確な3次元形状・距離を、照明条件に左右されずに計測する
- Radar: 雨・霧・逆光などカメラやLiDARが苦手な悪天候下でも機能し、物体の相対速度をドップラー効果から直接計測する
- IMU: 車両自体の姿勢変化・加減速を高頻度に捉え、他センサーの観測の合間を補間する
- GNSS: 地図とのグローバルな位置合わせの基準を与える(通常RTK等で高精度化した上でIMUと統合される)
この4〜5種類のセンサーは、どれか1つが苦手な状況を、他のセンサーがほぼ確実にカバーする関係になっている。濃霧でカメラとLiDARの精度が落ちてもRadarは機能し、GPSが届かないトンネル内でもIMUとLiDARオドメトリで自己位置を維持できる、といった具合である。この「ある状況で複数のセンサーが同時に機能を失う可能性を減らす」という冗長性の設計が、自動運転のように失敗が許されない用途でマルチセンサーFusionが不可欠とされる最大の理由である。
11. Fusionで難しいこと
Sensor Fusionは理論上美しくまとまっていても、実装においてはいくつもの現実的な難しさに直面する。
- Time Synchronization(時刻同期): 各センサーは異なる周期・異なる遅延でデータを出力するため、「同じ瞬間」のデータとして扱うには正確な時刻合わせが必要になる。ハードウェアレベルでのトリガー同期や、タイムスタンプの補間が使われる。
- Calibration(キャリブレーション): センサー間の相対位置・姿勢(外部パラメータ)と、各センサー自体の内部パラメータを正確に求める必要がある。取り付け位置がわずかにズレるだけでFusion結果全体に系統的な誤差が乗る。
- Coordinate Transformation(座標変換): センサーごとに異なる座標系(カメラ座標系、LiDAR座標系、車両座標系、グローバル座標系)を、一貫性を保ったまま変換し続ける必要がある。
- Sensor Noise(センサーノイズ): 各センサーのノイズ特性(ガウス性かどうか、バイアスの有無)はセンサーごとに異なり、これを正しくモデル化しないと、確率的Fusionの重み付け(Kalman Gain等)が不適切になる。
- Outlier(外れ値): センサーの誤検出や、動的物体によるマッチングミスなど、モデルが想定しないノイズが混入することがあり、単純な統計モデルだけでは対処しきれない。RANSACのような頑健推定手法や、外れ値を明示的に棄却する仕組みが必要になる。
- Uncertainty(不確実性の伝播): 個々のセンサーの不確実性(共分散)を、Fusion全体を通じて正しく伝播させないと、最終的な推定が「実際にはどれだけ信頼できるか」を見誤る。過信も過小評価も、後段の意思決定を誤らせる。
これらはどれも、教科書的なKalman Filterの式を実装するだけでは自動的に解決しない、システム設計・エンジニアリングの領域の課題である。
12. 最終的なWorld Model
ここまで見てきた個々のセンサー・個々のFusion手法を統合すると、最終的にロボットや自動運転車が持つのは、次の図のような、複数センサーの得意分野を1つに束ねた「世界モデル」である。
図2 — Camera・LiDAR・IMU・Radar・GNSSがそれぞれ異なる情報を提供し、Fusion層で統合されて1つの一貫したWorld Modelになる。
World Modelには、単なる「自分の位置」だけでなく、周囲の物体の幾何形状(LiDAR由来)、その物体が何であるかという意味情報(Camera由来)、物体の相対速度(Radar由来)、自分自身の姿勢変化(IMU由来)、地図上のグローバルな位置(GNSS由来)が、時間的・空間的な矛盾なく統合されている。ロボットの経路計画や自動運転車の運転判断は、生のセンサーデータではなく、このWorld Modelを参照して行われる——Sensor Fusionは、個々のセンサーの認識能力を、行動可能な1つの一貫した表現へ変換する、認識と行動の間の橋渡し役だと言える。
13. 実用上の設計方法
実際にSensor Fusionを設計する際は、次の4つの問いにそって考えると判断がしやすい。
| 設計判断 | 検討すべき点 |
|---|---|
| 何を融合するか | 用途上、どのセンサーの弱点をカバーする必要があるか(悪天候対応が必須ならRadar、絶対位置が必要ならGNSSなど)を先に決める |
| どの段階で融合するか | 精度最優先ならEarly/Mid-level、開発の独立性・保守性を優先するならLate Fusion |
| Filter vs Optimization | 逐次・低遅延・低計算資源が要件ならFilter(EKF等)、精度最優先で計算資源に余裕があり、後から過去の推定を修正したいなら最適化ベース(Factor Graph等) |
| 精度 vs 計算量 | 密結合(tightly-coupled)は一般に高精度だが実装・計算コストが高く、疎結合(loosely-coupled、各センサーで別々に推定してから混ぜる)は実装が容易で計算量も少ないが精度で劣る |
これらの判断は独立ではなく、互いに絡み合っている。例えば車載の量産システムでは、計算資源の制約からFilterベース・疎結合を選ばざるを得ないことが多い一方、研究用途やオフラインでの地図生成では、最適化ベース・密結合で最大限の精度を追求できる。「完璧なFusion手法」は存在せず、搭載できるセンサー・利用できる計算資源・求められる精度とレイテンシのバランスの中で、最も妥当な組み合わせを選ぶことが、実務上のSensor Fusion設計の本質である。
14. まとめ
Sensor Fusionは、Camera・LiDAR・IMU・Radar・GNSSという、それぞれ得意・不得意が異なるセンサーの観測値を、確率的手法(Kalman Filter/EKF/UKF/Particle Filter)または最適化ベース手法(Factor Graph/Bundle Adjustment)によって数学的に統合し、単独では得られない精度と信頼性を持つ状態推定を作り出す技術である。VIO(Camera×IMU)・LIO(LiDAR×IMU)・BEV Fusion(Camera×LiDAR)といった具体的な組み合わせはいずれも、この「相補性を数式に落とし込む」という同じ発想の上に成り立っており、どの段階で・どの手法で融合するかという設計判断が、最終的な精度・計算コスト・実装の複雑さを決める。