多传感器融合定位Localization这个方向看起来是算法问题真正落地时往往卡在工程细节上。很多人在卫星、惯性测量单元、相机、激光雷达都齐全之后依然做不出稳定不跳的定位结果问题通常不是某个传感器不行而是时间同步、外参标定、状态估计框架和故障语义这几层没有搭好。这篇文章适合正在做机器人、智能驾驶、无人机或者便携设备定位的工程师也适合刚开始学 SLAM 与组合导航的人。我先说一个核心判断多传感器融合定位的价值不是简单地把多个定位结果取平均而是利用不同传感器的互补特性在某一类传感器退化的场景下仍然能输出连续、平滑、全局一致的位置和姿态。最值得花时间研究的不是某个滤波公式而是“状态怎么定义、观测怎么进、异常怎么剔除、协方差怎么维护”这一整套流程。下面按我从单传感器到多传感器融合的落地顺序拆开讲。1. 为什么说多传感器融合定位的关键不是“多”而是“互补”1.1 单一传感器各自有短板先看最常见的几类定位传感器。GNSS 的优点是全局无漂移长期误差有界。缺点是更新频率低室内收不到信号城市峡谷容易多径反射高架桥下容易丢星。单点定位的误差经常在米级即使加了 RTK遮挡和跳变也会让定位结果突然偏出去几米。IMU 的优点是频率高短期相对位置和姿态平滑不受外界信号干扰。缺点非常明显加速度计和陀螺仪都有偏置积分之后位置误差会随时间快速增长。严格来说IMU 不适合单独做长时间定位。相机在纹理丰富的场景里可以提供稳定的视觉里程计但光照变化、动态物体、玻璃墙面、重复纹理都会让特征匹配出问题。单目相机还存在尺度不可观的问题需要和其他传感器配合才能拿到真正的米制轨迹。LiDAR 在结构化环境中能给出很直观的点云匹配结果但空旷平直场景下沿前进方向的约束很弱容易退化。雪天、灰尘、镜面反射也会让点云质量明显下降。这些短板相互独立所以融合的意义不是“多加一个保险”而是让传感器之间互相补足。GNSS 负责抑制长期漂移IMU 负责短期平滑和姿态跟踪视觉和 LiDAR 负责提供局部几何约束与回环修正。1.2 融合定位真正要解决的问题真正要解决的问题可以拆成四层状态层你估计的是位置、速度、姿态还是还要估计 IMU 偏置、GNSS 接收机钟差、相机与 LiDAR 的外参数据层各传感器的时间戳对齐到什么精度坐标系是否统一哪些观测是离群值估计层使用滤波还是优化如何处理非线性误差观测更新频率不一致怎么办故障层某个传感器突然丢数据或给出错误数据时融合结果应该退化还是拒绝该观测量判断一套融合定位系统是否合格最简单的三条标准是长时间跑不漂、动态场景不跳、传感器丢失时不崩。如果只看单帧精度不连续看轨迹和速度曲线很容易被局部结果误导。我建议在动手写代码之前先用测量数据画一张各传感器轨迹对比图。把 GNSS 轨迹、IMU 积分轨迹、视觉里程计轨迹画在同一张图里观察各自的漂移方向和跳变频率。这会直接影响后续融合策略的选择比直接套卡尔曼滤波公式更有效。注意很多定位异常不是公式推错而是某个传感器输入已经明显异常却没有被识别出来。2. 先搭好融合框架从松耦合到紧耦合再从滤波到因子图2.1 松耦合、紧耦合和深耦合怎么选多传感器融合定位的实现方式通常分成三类。松耦合是指每个传感器先独立输出位姿或速度再把这些结果放进滤波器或优化器里融合。优点是实现简单、模块间解耦缺点是信息损失较大。例如视觉里程计已经用内部优化压缩成一个位姿融合时不再有机会使用原始图像特征。紧耦合是指把原始观测直接放进同一个估计问题中。相机特征点、LiDAR 点云、GNSS 伪距与载波相位、IMU 预积分残差都作为观测项参与联合估计。优点是精度上限高缺点是计算复杂、调参难任何一个子模块出错都不好定位。深耦合大多出现在 GNSS/INS 组合里指 GNSS 接收机内部的跟踪环与惯性导航互相辅助载波环和码环可以利用 IMU 的预测信息在信号遮挡或高动态场景下提升跟踪能力。对多数做机器人定位的团队来说第一步不太需要直接上深耦合松耦合和紧耦合已经能覆盖大部分场景。从工程经验看可以先从松耦合起步跑通数据和系统后再逐步把弱约束模块替换成紧耦合。不要一上来就同时紧耦合视觉、LiDAR 和 GNSS那样一旦效果不好根本分不清是哪个残差项出了问题。2.2 卡尔曼滤波与因子图的取舍滤波和因子图是目前最常用的两种状态估计框架。卡尔曼滤波适合在线实时处理状态量不大算力要求低。实际工程里很少直接用标准卡尔曼滤波处理非线性系统更多使用扩展卡尔曼滤波、误差状态卡尔曼滤波、无损卡尔曼滤波。误差状态卡尔曼滤波在 IMU 融合场景中很常用因为误差状态比全状态更容易实现线性化姿态更新也更稳定。因子图优化适合多传感器、多时刻、多约束的场景。核心思想是把每个状态节点和每个观测残差画成图然后通过迭代优化求得所有状态的最优估计。因子图的优势是容易加入回环约束、容易增加新的传感器因子、方便做历史状态重估计。代价是计算复杂度比滤波高而且需要管理优化窗口和边缘化策略。我常用的选择方法是如果主要传感器是 GNSS 和 IMU实时性要求高状态量较小先用误差状态卡尔曼滤波。如果加入了视觉或 LiDAR并且需要回环检测、多帧优化优先考虑因子图。如果系统里已经有实时滤波结果同时还有离线地图优化需求可以做成滤波和因子图并存前端滤波维持实时输出后端因子图定期修正长期漂移。2.3 新手建议的最短可行链路第一次做多传感器融合定位时不要追求一步到位。我的建议是拆成三条验证路径。第一路径GNSS IMU 的松耦合融合。先用 RTK/千寻或普通 GNSS 提供绝对位置IMU 提供高频位姿增量观察位置曲线是否比单 GNSS 更平滑。这一关过了说明坐标系、时间戳、状态定义基本正确。第二路径在第一条路径上加入视觉或 LiDAR 里程计。视觉和 LiDAR 提供短时间内的相对位姿约束融合后应该有两个明显变化定位频率提高局部轨迹更平滑。第三路径加入回环检测或地图匹配。当机器人重复经过同一地点时回环约束会把累积漂移拉回来。这一步是长期稳定性的关键。每条路径都需要准备一份小数据集数据长度控制在 5 到 10 分钟先跑通再扩大。不要直接拿几百 GB 的自动驾驶路测数据做验证。3. IMU GNSS 融合实操误差状态卡尔曼滤波的落地细节3.1 为什么优先用误差状态卡尔曼滤波GNSS 与 IMU 的组合导航是多传感器定位里最经典、也最适合先落地的方案。IMU 输出高频加速度和角速度GNSS 输出低频率但绝对准确的位置和速度。两者天然互补。如果直接用全状态扩展卡尔曼滤波处理姿态旋转矩阵或四元数在更新时容易遇到归一化问题而且线性化误差较大。更稳的做法是用误差状态卡尔曼滤波将状态拆成标称状态和误差状态标称状态用 IMU 机械编排更新误差状态用卡尔曼滤波估计然后反馈修正。误差状态的好处是误差量通常很小旋转误差可以近似为三维小角线性化假设更可靠。同时 IMU 偏置也可以作为误差状态的一部分估计出来这对长时间定位非常关键。我建议状态至少包含位置3 维速度3 维姿态误差3 维陀螺仪偏置3 维加速度计偏置3 维如果需要和 GNSS 接收机本身联合建模还可以加入接收机钟差和钟漂状态。不过初学者先不加避免状态维度过大导致可观测性变差。3.2 参数、噪声矩阵和初始对齐落地时最容易出错的不是公式而是参数和坐标系。坐标系方面最常用的是以某个参考点为原点的 ENU 局部坐标系或者 WGS84 下的 ECEF 坐标系。如果场景范围不大ENU 更直观如果做跨城市或全球应用ECEF 更合适。关键是 IMU 加速度计输出要转换到导航坐标系GNSS 位置也要转换到同一坐标系。时间戳方面GNSS 输出一般自带 UTC 或 GPS 时间IMU 是接收时刻的本地时间。两者需要统一到同一时间轴最好在数据采集时就用硬件同步或统一打戳。如果时间戳不对齐融合结果必然高频震荡。噪声矩阵方面需要设置过程噪声和测量噪声。常见做法是查看 IMU 手册中的噪声密度和随机游走参数并把 GNSS 的位置、速度噪声根据实际定位模式设置。RTK 固定解可以给到厘米到分米级单点定位要放到米级浮点解则介于两者之间。这个差异如果不区分融合结果会被错误的高权重信号带偏。初始对齐方面静止状态下先采集几十秒 IMU 数据计算初始三轴角度和陀螺仪偏置粗值。GNSS 也要在定位精度较好时记录初始位置。不要边运动边初始化除非系统支持动态对准否则收敛时间会很长。3.3 结果判断和常见坑怎么判断融合结果是否正常第一看位置轨迹是否连续。GNSS 偶尔跳变时融合位置应当被 IMU 平滑掉一部分而不是完全跟随跳变。第二看速度曲线是否合理。正常行驶或机器人移动时速度曲线不应出现高频大幅抖动如果出现多半是时间戳错位或噪声矩阵过小。第三看姿态是否稳定。静态和直线运动时俯仰和横滚应稳定在重力方向附近航向角虽然会漂移但不应出现毫无规律的瞬跳。第四看协方差是否发散。如果滤波协方差一直涨到非常大说明观测模型或过程模型有一个不对常见原因是单位不统一、坐标系反了、观测噪声设置过小。常见坑包括IMU 数据的加速度和角速度没有经过重力补偿判断初始姿态错误导致位置发散。GNSS 坐标写错单位例如把经纬度直接当米使用。时间戳时间轴不一致导致 GNSS 观测与 IMU 状态错位一个周期。更新顺序错乱先做 GNSS 更新又用同一时刻 IMU 数据做预测造成信息重复使用。静止时出现速度漂移但位置还能稳定因为偏置估计还没收敛。如果出现这些现象不要急着调大过程噪声“强行压平”先依次检查时间同步、坐标系、单位、初始姿态和观测噪声。注意噪声矩阵不是随便调的。小样本先跑 5 分钟对比融合轨迹和真实轨迹再根据误差曲线调整。4. 加入视觉和 LiDAR前端里程计、回环检测与因子图优化4.1 视觉和 LiDAR 在定位中扮演什么角色GNSS 覆盖很好的地方GNSS IMU 基本够用。但室内、地下车库、高架桥、林荫道这些场景里GNSS 不可用或者质量差就需要视觉和 LiDAR 提供环境约束。视觉和 LiDAR 在定位里主要做两件事。第一件事是里程计估计相邻时刻之间的相对位姿。视觉里程计通过特征点、光流或直接法估计相机运动LiDAR 里程计通过点云配准估计激光雷达运动。里程计提供的是局部一致性频率可以到 10Hz 到 30Hz但会随距离累积漂移。第二件事是地图匹配与回环检测。当系统进入一个之前访问过的区域时可以通过图像特征或点云特征识别出“我来过这里”然后添加一个回环约束把当前位姿和历史位姿对齐从而消除累计漂移。在融合框架里GNSS 是全局约束IMU 是高频约束视觉和 LiDAR 是几何约束。不同约束来源不同权重也应该随着环境和质量动态变化。4.2 前端里程计和后端优化怎么配合工程上比较成熟的路线是“前端里程计 后端因子图优化”。前端负责实时输出相对运动和必要的观测信息。视觉前端输出两帧之间的相对位姿以及该约束的不确定性LiDAR 前端输出帧间匹配的相对位姿和匹配质量。只输出位姿不输出内部的点云、特征和原始测量在融合层看起来就是一个相对位姿因子。后端负责把这些相对约束和绝对约束一起优化。IMU 预积分可以作为帧间的惯性因子GNSS 位置可以作为全局位置先验视觉和 LiDAR 相对位姿作为相邻帧因子回环检测作为长距离约束。关键点在于不是所有前端输出都能直接当成高精度约束。视觉里程计在纹理弱、动态物体多的情况下误差会变大LiDAR 里程计在走廊、隧道等退化环境中会出现沿某个方向的大漂移。因此后端必须能判断每个约束的质量。一种简单做法是给每个前端约束设置一个置信度或鲁棒核。例如视觉约束在特征点数量少或重投影误差大时降权LiDAR 约束在退化方向检测到约束不足时降权。4.3 批量场景下的回环和地图一致性处理只做前端和后端优化长距离定位依然可能漂移。回环检测能解决“位置回到原点后轨迹是否闭合”的问题但处理不好也会引入错误回环。判断回环质量时我会优先看三样东西描述子匹配得分图像 BoW 模型得分是否显著高于阈值。几何校验结果通过 PnP 或 ICP 计算相对位姿时的内点比例。时空一致性当前位姿和候选回环历史位姿是否在合理范围内。如果三个条件都满足再让回环因子进入优化器如果有一个明显不满足这个回环大概率是误检。地图一致性常见的坑包括不同时刻采集的点云没有对齐到统一地图坐标系视觉地图和 LiDAR 地图分别构建然后互相错位动态物体被当作静态环境加入地图。这些问题在单传感器测试中不明显多传感器融合后会变成长期漂移和跳变的来源。解决思路是不要只维护一个位姿输出还要维护一个可更新的局部地图或关键帧数据库。每次优化后把关键帧位姿、地图点、因子残差一起更新保证后续匹配基于一致的地图。另外如果数据足够长建议把后端从固定滑窗扩展成带边缘化的滑动窗口或者使用增量式因子图求解器。这样可以兼顾实时性和全局一致性。5. 时间同步与传感器标定比算法更影响融合效果5.1 时间同步的四种常见做法时间同步是融合定位里最容易被低估的环节。传感器之间的时间差达到几十毫秒在高速行驶或剧烈运动时就会产生明显的位置误差。常见的同步做法有四类。第一类硬件同步。GNSS 接收机输出 PPS 秒脉冲作为各传感器的统一时间基准同时通过串口输出 NMEA 协议中的 UTC 时间。视觉、LiDAR、IMU 在收到 PPS 时打上硬件时间戳。这种方式精度最高可以达到微秒到毫秒级。第二类统一主时钟。在机器人或车辆上使用同一个时间同步板卡为所有传感器提供同步触发信号。相机曝光时刻、LiDAR 旋转方位角、IMU 采样时刻都能对齐到同一时间轴。第三类软件同步。各传感器自带时间戳在融合前用插值或查找最近邻将观测对齐到目标时间轴。例如 IMU 是 200Hz相机是 30Hz可以把相机的曝光时刻作为目标时间对 IMU 数据做插值。第四类离线后处理同步。预先采集数据保存每帧原始时间戳在算法入口处统一转换。适合算法验证和数据集回放不适合实时系统。不管用哪种方式第一步都是先看各传感器的延迟分布。把同一运动时刻下的加速度计峰值和相机帧时间戳画出来可以看到大致延迟。若能曝光时间已知最好把相机曝光中点作为对齐点。5.2 外参标定和 IMU 内参偏置时间同步解决“什么时候”外参标定解决“在哪朝哪里”。融合定位中至少需要确认几组外参IMU 坐标系到 GNSS 天线相位中心的杆臂。IMU 坐标系到相机光心的旋转和平移。IMU 坐标系到 LiDAR 中心的旋转和平移。相机和 LiDAR 之间的相对外参如果它们都参与定位。外参不准会造成严重的投影和匹配误差。尤其是 IMU 与 GNSS 天线之间的杆臂在车辆转弯时会引入一条虚拟的速度误差导致位置轨迹出现向外扩张的弯曲。IMU 内参包括加速度计尺度、惯性轴不正交角、陀螺仪尺度、零偏重复性与温度漂移。内参虽然没有外参那么显眼但长时间运行后会直接影响累积误差。建议用 Allan 方差分析陀螺仪和加速度计的随机游走特性并在温箱或静止数据上做零偏稳定性评估。标定方法可以是基于标定板的视觉标定、手眼标定、基于自然场景的联合优化。实际工程中可以先用简单的旋转激励和位姿残差做粗标定再用融合优化做精标定。5.3 验证标定结果的方法标定完不能只看重投影误差还要看融合后的闭环效果。我通常用以下方法验证。方法一把激光点云投影到图像上。如果相机与 LiDAR 外参正确物体边缘应该对齐道路标线边缘不应出现明显错位。方法二做原地和“8”字运动。用更新后的外参融合数据观察 GNSS 天线速度和 IMU 速度的一致性。如果杆臂或外参不对转弯时速度残差会出现周期波动。方法三反复通过同一路段。对比两次经过时融合轨迹与地图的重合度。重合度越高说明外参和回环约束越可靠。方法四检查优化后的残差分布。将各类观测残差按时间和传感器类型统计如果某类残差始终偏大优先怀疑标定或时间同步而不是滤波增益。6. 落地排查清单从“能跑”到“跑得稳”6.1 按现象分类的排查顺序多传感器定位系统出问题时先看现象再开单顺序很重要。现象一启动后位置直接飞掉。优先检查初始位置、初始姿态、坐标系和单位。IMU 的 z 轴是否向上加速度是否补了重力GNSS 经纬度是否转换正确。现象二运行时位置抖动厉害。优先检查时间同步、观测噪声和更新频率。如果 GNSS 和 IMU 的时间戳错位抖动通常会在低通滤波下也压不下来。现象三轨迹整体漂移越来越远。优先检查 IMU 偏置是否收敛、GNSS 约束是否被降权、视觉或 LiDAR 里程计是否退化。正常时绝对约束应该能把长期漂移锁住。现象四运行一段时间后协方差发散。优先检查状态可观测性。比如只有 GNSS 位置更新没有速度观测速度状态可能出现不可观只有 IMU 而没有绝对角度观测时航向角可能不可观。现象五某个传感器短时间丢失后再恢复定位跳变。优先检查故障识别逻辑是否有迟滞以及恢复时是否需要重新初始化观测。直接让一个刚恢复的传感器以很大权重进入滤波器通常会造成大幅跳变。排查时不要同时改多个参数。一次只改一个变量观察输出变化。用同一段数据反复回放才能对比出真实差异。6.2 资源占用、日志和结果保存融合定位的实时性不能只看单帧耗时。还要看滤波器或优化器在最大观测数量下的耗时上限。不同传感器话题或数据通道是否存在队列积压。数据回放时能否保证时间戳顺序不被打乱。日志写入磁盘是否会影响实时循环尤其在长时间路测时。我一般会把每次测试的原始数据、外参、噪声参数、版本号、融合结果和残差曲线一起保存。这样同一个数据包可以反复验证不同参数下的差异也会让后期排查问题少很多弯路。输出保存格式建议使用二进制或高性能序列化格式不要直接落成千上万行的文本。每条轨迹记录可以包含时间戳、位置、速度、姿态、协方差对角线和每个传感器状态标志。带有状态标志的好处是当定位跳变时能快速判断是哪个传感器先异常。6.3 几个容易误判的经验最后说几点我踩过很多次的经验。低配置环境能跑通联合优化不代表能实时跑。优化器在离线数据集上收敛很快但在线运行时每次插入新因子都要控制窗口大小。如果发现实时性不够优先做边缘化或降低优化频率而不是阉割传感器数量。“支持多传感器”不等于“任意组合都稳定”。GNSS IMU 融合和 GNSS IMU LiDAR 融合是完全不同的调参难度。新增一个传感器需要重新评估它的坐标、时间同步、外参、故障模式不要默认系统会自适应。报错不一定是算法问题。很多“融合发散”来自外参在采集时发生过改动、地面坐标系和车辆坐标系定义不一致、数据包中包含重复时间戳。先清理数据再调参数。还要注意同一类型的传感器也有退化叠加风险。比如视觉和 LiDAR 在逆光或烟尘中同时退化GNSS 在隧道里失效此时如果没有可靠的 IMU 和里程计约束融合结果会退化到纯惯导。给系统设计一个清晰的退化路径明显优于硬撑着输出不可信定位。真正跑稳多传感器融合定位我见过的最短路径就是先用小数据集验证每条传感器通道再逐层加入融合约束最后用长时间和复杂场景数据去压测。这个顺序看起来慢但排错时间反而最少。
多传感器融合定位Localization这个方向看起来是算法问题真正落地时往往卡在工程细节上。很多人在卫星、惯性测量单元、相机、激光雷达都齐全之后依然做不出稳定不跳的定位结果问题通常不是某个传感器不行而是时间同步、外参标定、状态估计框架和故障语义这几层没有搭好。这篇文章适合正在做机器人、智能驾驶、无人机或者便携设备定位的工程师也适合刚开始学 SLAM 与组合导航的人。我先说一个核心判断多传感器融合定位的价值不是简单地把多个定位结果取平均而是利用不同传感器的互补特性在某一类传感器退化的场景下仍然能输出连续、平滑、全局一致的位置和姿态。最值得花时间研究的不是某个滤波公式而是“状态怎么定义、观测怎么进、异常怎么剔除、协方差怎么维护”这一整套流程。下面按我从单传感器到多传感器融合的落地顺序拆开讲。1. 为什么说多传感器融合定位的关键不是“多”而是“互补”1.1 单一传感器各自有短板先看最常见的几类定位传感器。GNSS 的优点是全局无漂移长期误差有界。缺点是更新频率低室内收不到信号城市峡谷容易多径反射高架桥下容易丢星。单点定位的误差经常在米级即使加了 RTK遮挡和跳变也会让定位结果突然偏出去几米。IMU 的优点是频率高短期相对位置和姿态平滑不受外界信号干扰。缺点非常明显加速度计和陀螺仪都有偏置积分之后位置误差会随时间快速增长。严格来说IMU 不适合单独做长时间定位。相机在纹理丰富的场景里可以提供稳定的视觉里程计但光照变化、动态物体、玻璃墙面、重复纹理都会让特征匹配出问题。单目相机还存在尺度不可观的问题需要和其他传感器配合才能拿到真正的米制轨迹。LiDAR 在结构化环境中能给出很直观的点云匹配结果但空旷平直场景下沿前进方向的约束很弱容易退化。雪天、灰尘、镜面反射也会让点云质量明显下降。这些短板相互独立所以融合的意义不是“多加一个保险”而是让传感器之间互相补足。GNSS 负责抑制长期漂移IMU 负责短期平滑和姿态跟踪视觉和 LiDAR 负责提供局部几何约束与回环修正。1.2 融合定位真正要解决的问题真正要解决的问题可以拆成四层状态层你估计的是位置、速度、姿态还是还要估计 IMU 偏置、GNSS 接收机钟差、相机与 LiDAR 的外参数据层各传感器的时间戳对齐到什么精度坐标系是否统一哪些观测是离群值估计层使用滤波还是优化如何处理非线性误差观测更新频率不一致怎么办故障层某个传感器突然丢数据或给出错误数据时融合结果应该退化还是拒绝该观测量判断一套融合定位系统是否合格最简单的三条标准是长时间跑不漂、动态场景不跳、传感器丢失时不崩。如果只看单帧精度不连续看轨迹和速度曲线很容易被局部结果误导。我建议在动手写代码之前先用测量数据画一张各传感器轨迹对比图。把 GNSS 轨迹、IMU 积分轨迹、视觉里程计轨迹画在同一张图里观察各自的漂移方向和跳变频率。这会直接影响后续融合策略的选择比直接套卡尔曼滤波公式更有效。注意很多定位异常不是公式推错而是某个传感器输入已经明显异常却没有被识别出来。2. 先搭好融合框架从松耦合到紧耦合再从滤波到因子图2.1 松耦合、紧耦合和深耦合怎么选多传感器融合定位的实现方式通常分成三类。松耦合是指每个传感器先独立输出位姿或速度再把这些结果放进滤波器或优化器里融合。优点是实现简单、模块间解耦缺点是信息损失较大。例如视觉里程计已经用内部优化压缩成一个位姿融合时不再有机会使用原始图像特征。紧耦合是指把原始观测直接放进同一个估计问题中。相机特征点、LiDAR 点云、GNSS 伪距与载波相位、IMU 预积分残差都作为观测项参与联合估计。优点是精度上限高缺点是计算复杂、调参难任何一个子模块出错都不好定位。深耦合大多出现在 GNSS/INS 组合里指 GNSS 接收机内部的跟踪环与惯性导航互相辅助载波环和码环可以利用 IMU 的预测信息在信号遮挡或高动态场景下提升跟踪能力。对多数做机器人定位的团队来说第一步不太需要直接上深耦合松耦合和紧耦合已经能覆盖大部分场景。从工程经验看可以先从松耦合起步跑通数据和系统后再逐步把弱约束模块替换成紧耦合。不要一上来就同时紧耦合视觉、LiDAR 和 GNSS那样一旦效果不好根本分不清是哪个残差项出了问题。2.2 卡尔曼滤波与因子图的取舍滤波和因子图是目前最常用的两种状态估计框架。卡尔曼滤波适合在线实时处理状态量不大算力要求低。实际工程里很少直接用标准卡尔曼滤波处理非线性系统更多使用扩展卡尔曼滤波、误差状态卡尔曼滤波、无损卡尔曼滤波。误差状态卡尔曼滤波在 IMU 融合场景中很常用因为误差状态比全状态更容易实现线性化姿态更新也更稳定。因子图优化适合多传感器、多时刻、多约束的场景。核心思想是把每个状态节点和每个观测残差画成图然后通过迭代优化求得所有状态的最优估计。因子图的优势是容易加入回环约束、容易增加新的传感器因子、方便做历史状态重估计。代价是计算复杂度比滤波高而且需要管理优化窗口和边缘化策略。我常用的选择方法是如果主要传感器是 GNSS 和 IMU实时性要求高状态量较小先用误差状态卡尔曼滤波。如果加入了视觉或 LiDAR并且需要回环检测、多帧优化优先考虑因子图。如果系统里已经有实时滤波结果同时还有离线地图优化需求可以做成滤波和因子图并存前端滤波维持实时输出后端因子图定期修正长期漂移。2.3 新手建议的最短可行链路第一次做多传感器融合定位时不要追求一步到位。我的建议是拆成三条验证路径。第一路径GNSS IMU 的松耦合融合。先用 RTK/千寻或普通 GNSS 提供绝对位置IMU 提供高频位姿增量观察位置曲线是否比单 GNSS 更平滑。这一关过了说明坐标系、时间戳、状态定义基本正确。第二路径在第一条路径上加入视觉或 LiDAR 里程计。视觉和 LiDAR 提供短时间内的相对位姿约束融合后应该有两个明显变化定位频率提高局部轨迹更平滑。第三路径加入回环检测或地图匹配。当机器人重复经过同一地点时回环约束会把累积漂移拉回来。这一步是长期稳定性的关键。每条路径都需要准备一份小数据集数据长度控制在 5 到 10 分钟先跑通再扩大。不要直接拿几百 GB 的自动驾驶路测数据做验证。3. IMU GNSS 融合实操误差状态卡尔曼滤波的落地细节3.1 为什么优先用误差状态卡尔曼滤波GNSS 与 IMU 的组合导航是多传感器定位里最经典、也最适合先落地的方案。IMU 输出高频加速度和角速度GNSS 输出低频率但绝对准确的位置和速度。两者天然互补。如果直接用全状态扩展卡尔曼滤波处理姿态旋转矩阵或四元数在更新时容易遇到归一化问题而且线性化误差较大。更稳的做法是用误差状态卡尔曼滤波将状态拆成标称状态和误差状态标称状态用 IMU 机械编排更新误差状态用卡尔曼滤波估计然后反馈修正。误差状态的好处是误差量通常很小旋转误差可以近似为三维小角线性化假设更可靠。同时 IMU 偏置也可以作为误差状态的一部分估计出来这对长时间定位非常关键。我建议状态至少包含位置3 维速度3 维姿态误差3 维陀螺仪偏置3 维加速度计偏置3 维如果需要和 GNSS 接收机本身联合建模还可以加入接收机钟差和钟漂状态。不过初学者先不加避免状态维度过大导致可观测性变差。3.2 参数、噪声矩阵和初始对齐落地时最容易出错的不是公式而是参数和坐标系。坐标系方面最常用的是以某个参考点为原点的 ENU 局部坐标系或者 WGS84 下的 ECEF 坐标系。如果场景范围不大ENU 更直观如果做跨城市或全球应用ECEF 更合适。关键是 IMU 加速度计输出要转换到导航坐标系GNSS 位置也要转换到同一坐标系。时间戳方面GNSS 输出一般自带 UTC 或 GPS 时间IMU 是接收时刻的本地时间。两者需要统一到同一时间轴最好在数据采集时就用硬件同步或统一打戳。如果时间戳不对齐融合结果必然高频震荡。噪声矩阵方面需要设置过程噪声和测量噪声。常见做法是查看 IMU 手册中的噪声密度和随机游走参数并把 GNSS 的位置、速度噪声根据实际定位模式设置。RTK 固定解可以给到厘米到分米级单点定位要放到米级浮点解则介于两者之间。这个差异如果不区分融合结果会被错误的高权重信号带偏。初始对齐方面静止状态下先采集几十秒 IMU 数据计算初始三轴角度和陀螺仪偏置粗值。GNSS 也要在定位精度较好时记录初始位置。不要边运动边初始化除非系统支持动态对准否则收敛时间会很长。3.3 结果判断和常见坑怎么判断融合结果是否正常第一看位置轨迹是否连续。GNSS 偶尔跳变时融合位置应当被 IMU 平滑掉一部分而不是完全跟随跳变。第二看速度曲线是否合理。正常行驶或机器人移动时速度曲线不应出现高频大幅抖动如果出现多半是时间戳错位或噪声矩阵过小。第三看姿态是否稳定。静态和直线运动时俯仰和横滚应稳定在重力方向附近航向角虽然会漂移但不应出现毫无规律的瞬跳。第四看协方差是否发散。如果滤波协方差一直涨到非常大说明观测模型或过程模型有一个不对常见原因是单位不统一、坐标系反了、观测噪声设置过小。常见坑包括IMU 数据的加速度和角速度没有经过重力补偿判断初始姿态错误导致位置发散。GNSS 坐标写错单位例如把经纬度直接当米使用。时间戳时间轴不一致导致 GNSS 观测与 IMU 状态错位一个周期。更新顺序错乱先做 GNSS 更新又用同一时刻 IMU 数据做预测造成信息重复使用。静止时出现速度漂移但位置还能稳定因为偏置估计还没收敛。如果出现这些现象不要急着调大过程噪声“强行压平”先依次检查时间同步、坐标系、单位、初始姿态和观测噪声。注意噪声矩阵不是随便调的。小样本先跑 5 分钟对比融合轨迹和真实轨迹再根据误差曲线调整。4. 加入视觉和 LiDAR前端里程计、回环检测与因子图优化4.1 视觉和 LiDAR 在定位中扮演什么角色GNSS 覆盖很好的地方GNSS IMU 基本够用。但室内、地下车库、高架桥、林荫道这些场景里GNSS 不可用或者质量差就需要视觉和 LiDAR 提供环境约束。视觉和 LiDAR 在定位里主要做两件事。第一件事是里程计估计相邻时刻之间的相对位姿。视觉里程计通过特征点、光流或直接法估计相机运动LiDAR 里程计通过点云配准估计激光雷达运动。里程计提供的是局部一致性频率可以到 10Hz 到 30Hz但会随距离累积漂移。第二件事是地图匹配与回环检测。当系统进入一个之前访问过的区域时可以通过图像特征或点云特征识别出“我来过这里”然后添加一个回环约束把当前位姿和历史位姿对齐从而消除累计漂移。在融合框架里GNSS 是全局约束IMU 是高频约束视觉和 LiDAR 是几何约束。不同约束来源不同权重也应该随着环境和质量动态变化。4.2 前端里程计和后端优化怎么配合工程上比较成熟的路线是“前端里程计 后端因子图优化”。前端负责实时输出相对运动和必要的观测信息。视觉前端输出两帧之间的相对位姿以及该约束的不确定性LiDAR 前端输出帧间匹配的相对位姿和匹配质量。只输出位姿不输出内部的点云、特征和原始测量在融合层看起来就是一个相对位姿因子。后端负责把这些相对约束和绝对约束一起优化。IMU 预积分可以作为帧间的惯性因子GNSS 位置可以作为全局位置先验视觉和 LiDAR 相对位姿作为相邻帧因子回环检测作为长距离约束。关键点在于不是所有前端输出都能直接当成高精度约束。视觉里程计在纹理弱、动态物体多的情况下误差会变大LiDAR 里程计在走廊、隧道等退化环境中会出现沿某个方向的大漂移。因此后端必须能判断每个约束的质量。一种简单做法是给每个前端约束设置一个置信度或鲁棒核。例如视觉约束在特征点数量少或重投影误差大时降权LiDAR 约束在退化方向检测到约束不足时降权。4.3 批量场景下的回环和地图一致性处理只做前端和后端优化长距离定位依然可能漂移。回环检测能解决“位置回到原点后轨迹是否闭合”的问题但处理不好也会引入错误回环。判断回环质量时我会优先看三样东西描述子匹配得分图像 BoW 模型得分是否显著高于阈值。几何校验结果通过 PnP 或 ICP 计算相对位姿时的内点比例。时空一致性当前位姿和候选回环历史位姿是否在合理范围内。如果三个条件都满足再让回环因子进入优化器如果有一个明显不满足这个回环大概率是误检。地图一致性常见的坑包括不同时刻采集的点云没有对齐到统一地图坐标系视觉地图和 LiDAR 地图分别构建然后互相错位动态物体被当作静态环境加入地图。这些问题在单传感器测试中不明显多传感器融合后会变成长期漂移和跳变的来源。解决思路是不要只维护一个位姿输出还要维护一个可更新的局部地图或关键帧数据库。每次优化后把关键帧位姿、地图点、因子残差一起更新保证后续匹配基于一致的地图。另外如果数据足够长建议把后端从固定滑窗扩展成带边缘化的滑动窗口或者使用增量式因子图求解器。这样可以兼顾实时性和全局一致性。5. 时间同步与传感器标定比算法更影响融合效果5.1 时间同步的四种常见做法时间同步是融合定位里最容易被低估的环节。传感器之间的时间差达到几十毫秒在高速行驶或剧烈运动时就会产生明显的位置误差。常见的同步做法有四类。第一类硬件同步。GNSS 接收机输出 PPS 秒脉冲作为各传感器的统一时间基准同时通过串口输出 NMEA 协议中的 UTC 时间。视觉、LiDAR、IMU 在收到 PPS 时打上硬件时间戳。这种方式精度最高可以达到微秒到毫秒级。第二类统一主时钟。在机器人或车辆上使用同一个时间同步板卡为所有传感器提供同步触发信号。相机曝光时刻、LiDAR 旋转方位角、IMU 采样时刻都能对齐到同一时间轴。第三类软件同步。各传感器自带时间戳在融合前用插值或查找最近邻将观测对齐到目标时间轴。例如 IMU 是 200Hz相机是 30Hz可以把相机的曝光时刻作为目标时间对 IMU 数据做插值。第四类离线后处理同步。预先采集数据保存每帧原始时间戳在算法入口处统一转换。适合算法验证和数据集回放不适合实时系统。不管用哪种方式第一步都是先看各传感器的延迟分布。把同一运动时刻下的加速度计峰值和相机帧时间戳画出来可以看到大致延迟。若能曝光时间已知最好把相机曝光中点作为对齐点。5.2 外参标定和 IMU 内参偏置时间同步解决“什么时候”外参标定解决“在哪朝哪里”。融合定位中至少需要确认几组外参IMU 坐标系到 GNSS 天线相位中心的杆臂。IMU 坐标系到相机光心的旋转和平移。IMU 坐标系到 LiDAR 中心的旋转和平移。相机和 LiDAR 之间的相对外参如果它们都参与定位。外参不准会造成严重的投影和匹配误差。尤其是 IMU 与 GNSS 天线之间的杆臂在车辆转弯时会引入一条虚拟的速度误差导致位置轨迹出现向外扩张的弯曲。IMU 内参包括加速度计尺度、惯性轴不正交角、陀螺仪尺度、零偏重复性与温度漂移。内参虽然没有外参那么显眼但长时间运行后会直接影响累积误差。建议用 Allan 方差分析陀螺仪和加速度计的随机游走特性并在温箱或静止数据上做零偏稳定性评估。标定方法可以是基于标定板的视觉标定、手眼标定、基于自然场景的联合优化。实际工程中可以先用简单的旋转激励和位姿残差做粗标定再用融合优化做精标定。5.3 验证标定结果的方法标定完不能只看重投影误差还要看融合后的闭环效果。我通常用以下方法验证。方法一把激光点云投影到图像上。如果相机与 LiDAR 外参正确物体边缘应该对齐道路标线边缘不应出现明显错位。方法二做原地和“8”字运动。用更新后的外参融合数据观察 GNSS 天线速度和 IMU 速度的一致性。如果杆臂或外参不对转弯时速度残差会出现周期波动。方法三反复通过同一路段。对比两次经过时融合轨迹与地图的重合度。重合度越高说明外参和回环约束越可靠。方法四检查优化后的残差分布。将各类观测残差按时间和传感器类型统计如果某类残差始终偏大优先怀疑标定或时间同步而不是滤波增益。6. 落地排查清单从“能跑”到“跑得稳”6.1 按现象分类的排查顺序多传感器定位系统出问题时先看现象再开单顺序很重要。现象一启动后位置直接飞掉。优先检查初始位置、初始姿态、坐标系和单位。IMU 的 z 轴是否向上加速度是否补了重力GNSS 经纬度是否转换正确。现象二运行时位置抖动厉害。优先检查时间同步、观测噪声和更新频率。如果 GNSS 和 IMU 的时间戳错位抖动通常会在低通滤波下也压不下来。现象三轨迹整体漂移越来越远。优先检查 IMU 偏置是否收敛、GNSS 约束是否被降权、视觉或 LiDAR 里程计是否退化。正常时绝对约束应该能把长期漂移锁住。现象四运行一段时间后协方差发散。优先检查状态可观测性。比如只有 GNSS 位置更新没有速度观测速度状态可能出现不可观只有 IMU 而没有绝对角度观测时航向角可能不可观。现象五某个传感器短时间丢失后再恢复定位跳变。优先检查故障识别逻辑是否有迟滞以及恢复时是否需要重新初始化观测。直接让一个刚恢复的传感器以很大权重进入滤波器通常会造成大幅跳变。排查时不要同时改多个参数。一次只改一个变量观察输出变化。用同一段数据反复回放才能对比出真实差异。6.2 资源占用、日志和结果保存融合定位的实时性不能只看单帧耗时。还要看滤波器或优化器在最大观测数量下的耗时上限。不同传感器话题或数据通道是否存在队列积压。数据回放时能否保证时间戳顺序不被打乱。日志写入磁盘是否会影响实时循环尤其在长时间路测时。我一般会把每次测试的原始数据、外参、噪声参数、版本号、融合结果和残差曲线一起保存。这样同一个数据包可以反复验证不同参数下的差异也会让后期排查问题少很多弯路。输出保存格式建议使用二进制或高性能序列化格式不要直接落成千上万行的文本。每条轨迹记录可以包含时间戳、位置、速度、姿态、协方差对角线和每个传感器状态标志。带有状态标志的好处是当定位跳变时能快速判断是哪个传感器先异常。6.3 几个容易误判的经验最后说几点我踩过很多次的经验。低配置环境能跑通联合优化不代表能实时跑。优化器在离线数据集上收敛很快但在线运行时每次插入新因子都要控制窗口大小。如果发现实时性不够优先做边缘化或降低优化频率而不是阉割传感器数量。“支持多传感器”不等于“任意组合都稳定”。GNSS IMU 融合和 GNSS IMU LiDAR 融合是完全不同的调参难度。新增一个传感器需要重新评估它的坐标、时间同步、外参、故障模式不要默认系统会自适应。报错不一定是算法问题。很多“融合发散”来自外参在采集时发生过改动、地面坐标系和车辆坐标系定义不一致、数据包中包含重复时间戳。先清理数据再调参数。还要注意同一类型的传感器也有退化叠加风险。比如视觉和 LiDAR 在逆光或烟尘中同时退化GNSS 在隧道里失效此时如果没有可靠的 IMU 和里程计约束融合结果会退化到纯惯导。给系统设计一个清晰的退化路径明显优于硬撑着输出不可信定位。真正跑稳多传感器融合定位我见过的最短路径就是先用小数据集验证每条传感器通道再逐层加入融合约束最后用长时间和复杂场景数据去压测。这个顺序看起来慢但排错时间反而最少。
