1. 为什么非要组合——INS和GPS各自的死穴干惯导这行久了我经常给新人打一个比方INS是个学习能力很强但方向感极差的新人能精准感知自己每一步迈了多大、转了哪个角度却不知道自己到底在哪条街上GPS是个认路的老司机但信号一进隧道就两眼一抹黑偶尔还会被高架桥反射的信号带偏到隔壁车道。组合导航要做的就是把这两者绑在一起让老司机带着新人认路让新人用自己连续的运动感知帮老司机度过没有信号的盲区。这套逻辑听起来简单真正落地的时候你得先把两个系统的误差特性吃透才知道组合的时候该信谁多一点。这是整个INS/GPS组合导航里最底层的判断依据也是我见过最多人一上来就调滤波器参数、结果越调越飘的根本原因。1.1 GPS误差源拆解不是信号不好是误差太多GPS定位的原始观测量是伪距和载波相位。伪距的测量原理是卫星发信号的时间戳和接收机本地时间的差乘以光速算一个距离出来但这里面叠加的误差源远远超出很多入门者的想象。星历误差是卫星播发的轨道参数和卫星真实位置之间的偏差通常等效到几米量级SA政策取消后这个误差小了不少但精密单点定位PPP仍然是靠事后精密星历才能把轨道误差压到厘米级可见星历误差并没有消失只是被削弱了。电离层延迟是对流层以上带电粒子对信号传播速度的影响等效距离误差在白天最严重的时候能到十几米甚至几十米双频接收机可以用两个频率上的延迟差异把一阶项消掉单频机只能靠Klobuchar模型粗略修正。对流层延迟则分干分量和湿分量干分量占90%且可以用模型算得很准湿分量是天顶湿延迟变化快、难建模是精密定位里最难啃的骨头之一。多径效应则是信号经过建筑物、地面反射后和直射信号叠加产生几米到几十米的鬼影误差而且完全随机没有任何模型能提前预测它。更麻烦的是GPS误差里有一类系统性的共同误差——同一颗卫星的信号到达同一区域内的不同接收机时电离层、对流层、星历误差是高度相关的。这就是RTK和差分GPS能大幅消掉公共误差的理论基础。组合导航里如果你接了RTK信号那么GPS量测的噪声特性会和非差分模式下完全不一样滤波器的量测噪声矩阵也要跟着改不然卡尔曼滤波会把明明很准的位置当成带噪声的观测来平滑反而把精度拖下去。这一点我在后文调参部分会专门讲。1.2 INS的漂移特性数学上完美物理上拉胯INS的核心是陀螺仪和加速度计。陀螺测角速度加速度计测比力然后通过姿态解算把比力投影到导航坐标系积分一次得到速度再积分一次得到位置。这个过程完全自主不看天不看地不依赖任何外部信号所以抗干扰、高动态、全环境覆盖是它天生的优势。但问题就出在积分这两个字上。你对一个带偏差的量做积分误差会随时间线性增长再做一次积分误差会随时间平方增长。如果陀螺零偏没有完全补偿掉比如残余零偏是0.01度/小时姿态角误差就会像一个缓慢累积的倾角让加速度计测量的重力分量混进水平加速度里。这个混入的重力误差再积分位置误差甚至会随时间三次方增长。这就是为什么纯惯导跑几分钟还能看跑半小时之后在图上就是一条完全离谱的曲线。更麻烦的是INS的误差是低频主导的漂移曲线平滑但趋势性强。GPS的误差是高频主导的单点定位结果噪声大但长期均值稳定。这两种误差在频域上几乎是互补的——INS在短时间内的相对位置变化非常准GPS在长时间内的绝对位置非常稳。组合导航本质上是一个频域互补滤波的思想通过卡尔曼滤波器把两个传感器的优势段拼起来用GPS的低频信息去估计并修正INS的误差状态用INS的高频响应去填充GPS被遮挡、被干扰时段的定位盲区。1.3 组合不是简单相加而是用互斥缺点换双重优点很多人看到这里会问那我直接把GPS位置和INS位置做个加权平均不就行了不行。加权平均只能让两个噪声互相折中但无法解决INS漂移的根源——你根本不知道INS当前的三维姿态误差是多少、加速度计零偏是多少、陀螺零漂是多少这些误差状态不估计出来光在输出端做融合等于只治标不治本。卡尔曼滤波在这里做的事情是用一个误差状态作为系统状态——包括位置误差、速度误差、姿态误差、陀螺零漂、加速度计零偏等——然后用GPS量测和INS递推结果之间的差异作为观测反推这些误差状态的最优估计再把它们反馈回INS的捷联解算结果里修正姿态矩阵、位置和速度。这个过程每做一次INS就被校准一次它的漂移被持续压制在很小的范围内而GPS被遮挡时系统仍然能用被修正过的INS做外推短时间内的精度不会像纯惯导那样迅速发散。这其实就是我给你这篇文章定下的核心逻辑组合导航不是两个系统结果的加法而是用GPS的绝对信息锁定INS误差状态的滤波器。理解了这一点后面所有的状态方程、量测方程、调参流程都是在一个框架下展开。2. 松组合、紧组合、深组合——三种架构的差异与选型逻辑我接触的很多项目里组合导航被当成一个黑盒来用买来模块直接输出位置从来不关心里面是松还是紧、是位置域还是伪距域组合。等到精度不达标、或者需要在高动态场景下排查问题的时候才发现自己连系统架构都没搞清楚根本无从下手。这一章我把三种架构的建模逻辑讲清楚顺便给出选型建议。松组合Loosely Coupled是最常见的架构。它把GPS接收机当作一个完整的位置速度传感器输出经度、纬度、高度和速度和INS递推出来的位置速度做差作为卡尔曼滤波的量测更新。优点是实现简单、GPS接收机可以完全独立工作、接口标准市面上的消费级GPS模块和组合导航板卡大多支持这种模式。缺点是如果GPS接收机内部自己也在做滤波比如PVT解算里的最小二乘或卡尔曼平滑那么输出的位置速度噪声就不再是白噪声而是时间上相关的有色噪声。这违反了卡尔曼滤波对量测噪声的假设会导致滤波器过度自信协方差矩阵越滚越小实际误差却并没有那么小。用工程黑话叫滤波发散前的虚假收敛。紧组合Tightly Coupled则把GPS接收机的原始测量值——伪距、伪距率——直接引入组合滤波器的量测方程。INS解算的位置和卫星星历计算出预测伪距和GPS实测伪距做差同理伪距率。这样做的第一个好处是即便可见卫星数少于4颗GPS接收机无法输出独立的定位结果紧组合依然能用单颗卫星的伪距约束INS实现部分的量测更新这对城市峡谷、山谷、树林遮挡环境是巨大的优势。第二个好处是量测噪声更接近白噪声假设滤波器更诚实。代价是系统复杂度明显上升——你需要知道每颗卫星的星历、钟差参数、伪距率计算方式还要处理GPS接收机内部环路延迟和INS数据帧的时间对准。这是目前车载、无人机、测绘产品里主流的高性能方案。深组合Deeply Coupled / Ultra-Tightly Coupled则是把INS的信息一直深入到GPS接收机的跟踪环路层面。GPS接收机的码环和载波环不再是独立跟踪而是由INS预测的多普勒频移和码相位来辅助形成一个矢量跟踪环路。每个通道的误差状态都进入同一个卡尔曼滤波器滤波器的状态向量不仅包含导航误差还包含GPS环路误差参数。深组合的优势是在高动态、低信噪比环境下环路不容易失锁信号被遮挡后重新捕获的速度也快得多。缺点是实现难度极高基本需要自己设计GPS基带市面上绝大多数组合导航产品做的其实是紧耦合辅助环路并不是理论意义上的完全深组合。如果你想在导弹、高速无人机这类极端场景里做建议先老老实实把松组合、紧组合的工程问题吃透再考虑深组合。3. 组合导航的数学内核——滤波器的状态方程和量测方程推导思路这一章是文章的重头戏。我不打算把完整的公式推导抄一遍——教材上全都有我重点讲推导过程中那些书上不写但实际建模时必须想清楚的点还有离散化、噪声矩阵、量测更新的具体处理方式。3.1 15维状态向量的物理含义我们先从最经典的15维状态向量开始。它由5组三维分量构成位置误差δP纬度、经度、高度误差速度误差δV东北天方向姿态误差δΨ平台失准角陀螺零漂ε东北天方向的等效陀螺漂移加速度计零偏∇东北天方向的等效加速度计偏置。为什么要用误差而不是绝对量作为状态这是组合导航里很关键的一个设计选择。如果直接用位置、速度、姿态作为状态系统的动态范围非常大数值上容易出现病态更重要的是INS解算的物理模型是高度非线性的而卡尔曼滤波要求线性模型。但INS的误差在一个小范围内近似线性误差传播方程可以很好地线性化。所以组合导航几乎都用误差状态作为滤波器的状态向量INS本身负责相对精确的递推滤波器负责估计并修正这个递推过程里的误差状态。这相当于把一个大尺度非线性问题拆成一个小尺度线性问题数学上漂亮得多工程上数值稳定性也好得多。3.2 状态转移矩阵的离散化处理连续时间下的状态方程是δẋ F·δx G·w其中F是系统矩阵它包含了惯性导航误差传播的物理关系。比如速度误差方程里有地球自转角速度、哥里奥利项、重力异常项等姿态误差方程里有陀螺噪声耦合项和加速度计零偏耦合项。工程实现的时候你要把这个连续系统离散成δx(k1) Φ·δx(k) w(k)其中的状态转移矩阵Φ通常用泰勒展开近似取到二阶项Φ ≈ I F·Δt (F·Δt)²/2这里有个实际经验在IMU输出频率为100Hz到200Hz的典型场景下Δt通常在0.005到0.01秒之间取到二阶项已经足够精确再高阶的项对精度贡献不大反而增加计算量。很多老工程师在码里只写了Φ I F·Δt跑中低精度MEMS惯导时室内静态精度差别不大但一旦上光纤陀螺或者激光惯导、或者载体本身有强振动时二阶项的影响就会暴露出来协方差矩阵的一致性会变差。系统噪声协方差矩阵Q的离散化同样容易被低估。你用的陀螺噪声功率谱密度、加速度计噪声功率谱密度必须根据器件手册里的角度随机游走ARW和速度随机游走VRW参数来换算而不是随手填一个能跑通的数字。Q矩阵设小了滤波器对新量测的信任度过高容易震荡设大了量测更新权重变低组合后的轨迹偏向INS的漂移特性。我在调参阶段通常先把Q按照器件手册的理论值放进去然后用静止状态下的滤波协方差收敛曲线来校验一致性——如果协方差收敛得过快且最终值异常小说明Q偏小如果速度误差协方差一直压不下来说明Q偏大。3.3 量测方程的建立与噪声矩阵的工程整定松组合的量测方程本身不复杂量测向量Z [P_GPS - P_INS; V_GPS - V_INS]对应的观测矩阵H [I₃ 0 0 0 0; 0 I₃ 0 0 0]也就是量测值直接对应位置误差状态和速度误差状态。但工程里麻烦的是量测噪声矩阵R的取值。先说位置量测。如果你的GPS是单频单点定位水平位置误差典型值在3到5米1σ高度误差7到10米R矩阵的对角元就可以按这个量级取。如果是RTK模式厘米级精度R要缩小三个数量级以上。这里有个容易踩的坑很多人直接把R设成一个固定值但GPS在城市环境下的误差其实在随时变化。多径严重时位置误差可能到几十米此时固定小R值会让滤波器把巨大的错误量测当成高精度观测来强行拉偏轨迹造成俗称的跳点效应。合理的做法是用GPS接收机输出的定位精度因子PDOP或HDOP和位置标准差sAcc来在线调节R矩阵比如R R_base × (PDOP/PDOP_nominal)²。我实测过在楼宇密集的街道上这套自适应策略能把跳点次数减少大约一半且轨迹平滑度明显提高。速度量测相对干净GPS测速用的是多普勒频移而不是位置差分噪声水平通常在0.03到0.1米/秒左右比位置量测可靠得多。所以在GPS位置误差评级比较大的时候我倾向于让滤波器更快地信任速度量测利用速度约束和INS姿态的耦合关系来间接修正位置漂移。这算是一个不太被写进文档的实用技巧。紧组合的量测方程就要涉及伪距和伪距率了。伪距量测是POS行列ρ_meas |r_sat - r_rec| c·δt_u - c·δt_sat ε_iono ε_trop ε_multipath。INS这边用估计出的接收机位置和卫星星历计算预测伪距ρ_pred |r_sat - r_ins_pred| c·δt_u_pred。两者的差就是滤波器观测量。这里最关键的一点是接收机钟差δt_u必须放进滤波器的状态里否则伪距误差会全部被钟差吸收导致位置修正失真。所以紧组合的典型状态向量是16维而不是15维多出来的一个维度就是接收机钟差等效距离。4. 工程落地最容易翻车的环节——标定、时间同步、杆臂补偿很多人在MATLAB里仿真组合导航跑出来精度漂亮得不得一上真机就开始飘。问题基本不在滤波器算法本身而在工程细节。这一章我把自己在多次实际项目里踩过的坑、总结的经验讲透。4.1 时间同步PPS秒脉冲和IMU数据帧的握手协议GPS接收机一般会输出一个PPSPulse Per Second秒脉冲作为精确的时间基准。IMU数据则有自己的采样频率比如200Hz。组合导航算法在融合时必须知道每一帧IMU数据对应的精确时间戳和每一帧GPS解算对应的精确时间戳否则GPS位置和INS位置之间会存在几毫秒到几十毫秒的时间错位。时间错位在高动态场景下的影响非常显著。假设载体以50米/秒的速度运动10毫秒的时间错位意味着位置量测和INS递推之间差了0.5米。更严重的是这个偏差是速度方向的函数——方向一变误差方向跟着变滤波器会看到一个无法被状态向量解释的随机跳动量测结果就是协方差收缩不稳定、轨迹出现锯齿。工程上常用的做法分三层。第一层是硬件方案IMU和GPS接收机都通过同一个PPS信号做硬件触发同步IMU帧号和PPS时刻对齐GRCGNSS-INS时间偏差在硬件层面就压到微秒级。第二层是软件补偿如果硬件上做不到硬触发可以用GPS的PPS和IMU帧率做时间戳折算估计出每一帧IMU相对GPS绝对时间的偏差在滤波时间更新中做内插外推补偿。第三层是状态扩增把时间偏差本身作为一个状态放进滤波器估计。三层方案按精度需求依次递进消费级模块用第二层就够了但做测绘级产品建议直接上第一层。4.2 杆臂效应与天线相位中心偏差杆臂Lever Arm效应是另一个经常被忽略却非常致命的误差来源。GPS天线安装在车顶IMU安装在车辆重心附近两者之间有一个固定的空间矢量。GPS测量的是天线相位中心的位置和速度INS测量的是IMU中心的位置和速度。如果不补偿这个臂长GPS位置量测和INS预测位置之间就会有一个固定偏差。在静态或低速场景下这个固定偏差还能看成一个常数偏置影响有限。但在车辆转弯、飞机做横滚机动时杆臂会通过姿态变化产生显著的线速度误差。举个例子天线在IMU前方1米处车辆以20度/秒的航向角速度转弯天线和IMU之间的速度差可以到0.35米/秒左右。如果不补偿这个速度差会被滤波器当成测量噪声让姿态估计产生偏差进而影响整个导航解。杆臂补偿的正确做法是在GPS速度量测进入滤波器之前把天线速度换算到IMU中心V_imu V_antenna - ω_nb × L其中ω_nb是载体系相对导航系的角速度L是杆臂向量在载体系中的投影。这个公式看着简单但杆臂向量必须精确测量——用卷尺量天线的安装位置时记得把天线相位中心到安装面的距离也算进去很多天线手册里标的有Phase Center Offset参数装完之后最好做一个简单的静态角点标定把杆臂三维分量精确求出来。我的经验是不标定杆臂直接就跑动态测试的位置误差在转弯工况下非常容易被误判为GPS飘了折腾半天发现是自己没做基础工作。4.3 安装标定与内参校准不做等着飘INS/GPS组合导航里标定这个词至少涵盖三层含义。第一层是IMU自身的零偏、标度因数、安装误差、正交误差在出厂前或使用前用转台标定。对于消费级MEMS器件出厂离散性很大最好用六位置法做一次加速度计标定用角速率法做一次陀螺标定。第二层是IMU和GPS天线之间的杆臂和失准角标定——失准角是指IMU坐标系的三个轴和载体坐标系的三个轴之间存在的小角度错位这个错位会导致速度、姿态解算和GPS量测之间存在投影误差。常规做法是让载体做S型路线或8字路线行驶几分钟然后用估计器把这几个角度估计出来。第三层是在线标定即滤波器运行过程中持续估计陀螺零漂和加速度计零偏——这是15维状态向量里面后两个分量的职责。还有一个容易被忽视的点天线相位中心偏差PCO和天线相位中心变化PCV。高精度测绘天线出厂时会给一套PCO/PCV修正参数如果你的组合导航系统精度目标是厘米级甚至毫米级这些参数必须加载。消费级天线虽然没有正式标定数据但天线相位中心相对天线外壳通常有几个毫米的偏差对亚米级精度影响不大对RTK级别的精度就必须考虑了。5. 实测调试中的典型问题与排查思路组合导航没有一次跑通的。我几乎每次转场到新环境、新载体都会遇到新的问题。这一章我挑几个高频率的现场问题把排查思路写清楚。5.1 静止时轨迹缓慢漂移先查的不是滤波器是标定和噪声一个非常常见的现场现象设备放在桌上静止不动组合导航输出的位置还是缓慢地往一个方向飘速度也不归零。很多人第一反应是滤波参数没调好其实这个现象背后往往是陀螺零偏或加速度计零偏的残余太大。在静止条件下GPS位置是准的滤波器会持续估计并反馈修正误差状态如果几分钟内漂移还在持续大概率是初始零偏估计偏差过大、或者状态方程里的激励噪声Q设置不当导致滤波器对零偏的估计收敛得很慢。排查手段很简单先看滤波器估计的陀螺零偏和加速度计零偏曲线是否收敛。正常情况上电初期的零偏估计会在几十秒到几分钟内从初始值收敛到稳定值附近。如果曲线一直在缓慢爬升或抖动检查一下IMU原始数据的方差和温度漂移情况如果零偏估计收敛到一个明显很大的值比如MEMS陀螺零偏估计到0.1度/秒以上那可能是IMU数据本身没做温度补偿或者器件已经受了冲击。把器件手册的零偏重复性指标拿出来对比能帮你判断当前估计值是否合理。静止漂移的另一个原因可能是GPS天线收到了多径干扰——室内或窗口附近的多径反射会让GPS位置出现系统性偏移滤波器会误以为INS在漂移。把设备搬到一个开阔的楼顶做一次对照测试能快速区分是GPS问题还是INS问题。5.2 城市峡谷GPS丢星后重新锁定瞬间轨迹跳变在城市高楼间行驶GPS被遮挡、反射导致的跳星和重新捕获很常见。丢星期间INS纯惯性外推轨迹精度会随时间下降重新捕获之后如果GPS接收机输出的位置和INS外推位置相差很大滤波器在量测更新时可能会产生一个明显的轨迹跳变。解决这个问题有几个层面的手段。软件层面可以在组合滤波器中做一个量测有效性判断——当GPS位置和INS预测位置之间的新息Innovation超过了阈值比如按当前滤波协方差计算的3倍标准差时暂时不给这个量测过大的权重或者直接跳过这次更新。这个方法通常被称为新息卡方检测我在城市测试里验证过能有效抑制大多数跳点。硬件层面GPS接收机自身的稳健性也很关键支持多频多星座的接收机在高楼环境里的抗多径能力明显优于老式单频机出跳点的概率低很多。值得说明的是新息卡方检测的阈值设置要小心阈值太紧会把正常的GPS校正也挡在外面导致INS漂移越来越严重太松又起不到保护作用。我的经验是离线跑一遍数据统计不同环境下的新息分布再按3到5倍标准差设定阈值并在定位模质量指标比如GPS的sAcc做二次校验。5.3 欺骗干扰下的失效保护与基于一致性的检测思路组合导航行业最近这两年对欺骗干扰的讨论越来越多。GPS生成式欺骗干扰会播发和真实星座结构相似但位置被篡改的假信号接收机如果完全锁定在假信号上输出的GPS位置就是攻击者设定好的错误位置。组合导航的优势在于INS有短时间内的记忆能力不会立刻跟着假信号跳变但如果滤波器没有异常检测机制持续的被欺骗量测会逐渐把整个导航解拉过去。业界目前主流的防护策略之一叫多源一致性检测。大框架是把GPS位置、INS外推位置、以及可能的其他辅助源比如视觉里程计、轮速计、气压高度计放在一个一致性检验框架里任何单个信号源给出的信息和其余源相差过大时系统自动降权或者剔除这个信号源。更深一步的做法是把GPS原始测量值的质量指标比如载噪比、每颗星的伪距残差纳入监测如果所有通道的载噪比同时异常地高、伪距残差分布不符合正常噪声分布就要高度怀疑在遭受欺骗干扰。当然这一套做下来复杂度不小但如果在做的是自动驾驶或者无人系统这个保护机制值得认真考虑。我见过一些产品把基于新息卡方检测的故障检测和多源一致性监测结合起来做冗余投票效果会比单一策略可靠很多。6. 选型与调参中的几个实用心得最后分享几个贯穿全程、不太容易被文档收录的心得。关于器件选型我的建议是不要把IMU的指标孤立地看。陀螺零偏稳定性和角度随机游走这两个指标决定INS在GPS丢星后的外推精度加速度计零偏稳定性和速度随机游走决定系统对动态加速度的响应能力而GPS接收机支持不支持RTK、多频多星座、输出频率多高则决定量测域的可用性。两者要匹配。你用消费级MEMS配RTK厘米级精度精度上限基本上还是被IMU的姿态误差和杆臂误差卡住RTK的优势发挥不出来反过来你用光纤陀螺IMU去配单频GPS量测噪声太大组合出来的系统精度也不会好到哪里去。调参方面我强烈建议不要一上来就追求整套系统跑得最好而是先把滤波器改成分析模式——把量测更新暂时关闭或者把R设得极大让系统只在纯INS模式下跑看INS本身的漂移特性是否符合器件手册的水平。这一步能帮你把所有错误从算法层面和传感器层面分开。确认INS基本没问题后再逐步缩R观察组合后轨迹的变化。每一次只改一个参数记录效果再改下一个。我在团队里一直推行这个流程排查问题的效率比漫无目的地试参高得多。还有一个小技巧组合导航输出里一定要保留滤波器协方差矩阵的实时记录。很多时候轨迹看起来正常但协方差已经在悄悄涨大等你发现导航解飘了的时候数据已经跑完了。通过分析协方差变化和时间段的对应关系能很准确地定位是哪个量测出现了异常这是排查现场问题最有效的证据之一。 INS/GPS组合导航原理与工程实践：从误差分析到滤波器调参
1. 为什么非要组合——INS和GPS各自的死穴干惯导这行久了我经常给新人打一个比方INS是个学习能力很强但方向感极差的新人能精准感知自己每一步迈了多大、转了哪个角度却不知道自己到底在哪条街上GPS是个认路的老司机但信号一进隧道就两眼一抹黑偶尔还会被高架桥反射的信号带偏到隔壁车道。组合导航要做的就是把这两者绑在一起让老司机带着新人认路让新人用自己连续的运动感知帮老司机度过没有信号的盲区。这套逻辑听起来简单真正落地的时候你得先把两个系统的误差特性吃透才知道组合的时候该信谁多一点。这是整个INS/GPS组合导航里最底层的判断依据也是我见过最多人一上来就调滤波器参数、结果越调越飘的根本原因。1.1 GPS误差源拆解不是信号不好是误差太多GPS定位的原始观测量是伪距和载波相位。伪距的测量原理是卫星发信号的时间戳和接收机本地时间的差乘以光速算一个距离出来但这里面叠加的误差源远远超出很多入门者的想象。星历误差是卫星播发的轨道参数和卫星真实位置之间的偏差通常等效到几米量级SA政策取消后这个误差小了不少但精密单点定位PPP仍然是靠事后精密星历才能把轨道误差压到厘米级可见星历误差并没有消失只是被削弱了。电离层延迟是对流层以上带电粒子对信号传播速度的影响等效距离误差在白天最严重的时候能到十几米甚至几十米双频接收机可以用两个频率上的延迟差异把一阶项消掉单频机只能靠Klobuchar模型粗略修正。对流层延迟则分干分量和湿分量干分量占90%且可以用模型算得很准湿分量是天顶湿延迟变化快、难建模是精密定位里最难啃的骨头之一。多径效应则是信号经过建筑物、地面反射后和直射信号叠加产生几米到几十米的鬼影误差而且完全随机没有任何模型能提前预测它。更麻烦的是GPS误差里有一类系统性的共同误差——同一颗卫星的信号到达同一区域内的不同接收机时电离层、对流层、星历误差是高度相关的。这就是RTK和差分GPS能大幅消掉公共误差的理论基础。组合导航里如果你接了RTK信号那么GPS量测的噪声特性会和非差分模式下完全不一样滤波器的量测噪声矩阵也要跟着改不然卡尔曼滤波会把明明很准的位置当成带噪声的观测来平滑反而把精度拖下去。这一点我在后文调参部分会专门讲。1.2 INS的漂移特性数学上完美物理上拉胯INS的核心是陀螺仪和加速度计。陀螺测角速度加速度计测比力然后通过姿态解算把比力投影到导航坐标系积分一次得到速度再积分一次得到位置。这个过程完全自主不看天不看地不依赖任何外部信号所以抗干扰、高动态、全环境覆盖是它天生的优势。但问题就出在积分这两个字上。你对一个带偏差的量做积分误差会随时间线性增长再做一次积分误差会随时间平方增长。如果陀螺零偏没有完全补偿掉比如残余零偏是0.01度/小时姿态角误差就会像一个缓慢累积的倾角让加速度计测量的重力分量混进水平加速度里。这个混入的重力误差再积分位置误差甚至会随时间三次方增长。这就是为什么纯惯导跑几分钟还能看跑半小时之后在图上就是一条完全离谱的曲线。更麻烦的是INS的误差是低频主导的漂移曲线平滑但趋势性强。GPS的误差是高频主导的单点定位结果噪声大但长期均值稳定。这两种误差在频域上几乎是互补的——INS在短时间内的相对位置变化非常准GPS在长时间内的绝对位置非常稳。组合导航本质上是一个频域互补滤波的思想通过卡尔曼滤波器把两个传感器的优势段拼起来用GPS的低频信息去估计并修正INS的误差状态用INS的高频响应去填充GPS被遮挡、被干扰时段的定位盲区。1.3 组合不是简单相加而是用互斥缺点换双重优点很多人看到这里会问那我直接把GPS位置和INS位置做个加权平均不就行了不行。加权平均只能让两个噪声互相折中但无法解决INS漂移的根源——你根本不知道INS当前的三维姿态误差是多少、加速度计零偏是多少、陀螺零漂是多少这些误差状态不估计出来光在输出端做融合等于只治标不治本。卡尔曼滤波在这里做的事情是用一个误差状态作为系统状态——包括位置误差、速度误差、姿态误差、陀螺零漂、加速度计零偏等——然后用GPS量测和INS递推结果之间的差异作为观测反推这些误差状态的最优估计再把它们反馈回INS的捷联解算结果里修正姿态矩阵、位置和速度。这个过程每做一次INS就被校准一次它的漂移被持续压制在很小的范围内而GPS被遮挡时系统仍然能用被修正过的INS做外推短时间内的精度不会像纯惯导那样迅速发散。这其实就是我给你这篇文章定下的核心逻辑组合导航不是两个系统结果的加法而是用GPS的绝对信息锁定INS误差状态的滤波器。理解了这一点后面所有的状态方程、量测方程、调参流程都是在一个框架下展开。2. 松组合、紧组合、深组合——三种架构的差异与选型逻辑我接触的很多项目里组合导航被当成一个黑盒来用买来模块直接输出位置从来不关心里面是松还是紧、是位置域还是伪距域组合。等到精度不达标、或者需要在高动态场景下排查问题的时候才发现自己连系统架构都没搞清楚根本无从下手。这一章我把三种架构的建模逻辑讲清楚顺便给出选型建议。松组合Loosely Coupled是最常见的架构。它把GPS接收机当作一个完整的位置速度传感器输出经度、纬度、高度和速度和INS递推出来的位置速度做差作为卡尔曼滤波的量测更新。优点是实现简单、GPS接收机可以完全独立工作、接口标准市面上的消费级GPS模块和组合导航板卡大多支持这种模式。缺点是如果GPS接收机内部自己也在做滤波比如PVT解算里的最小二乘或卡尔曼平滑那么输出的位置速度噪声就不再是白噪声而是时间上相关的有色噪声。这违反了卡尔曼滤波对量测噪声的假设会导致滤波器过度自信协方差矩阵越滚越小实际误差却并没有那么小。用工程黑话叫滤波发散前的虚假收敛。紧组合Tightly Coupled则把GPS接收机的原始测量值——伪距、伪距率——直接引入组合滤波器的量测方程。INS解算的位置和卫星星历计算出预测伪距和GPS实测伪距做差同理伪距率。这样做的第一个好处是即便可见卫星数少于4颗GPS接收机无法输出独立的定位结果紧组合依然能用单颗卫星的伪距约束INS实现部分的量测更新这对城市峡谷、山谷、树林遮挡环境是巨大的优势。第二个好处是量测噪声更接近白噪声假设滤波器更诚实。代价是系统复杂度明显上升——你需要知道每颗卫星的星历、钟差参数、伪距率计算方式还要处理GPS接收机内部环路延迟和INS数据帧的时间对准。这是目前车载、无人机、测绘产品里主流的高性能方案。深组合Deeply Coupled / Ultra-Tightly Coupled则是把INS的信息一直深入到GPS接收机的跟踪环路层面。GPS接收机的码环和载波环不再是独立跟踪而是由INS预测的多普勒频移和码相位来辅助形成一个矢量跟踪环路。每个通道的误差状态都进入同一个卡尔曼滤波器滤波器的状态向量不仅包含导航误差还包含GPS环路误差参数。深组合的优势是在高动态、低信噪比环境下环路不容易失锁信号被遮挡后重新捕获的速度也快得多。缺点是实现难度极高基本需要自己设计GPS基带市面上绝大多数组合导航产品做的其实是紧耦合辅助环路并不是理论意义上的完全深组合。如果你想在导弹、高速无人机这类极端场景里做建议先老老实实把松组合、紧组合的工程问题吃透再考虑深组合。3. 组合导航的数学内核——滤波器的状态方程和量测方程推导思路这一章是文章的重头戏。我不打算把完整的公式推导抄一遍——教材上全都有我重点讲推导过程中那些书上不写但实际建模时必须想清楚的点还有离散化、噪声矩阵、量测更新的具体处理方式。3.1 15维状态向量的物理含义我们先从最经典的15维状态向量开始。它由5组三维分量构成位置误差δP纬度、经度、高度误差速度误差δV东北天方向姿态误差δΨ平台失准角陀螺零漂ε东北天方向的等效陀螺漂移加速度计零偏∇东北天方向的等效加速度计偏置。为什么要用误差而不是绝对量作为状态这是组合导航里很关键的一个设计选择。如果直接用位置、速度、姿态作为状态系统的动态范围非常大数值上容易出现病态更重要的是INS解算的物理模型是高度非线性的而卡尔曼滤波要求线性模型。但INS的误差在一个小范围内近似线性误差传播方程可以很好地线性化。所以组合导航几乎都用误差状态作为滤波器的状态向量INS本身负责相对精确的递推滤波器负责估计并修正这个递推过程里的误差状态。这相当于把一个大尺度非线性问题拆成一个小尺度线性问题数学上漂亮得多工程上数值稳定性也好得多。3.2 状态转移矩阵的离散化处理连续时间下的状态方程是δẋ F·δx G·w其中F是系统矩阵它包含了惯性导航误差传播的物理关系。比如速度误差方程里有地球自转角速度、哥里奥利项、重力异常项等姿态误差方程里有陀螺噪声耦合项和加速度计零偏耦合项。工程实现的时候你要把这个连续系统离散成δx(k1) Φ·δx(k) w(k)其中的状态转移矩阵Φ通常用泰勒展开近似取到二阶项Φ ≈ I F·Δt (F·Δt)²/2这里有个实际经验在IMU输出频率为100Hz到200Hz的典型场景下Δt通常在0.005到0.01秒之间取到二阶项已经足够精确再高阶的项对精度贡献不大反而增加计算量。很多老工程师在码里只写了Φ I F·Δt跑中低精度MEMS惯导时室内静态精度差别不大但一旦上光纤陀螺或者激光惯导、或者载体本身有强振动时二阶项的影响就会暴露出来协方差矩阵的一致性会变差。系统噪声协方差矩阵Q的离散化同样容易被低估。你用的陀螺噪声功率谱密度、加速度计噪声功率谱密度必须根据器件手册里的角度随机游走ARW和速度随机游走VRW参数来换算而不是随手填一个能跑通的数字。Q矩阵设小了滤波器对新量测的信任度过高容易震荡设大了量测更新权重变低组合后的轨迹偏向INS的漂移特性。我在调参阶段通常先把Q按照器件手册的理论值放进去然后用静止状态下的滤波协方差收敛曲线来校验一致性——如果协方差收敛得过快且最终值异常小说明Q偏小如果速度误差协方差一直压不下来说明Q偏大。3.3 量测方程的建立与噪声矩阵的工程整定松组合的量测方程本身不复杂量测向量Z [P_GPS - P_INS; V_GPS - V_INS]对应的观测矩阵H [I₃ 0 0 0 0; 0 I₃ 0 0 0]也就是量测值直接对应位置误差状态和速度误差状态。但工程里麻烦的是量测噪声矩阵R的取值。先说位置量测。如果你的GPS是单频单点定位水平位置误差典型值在3到5米1σ高度误差7到10米R矩阵的对角元就可以按这个量级取。如果是RTK模式厘米级精度R要缩小三个数量级以上。这里有个容易踩的坑很多人直接把R设成一个固定值但GPS在城市环境下的误差其实在随时变化。多径严重时位置误差可能到几十米此时固定小R值会让滤波器把巨大的错误量测当成高精度观测来强行拉偏轨迹造成俗称的跳点效应。合理的做法是用GPS接收机输出的定位精度因子PDOP或HDOP和位置标准差sAcc来在线调节R矩阵比如R R_base × (PDOP/PDOP_nominal)²。我实测过在楼宇密集的街道上这套自适应策略能把跳点次数减少大约一半且轨迹平滑度明显提高。速度量测相对干净GPS测速用的是多普勒频移而不是位置差分噪声水平通常在0.03到0.1米/秒左右比位置量测可靠得多。所以在GPS位置误差评级比较大的时候我倾向于让滤波器更快地信任速度量测利用速度约束和INS姿态的耦合关系来间接修正位置漂移。这算是一个不太被写进文档的实用技巧。紧组合的量测方程就要涉及伪距和伪距率了。伪距量测是POS行列ρ_meas |r_sat - r_rec| c·δt_u - c·δt_sat ε_iono ε_trop ε_multipath。INS这边用估计出的接收机位置和卫星星历计算预测伪距ρ_pred |r_sat - r_ins_pred| c·δt_u_pred。两者的差就是滤波器观测量。这里最关键的一点是接收机钟差δt_u必须放进滤波器的状态里否则伪距误差会全部被钟差吸收导致位置修正失真。所以紧组合的典型状态向量是16维而不是15维多出来的一个维度就是接收机钟差等效距离。4. 工程落地最容易翻车的环节——标定、时间同步、杆臂补偿很多人在MATLAB里仿真组合导航跑出来精度漂亮得不得一上真机就开始飘。问题基本不在滤波器算法本身而在工程细节。这一章我把自己在多次实际项目里踩过的坑、总结的经验讲透。4.1 时间同步PPS秒脉冲和IMU数据帧的握手协议GPS接收机一般会输出一个PPSPulse Per Second秒脉冲作为精确的时间基准。IMU数据则有自己的采样频率比如200Hz。组合导航算法在融合时必须知道每一帧IMU数据对应的精确时间戳和每一帧GPS解算对应的精确时间戳否则GPS位置和INS位置之间会存在几毫秒到几十毫秒的时间错位。时间错位在高动态场景下的影响非常显著。假设载体以50米/秒的速度运动10毫秒的时间错位意味着位置量测和INS递推之间差了0.5米。更严重的是这个偏差是速度方向的函数——方向一变误差方向跟着变滤波器会看到一个无法被状态向量解释的随机跳动量测结果就是协方差收缩不稳定、轨迹出现锯齿。工程上常用的做法分三层。第一层是硬件方案IMU和GPS接收机都通过同一个PPS信号做硬件触发同步IMU帧号和PPS时刻对齐GRCGNSS-INS时间偏差在硬件层面就压到微秒级。第二层是软件补偿如果硬件上做不到硬触发可以用GPS的PPS和IMU帧率做时间戳折算估计出每一帧IMU相对GPS绝对时间的偏差在滤波时间更新中做内插外推补偿。第三层是状态扩增把时间偏差本身作为一个状态放进滤波器估计。三层方案按精度需求依次递进消费级模块用第二层就够了但做测绘级产品建议直接上第一层。4.2 杆臂效应与天线相位中心偏差杆臂Lever Arm效应是另一个经常被忽略却非常致命的误差来源。GPS天线安装在车顶IMU安装在车辆重心附近两者之间有一个固定的空间矢量。GPS测量的是天线相位中心的位置和速度INS测量的是IMU中心的位置和速度。如果不补偿这个臂长GPS位置量测和INS预测位置之间就会有一个固定偏差。在静态或低速场景下这个固定偏差还能看成一个常数偏置影响有限。但在车辆转弯、飞机做横滚机动时杆臂会通过姿态变化产生显著的线速度误差。举个例子天线在IMU前方1米处车辆以20度/秒的航向角速度转弯天线和IMU之间的速度差可以到0.35米/秒左右。如果不补偿这个速度差会被滤波器当成测量噪声让姿态估计产生偏差进而影响整个导航解。杆臂补偿的正确做法是在GPS速度量测进入滤波器之前把天线速度换算到IMU中心V_imu V_antenna - ω_nb × L其中ω_nb是载体系相对导航系的角速度L是杆臂向量在载体系中的投影。这个公式看着简单但杆臂向量必须精确测量——用卷尺量天线的安装位置时记得把天线相位中心到安装面的距离也算进去很多天线手册里标的有Phase Center Offset参数装完之后最好做一个简单的静态角点标定把杆臂三维分量精确求出来。我的经验是不标定杆臂直接就跑动态测试的位置误差在转弯工况下非常容易被误判为GPS飘了折腾半天发现是自己没做基础工作。4.3 安装标定与内参校准不做等着飘INS/GPS组合导航里标定这个词至少涵盖三层含义。第一层是IMU自身的零偏、标度因数、安装误差、正交误差在出厂前或使用前用转台标定。对于消费级MEMS器件出厂离散性很大最好用六位置法做一次加速度计标定用角速率法做一次陀螺标定。第二层是IMU和GPS天线之间的杆臂和失准角标定——失准角是指IMU坐标系的三个轴和载体坐标系的三个轴之间存在的小角度错位这个错位会导致速度、姿态解算和GPS量测之间存在投影误差。常规做法是让载体做S型路线或8字路线行驶几分钟然后用估计器把这几个角度估计出来。第三层是在线标定即滤波器运行过程中持续估计陀螺零漂和加速度计零偏——这是15维状态向量里面后两个分量的职责。还有一个容易被忽视的点天线相位中心偏差PCO和天线相位中心变化PCV。高精度测绘天线出厂时会给一套PCO/PCV修正参数如果你的组合导航系统精度目标是厘米级甚至毫米级这些参数必须加载。消费级天线虽然没有正式标定数据但天线相位中心相对天线外壳通常有几个毫米的偏差对亚米级精度影响不大对RTK级别的精度就必须考虑了。5. 实测调试中的典型问题与排查思路组合导航没有一次跑通的。我几乎每次转场到新环境、新载体都会遇到新的问题。这一章我挑几个高频率的现场问题把排查思路写清楚。5.1 静止时轨迹缓慢漂移先查的不是滤波器是标定和噪声一个非常常见的现场现象设备放在桌上静止不动组合导航输出的位置还是缓慢地往一个方向飘速度也不归零。很多人第一反应是滤波参数没调好其实这个现象背后往往是陀螺零偏或加速度计零偏的残余太大。在静止条件下GPS位置是准的滤波器会持续估计并反馈修正误差状态如果几分钟内漂移还在持续大概率是初始零偏估计偏差过大、或者状态方程里的激励噪声Q设置不当导致滤波器对零偏的估计收敛得很慢。排查手段很简单先看滤波器估计的陀螺零偏和加速度计零偏曲线是否收敛。正常情况上电初期的零偏估计会在几十秒到几分钟内从初始值收敛到稳定值附近。如果曲线一直在缓慢爬升或抖动检查一下IMU原始数据的方差和温度漂移情况如果零偏估计收敛到一个明显很大的值比如MEMS陀螺零偏估计到0.1度/秒以上那可能是IMU数据本身没做温度补偿或者器件已经受了冲击。把器件手册的零偏重复性指标拿出来对比能帮你判断当前估计值是否合理。静止漂移的另一个原因可能是GPS天线收到了多径干扰——室内或窗口附近的多径反射会让GPS位置出现系统性偏移滤波器会误以为INS在漂移。把设备搬到一个开阔的楼顶做一次对照测试能快速区分是GPS问题还是INS问题。5.2 城市峡谷GPS丢星后重新锁定瞬间轨迹跳变在城市高楼间行驶GPS被遮挡、反射导致的跳星和重新捕获很常见。丢星期间INS纯惯性外推轨迹精度会随时间下降重新捕获之后如果GPS接收机输出的位置和INS外推位置相差很大滤波器在量测更新时可能会产生一个明显的轨迹跳变。解决这个问题有几个层面的手段。软件层面可以在组合滤波器中做一个量测有效性判断——当GPS位置和INS预测位置之间的新息Innovation超过了阈值比如按当前滤波协方差计算的3倍标准差时暂时不给这个量测过大的权重或者直接跳过这次更新。这个方法通常被称为新息卡方检测我在城市测试里验证过能有效抑制大多数跳点。硬件层面GPS接收机自身的稳健性也很关键支持多频多星座的接收机在高楼环境里的抗多径能力明显优于老式单频机出跳点的概率低很多。值得说明的是新息卡方检测的阈值设置要小心阈值太紧会把正常的GPS校正也挡在外面导致INS漂移越来越严重太松又起不到保护作用。我的经验是离线跑一遍数据统计不同环境下的新息分布再按3到5倍标准差设定阈值并在定位模质量指标比如GPS的sAcc做二次校验。5.3 欺骗干扰下的失效保护与基于一致性的检测思路组合导航行业最近这两年对欺骗干扰的讨论越来越多。GPS生成式欺骗干扰会播发和真实星座结构相似但位置被篡改的假信号接收机如果完全锁定在假信号上输出的GPS位置就是攻击者设定好的错误位置。组合导航的优势在于INS有短时间内的记忆能力不会立刻跟着假信号跳变但如果滤波器没有异常检测机制持续的被欺骗量测会逐渐把整个导航解拉过去。业界目前主流的防护策略之一叫多源一致性检测。大框架是把GPS位置、INS外推位置、以及可能的其他辅助源比如视觉里程计、轮速计、气压高度计放在一个一致性检验框架里任何单个信号源给出的信息和其余源相差过大时系统自动降权或者剔除这个信号源。更深一步的做法是把GPS原始测量值的质量指标比如载噪比、每颗星的伪距残差纳入监测如果所有通道的载噪比同时异常地高、伪距残差分布不符合正常噪声分布就要高度怀疑在遭受欺骗干扰。当然这一套做下来复杂度不小但如果在做的是自动驾驶或者无人系统这个保护机制值得认真考虑。我见过一些产品把基于新息卡方检测的故障检测和多源一致性监测结合起来做冗余投票效果会比单一策略可靠很多。6. 选型与调参中的几个实用心得最后分享几个贯穿全程、不太容易被文档收录的心得。关于器件选型我的建议是不要把IMU的指标孤立地看。陀螺零偏稳定性和角度随机游走这两个指标决定INS在GPS丢星后的外推精度加速度计零偏稳定性和速度随机游走决定系统对动态加速度的响应能力而GPS接收机支持不支持RTK、多频多星座、输出频率多高则决定量测域的可用性。两者要匹配。你用消费级MEMS配RTK厘米级精度精度上限基本上还是被IMU的姿态误差和杆臂误差卡住RTK的优势发挥不出来反过来你用光纤陀螺IMU去配单频GPS量测噪声太大组合出来的系统精度也不会好到哪里去。调参方面我强烈建议不要一上来就追求整套系统跑得最好而是先把滤波器改成分析模式——把量测更新暂时关闭或者把R设得极大让系统只在纯INS模式下跑看INS本身的漂移特性是否符合器件手册的水平。这一步能帮你把所有错误从算法层面和传感器层面分开。确认INS基本没问题后再逐步缩R观察组合后轨迹的变化。每一次只改一个参数记录效果再改下一个。我在团队里一直推行这个流程排查问题的效率比漫无目的地试参高得多。还有一个小技巧组合导航输出里一定要保留滤波器协方差矩阵的实时记录。很多时候轨迹看起来正常但协方差已经在悄悄涨大等你发现导航解飘了的时候数据已经跑完了。通过分析协方差变化和时间段的对应关系能很准确地定位是哪个量测出现了异常这是排查现场问题最有效的证据之一。