多传感器融合

概述

多传感器融合（Multi-Sensor Fusion）是将来自不同传感器的数据进行综合处理，以获得比任何单一传感器更准确、更鲁棒的状态估计。它是机器人导航和自动驾驶系统的核心技术。

融合层级

graph TD
    A[传感器融合层级] --> B[数据级融合<br/>Data-level]
    A --> C[特征级融合<br/>Feature-level]
    A --> D[决策级融合<br/>Decision-level]

    B --> B1[直接融合原始数据<br/>如：点云+图像着色]
    C --> C1[融合提取的特征<br/>如：VIO 紧耦合]
    D --> D1[融合各传感器的独立结果<br/>如：多传感器投票]

层级	信息利用率	计算量	灵活性	示例
数据级	最高	最大	低	点云-图像融合
特征级	高	中	中	VIO、LIO
决策级	低	小	高	松耦合融合

扩展卡尔曼滤波（EKF）

标准 EKF

EKF 是最经典的非线性状态估计方法，通过一阶泰勒展开线性化非线性系统。

系统模型：

\[ \mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_k) + \mathbf{w}_k, \quad \mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_k) \]

\[ \mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k, \quad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_k) \]

预测步骤（Predict）：

\[ \hat{\mathbf{x}}_{k|k-1} = f(\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_k) \]

\[ \mathbf{P}_{k|k-1} = \mathbf{F}_k \mathbf{P}_{k-1|k-1} \mathbf{F}_k^T + \mathbf{Q}_k \]

其中 \(\mathbf{F}_k = \left.\frac{\partial f}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}_{k-1}}\) 为状态转移雅可比矩阵。

更新步骤（Update）：

\[ \mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^T \left(\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T + \mathbf{R}_k\right)^{-1} \]

\[ \hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k \left(\mathbf{z}_k - h(\hat{\mathbf{x}}_{k|k-1})\right) \]

\[ \mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1} \]

其中 \(\mathbf{H}_k = \left.\frac{\partial h}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}_{k|k-1}}\) 为观测雅可比矩阵。

EKF 融合流程

graph TD
    A["初始化<br/>x₀, P₀"] --> B{"接收数据"}
    B --> |"IMU (高频)"| C["预测步骤<br/>x̂ = f(x, u)<br/>P = FPFᵀ + Q"]
    B --> |"GPS/LiDAR/视觉 (低频)"| D["更新步骤<br/>K = PHᵀ(HPHᵀ+R)⁻¹<br/>x̂ = x̂ + K(z - h(x̂))<br/>P = (I-KH)P"]
    C --> B
    D --> B

    C --> E["输出状态估计"]
    D --> E

多传感器 EKF 更新

当有多个传感器时，可以顺序更新（Sequential Update）：

\[ \text{IMU predict} \to \text{GPS update} \to \text{LiDAR update} \to \text{Wheel odom update} \]

每个传感器的更新只影响其观测相关的状态分量。

无迹卡尔曼滤波（UKF）

动机

EKF 使用一阶线性化，对强非线性系统精度不足。UKF 使用 Sigma 点（无迹变换）更好地近似非线性变换后的分布。

Sigma 点生成

对于 \(n\) 维状态，生成 \(2n+1\) 个 Sigma 点：

\[ \boldsymbol{\chi}_0 = \hat{\mathbf{x}} \]

\[ \boldsymbol{\chi}_i = \hat{\mathbf{x}} + \sqrt{(n + \lambda) \mathbf{P}}_i, \quad i = 1, \ldots, n \]

\[ \boldsymbol{\chi}_{i+n} = \hat{\mathbf{x}} - \sqrt{(n + \lambda) \mathbf{P}}_i, \quad i = 1, \ldots, n \]

其中 \(\lambda = \alpha^2(n + \kappa) - n\) 为缩放参数。

UKF vs EKF

特性	EKF	UKF
线性化方式	雅可比矩阵（一阶）	Sigma 点（二阶）
精度	弱非线性好	强非线性也好
实现难度	需要推导雅可比	无需雅可比
计算量	较小	稍大（\(2n+1\) 次函数求值）
适用场景	大多数情况	强非线性系统

互补滤波器（Complementary Filter）

原理

互补滤波器是最简单的传感器融合方法，特别适合 IMU 姿态估计。它利用加速度计的低频准确性和陀螺仪的高频准确性：

\[ \hat{\theta}_k = \alpha \cdot (\hat{\theta}_{k-1} + \omega_k \Delta t) + (1 - \alpha) \cdot \theta_{\text{accel},k} \]

其中：

\(\alpha\) 为滤波系数（通常 0.95–0.99）
\(\omega_k\) 为陀螺仪角速度
\(\theta_{\text{accel},k}\) 为加速度计计算的倾角

频域理解

graph LR
    A[陀螺仪<br/>角速度] --> B[积分] --> C[高通滤波<br/>α]
    D[加速度计<br/>倾角] --> E[低通滤波<br/>1-α]
    C --> F[求和]
    E --> F
    F --> G[融合姿态角]

互补滤波器本质上是：

对陀螺仪数据做高通滤波（信任高频分量，消除漂移）
对加速度计数据做低通滤波（信任低频分量，消除振动噪声）

实现

