概要
センサーフュージョン（Sensor Fusion）とは、複数のセンサーから得られるデータを組み合わせ・統合することで、単一センサーでは実現できない高精度・高信頼性の計測・推定を行う技術です。各センサーの弱点を他のセンサーで補完し、また複数センサーの冗長性で故障耐性を高めるという原則に基づいています。
典型的なセンサーフュージョンの例：
- IMUフュージョン（加速度＋ジャイロ＋磁気）：加速度センサーの低周波精度とジャイロスコープの高周波精度を相補フィルターやカルマンフィルターで統合し、ドリフトの少ない姿勢推定を実現
- GNSS/IMU統合：GPSの絶対位置精度とIMUの高サンプルレート・デッドレコニング能力を統合し、屋内・トンネルでのGPS途絶時でも位置を推定
- 自動運転センシング：LiDAR・カメラ・ミリ波レーダー・IMUを統合し、全天候・高精度な周囲環境認識
- ヘルスモニタリング：加速度・心拍（PPG）・皮膚温を統合して活動量・睡眠状態・ストレスを推定
フュージョンのアルゴリズムとしては、相補フィルター・カルマンフィルター（KF）・拡張カルマンフィルター（EKF）・無香料カルマンフィルター（UKF）・粒子フィルター（PF）・深層学習ベースの統合手法など、応用に応じた様々な手法が使われます。
歴史・背景
センサーフュージョンの概念は1950〜60年代の軍事・航空宇宙分野での慣性航法システム（INS）の研究から始まります。ルドルフ・カルマンが1960年に発表したカルマンフィルター（Kalman, 1960, “A New Approach to Linear Filtering and Prediction Problems”）は、センサーノイズと状態推定誤差を統計的に最適化する手法として革命的でした。アポロ計画（1969年の月面着陸）でカルマンフィルターによるIMU/レーダー統合が使われたことで、宇宙工学での有用性が実証されました。
1980〜90年代には軍事レーダーシステムや自動化工場での多センサー統合が研究の中心でした。民生分野では2000年代のスマートフォン普及が転機となり、加速度・ジャイロ・磁気・気圧センサーを統合した姿勢推定が標準化されました。AndroidとiOSの標準センサーAPIで姿勢データが提供されるようになり、アプリ開発者がセンサーフュージョン結果を容易に利用できるようになりました。
2010年代の自動運転・ドローン・VR/ARの発展とともに、より高度なセンサーフュージョン（3D LiDAR＋カメラ＋レーダー）が研究・実用化されました。現在はエッジAIと組み合わせた深層学習ベースのセンサーフュージョンが新しいフロンティアとなっています。
技術仕様
センサーフュージョンの抽象レベル分類
| レベル | 名称 | 説明 | 例 |
|---|---|---|---|
| Level 0 | データフュージョン | 生データのアライメント・タイムスタンプ統一 | マルチカメラの時刻同期 |
| Level 1 | 特徴レベルフュージョン | センサーデータから抽出した特徴を統合 | LiDAR点群＋画像特徴の統合 |
| Level 2 | 状態推定フュージョン | 推定した状態（位置・速度・姿勢）を統合 | GNSS/IMUのカルマンフィルター |
| Level 3 | 決定レベルフュージョン | 各センサーの判断結果を多数決・重み付けで統合 | 異常検知の多数決 |
| Level 4 | AI推論フュージョン | 生データをDNNに入力してエンドツーエンド統合 | 自動運転のend-to-end DNN |
主要フュージョンアルゴリズムの比較
| アルゴリズム | 計算量 | 非線形対応 | 精度 | 適用例 |
|---|---|---|---|---|
| 相補フィルター | 非常に低 | 部分的 | 中 | IMU姿勢（MCU向け） |
| カルマンフィルター（KF） | 低〜中 | 不可（線形のみ） | 高（線形モデル） | GPS/IMU |
| 拡張カルマンフィルター（EKF） | 中 | 可（ヤコビアン線形化） | 中〜高 | SLAM, GNSS/IMU |
| 無香料カルマンフィルター（UKF） | 中〜高 | 可（シグマ点） | 高 | ロボット姿勢推定 |
| 粒子フィルター（PF） | 高（粒子数依存） | 可（非ガウスも） | 非常に高 | 非線形・非ガウスSLAM |
| マドウィックフィルター | 低 | 可 | 中〜高 | AHRS（MCU向け） |
| Mahonyフィルター | 非常に低 | 可 | 中 | 超低リソースMCU |
動作原理
相補フィルターの理論と実装
加速度センサーはDC〜低周波で姿勢推定精度が高く（重力を使った傾き計算）、高周波ノイズに弱い。ジャイロは高周波の角速度変化に精密だが、積分誤差（ドリフト）が低周波で蓄積する。相補フィルターはこの周波数特性の違いを活かした統合手法です：
総合推定 = α × ジャイロ積分値（高周波） + (1-α) × 加速度計算値（低周波）
α = τ / (τ + Δt) （τ: カットオフ時間定数、Δt: サンプリング周期）
例: α=0.98 → τ = 0.98 × 0.01 / (1 - 0.98) = 0.49秒
→ 0.49秒より短い変化はジャイロ優先、長い変化は加速度優先// 3軸相補フィルター（Roll / Pitch 推定）
typedef struct {
float roll; // ロール角 [deg]
float pitch; // ピッチ角 [deg]
float alpha; // フィルター係数
} complementary_filter_t;
void cf_update(complementary_filter_t *cf,
float ax, float ay, float az,
float gx, float gy,
float dt) {
// 加速度から傾き計算
float accel_roll = atan2f(ay, sqrtf(ax*ax + az*az)) * 180.0f / M_PI;
float accel_pitch = atan2f(-ax, sqrtf(ay*ay + az*az)) * 180.0f / M_PI;
// ジャイロ積分
float gyro_roll = cf->roll + gx * dt;
float gyro_pitch = cf->pitch + gy * dt;
// 相補統合
cf->roll = cf->alpha * gyro_roll + (1.0f - cf->alpha) * accel_roll;
cf->pitch = cf->alpha * gyro_pitch + (1.0f - cf->alpha) * accel_pitch;
}カルマンフィルターの実装（1次元例：高度推定）
// GPS高度 + 気圧高度のカルマンフィルター統合
// 状態: [高度, 垂直速度], 観測: [GPS高度, 気圧高度]
typedef struct {
float x[2]; // 状態ベクトル [高度, 垂直速度]
float P[2][2]; // 誤差共分散行列
float Q[2][2]; // プロセスノイズ共分散
float R[2][2]; // 観測ノイズ共分散
} kalman2d_t;
void kalman2d_predict(kalman2d_t *kf, float dt) {
// 状態遷移行列 F = [[1, dt], [0, 1]]
float x_new[2];
x_new[0] = kf->x[0] + kf->x[1] * dt; // 高度 += 速度 × dt
x_new[1] = kf->x[1]; // 速度（一定と仮定）
kf->x[0] = x_new[0];
kf->x[1] = x_new[1];
// 共分散予測: P = F*P*F^T + Q
// （簡略化のため2×2行列演算を展開）
float p00 = kf->P[0][0] + dt*kf->P[1][0] + kf->P[0][1]*dt + dt*dt*kf->P[1][1] + kf->Q[0][0];
float p01 = kf->P[0][1] + dt*kf->P[1][1] + kf->Q[0][1];
float p10 = kf->P[1][0] + dt*kf->P[1][1] + kf->Q[1][0];
float p11 = kf->P[1][1] + kf->Q[1][1];
kf->P[0][0]=p00; kf->P[0][1]=p01; kf->P[1][0]=p10; kf->P[1][1]=p11;
}
void kalman2d_update_gps(kalman2d_t *kf, float gps_altitude) {
// 観測行列 H = [1, 0]（高度のみ観測）
float innovation = gps_altitude - kf->x[0];
float S = kf->P[0][0] + kf->R[0][0]; // 観測残差共分散
float K0 = kf->P[0][0] / S; // カルマンゲイン（高度）
float K1 = kf->P[1][0] / S; // カルマンゲイン（速度）
kf->x[0] += K0 * innovation;
kf->x[1] += K1 * innovation;
kf->P[0][0] -= K0 * kf->P[0][0];
kf->P[0][1] -= K0 * kf->P[0][1];
kf->P[1][0] -= K1 * kf->P[0][0];
kf->P[1][1] -= K1 * kf->P[0][1];
}自動運転向けLiDAR＋カメラフュージョン
# ROS 2 + Python: LiDAR点群とカメラ画像のセンサーフュージョン（概念コード）
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2, Image
from message_filters import ApproximateTimeSynchronizer, Subscriber
import numpy as np
import cv2
class LidarCameraFusionNode(Node):
def __init__(self):
super().__init__('lidar_camera_fusion')
# カメラ内部行列（キャリブレーション済み）
self.camera_matrix = np.array([
[fx, 0, cx],
[0, fy, cy],
[0, 0, 1]
], dtype=np.float32)
# LiDAR→カメラ座標変換行列（外部キャリブレーション）
self.T_lidar_to_cam = np.array([...]) # 4×4変換行列
# 時刻同期付きサブスクライバ（最大50ms誤差を許容）
self.lidar_sub = Subscriber(self, PointCloud2, '/velodyne_points')
self.image_sub = Subscriber(self, Image, '/camera/image_raw')
self.sync = ApproximateTimeSynchronizer(
[self.lidar_sub, self.image_sub], queue_size=10, slop=0.05)
self.sync.registerCallback(self.fusion_callback)
def fusion_callback(self, lidar_msg, image_msg):
# 1. LiDAR点群をカメラ画像平面に投影
points_3d = self.decode_pointcloud2(lidar_msg) # (N, 3)
# カメラ座標へ変換
pts_cam = self.T_lidar_to_cam[:3, :3] @ points_3d.T + \
self.T_lidar_to_cam[:3, 3:4]
# カメラ前方の点のみ（z > 0）
mask = pts_cam[2] > 0
pts_cam = pts_cam[:, mask]
# 画像平面に投影
pts_2d, _ = cv2.projectPoints(
pts_cam.T, np.zeros(3), np.zeros(3),
self.camera_matrix, np.zeros(4))
# 2. 画像内に投影された点の色情報を取得（テクスチャマッピング）
image = self.decode_image(image_msg)
h, w = image.shape[:2]
colored_points = []
for i, (x, y) in enumerate(pts_2d.reshape(-1, 2)):
if 0 <= x < w and 0 <= y < h:
color = image[int(y), int(x)] # BGR
depth = pts_cam[2, i]
colored_points.append((*pts_cam[:, i], *color, depth))
# 3. 色付き3D点群として物体検知・分類に利用
self.process_colored_pointcloud(colored_points)歩行者デッドレコニング（PDR）
# スマートフォンIMU → 屋内位置推定（PDR）
import numpy as np
from scipy import signal
class PedestrianDeadReckoning:
def __init__(self):
self.position = np.array([0.0, 0.0]) # X, Y [m]
self.heading = 0.0 # [rad]
self.step_length = 0.7 # デフォルト歩幅 [m]
def detect_step(self, accel_z_arr, fs=100):
"""加速度Z軸のピーク検出で歩数を計数"""
# バンドパスフィルター（0.5〜3Hz：歩行周波数帯域）
b, a = signal.butter(4, [0.5, 3.0], btype='bandpass', fs=fs)
filtered = signal.filtfilt(b, a, accel_z_arr)
# ピーク検出（閾値: 加速度標準偏差の1.5倍以上）
threshold = 1.5 * np.std(filtered)
peaks, _ = signal.find_peaks(filtered, height=threshold, distance=fs//3)
return len(peaks) # 歩数
def update_position(self, steps, heading_rad):
"""歩数と向きから位置を更新"""
distance = steps * self.step_length
self.position[0] += distance * np.cos(heading_rad)
self.position[1] += distance * np.sin(heading_rad)
return self.position.copy()
def fuse_with_wifi(self, wifi_position, wifi_confidence):
"""WiFiフィンガープリント位置とのフュージョン"""
# 加重平均（PDRの累積誤差が大きい場合はWiFiを重視）
pdr_weight = 0.7 if self.steps_since_wifi < 50 else 0.3
wifi_weight = 1.0 - pdr_weight
self.position = pdr_weight * self.position + wifi_weight * np.array(wifi_position)用途・ユースケース
自動運転（Level 3〜4）
自動運転車は3D LiDAR（Velodyne/Ouster）・高解像度カメラ（複数方向）・ミリ波レーダー・GNSS/RTK・IMUを統合します。各センサーの特性：LiDAR（精密3D点群・悪天候に弱）、カメラ（テクスチャ・色・テキスト認識）、レーダー（全天候・速度計測）を組み合わせ、どのセンサーが単体で失敗しても全体が機能するロバスト設計が求められます。
ドローン自律飛行
ドローンのセンサーフュージョンスタック例：IMU（1kHz姿勢推定）＋ 気圧センサー（高度）＋ GNSS（絶対位置）＋ 光学フロー（低高度速度）＋ 超音波（地面近接）。オープンソースのPX4/ArduPilotがEKFベースのフュージョンを実装しています。
ウェアラブル・ヘルスケア
Apple Watch・Garminウォッチは加速度・ジャイロ・心拍PPG・皮膚温・SpO2を統合し、活動量・睡眠ステージ・ストレス・血中酸素飽和度を推定します。センサー間の相関を機械学習で学習し、精度を向上させています。
産業機器の予知保全
工作機械の振動センサー・温度センサー・電流センサー・音響センサーを統合し、ベアリング異常・ツール摩耗・加工精度低下を早期に検知します。エッジAIでリアルタイム異常スコアを計算し、閾値超過時に警報を発します。
農業ドローン（マルチスペクトル）
可視光カメラ＋NIRカメラ＋熱赤外線カメラを搭載したドローンでマルチスペクトル画像を取得し、NDVI・NDRE・葉面温度をフュージョンして作物の生育状況・水ストレスを診断します。
実装・開発のポイント
時刻同期（タイムスタンプ管理）
センサーフュージョンで最も重要な基盤は時刻同期です。異なるセンサーのデータを正確な時刻でアライメントしないと、動く物体の位置誤差が大きくなります：
// ハードウェア同期: 共通タイムベースから全センサーにトリガーパルスを送る
// GNSSの1PPS信号を基準としてMCUタイマーをキャリブレーション
// 各センサーデータに高精度タイムスタンプを付与
// ソフトウェア時刻補間（後処理でのアライメント）
float interpolate_sensor_value(float t, float t0, float v0, float t1, float v1) {
return v0 + (v1 - v0) * (t - t0) / (t1 - t0);
}キャリブレーション（外部・内部）
カメラとLiDARを統合するには外部キャリブレーション（座標変換行列）が必要です：
# カメラ-LiDARキャリブレーション（チェッカーボードを使う方法）
# 1. チェッカーボードをカメラ画像とLiDAR点群で同時に計測
# 2. 対応点ペアから最適な剛体変換（R, t）を求める（ICP, SVDなど）
import cv2
import numpy as np
def calibrate_extrinsics(image_points, lidar_points, camera_matrix):
"""PnP法でLiDAR→カメラ外部パラメーター推定"""
ret, rvec, tvec = cv2.solvePnP(
lidar_points.astype(np.float32), # 3Dコーナー（LiDAR座標）
image_points.astype(np.float32), # 2Dコーナー（画像座標）
camera_matrix,
None # 歪み係数（0想定）
)
R, _ = cv2.Rodrigues(rvec)
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = tvec.ravel()
return T # 4×4変換行列ノイズモデルの適切な設定
カルマンフィルターの性能はプロセスノイズQ（モデル不確かさ）と観測ノイズR（センサーノイズ）の設定に大きく依存します：
# Q, R の設定指針
# Q が大きい: 状態モデルへの信頼度が低い → センサー観測を重視
# Q が小さい: 状態モデルを信頼 → カルマンゲインが小さく、センサー変化に鈍感
# R が大きい: センサーノイズが大きい → 状態モデル予測を重視
# R が小さい: センサーを信頼 → カルマンゲインが大きく、センサーに追従しやすい
Q = np.diag([0.01, 0.001]) # プロセスノイズ: 高度σ=0.1m, 速度σ=0.032m/s
R = np.diag([4.0, 0.25]) # 観測ノイズ: GPSσ=2m, 気圧高度σ=0.5m他技術との比較
フュージョン手法の選定指針
| 要件 | 推奨手法 | 理由 |
|---|---|---|
| MCUでリアルタイム（メモリ<10KB） | 相補フィルター、Mahonyフィルター | 計算量が極めて少ない |
| 組み込みLinux・高精度姿勢推定 | EKF（マドウィック比） | 非線形対応・中程度の計算量 |
| 非ガウス雑音・多峰性分布 | 粒子フィルター | 非ガウス分布をモデル化 |
| 学習データが豊富 | 深層学習（LSTM/Transformer） | エンドツーエンド・高精度 |
| 説明可能性が必要（安全系） | EKF | 数学的に解釈可能 |
| GNSS/IMU統合 | EKF/UKFベースTightly/Loosely Coupled | 実績ある手法 |
センサーフュージョンは単なるアルゴリズムの選択だけでなく、センサーの配置・キャリブレーション・時刻同期・ノイズモデリングが一体となって成果を決定します。各センサーの物理特性と限界を深く理解した上でフュージョン設計を行うことが、高精度・高信頼なシステム実現の鍵です。