2. 融合定位基础

融合定位的本质，是在同一时刻用多个来源不同、误差特性各异的传感器，共同估计载体的位姿（位置、速度、姿态）。单一传感器往往存在固有短板：GNSS 在开阔环境精度高但易受遮挡和多路径影响，IMU 短时高频但会随时间漂移，LiDAR 与相机提供相对观测却依赖环境特征，轮速计在打滑时失真。融合定位正是通过互补与冗余，把这些"各有缺陷"的观测组合成更稳健、更连续的位姿估计，而故障检测与隔离（FDI）就建立在这一融合框架之上。

一、多传感器融合定位的核心原理

融合定位通常采用"预测—观测—更新"的递推结构。以惯性递推为骨架，IMU 提供高频的角速度和加速度，通过积分得到短时位姿预测；GNSS、LiDAR、相机、轮速计等提供低频或事件式的观测，用于修正预测误差。其数学核心是状态估计：定义状态向量 x = [p, v, q, b_a, b_g]，其中 p 为位置、v 为速度、q 为姿态四元数、b_a 与 b_g 分别为加速度计和陀螺仪零偏，再通过扩展卡尔曼滤波（EKF）或因子图优化进行递推与批量求解。

融合层次可分为三类：数据级融合直接合并原始测量，信息量最大但对时间同步和标定要求极高；特征级融合先提取特征再关联，兼顾信息量与鲁棒性；决策级融合对各传感器独立解算结果加权或投票，实现简单但信息损失较大。工程中 GNSS/IMU 组合导航多采用数据级或特征级，而多源容错架构常采用决策级作为兜底。

二、常见传感器类型及其特性

不同传感器在频率、精度、误差特性和环境依赖上差异显著，理解这些差异是设计融合与故障检测策略的前提。下表汇总了典型特性：

从故障检测视角看，GNSS 的突变型故障（如多路径跳变）与 IMU 的缓变型故障（零偏漂移）需要不同的检测策略：前者适合残差卡方检验，后者更适合长时间窗口的趋势监测。LiDAR 与相机则常因环境退化导致观测质量下降，需要通过特征数量、匹配内点率等指标进行健康评估。

三、坐标系与时间同步基础

融合定位中所有观测必须统一到同一坐标系和同一时间基准，否则融合结果会出现系统性偏差。常用坐标系包括：ECEF（地心地固）、ENU（东-北-天局部切平面）、机体坐标系（IMU 本体）、相机坐标系与LiDAR 坐标系。外参标定确定各传感器到机体坐标系的旋转与平移，内参标定确定相机焦距、畸变等。

时间同步分为硬同步与软同步。硬同步通过 PPS（秒脉冲）或触发信号实现硬件级对齐；软同步通过时间戳插值对齐。工程中常用做法是：以 IMU 为时间基准，对低频观测做线性插值或样条插值，再在滤波更新中使用对应时刻的状态。

# 时间同步示例：将 GNSS 观测对齐到 IMU 时间轴
gnss_t = 12.340  # GNSS 时间戳
imu_t  = [12.330, 12.340, 12.350]
imu_p  = [(x0,y0,z0), (x1,y1,z1), (x2,y2,z2)]

# 线性插值得到 gnss_t 时刻的 IMU 预测位置
alpha = (gnss_t - imu_t[0]) / (imu_t[1] - imu_t[0])
p_pred = lerp(imu_p[0], imu_p[1], alpha)

# 计算残差，用于后续卡方检验
residual = gnss_pos - p_pred

四、典型场景与常见误区复盘

以城市峡谷中的自动驾驶为例：GNSS 因高楼遮挡频繁失锁，IMU 递推维持短时连续位姿，LiDAR 与相机提供相对观测修正漂移，轮速计在低速时补充里程。此时若不做时间同步，GNSS 恢复瞬间的观测会与 IMU 预测严重错位，导致滤波器误判为故障而错误隔离。正确做法是：在 GNSS 失锁期间维持 IMU 递推并扩大协方差，恢复后先做一致性检验再逐步纳入观测。

常见误区包括：其一，忽视外参标定误差，把标定偏差误判为传感器故障；其二，时间戳来源不统一，混用传感器本地时间与系统时间；其三，过度信任单一传感器，未建立冗余校验机制；其四，故障检测阈值固定不变，未随环境与运动状态自适应调整。复盘时应重点核查标定链路、时间链路与残差统计特性，而不是只盯着融合输出曲线。

综上，融合定位的工程实现可以概括为一条链路：多源观测 → 时空对齐 → 状态估计 → 残差检验 → 故障隔离。理解各传感器的频率、精度与失效模式，掌握坐标系变换与时间同步方法，是后续开展故障检测与隔离设计的基础。只有在时空一致的前提下，残差才具有可比性，故障判据才具备可信度。