1. 为什么轨迹跟踪必须走到多传感器融合这一步做轨迹跟踪的人早晚会遇到同一个问题单一传感器到了复杂工况下数据根本没法信。跑着跑着GNSS信号被高楼挡住了惯导器件零偏漂得自己都不知道自己在哪视觉里程计一到逆光或者纹理稀疏的场景就疯狂跳变。这个时候再好的单源跟踪算法也是白搭因为上游数据本身就是错的。我最早做无人机室内定位的时候用的是纯光流加IMU的组合室内没有卫星信号光流在瓷砖地面上还行一到地毯或者强光直射的区域就开始原地转圈。后来改成光流、IMU加UWB锚点测距的三源融合才勉强压住轨迹漂移。但融合不是简单把数据平均一下做加法之前得先解决一个根本问题谁的数据在什么时刻是可信的可信到什么程度。这正是卡尔曼滤波体系的核心价值——它不是在帮你说哪个传感器对而是在帮你计算每个传感器的不确定性到底是多少再按不确定性加权。这篇文章要聊的就是我在多传感器轨迹跟踪项目里实际趟过的三条技术路线标准卡尔曼滤波KF、自适应扩展卡尔曼滤波AEKF和自适应无迹卡尔曼滤波AUKF。三种算法解决的是同一个问题——多源量测数据融合下的状态估计但适用场景、实现复杂度、调参难度完全不在一个量级。我会把每种算法的原理、推导关键点、工程实现细节、容易踩的坑以及我自己的选型依据都写清楚。适合刚接触传感器融合的初学者也适合已经在做轨迹估计但想在稳定性上再进一步的人。2. 先从最基础的线性卡尔曼滤波说起它是所有变体的地基2.1 线性KF到底在算什么标准卡尔曼滤波做的事情可以概括成一句话在运动模型的预测值和传感器的观测值之间找一个按方差加权的折中。它假设系统是线性的、噪声是高斯的然后通过递推的方式不断修正状态估计。具体的递推分两步。第一步叫预测用上一时刻的状态和运动模型推算出当前时刻的状态先验估计同时推算出状态协方差矩阵的先验值[ \hat{x}{k|k-1} A\hat{x}{k-1|k-1} Bu_{k-1} ] [ P_{k|k-1} AP_{k-1|k-1}A^T Q ]第二步叫更新把传感器的量测残差按卡尔曼增益K融合进来[ K_k P_{k|k-1}H^T(HP_{k|k-1}H^T R)^{-1} ] [ \hat{x}{k|k} \hat{x}{k|k-1} K_k(z_k - H\hat{x}{k|k-1}) ] [ P{k|k} (I - K_kH)P_{k|k-1} ]公式不复杂关键要理解K的物理含义。K是一个0到1之间的信任度分配器当量测噪声R很小、传感器很可信时K趋近于1滤波器更听从观测当过程噪声Q很小、运动模型很准时K趋近于0滤波器更相信预测。整个算法本质上就是一个动态求加权平均的过程。2.2 线性KF在实际轨迹跟踪中的两个硬伤第一轨迹跟踪系统几乎没有真正线性的。拿一个典型的无人机水平位置估计来说状态可能是 [位置x, 速度vx, 位置y, 速度vy]运动模型可以用匀速模型近似但如果无人机在做圆周运动或者强机动匀速模型的预测误差会大幅增加。这时候如果Q给小了滤波器会持续相信错误预测轨迹会出现明显滞后Q给大了轨迹又会抖动。第二R和Q都是提前定死的常量。实际工程里传感器的噪声特性是随环境变化的。GNSS在开阔地误差两厘米到了树荫下可能变成几十厘米视觉里程计在纹理丰富区域精度很高遇到白墙就发散。固定R的KF在这种情况下会做出错误判断——它要么过度相信已经失准的传感器要么过度怀疑仍然精准的传感器。这就是为什么后来大家普遍转向EKF——用泰勒展开把非线性系统在工作点附近线性化公式形式和线性KF一致只是A和H矩阵变成了雅可比矩阵。2.3 我在线性KF上踩过的具体坑最早我用KF做AGV小车的轨迹跟踪状态是二维位置加二维速度量测来源是编码器推算和地面二维码识别。刚开始测试一切正常一到二维码布置稀疏的区域轨迹就开始抽风。排查了很久才发现问题不在算法本身而在时间同步——编码器的数据是50Hz二维码识别是2Hz我直接把两组数据按到达顺序丢进滤波器导致同一时刻的两类量测其实描述的是不同时间点的位置。做多传感器融合时间对齐永远是第一个要处理的问题。KF假设所有量测在同一时刻到达实际工程必须做插值或缓冲把不同频率的量测对齐到同一时间戳否则融合结果还不如单一传感器。3. AEKF的核心思路让噪声协方差学会自己调节3.1 AEKF到底自适应在哪里AEKF的自适应机制有很多流派我做轨迹跟踪时用的是基于残差的新息协方差匹配法。核心思想是理论上滤波器的新息量测残差协方差应该等于量测矩阵、状态协方差和量测噪声协方差的组合[ S_k HP_{k|k-1}H^T R_k ]而实际工程里我们可以用一段滑窗内的新息序列直接估算出新息协方差的实测值[ \hat{S}k \frac{1}{N}\sum{ik-N1}^{k} e_i e_i^T, \quad e_i z_i - H\hat{x}_{i|i-1} ]如果实测值比理论值大说明量测噪声被低估了应该调大R反过来则调小R。AEKF就是在每个滤波周期做一次这样的比较然后在线修正R矩阵有的实现也修正Q让滤波器对噪声变化保持敏感。3.2 自适应调节的实际效果一个直观的案例我在一个植保无人机项目里做过对比实验。无人机沿田垄飞行轨迹跟踪系统融合RTK-GNSS和机载IMU。正常情况下RTK精度在1-2厘米GNSS量测噪声R设为0.01量级就能跑得不错。但无人机飞到田边树林附近时RTK信号开始出现间歇性多路径效应定位误差短时间跳到0.5米以上。用固定R的EKF时轨迹在树林段明显鼓起一个包因为滤波器仍然以0.01的方差去相信这批已经被污染的观测数据。切换到AEKF后滑窗新息协方差检测到残差突然变大自动把R调高了几个数量级滤波器转为更信任IMU的预测值轨迹包被压下去了。图中那个位置轨迹偏差从0.4米降到0.15米以内。但这套方法不是免费的。滑窗长度N的选择直接影响自适应速度。N太小单个异常残差就会剧烈扰动R的估计导致权重分配震荡N太大R的修正滞后等滤波器反应过来传感器噪声已经恢复原状了。我试过N取5、10、20、50四档实测下来在20Hz量测频率下N取10-15表现最稳既能在一秒内响应噪声突变又不会因为单点异常产生较大波动。3.3 AEKF的工程实现时序下面是我实际用过的AEKF实现流程用的是位置速度模型加GNSS量测预测步骤用上一时刻状态和IMU积分推进状态先验同步更新协方差先验。量测到达后计算新息(e_k z_k - H\hat{x}_{k|k-1})。维护一个固定长度的新息滑窗窗口长度N取10。用窗口内所有新息计算实测新息协方差。理论新息协方差与实测值对比按比例调整R矩阵对角元素并做上下限限幅。用调整后的R计算卡尔曼增益完成状态和协方差更新。限幅这步非常关键。我踩过的坑是把R调没了下限结果连续几个异常残差把R推到极小值滤波器之后狂信已经失准的量测轨迹反而发散。后来给R的每个对角元设了(10^{-6})到(10^0)的范围稳定性立刻上来了。AEKF适合处理的是量测噪声变化较慢、有统计规律的场景。如果你的传感器噪声是瞬间阶跃的而且阶跃幅度很大AEKF的滑窗响应可能不够快这时候需要的是多模型切换或者鲁棒滤波的思路。4. AUKF的进阶没有雅可比也能处理强非线性4.1 无迹变换的理念用采样点逼近分布EKF和AEKF都依赖雅可比矩阵做线性化这带来两个问题一是复杂模型的雅可比推导非常痛苦容易出错二是线性化在强非线性区域误差很大泰勒展开丢掉的高阶项可能就是决定成败的部分。UKF的思路完全绕开了线性化。它不再对函数做近似而是通过对状态分布做近似——选取一组带权重的sigma点让这些点的均值和协方差等于当前状态分布的均值和协方差然后把每个点都经过非线性函数传递用传递后的点重新计算均值和协方差。在轨迹跟踪里最典型的强非线性场景是动目标的纯方位跟踪量测是方位角 (\theta atan2(y - y_s, x - x_s))这个非线性函数在目标经过传感器正侧方时方位角变化率急剧增大EKF在这个区域的线性化误差会明显放大。UKF因为直接采样传递对这类非线性有天然优势。4.2 AUKF的自适应和AEKF的差异AUKF的自适应机制和AEKF类似也是通过新息协方差匹配来在线调整噪声矩阵。区别在于基础滤波器换成了UKF预测和更新步骤的处理对象从高斯分布的解析传递变成了sigma点的样本传递。一组常用的尺度参数配置是(\alpha0.001)控制sigma点的散布范围(\beta2)用于高斯分布的最优拟合(\kappa0)或取(3-n)保证协方差半正定。实测下来(\alpha)取值非常敏感小于(10^{-4})时sigma点分布太集中非线性传递的信息量不足大于0.01时分布过宽协方差被高估增益过小导致响应迟钝。我在多源融合里选的是(\alpha0.001)(\beta2)效果稳定。4.3 计算代价与工程权衡要提醒的是AUKF不是免费的午餐。UKF需要(2n1)个sigma点逐一通过非线性函数n是状态维度。如果状态是6维位置3速度3那就是13个点要跑一遍量测模型计算量是EKF的好几倍。对实时性要求高的系统这个开销和稳定性收益需要仔细平衡。我在一个地面无人车的轨迹跟踪项目里做过多算法对比。融合数据是轮速计、IMU和LiDAR位置量测状态维度12维位置、姿态、速度、角速度轨迹包括急转弯和加减速段。AUKF在急转弯段的位置误差比EKF低约23%但单周期耗时从0.3ms涨到1.1ms。100Hz的控制周期下依然远有余量但如果换到200Hz以上的嵌入式环境就得考虑降维或者改用平方根形式。5. 三种算法的对比实验与选型指南5.1 我在同一套数据上的实测对比我在公开的KITTI数据集和自己采集的AGV数据集上都跑了三类算法的对比实验评价指标是RMSE位置误差、最大误差和单帧耗时。实验设置是状态量位置加速度量测源是GPS风格的低频绝对定位加高频惯导推算模拟场景包含直线、大曲率弯道、短时信号丢失去和噪声突变。算法位置RMSEm最大误差m单帧耗时ms调参难度固定R的KF/EKF0.321.200.2低AEKF0.240.620.3中AUKF0.190.451.0高AEKF相对EKF的RMSE提升约25%最大误差降低约50%核心收益来自对噪声突变的自适应响应。AUKF相对AEKF的RMSE又提升约20%主要收益来自强非线性段的估计精度改善。5.2 我自己的选型法则纯从工程角度我的选型逻辑是分级的系统线性度好传感器噪声稳定RTK加惯导的组合直接用固定R的KF就够了没必要上自适应多一层机制多一个故障源。系统线性度还行但传感器会受环境影响波动GNSS进树荫、视觉进弱纹理区AEKF性价比最高改动小、收益明显。系统存在明显非线性环节大曲率转弯、纯方位跟踪、视觉重投影模型或者传感器噪声变化剧烈、间歇性失效值得上AUKF。但前提是你有足够的算力和调参耐心。调参顺序上我的建议是先用固定R的EKF把基础状态模型调顺确认运动模型和量测模型本身没错再切换到AEKF测试自适应效果最后才考虑AUKF。很多人一上来就AUKF结果调了两个月发现是状态定义有问题白白浪费时间。6. 实战案例分析AGV轨迹跟踪的多传感器融合设计6.1 系统构成和数据流我在一个工厂AGV项目里完整落地过这套方案。AGV在仓库里以最高1.5m/s的速度行驶轨迹跟踪需要提供20Hz的位置输出用于闭环控制。传感器配置是编码器轮速计50Hz短时精度高但有累计漂移IMU100Hz提供角速度和加速度用于状态预测反光板激光定位2Hz绝对位置量测硬币大小精度整套系统最核心的设计决策是高频的轮速计和IMU负责状态预测低频的激光定位只负责修正漂移中间加一层AEKF做噪声自适应。一开始用固定R跑AGV在反光板密集区域效果不错走到货架深处反光板被遮挡时激光量测频繁失效轨迹逐渐偏出车道。后来给AEKF加上量测有效性判断激光定位的质量评估分数低于阈值时直接把对应的量测噪声调到极大值相当于丢弃该量测靠惯导推算撑过遮挡区。这个策略让系统在遮挡区的最大偏差从0.6m降到了0.2m完全满足AGV对接精度要求。6.2 数据对齐的关键实现时间对齐这个坑再强调一遍。我是用一个环形缓存做的激光定位消息带时间戳到达后在缓存里找到最近两个IMU/轮速计状态线性插值出恰好和激光定位同一时刻的预测状态然后拿这个插值状态去做量测更新。插值公式很简单[ x(t_{lidar}) x(t_1) (x(t_2) - x(t_1)) \cdot \frac{t_{lidar} - t_1}{t_2 - t_1} ]但t1和t2的选择很讲究。我一开始用缓存里最新一帧和前一帧插值后来发现问题在于IMU积分本身有漂移插值出来等于在两个有漂移的状态之间做线性组合误差没有消除反而被平均化了轨迹会出现小幅锯齿。最后改成在滤波器预测阶段就记录中间状态每次预测之后把状态缓存一份插值时用缓存里的状态而非单独维护的里程计状态锯齿消失了。不确定度也要按时间对齐做处理。不同传感器的量测噪声标准差必须在同一时间基准下定义才有意义。我的做法是在离线标定时先让AGV走一段已知真值轨迹统计每个传感器在每个时间偏移下的误差特性再在在线阶段用查表方式设置初始RAEKF在此基础上做动态修正。6.3 初始化阶段的一个细节多传感器融合系统的初始协方差矩阵不能随便设。设太大滤波器初始收敛慢前几秒钟的轨迹是反的或者严重滞后设太小又过早锁死先验值如果初始状态给偏了会很难拉回来。我的经验是初始P设置为协方差对角元素([\sigma_p^2, \sigma_v^2])其中(\sigma_p)取初始定位不确定度比如激光定位精度0.02m(\sigma_v)取0.5m/s量级。然后前50个滤波周期用较大的量测噪声R让系统快速收敛之后再切入正常R值。实测下来这个策略比从一开始就用精确R的收敛速度快很多而且不会出现初始震荡。很多人忽略这个细节融合系统一启动就轨迹飞出去然后又怀疑算法本身有问题其实只是初始协方差没给对。7. 工程落地的其他拦路虎协方差矩阵退化与数值稳定性7.1 协方差矩阵是怎么退化掉的做多传感器融合做得久了一定会遇到协方差矩阵退化问题。症状是滤波器运行一段时间后状态协方差矩阵的某些对角元变得异常小或者矩阵不再对称正定卡尔曼增益计算出现NaN整个估计器当场失效。我遇到过一个真实案例AGV长时间直行横向位置没有量测激励协方差的横向分量被不断压缩到接近机器精度这时候一个微小的数值误差就能让矩阵失去正定性。后来我加了两个防护一是对P矩阵做周期性对称化(P (P P^T)/2)并在对角元上加(10^{-9})的极小值保证正定性二是对Q矩阵做下限限幅防止过程噪声在某些维度被压到零从而导致协方差冻结。7.2 平方根滤波是更稳的备选方案如果上述防护还挡不住数值风险可以考虑平方根UKFSR-UKF或平方根KF。它的核心思路是不直接传递协方差P而是传递其Cholesky分解的下三角矩阵S每次更新直接对S做QR分解和Cholesky更新从数学上保证协方差矩阵半正定。我在一个需要连续运行8小时以上的巡检机器人项目里用过SR-UKF和普通UKF对比了协方差矩阵的最小特征值变化趋势普通UKF在4小时后最小特征值就跌到(10^{-12})量级SR-UKF一直在(10^{-6})以上数值稳定性差距肉眼可见。代价是实现复杂度更高sigma点更新和Cholesky更新的代码量大约多出40%。7.3 量测一致性检验别让坏数据进入滤波器多传感器融合的最后一个隐形杀手是数据异常没有过滤就进入滤波器。自适应滤波能应对噪声的渐变但对突发的粗大误差比如激光误匹配、GNSS周跳应对能力有限。我在所有项目里都会加一个量测门控[ e_k^T S_k^{-1} e_k \chi^2_{0.99} ]这里(e_k)是新息向量(S_k)是新息协方差左侧计算出来的是马氏距离的平方。如果超过(\chi^2_{0.99})门限说明这个量测和当前估计严重冲突直接丢弃。这个门控逻辑实现成本极低但对系统稳定性的收益非常大。实测中有门控的系统在激光偶尔误匹配的情况下连续运行一周不发散没有门控的系统平均两三个小时就会出一次大跳变。我在AGV项目里用的门限取值是位置量测2维自由度(\chi^2_{0.99} 9.21)超过即丢弃。这个值也可以根据调试情况放宽但不要超过15否则门控就失去意义了。8. 把三种算法的选型思路串起来一个可落地的决策框架到这一步整体的思路已经比较清楚了。KF、AEKF、AUKF三者不是替代关系而是针对不同传感器噪声特性、不同系统非线性程度的三档工具。工程上不用执着于一定要用最新的算法我见过很多量产项目用固定R的EKF跑得非常稳也见过硬上UKF结果调参调崩的项目。如果让我给一个直接的落地建议大概是这样的第一天做融合直接用标准KF把状态定义和传感器时间对齐问题解决清楚这是整个系统能不能跑通的地基。第一周做优化确认基础功能稳定后引入AEKF的自适应机制重点看量测噪声变化场景下的轨迹改善。后续按需升级如果发现系统存在明显的非线性环节导致EKF线性化误差不可接受再迁移到AUKF。我在实际项目中还有一个体会调参时不要把注意力全放在算法参数上传感器本身的误差特性摸底越透滤波参数越容易确定。花半天时间让传感器动起来记录不同工况下误差分布的真实数据比盲调R和Q十天都有用。量测噪声到底给0.01还是0.1不是拍脑袋定的是从误差分析中算出来的。另外建议所有做轨迹跟踪的团队都建立一个离线回放调试环境记录传感器原始数据离线跑同样的融合算法这样每次调整参数都能在同一段数据上对比效果。我吃过太多亏——在实车上改参数改完环境变了根本分不清是参数变好还是工况变好。离线回放加上定量指标才是调参的正确姿势。最后再分享一个小技巧AEKF和AUKF自适应调节R时对R矩阵的每个对角元分别做限幅和滤波而不要让不同维度的量测噪声互相影响。比如视觉量测的噪声和GNSS量测的噪声应该完全独立地自适应如果用一个标量统一缩放所有维度某个维度异常会把其他正常维度的权重也带偏。这个细节我在多个项目里都验证过分开调节的效果稳定得多。