class ComplementaryFilter:
    def __init__(self, alpha=0.98, dt=0.01):
        self.alpha = alpha
        self.dt = dt
        self.roll = 0.0
        self.pitch = 0.0

    def update(self, gyro, accel):
        """
        gyro: (gx, gy, gz) rad/s
        accel: (ax, ay, az) m/s^2
        """
        import math

        # 加速度计计算倾角
        accel_roll = math.atan2(accel[1], accel[2])
        accel_pitch = math.atan2(-accel[0], 
                       math.sqrt(accel[1]**2 + accel[2]**2))

        # 互补滤波
        self.roll = self.alpha * (self.roll + gyro[0] * self.dt) \
                    + (1 - self.alpha) * accel_roll
        self.pitch = self.alpha * (self.pitch + gyro[1] * self.dt) \
                     + (1 - self.alpha) * accel_pitch

        return self.roll, self.pitch

互补滤波 vs EKF

互补滤波器实现简单、计算量极小，适合嵌入式系统。但它不提供不确定性估计，且难以融合多个传感器。对于复杂系统，EKF/UKF 是更好的选择。

ROS2 robot_localization

robot_localization 是 ROS2 中最常用的传感器融合包，提供 EKF 和 UKF 实现。

基本架构

graph TD
    A[IMU<br/>/imu/data] --> E[ekf_node]
    B[轮式里程计<br/>/odom] --> E
    C[GPS<br/>/gps/fix] --> F[navsat_transform_node]
    F --> E
    D[视觉里程计<br/>/vo] --> E

    E --> G[/odometry/filtered<br/>融合后位姿]
    E --> H[/tf<br/>odom → base_link]

配置示例

# ekf.yaml
ekf_node:
  ros__parameters:
    frequency: 50.0
    sensor_timeout: 0.1
    two_d_mode: false

    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

    # IMU 配置
    imu0: /imu/data
    imu0_config: [false, false, false,  # x, y, z
                   true,  true,  true,   # roll, pitch, yaw
                   false, false, false,  # vx, vy, vz
                   true,  true,  true,   # vroll, vpitch, vyaw
                   true,  true,  true]   # ax, ay, az
    imu0_differential: false
    imu0_remove_gravitational_acceleration: true

    # 轮式里程计配置
    odom0: /odom
    odom0_config: [true,  true,  false,  # x, y, z
                    false, false, false,  # roll, pitch, yaw
                    false, false, false,  # vx, vy, vz
                    false, false, true,   # vroll, vpitch, vyaw
                    false, false, false]  # ax, ay, az

    # 过程噪声协方差
    process_noise_covariance: [0.05, 0.0, ...]

双 EKF 配置（带 GPS）

# 局部 EKF：odom 坐标系
ekf_local:
  ros__parameters:
    world_frame: odom
    odom0: /wheel_odom
    imu0: /imu/data

# 全局 EKF：map 坐标系
ekf_global:
  ros__parameters:
    world_frame: map
    odom0: /odometry/gps  # 来自 navsat_transform
    imu0: /imu/data

# GPS 坐标转换
navsat_transform:
  ros__parameters:
    magnetic_declination_radians: 0.0
    yaw_offset: 0.0
    broadcast_utm_transform: true

启动

ros2 launch robot_localization dual_ekf_navsat.launch.py

常见融合配置

室内移动机器人

传感器	提供信息	融合方式
轮式编码器	速度、里程	EKF 预测
IMU	角速度、加速度	EKF 预测 + 更新
2D LiDAR	位置（SLAM）	EKF 更新

室外自动驾驶

传感器	提供信息	融合方式
IMU	高频运动	EKF 预测
RTK-GPS	全局位置	EKF 更新
3D LiDAR	相对位姿、障碍物	EKF 更新
相机	语义、车道线	特征级融合
轮速计	速度	EKF 更新

无人机

传感器	提供信息	融合方式
IMU	高频姿态/加速度	EKF 预测
GPS	位置	EKF 更新
气压计	高度	EKF 更新
光流	速度（低空）	EKF 更新
磁力计	航向	EKF 更新

高级融合方法

因子图优化（Factor Graph）

因子图将融合问题建模为概率图模型，通过非线性优化求解：

\[ \mathbf{x}^* = \arg\min_{\mathbf{x}} \sum_i \| \mathbf{r}_i(\mathbf{x}) \|_{\Sigma_i}^2 \]

优势：可以利用全部历史信息、支持回环检测、更高精度。

常用工具：GTSAM、g2o、Ceres Solver。

粒子滤波

适合多模态分布和强非线性系统。常用于机器人全局定位（AMCL）。

深度学习融合

利用神经网络学习传感器融合策略：

PointPainting：将图像语义投影到点云
BEVFusion：在 BEV 空间融合相机和 LiDAR 特征
TransFusion：基于 Transformer 的多模态融合

调试与问题排查

现象	可能原因	排查方法
位姿发散	过程噪声/观测噪声设置不当	检查协方差矩阵
位姿跳变	传感器时间戳不同步	检查时间同步
原地旋转漂移	IMU 偏置未校准	静止校准陀螺偏置
GPS 跳变	多径效应/卫星切换	增大 GPS 观测噪声
高度漂移	气压计/GPS 高度不稳	约束高度为 0（地面机器人）

参考资料

《Probabilistic Robotics》 - Thrun, Burgard, Fox
《State Estimation for Robotics》 - Tim Barfoot
ROS2 robot_localization 文档：https://docs.ros.org/en/humble/p/robot_localization/
GTSAM 文档：https://gtsam.org
Sola, "Quaternion kinematics for the error-state Kalman filter", 2017