机器人依靠多种传感器“看”世界，就像人眼、耳朵和内耳前庭协同工作一样。激光雷达提供精确的距离，摄像头提供丰富的纹理，IMU感知自身的加速度和角速度。但每个传感器都有自己的坐标系、测量频率和噪声特性。传感器融合，就是将这些分散、不完美、有时甚至矛盾的“感官信息”，融合成一个关于机器人自身位置、姿态和速度的、更准确、更可靠的统一认知。
在项目中，我曾尝试仅用轮式里程计进行定位。在光滑地面上，机器人打滑了，里程计读数却显示它在前进，导致定位迅速漂移，机器人“以为”自己在一个不存在的位置上。这就是单一传感器的典型缺陷：它无法感知自身模型的失效。
每种传感器都有其固有的优缺点，将它们组合起来可以取长补短。下表对比了几种常见导航传感器的特性：
| 传感器 | 优点 | 缺点 | 典型坐标系 |
|---|---|---|---|
| 轮式里程计 | 高频、相对位姿平滑、无外部依赖 | 累积误差、受打滑/颠簸影响大 | 机器人本体坐标系 (odom) |
| 惯性测量单元 (IMU) | 高频、测量本体角速度和加速度 | 加速度积分产生巨大漂移、对振动敏感 | IMU传感器坐标系 (imu_link) |
| 激光雷达 (LiDAR) | 测距精度高、不受光照影响 | 成本高、在特征稀疏环境（长走廊）易失效 | 激光传感器坐标系 (laser_link) |
| 视觉里程计 (VO)/摄像头 | 信息丰富、可识别回环 | 受光照、纹理、动态物体影响大、计算量大 | 相机光学坐标系 (camera_optical_frame) |
| 全球定位系统 (GPS) | 全局、无累积误差 | 室内/桥下无信号、更新频率低、精度有限（民用） | 世界坐标系 (如 WGS-84) |
融合的目标很明确：用高频的里程计/IMU提供平滑的短期运动估计，用绝对观测（激光、视觉、GPS）来周期性地校正累积误差，从而得到一个既全局一致又局部平滑的机器人状态估计。
传感器融合算法大多遵循“预测-校正”的框架。算法内部维护一个关于机器人状态（位置、姿态、速度等）的估计。在每一个周期，它首先根据运动模型（如里程计数据）预测状态会如何变化；然后，当其他传感器（如激光匹配到地图）提供观测数据时，算法将预测值与观测值进行比较，并按照某种最优准则校正自己的状态估计。卡尔曼滤波器及其变种是这一思想的经典实现。
根据原始数据参与融合的层次，融合架构主要分为松耦合和紧耦合。这类似于做菜：松耦合是把已经炒好的几个菜（各自处理完的数据）拼在一个盘子里；紧耦合是把所有生鲜食材（原始数据）一起下锅烹饪。
对于大多数工业移动机器人项目，从松耦合开始是更务实的选择。它让你可以快速验证系统，并逐个模块地排查问题。我在一个AGV项目中就采用了松耦合：将激光SLAM输出的位姿和轮式里程计的数据送入一个扩展卡尔曼滤波器，效果和稳定性都远超单一传感器。
卡尔曼滤波是实现“预测-校正”框架的最著名算法。你可以把它想象成一个有经验的导航员。他手里有一张地图（系统模型），知道机器人大概怎么运动（预测）。同时，他不断收到各种瞭望塔（传感器）发来的、带有误差的位置报告（观测）。这位导航员会根据自己对模型和传感器可靠度的信任程度（协方差），聪明地将预测和观测结合起来，得出一个最优的位置估计。
卡尔曼滤波针对的是线性高斯系统。但机器人的运动模型和观测模型往往是非线性的（比如涉及旋转）。这时就需要扩展卡尔曼滤波。EKF的核心思想是在当前估计点附近，对非线性模型进行一阶泰勒展开，将其线性化，然后再应用标准卡尔曼滤波的公式。
盲人摸象的故事里，每个盲人只摸到一部分（单一传感器），得出错误结论。如果让他们一起讨论，并知道每个人摸的位置和通常的触感可靠性（传感器模型和噪声协方差），他们就能通过一个“融合算法”更准确地推断出大象的真实形状。EKF就是这样一个让“局部感知”达成“全局共识”的数学化讨论规则。
技术定义：扩展卡尔曼滤波（EKF）是一种用于非线性系统的状态估计方法。它通过在当前状态估计处线性化系统的运动方程和观测方程，将非线性问题近似为线性问题，从而递归地融合预测和观测信息，得到系统状态的最优估计。
下面是一个高度简化的EKF更新步骤的概念性伪代码，展示了从预测到更新的流程：
// 假设状态x包含位置(px, py)和朝向(theta)
struct State {
double px, py, theta;
};
// 预测步骤 (基于里程计数据odom)
State predict(const State& x_old, const Odometry& odom) {
State x_pred;
x_pred.px = x_old.px + odom.distance * cos(x_old.theta);
x_pred.py = x_old.py + odom.distance * sin(x_old.theta);
x_pred.theta = x_old.theta + odom.yaw;
// 同时需要更新状态协方差矩阵P: P_pred = F * P_old * F^T + Q
// 其中F是预测模型的雅可比矩阵，Q是过程噪声协方差
return x_pred;
}
// 更新步骤 (基于激光匹配得到的观测z)
State update(const State& x_pred, const Observation& z) {
// 计算观测的预测值 (例如，预测激光在全局地图中的端点)
Observation z_pred = observationModel(x_pred);
// 计算观测残差 (新息)
Vector y = z - z_pred;
// 计算卡尔曼增益K (核心公式，权衡预测与观测)
// K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)
// 其中H是观测模型的雅可比矩阵，R是观测噪声协方差
Matrix K = computeKalmanGain(...);
// 状态校正
State x_new = x_pred + K * y;
// 更新协方差: P_new = (I - K*H) * P_pred
return x_new;
}在将数据送入滤波器之前，必须确保它们是在描述同一时刻、同一物理点的状态。这涉及到两个关键预处理：时间戳同步和坐标变换。
时间戳同步：不同传感器的数据到达融合节点的时间不同。简单取最新数据配对会导致严重的“时间扭曲”。例如，用10Hz的激光位姿去校正100Hz的IMU预测时，必须将激光位姿通过运动模型“前向预测”到最新的IMU时间戳上，或者将高频数据“插值”到低频观测的时刻。ROS中的 `message_filters` 模块就是用来做近似时间同步的。
坐标变换统一：这是本章“坐标统一”的核心。每个传感器的观测都位于其自身的传感器坐标系中。在融合前，必须通过已知的静态变换（由标定得到）将它们全部转换到一个共同的参考坐标系下，通常是机器人本体坐标系（`base_link`）。
编者提示：注意TF树的更新时机
在ROS中，利用TF2库进行坐标变换时，一个常见的坑是查询一个“未来”或“过去”的变换。例如，你在处理时间戳为t的激光数据时，需要查询`laser_link`到`base_link`在时间t的变换。如果TF广播器还在广播t时刻之前的变换，或者你请求了一个尚未广播的t时刻之后的变换，查询可能会失败或返回错误数据。务必确保你的数据时间戳在TF数据的时间范围内，对于高频融合，使用`tf2::doTransform`并传入精确的时间戳是关键。
整个传感器融合与坐标统一的数据流，可以概括为以下架构：
最终，融合模块的输出是一个稳定的、融合了多源信息的 `odom` 坐标系到 `base_link` 坐标系的变换。这个变换被导航栈中的路径规划器和控制器所使用，是机器人做出正确移动决策的基础。
如果你在使用ROS，可以尝试用 `robot_localization` 包进行一个简单的松耦合实验。