前阵子和一个做低速无人车的朋友聊到一个很常见的现象在高架桥下测试时卫星定位的轨迹直接穿过了桥墩而IMU推算的位置反倒一直贴着路面。两台传感器各报各的数谁也说服不了谁。这个场景估计搞过定位的人都遇到过——单靠卫星信号遮挡环境下会漂到让你怀疑人生单靠惯导十分钟后误差也能让你怀疑人生。把两者融合起来就是我们常说的组合导航。这篇文章想把INS与卫星信号的融合算法从头拆开讲清楚重点说说基于卡尔曼滤波技术的优化应用是怎么落地的以及中间那些文档里很少写清楚的坑。适合的读者主要有两类一类是刚接触组合导航正在搭第一个滤波器的学生或工程师另一类是已经能跑通基本流程但发现精度、稳定性总是差口气想系统排查问题的同行。我也会把传感器选型、耦合方式、参数调优、异常保护这些内容都串进来尽量做到看完能直接用来指导自己的项目。1. 单传感器各自的软肋恰恰是对方的机会1.1 卫星信号不是不行而是太“脆”GNSS定位的原理其实很直白接收机测量到多颗卫星的伪距代入最小二乘或卡尔曼解算出位置。开阔天空下实时动态差分RTK可以做到厘米级普通单频也可以做到米级而且误差不会随时间累积。这一点是惯导永远比不上的。但这个系统的短板也很明显。第一它对环境极其敏感。高楼峡谷、隧道、高架下、浓密树荫信号被遮挡后定位精度断崖式下降甚至直接失效。第二它怕多径。城市里到处是玻璃幕墙和金属护栏反射信号混进来产生十几米的偏差而且这个偏差是正负震荡的并不是单纯加个固定的偏移。第三更新率通常只有1020Hz中间一两百毫秒的轨迹全靠插值在急转弯或快速变道时会产生明显延迟。有个现象很能说明问题在城市里停车等红灯时静止状态下的卫星定位轨迹往往会在两三米范围内飘来飘去像喝醉酒一样。这是因为载噪比下降、多径和大气噪声都在起作用。你要是拿这种位置直接做控制车辆会一路“画龙”完全没有办法自动驾驶。所以卫星导航的结论是长期精度靠谱短期连续性差环境好时能当基准环境差时直接抓瞎。1.2 惯性导航很自主但误差会“滚雪球”惯性导航依靠IMU里的加速度计和陀螺仪分别测量比力和角速度。加速度积分得到速度速度再积分得到位置角速度积分得到姿态。整个推算过程完全自主不需要任何外部信号无遮挡问题更新率可以到200Hz甚至1000Hz输出也是毫秒级连续。但这个系统有个致命的毛病误差是积出来的。陀螺仪哪怕只有1°/h的零偏看着很小姿态误差却会随时间线性增长。姿态一旦有了角度误差重力分量就会被错误地分离到水平加速度上产生水平位置误差这个误差再经二次积分位置漂移会以时间的平方级扩大。加速度计零偏更直接0.1mg的零偏持续作用理论上60秒纯积分就能漂出好几米。我做过一个粗略估算一个中等精度的MEMS IMU零偏稳定性在10°/h级别不加任何修正90秒后位置漂移可以接近百米的量级。这在绝对定位眼里是完全不可接受的。惯性导航的结论是短期精度和连续性极好绝对位置会随时间越来越离谱。1.3 互补的本质一个管“长期不漂”一个管“短时精准”把两个传感器的特点放到一起看方向就非常明显了卫星系统给惯导提供一个稳定的绝对参考把随时间累积的漂移拽回来惯导在卫星信号被遮挡、多径、低更新率的间隙里提供高频、连续的相对运动和姿态信息。这就是组合导航最底层的动机也是这个领域存在的基础。卡尔曼滤波在这里的核心贡献不是简单地把两个位置取平均值而是根据各自的误差特性动态决定“谁的话更可信”。开阔天空下卫星观测噪声小滤波器会更信任卫星的绝对位置进入高架下卫星明显飘的时候通过观测残差可以把它的权重降下来转而依靠惯导的短时推算。两者交替补位的效果比任何一个单传感器都稳定得多。组合导航输出的也不只是“一条位置轨迹”而是一个完整的三维运动状态三维位置、三维速度、三维姿态以及陀螺和加速度计的零偏估计。这就是标题里的“三维立体组合导航”的实质——它在严格的数学框架里估计的是一个立体的运动状态而不只是二维平面上的x、y坐标。2. 卡尔曼滤波如何把两个不完美的信号“拧”成一条轨迹2.1 从贝叶斯直觉说起预测和更新就是“猜”加“改”很多讲卡尔曼滤波的资料一上来就列公式然后大家记住了公式却不知道它在干嘛。用大白话来说卡尔曼滤波干的事情就是两件先用系统模型预测当前状态再用观测数据对预测结果做修正。预测和修正各自带一个不确定性最后的结果是两者按不确定性加权折中出来的最优估计。你可以把它想成两个人估计一件事情一个人是根据经验和历史在猜另一个人拿仪器在测。如果猜的那个人历史表现很稳定而仪器今天噪声特别大你就更愿意相信猜的结果反过来仪器今天很准、猜的那个人一直在抖动你就要让测量的权重大一点。卡尔曼滤波里的协方差矩阵干的就是这个“判断谁更可信”的活。这也是它比互补滤波、加权平均高明的地方。加权平均只能给固定权重没办法根据信号质量实时变化卡尔曼滤波通过协方差传播能把“我有多相信这个估计”这件事随时算出来并且直接用协方差计算出最优权重矩阵——也就是卡尔曼增益。搞懂这一点后面的所有公式都只是在实现这个思路。2.2 组合导航中最常见的15维状态模型组合导航里最经典的模型不是只在二维平面里做位置融合而是把一个完整的三维运动状态作为估计对象。用得最多的状态向量是15维状态量维度单位说明位置北、东、地3m以当地导航坐标系为参考速度北、东、地3m/s对地速度姿态横滚、俯仰、航向3rad描述机体坐标系相对导航系的关系陀螺零偏3rad/s三轴陀螺的常值零偏估计加速度计零偏3m/s²三轴加速度计的常值零偏估计有人会加入更多维度比如陀螺和加速度计的标度因数、安装失准角、GNSS接收机时钟偏差以及杆臂误差等状态量可以扩到21维甚至更高。但初学者从15维起步最合适维度少可观性好参数容易调也足以支撑松耦合和大部分紧耦合应用。状态方程的来源就是惯性导航的机械编排。把IMU输出的比力和角速度作为输入用姿态矩阵把加速度折算到导航坐标系再积分得到速度和位置。陀螺零偏和加速度计零偏在模型里通常建模成随机游走也就是它们的值会缓慢漂移需要用GNSS观测不断修正。状态方程的矩阵形式可以写成x_k F * x_{k-1} G * u_k w其中x是15维状态F是状态转移矩阵u是IMU输入w是过程噪声。这一步做的是“我根据上一时刻的状态和IMU读数猜一下现在大概在哪里”。2.3 核心公式就这么几步但每一步都有物理含义卡尔曼滤波的标准五步是预测两步、更新三步全部列出来也就几行预测 x̂_k F * x̂_{k-1} B * u P_k F * P_{k-1} * Fᵀ Q 更新 K P_k * Hᵀ * (H * P_k * Hᵀ R)⁻¹ x̂_k x̂_k K * (z - H * x̂_k) P_k (I - K * H) * P_k逐个说它们的物理含义。P是状态协方差矩阵对角线元素可以粗略地理解为“当前每个状态估计的不确定程度”Q是过程噪声矩阵表示你对惯性导航模型本身的信任程度Q越大说明你认为模型误差越大R是观测噪声矩阵表示你对GNSS等外部观测的信任程度R越大说明你认为测量越不可靠。K是卡尔曼增益它是P、H、R共同作用的结果。当R相对小、P相对大的时候K会变大滤波器更偏信观测反过来K变小滤波器更偏信模型预测。z减H乘以x的那个括号数学上叫新息是“实际观测和预测观测之间的差别”也就是预测和测量不一致的地方在哪里。代入组合导航的场景看高架桥下卫星信号飘了十几米此时R应该被放大K就会降下来滤波依然信任惯导的推算卫星观测对位置的影响变小。开阔处卫星信号稳R小K相应变大惯导的漂移就被实时拽回来。这套机制不需要人为写一堆复杂的判断逻辑它自己就能根据不确定性去平衡。3. 松、紧、深三种耦合架构怎么选3.1 松耦合最适合作为第一套方案松耦合的架构是“先解算后融合”。GNSS接收机内部自己完成卫星信号的解算输出一组位置和速度导航滤波器拿到这组位置速度作为观测值和INS的机械编排结果做卡尔曼滤波融合。所有导航功能都在外部处理器上实现接收机只负责把结果递出来。优点是模块化程度非常高。换一个GNSS接收机、换成RTK、换成双天线测向对融合算法的影响很小只需要调整R矩阵里的噪声参数。调试也直观每个环节的错误都能单独定位先看GNSS单独输出的位置好不好再看INS单独积分的漂移大不大最后才看滤波融合出的结果。缺点是它把GNSS接收机内部解算出来的结果当成了“不可再分”的最终观测。一旦可见卫星少于4颗接收机就出不了定位结果此时松耦合系统拿不到任何更新只能靠INS自行推算。哪怕此时还有3颗星理论上还能给滤波提供有用的伪距约束松耦合也利用不上。如果是刚起步的项目建议直接采用松耦合。先把卡尔曼滤波的预测更新、坐标系转换、时间同步跑通有一个能稳定工作的基础组合导航系统再考虑要不要升级到紧耦合。这一步走稳了后面换架构只是改观测模型的问题。3.2 紧耦合少一颗星也能用的关键在于改“原料”紧耦合不再使用接收机解算出的位置速度而是直接用接收机输出的伪距、伪距率多普勒观测作为观测值。接收机对每一颗可见卫星单独输出一个观测导航滤波器自己统一处理这些原始测距信息。这个架构最大的价值体现在卫星部分遮挡的场景。正常定位要求至少4颗星才能解算出完整的三维位置和时钟偏差但在紧耦合结构里哪怕只有13颗星只要把这些原始伪距放进观测方程滤波器照样可以结合INS的状态预测获得一部分位置约束。结果就是信号环境差的时候紧耦合比松耦合更能“扛”。代价也很实在。第一接收机必须支持输出RTCM或厂商私有格式的原始观测数据选型受限。第二伪距观测方程里必须额外建立接收机时钟偏差模型把钟差当成一个状态量或者用已知参数消掉这一步处理不好会直接破坏滤波器的精度。第三可见卫星数量在复杂环境里变化很快滤波器的观测维度会动态变化代码实现和异常处理都更复杂。用一句话概括松耦合换的是成熟与简洁紧耦合换的是在恶劣环境下多出来的可用信息。3.3 深耦合硬件级的最终方案别轻易尝试深耦合是INS信息和GNSS接收机内部跟踪环路之间的深度融合。导航滤波器的输出会直接反馈到接收机的载波环路、码环路辅助接收机在弱信号下保持跟踪接收机内部更早的积分观测又被送回导航解算。这种结构在高动态飞行器、强干扰环境里有优势信号动态应力和抗欺骗能力都强。但它几乎不是一个软件层面能完全解决的问题通常需要专用的紧耦合板卡或一体化的导航芯片支持工程复杂度、硬件成本和调试难度都高出一个量级。对于绝大多数创业项目、科研原型、低速无人车或者无人机应用深耦合都不是必要的。我更建议把精力放在松耦合到紧耦合的过渡以及后面要讲的滤波细节优化上。架构观测输入主要优点主要缺点适用场景松耦合接收机解算的位速简单、模块化、易调试卫星少于4颗时无观测大部分无人车、机器人紧耦合伪距/伪距率低可见星下仍可用实现复杂、需原始观测量城市峡谷、无人机深耦合接收机内部环路抗高动态与干扰依赖专用硬件航空、导弹级应用4. 滤波器能跑起来只是开始这些工程细节决定精度4.1 时间同步GNSS数据到你手里时已经“过期”了这是新手最容易忽略、却对精度影响极大的问题。GNSS接收机从接收射频信号到做完解算再把结果通过串口发出来中间存在几十到几百毫秒的延迟。你在应用层收到的那组位置实际上是“过去某个时刻”的位置而不是当前时刻的位置。如果车辆正好在高速运动把这个过期位置当作当前观测直接和INS融合会产生与速度乘以延迟量相当的系统误差。车速18km/h、延迟200ms时位置就有1m的偏差车速72km/h时这个偏差到4m足以让融合结果看起来“拖尾”。工程上的处理办法有几类。最常见的是先把INS机械编排的时间基准对齐到GNSS时间戳在卡尔曼预测阶段把状态预测到GNSS数据的有效时刻再执行更新。另一种做法是利用GNSS的PPS秒脉冲硬件对齐时间配合时间戳做精确打点。对于已有GNSS原始观测但延迟不固定的情况可以把延迟量本身扩到状态向量里实时估计这属于进阶方案但很值得做。我早期做项目时曾偷懒没处理延迟结果在转弯路段融合轨迹明显比实际晚半拍查了快两天才发现是这个问题。4.2 坐标系和姿态初值融合前先把“坐标系语言”统一GNSS解算得到的位置通常在ECEF地心地固坐标系里而惯导的机械编排一般在地理坐标系NED或ENU里进行。滤波之前必须做一步坐标转换否则两个传感器说的根本不是一个坐标系里的位置融合结果就是乱的。坐标转换本身不难用经纬度和高程把ECEF转成当地的NED或ENU关键要保证一致。初值更麻烦。滤波器的初始位置可以由GNSS第一帧提供这个简单但初始姿态如果给错了系统会以这个错误姿态为基础不断积分位置误差前几分钟就会出现明显发散。静止初始化时可以用加速度计测量重力方向来估计横滚和俯仰重力方向是竖直的加速度计三轴读数里的分量分配就能反推角度。航向角在静止时没法用加速度计确定所以要么用磁力计但要注意磁场畸变要么在运动后借助GNSS速度方向来确定水平航向更稳妥的是使用双天线GNSS测向系统直接给定航向初值。多花几分钟做好初始化比一切参数调好之后再来排查“为什么起飞头几十秒就歪了”便宜得多。4.3 野值和多径学会对异常观测说不GNSS在城市环境下的观测值里经常混入异常。这种异常不像高斯噪声那样均匀分布而是表现为突然跳到几米外、或者持续一段时间的偏移。如果滤波器把这类异常当作正常观测去更新结果就是轨迹突然抽风。应对的关键不是调小R而是主动检测并拒绝异常观测。最常用的方法是基于新息的卡方检验。新息就是实际观测和预测观测之差理论上服从均值为零的高斯分布其协方差可以推导为S H * P * Hᵀ R。通过计算新息马氏距离d z⁻¹ * S⁻¹ * d再和卡方分布的分位数比较。对三维位置观测自由度是399%置信水平的阈值大约是11.34六维观测则取约16.81。超过阈值的观测直接判定为异常用一个极大的R值替代正常R相当于让这次观测在更新中失去作用。不过阈值不能设得太死。设得太严正常观测也会被频繁拒绝导致滤波器“饿死”设得太松异常观测又会混进来。我的习惯是在实际路测中统计新息分布把阈值设为理论值和实测值的折中。城市峡谷里宁可保守一点错的数据比没有数据更伤。4.4 Q和R的调参经验看着轨迹调参数是不行的滤波器能不能收敛、能不能稳住很大程度上取决于Q和R怎么取。但这组参数没有标准答案只能根据实际传感器特性和运行环境摸索。一个好的出发点是Q尽量依据IMU器件的Allan方差等指标来设R依据GNSS在开阔天空下的实测定位精度来设比如水平1m、垂直2m。调参的正确顺序应该是先固定R只调Q。观察GNSS可用时的整体位置误差和速度误差观察GNSS失锁后系统位置漂移速率。如果滤波器在失锁后很快漂走说明Q偏小模型过于自信你对惯导的信任放得太多如果轨迹被GNSS噪声拖得毛糙、高架桥下还跟着飘说明R偏小滤波器对观测的信任过度。比看轨迹更有说服力的是看新息序列。正常工作的新息应该是均值接近零、方差与S的预测值相近的白噪声。如果新息明显存在趋势性说明状态模型有系统误差比如加速度计零偏没建模好如果新息均值不为零但方差正常大概率是观测有固定偏差比如杆臂或者时间同步没对齐。通过新息序列判断滤波健康状态比“看着轨迹还不错”可靠得多能帮你把问题定位到具体环节。5. 从“能跑”到“好用”卡尔曼滤波的优化应用方向5.1 自适应滤波让滤波器自己学会调整噪声水平固定Q和R的组合导航在“信号环境恒定”的场景里没问题但真实环境往往是开阔地和城市峡谷交替出现。开阔地里GNSS精度高R应该小峡谷里多径严重R应该大。用固定R的滤波器要么在开阔地平滑不到位要么在峡谷里被异常观测带偏。自适应滤波的思路是让滤波器根据新息的现时统计情况动态修正R或Q。常见做法是维护一个滑动窗口统计一段时间内新息的实际协方差再和理论协方差S比较。如果实际协方差明显大于理论值说明观测噪声被低估了就适当放大R如果实际值接近理论值保持现有R不动。还可以配合渐消因子在检测到模型异常时主动放大P或者Q让滤波器重新“醒过来”恢复对观测的跟踪能力。自适应方法容易在强多径时被异常观测牵着走所以必须有保护机制。我通常给R设置一个下限通常是标称观测噪声的0.1倍左右防止滤波器自信过度、完全不跟着GNSS走了。5.2 多传感器接入里程计、气压计、视觉和激光怎么融进来卡尔曼滤波这个框架的好处是多一个传感器只是多一组观测方程无需撼动整体架构。实际项目里公里计的引入价值最大。轮式或履带式设备安装里程计后轮速乘以标度因子即为前进方向的速度观测还可以加转弯约束比如低速无人车通常假设后轴侧向速度接近零。这类非完整约束在地下停车场、隧道这些GNSS完全失效的场合可以把有效定位时间从几十秒拉长到几分钟非常实用。气压计对应的是垂直通道。GNSS垂直观测噪声一般比水平差2~3倍惯导的垂直通道又天然不稳所以纯GNSS/INS系统的高度波动比较大。气压计的短时差分可以当作高度变化率的观测比直接用绝对高度更稳因为气压计容易受天气和温度影响产生零点漂移但短时差分能把这个漂移大部分消掉。视觉或者激光接入时本质上分两类如果提供的是结构化地图匹配的绝对定位那和GNSS松耦合的高度观测没有区别直接作为位置观测如果提供的是视觉里程计那样的相对位姿则需要把它作为增量式约束加入通常会在状态模型里增加传感器相对主IMU的外参参数同时考虑延迟对齐。这一步经常成为最终精度瓶颈因为外参标定误差会导致两套传感器在大动态转弯或颠簸时互相矛盾前端看起来都“挺准”融合结果却在抖动。5.3 “三维立体”到底怎么落地垂直通道和高程稳定标题里强调“三维立体组合导航”很多人会以为只是把z坐标加进来算均值。实际上三维空间里的核心难点在垂直通道。卫星测量高程的能力受卫星几何限制GDOP恶化时垂直误差往往远大于水平误差惯性导航的垂直通道则更加糟糕——高度误差会通过重力补偿误差反馈进去形成不稳定的正反馈回路。处理办法是层次化接入垂直辅助。第一层是GNSS的高程观测基础框架必须有第二层是气压计的高度变化率辅助把绝对高度的缓慢漂移约束住第三层再进阶一点如果载体在地面行驶可以引入路面约束把高度锁定在一个平滑变化的通道附近。对飞行器来说气压计加GNSS的组合几乎是标准配置对无人车来说路面约束或者轮式里程计的垂向约束有很大帮助。这块做好的直观收益是坡道检测更加平滑、上立交桥和地面路的切换不抖动、悬停无人机不会莫名其妙往下掉高度。三维立体导航的价值正是在这些不太显眼但实际每天都在发生的工况里体现出来的。5.4 性能评估建议别只看轨迹贴不贴地图跑通以后最容易犯的错是觉得“轨迹贴在地图上、没有明显飞点”就是系统做好了。实际上定性地看看不出系统误差、延迟和协方差是否真实可信。我比较习惯的一套评估流程是这样的先在开阔处沿直线往返几个来回用RTK或者高精度后处理参考轨迹做基准再进入高架桥下、树荫、立交桥等遮挡环境多跑几趟记录GNSS原始观测。统计指标上至少要算水平位置误差RMS、95分位点误差垂直方向单独统计速度误差和姿态误差也要抽样统计尤其要关注转弯和加减速阶段。还要专门测试GNSS失锁后的表现选定一个区域人为切断GNSS观测30秒、60秒、120秒看混合导航的位置相对真实轨迹的漂移量这个数值直接反映惯导质量和滤波模型是否靠谱。在调试工具上务必保留原始GNSS观测数据、IMU原始数据和IMU的数据用统一时间戳回放对排查问题非常关键。做调参对比时把同一段数据的滤波结果拉出来对比分左右屏观察位置误差曲线和新息序列变化比单纯看地图直观得多。6. 几个我很少在文档里写的小经验做这套系统几年踩过不少坑有几个细节很少在技术文档里写明但对实际效果影响很大。一是滤波器的鲁棒性往往比峰值精度更值钱。很多团队花大力气把开阔环境下的误差从10cm压到5cm结果一到城市峡谷就散架。工程上一个可以接受的目标不是“每种环境下都最优”而是“95%的时间误差小于1米任何环境下都不会飞掉”。把异常观测保护、时间同步、姿态初值做好优先保障底线再去追求峰值。二是参数不是越多越好。状态扩维确实能提高理论精度但维度越高可观测性问题和调参难度也越大。很多场景里15维状态、松耦合、配合良好的野值处理已经能交出很漂亮的答卷。真正让系统变好用的是对时间同步和坐标系的严谨不是堆算法。三是别忽略仿真环境的对照。我在真实跑路之前习惯用一段录制的原始IMU和GNSS数据做离线回放把每次修改参数后的结果用同一组数据对比。这样既能快速回归测试也能避免在真车上反复试错消耗大量时间。把一段“问题数据”留作回归用例排查新问题时会非常安心。组合导航是一个把多个不完美信号拧成一条可信轨迹的工程卡尔曼滤波给了框架剩下的功夫都在理解和处理各种误差源上。希望这篇总结能帮你少走几个弯路尤其是那些我当年调了很久才发现的问题。