把“捷联惯导”这几个字拆开看无非就是要解决“我是谁、我在哪、我要去哪”的持续计算问题。但做过实际项目的人都知道捷联惯导从上电那一刻起每一步都在跟误差做斗争。这篇总结覆盖了捷联惯导的几个核心模块初始对准、位置标定、INS姿态更新、GPS/INS组合。这几块不是孤立的而是串成一条完整的数据链路——先知道初始姿态和初始位置然后高频递推姿态与位置再用GPS这种绝对观测来修正惯导的长期漂移。适合正在做惯性导航算法验证、刚入门的嵌入式导航工程师或者想把整条链路理清楚的同学参考。1. 捷联惯导的整体框架与数学基础1.1 先把坐标系这东西掰扯清楚搞捷联惯导第一个绕不开的就是坐标系。很多初学者上来就调算法姿态四元数写得飞起结果一上转台就错多半是坐标系定义没统一。捷联惯导常用的几个坐标系我习惯这么记i系惯性坐标系以地心为原点不随地球自转是牛顿力学成立的绝对参考。陀螺和加速度计测量的物理量本质上都是相对于这个系的。e系地球坐标系固连在地球上随地球自转。我们常说的经纬度就是载体相对这个系的位置。n系导航坐标系一般取当地水平坐标系东北天ENU或北东地NED。国内项目用东北天多一些但飞控圈子里北东地也很常见。我的建议是选一个全项目统一别混着来。b系载体坐标系固连在载体上一般是右前上x右、y前、z上也有前右下或者前右上。IMU的原始输出就是b系下的角增量和速度增量。m系地图坐标系某些组合导航产品里会用到和e系之间有固定的旋转关系主要服务地图显示。捷联惯导和平台惯导的本质区别就在这里。平台惯导用物理平台去模拟n系把加速度计和陀螺装在稳定平台上姿态角直接由平台框架读出。捷联惯导没有物理平台它用计算机里的姿态矩阵去“虚拟”一个数学平台把b系的传感器输出实时投影到n系。这个“虚拟平台”的精度完全取决于姿态更新算法的精度和陀螺的精度。1.2 捷联惯导的核心方程从工程实现的角度捷联惯导的每一拍计算都是围绕三组方程展开的姿态微分方程是捷联惯导的心脏。姿态矩阵 ( C_b^n ) 的变化率由载体相对导航系的角速度决定在四元数表示下写成[ \dot{q}n^b \frac{1}{2} q_n^b \otimes \omega{nb}^b ]这里的 ( \omega_{nb}^b ) 是载体相对导航系的角速度在b系的投影它不能直接用陀螺输出代替而是要扣除地球自转和载体在地球表面运动引起的牵连角速度[ \omega_{nb}^b \omega_{ib}^b - C_n^b (\omega_{ie}^n \omega_{en}^n) ]很多第一次写惯导程序的人会漏掉后面两项结果就是静态下姿态缓慢漂移转台测试过不了。比力方程用来更新速度[ \dot{v}^n C_b^n f^b - (2\omega_{ie}^n \omega_{en}^n) \times v^n g^n ]加速度计测的是比力不是纯加速度。要得到导航系下的真实加速度必须先去掉哥氏加速度和重力。这句说起来简单但 ( 2\omega_{ie}^n \omega_{en}^n ) 这两项的符号方向稍微搞错误差就会随速度增长。位置方程用来更新经纬度和高度。一般把速度投影到子午圈和卯酉圈曲率半径上求纬度和经度的变化率[ \dot{L} \frac{v_N}{R_M h}, \quad \dot{\lambda} \frac{v_E}{(R_N h)\cos L}, \quad \dot{h} v_U ]写到这里顺便说一句位置更新虽然公式简单但收敛性和数值稳定性同样要关注尤其是高动态载体和长时间惯性纯推演时位置误差是速度误差积分出来的二阶发散比姿态误差跑得快得多。2. 初始对准解决“从哪开始算”的问题2.1 初始对准到底在干什么捷联惯导的“初始对准”这四个字很多人理解成“让姿态角归零”这是不对的。初始对准的目标是确定载体坐标系相对导航坐标系的初始姿态矩阵 ( C_b^n(0) )。注意是“确定”不是“归零”。飞机停在机坪上机头朝东和朝北初始姿态矩阵完全不同。初始对准分两大类静基座对准和动基座对准。日常项目里静基座对准最常用也最基础动基座对准比如车辆跑起来之后才对准、飞机在舰船上对准属于进阶玩法。静基座对准又能分成粗对准和精对准两个阶段。粗对准在几十秒内完成精度几度到零点几度精对准在几分钟内把水平姿态对准到角分级、航向对准到0.1度级甚至更高。2.2 解析式粗对准的原理与实现粗对准的原理本质上是利用两个在n系下已知、在b系下可测的矢量来构建初始姿态矩阵。第一个矢量是重力。静基座下加速度计测到的比力在n系下就是 ( -g^n )方向指向地心在b系下的投影就是加速度计的实际输出 ( \bar{f}^b )。这个矢量能确定水平姿态俯仰和横滚。第二个矢量是地球自转角速度。陀螺静基座下输出 ( \omega_{ie}^b )在n系下 ( \omega_{ie}^n ) 是已知的水平分量为 ( \omega_{ie} \cos L ) 指向北向天向分量为 ( \omega_{ie} \sin L )。利用这个矢量就能确定航向角。工程实现上常用的做法是构建两个相互正交的矢量用双矢量定姿求解初始姿态矩阵取 ( \mathbf{v}_1 \bar{f}^b )重力矢量取 ( \mathbf{v}2 \bar{f}^b \times \bar{\omega}{ie}^b )重力与地球自转角速度的叉乘再归一化取 ( \mathbf{v}_3 \mathbf{v}_1 \times \mathbf{v}_2 ) 构成正交系分别求n系和b系下的这一组正交基然后直接解算出 ( C_b^n )。这个方法的推导并不复杂但有几个工程细节必须注意。第一加速度计和陀螺的输出必须先做平滑滤波取几十秒内的均值否则晃动噪声直接进姿态粗对准精度打折。第二陀螺噪声大的MEMS器件( \omega_{ie} ) 信号可能被噪声淹没航向精度很差这时候粗对准航向只用来做精对准的初值不需要强求精度。第三如果载体有轻微振动或阵风扰动可以用更长的平滑时间和更鲁棒的滤波策略来缓解。2.3 精对准罗经对准与卡尔曼滤波对准粗对准之后姿态误差还在度级尤其航向误差可能到几度这对导航来说不够。精对准的任务就是把残差压下去。经典的静基座精对准分水平对准和方位对准两条线水平精对准利用水平回路的舒拉调谐特性把失准角引起的水平加速度误差反馈到姿态修正中让水平姿态误差收敛到陀螺漂移的量级。简单说如果调平不准重力分量会投影到水平加速度计上形成虚假水平加速度位置环和速度环会产生振荡信号把振荡信号提取出来反向修正姿态角姿态就逐渐对正了。方位精对准利用罗经效应。静基座下航向误差会让水平陀螺敏感到地球自转的北向分量并通过水平回路形成一个等效的“罗经力矩”把航向误差慢慢拉向零。航向收敛速度很慢因为罗经回路的带宽很低一般需要几分钟到十几分钟。现代工程里除了经典控制回路方式更多用卡尔曼滤波来做静基座对准。把失准角、陀螺漂移、加速度计零偏放进状态量用速度误差作为量测更新。静基座下速度真值为零惯导计算速度就是速度误差这个量测干净且直观。卡尔曼滤波对准比经典罗经对准的调试效率高很多因为不用手工设计回路参数把噪声矩阵调合理就收敛。但代价是滤波器的可观性依赖可观测条件静基座下航向误差的可观性比较弱所以航向收敛慢这与罗经对准殊途同归。2.4 动基座对准与传递对准一句话总结如果载体一直在动比如车载惯导在行驶中开机静基座对准没法做。这时常用的手段是行进中对准依靠GPS速度或位置信息辅助利用车辆运动中的机动来激励航向角。直行时航向不可观有转向机动后航向误差才快速收敛。所以实际调试中常要求车辆跑一个S形或蛇形路线。传递对准高精度主惯导带动低精度子惯导利用杆臂补偿和时间同步通过速度或姿态匹配在短时间内让子惯导获得主惯导的精度。动基座对准的复杂度和调试成本比静基座高一个量级如果项目条件允许尽量在静止状态下完成初始对准再出车。3. 位置标定惯导的“原始坐标”必须立住3.1 位置标定不是“校准位置”而是“确定初始基准”标题里的“位置标定”我理解成两层含义一是惯导启动时需要装订初始位置经纬度和高度二是惯导安装到载体上之后安装位置和杆臂的标定。这两件事看着不相关实际都会直接决定捷联惯导的定位起点准不准。初始位置装订是常识但坑很多。惯导上电后如果初始位置误差很大比如差了几公里那接下来几分钟内的位置输出都会带一个巨大的固定偏置卡尔曼滤波收敛也救不回来因为初始位置误差直接进位置状态。初始位置怎么装订工程上有三招人工输入操作员通过界面输入当地经纬度和高度。简单但容易输错必须做合理性检查。GPS自动装订启动时用GNSS定位结果作为初始位置。最常用但要注意GPS定位可能有短时跳变取几十秒稳定值再做装订。存储回读如果载体是在已知地点定期运行比如公交场站、固定机位可以把上一帧的最终位置或预设位置存进非易失存储下次启动直接读取。3.2 杆臂标定GPS天线和惯导不在一处这是组合导航里特别容易忽略的问题。惯导安装在车辆后桥附近或机舱里GPS天线装在车顶或机身顶部两者之间的距离就是杆臂。如果把GPS的位置观测直接跟惯导的位置输出做差这个杆臂会以米级误差的形式进入组合滤波器的量测。杆臂补偿分静态和动态两种。静态杆臂是安装位置的三维偏移用卷尺都能量出来但量测精度有限。动态杆臂是载体晃动时GPS天线相对惯导的微米级运动通常建模为小量噪声。更稳妥的做法是把杆臂也放进卡尔曼滤波的状态量里估计尤其是对经常拆装、安装位置每次都变化的设备。有一种经验做法是让载体做一段直线加转弯的运动GPS位置和速度信息足够让杆臂误差在几十秒内收敛到厘米级。如果安装点固定标定一次以后就可以直接当作常量使用。3.3 安装误差角的标定安装误差角是另一个和“位置标定”容易混淆的概念。它指的是IMU的b系和载体实际轴线比如车辆航向轴、飞机机体轴之间的夹角。如果IMU装歪了1度那么姿态输出就带1度固定偏差组合导航的航向和位置也会被牵连拉偏。安装误差角的标定一般用转台或高精度真值转台标定把惯导固定在转台上转动到多个已知角度用姿态输出的偏差反算安装误差角。精度高但需要转台设备。车载标定借助高精度组合导航系统或RTK让载体跑直线和转弯用速度或航向差反算安装误差角。精度不如转台但完全够大多数车辆导航使用。标定完之后把安装误差矩阵 ( C_{install} ) 串进姿态链里先算IMU相对载体的姿态再转到导航系。3.4 高度位置的特殊性位置标定还有一个小话题高度。气压高度计、GNSS高程、惯导积分高度三者系统误差源完全不同。惯导高度通道本身是发散的纯惯性高度几分钟就飘所以组合导航里高度一般直接用GNSS高程或气压高度约束不给惯导高度通道太多权重。初始高度装订个粗略值就行滤波会快速修正。4. INS姿态更新一毫秒一毫秒地把姿态“转”出来4.1 姿态表示方法对比姿态更新是捷联惯导里计算量最大、最容易出数值问题的模块。先把表示方法说清楚。欧拉角直观三个角度横滚、俯仰、航向但存在万向节锁死问题且微分方程里含三角函数实时计算开销大。一般只用于显示和外部接口。方向余弦矩阵DCM不存在奇异问题但9个元素相互冗余归一是麻烦事。适合做姿态互转。四元数4个参数无奇异更新计算量小但是容易受到非正交误差影响需要周期归一化。工程上主算法基本都用四元数DCM只作为最终输出转换用。我的习惯是内部递推用四元数输出姿态角时再把四元数转成欧拉角。这个转换四元数到欧拉角的过程虽然简单但要特别注意角度范围的定义尤其是航向角0到360度和-180到180度的约定不同客户要求不一样。4.2 姿态更新的黑魔法旋转矢量与不可交换性误差捷联惯导姿态更新的核心问题是如何用陀螺输出的有限时间内的角增量去逼近真实的姿态变化。陀螺输出的是角增量不是角速度连续函数。如果载体在高动态环境中同时绕两个轴旋转比如既俯仰又横滚角速度矢量在空间中是旋转的这时直接把各轴角增量相加当成总旋转矢量来更新就会产生“不可交换性误差”。最经典的例子就是圆锥运动载体在锥面上绕顶点摆动每拍角增量都不大但累积的姿态漂移却相当可观。解决不可交换性误差的标准做法是旋转矢量法。姿态更新不再直接用角增量而是先构造旋转矢量 ( \boldsymbol{\phi} )然后通过Bortz方程或它的近似形式更新。工程里最常见的是单子样、双子样、三子样算法单子样算法每个周期只用一个角增量圆锥补偿项为零。适合低动态或高更新率的场景。双子样算法每个周期取两个角增量中间产生了圆锥补偿项可以在圆锥环境中大幅抑制漂移。多数中低动态项目够用。三子样算法每个周期取三个角增量圆锥补偿更精细适合高动态和高精度的组合。这类算法的核心思想是利用多个子样之间的角增量叉乘来估算角速度方向的变化率从而对不可交换性误差做补偿。写代码时有个教训子样算法里的补偿项不是把三个子样简单相加而是两两叉乘再乘系数。系数用错了补偿效果直接反折扣甚至会引入更大的误差。算法推导过程不复杂但建议直接把经过验证的嵌入式库函数拿过来用不要自己临时推导。4.3 速度更新里的划摇补偿姿态更新有不可交换性误差速度更新里对应的问题叫划摇效应sculling。想象一个物体在振动环境中既有线振动又有角振动比力积分时如果直接把加速度计输出当成简单的线性积分会产生额外的速度误差。速度更新的标准做法是把比力方程在积分周期内展开把角速度和比力的叉乘项做成划摇补偿项。高阶划摇补偿的推导思路跟圆锥补偿很相似都是利用多子样数据的叉乘乘积来逼近真实积分。很多教科书把圆锥补偿和划摇补偿分开讲代码实现时放在一起。尤其在高振动的车辆或无人机场景里划摇补偿不做好速度发散会很明显。我在实际测试中见过一辆怠速抖动的汽车MEMS级惯导的速度误差从0.05 m/s抖到0.3 m/s加了划摇补偿之后直接恢复到0.05 m/s级别。4.4 姿态更新周期的选择姿态更新周期不是越短越好要看陀螺采样率和CPU负载。MEMS陀螺输出率通常100 Hz到1 kHz高精度光纤陀螺可以到2 kHz以上。原则上姿态更新率至少要是载体最高动态频率的10倍以上。对普通车载应用100 Hz够了对无人机、导弹这类高机动载体建议400 Hz以上。我见过一个项目用100 Hz更新率跑旋转弹应用圆锥漂移大得离谱。把更新率提到500 Hz同样的算法代码姿态漂移直接砍半。算法再好采样率不足也白搭。4.5 归一化和反向运算四元数更新过程中浮点舍入误差会慢慢让四元数的模偏离1。每几百个周期做一次四元数归一化是必要的否则姿态矩阵会逐渐失去正交性姿态输出会产生慢漂。这里有一个容易被忽视的点四元数归一化的频率要足够高。如果只在进入组合滤波时才归一化那两次归一化之间累积的误差可能已经在高精度应用里不可接受了。我习惯每更新100到200拍就归一化一次开销很小但数值稳定性好不少。5. GPS/INS组合惯导不飘、GPS不跳的秘诀5.1 为什么一定要组合纯惯性导航的问题是误差随时间累积。陀螺零偏导致姿态漂移姿态漂移导致加速度计比力投影错误速度误差积分又导致位置误差二次方增长。哪怕高精度光纤陀螺纯惯性导航10分钟位置误差也可能到百米。GPS的问题则相反长期来看不飘绝对精度在米级到厘米级RTK但更新率低常见5到20 Hz高动态下可能丢星城市峡谷里信号遮挡严重。GPS/INS组合的本质是把这两个系统放在一个卡尔曼滤波器里用GPS的绝对观测去估计并补偿惯导的误差源同时用惯导的高频输出填补GPS丢星和低更新率的空缺。最后输出的导航解比单独任何一个系统都稳。5.2 松组合的架构与状态量设计工程里最常用的是松组合。它把GPS解算出来的位置和速度当作量测和惯导预测的位置、速度做差送到卡尔曼滤波器里去估计误差状态。一个典型的15维状态量包含3个姿态误差失准角3个速度误差3个位置误差3个陀螺漂移通常建模为一阶马尔可夫或随机游走3个加速度计零偏有的方案会扩展到18维、21维把杆臂误差、时间同步误差、GPS钟差等也加进去。状态量越多系统越复杂可观测性分析也越难。常规项目先用15维打底根据实际误差表现再扩展。5.3 卡尔曼滤波的工程细节卡尔曼滤波的核心公式谁都会写工程里真正的差距在模型噪声矩阵 ( Q ) 和量测噪声 ( R ) 的整定上。Q矩阵表示状态模型的过程噪声。陀螺零偏和加速度计零偏的噪声量级要参考IMU数据手册和艾伦方差分析。我的经验是初值先按手册给然后跑一段真实数据看估计出来的状态方差是否合理再微调放大或缩小Q。R矩阵表示量测噪声。GPS位置和速度的R值要根据GPS器件型号和差分状态设置。RTK模式下位置R可以给厘米级单点定位模式下要给3到5米的方差。R给太大修正力度弱惯导飘起来GPS拉不回来R给太小GPS噪声会直接灌进滤波输出导航曲线看起来会毛糙。还有一个工程细节是量测更新周期。GPS的更新率低惯导的预测更新率高滤波器是“高频预测、低频更新”。在GPS更新间隙卡尔曼滤波器只做时间更新把状态和协方差往前推收到新GPS数据后再做一次量测更新。这个机制理解清楚了代码结构就不会出错。5.4 反馈校正还是开环输出组合导航结果的输出有两种方式。开环方式误差反馈卡尔曼滤波估计的是误差状态最终的导航解在主惯导解算路径上加上修正量后输出。滤波器本身不改变捷联解算内部的状态稳定性好但误差状态线性化假设在大误差时可能失真。闭环方式全反馈把卡尔曼估计出的误差直接反馈修正惯导解算的姿态、速度、位置甚至修正陀螺和加速度计的零偏惯导内部一直保持一个干净的导航解。这种方式精度和稳定性都好但要注意反馈频率和控制律反馈过头会亏。我个人的做法是在INS更新和GPS量测更新之间做闭环反馈并且把反馈量做平滑和限幅处理避免一次GPS跳变打乱整个解。5.5 时间同步问题GPS/INS组合里最容易踩的坑是时间同步。GPS的位置和速度观测对应的是GPS接收机输出的脉冲时刻或数据帧时刻。惯导的预测结果对应的是惯导自己的解算时间戳。两者时间对不齐哪怕只差10毫秒在高速运动场景下就会产生显著的位置和速度误差。工程上有几个处理思路硬件脉冲对齐GPS接收机输出PPS脉冲惯导系统在PPS中断里采样把惯导状态打上GPS时间戳。这需要硬件支持精度最高。软件补偿根据时间戳差值用惯导的速度对GPS量测做外推或内插把它折算到惯导解算时刻。滤波状态扩展把时间同步误差当作一个状态量去估计。前两种最常见第三种适合对时间同步精度极其苛刻的场景。我建议做项目时优先保证时间戳的统一在输出层面就用同一个时间基准否则后面组合滤波器的调试会让你心力交瘁。5.6 紧组合和深组合的适用边界紧组合不再用GPS解算后的位置速度作为量测而是直接利用伪距、伪距率和载波相位观测。相比松组合紧组合在GPS卫星数不足时可以继续使用部分卫星数据观测量更原始信息量更大。但实现复杂度高需要自己维护卫星星历和接收机钟差。深组合把GPS接收机的跟踪环路和惯导紧耦合用惯导辅助接收机跟踪动态适应能力强抗干扰能力好。但这个方向主要是硬件和射频端的活软件算法反而是配角一般团队碰得少。中小项目我建议先做松组合它已经能解决90%的问题而且调试工具成熟。只有当松组合在弱信号、多径场景下明显不满足指标时再考虑紧组合。6. 常见问题与排查技巧实录6.1 静态下姿态缓慢漂移静止状态下姿态输出一直在缓慢变化幅度从几分到几度每小时不等。优先检查三件事陀螺零偏是否补偿干净。MEMS陀螺的零偏稳定性差温度变化也会引入额外零偏最好加温度补偿表。陀螺的比力敏感项和交叉轴灵敏度。有些低成本MEMS陀螺对加速度敏感静止时的重力会激励出虚假角速度。是否正确扣除了地球自转角速度。如果初始对准时把 ( \omega_{ie} ) 项忽略静态姿态就会缓慢漂移。6.2 GPS短时跳变拉偏导航解GPS在城市环境里多径、遮挡严重位置会发生几米到几十米的跳变。如果组合滤波器R设得太小量测更新会强行把导航解拉向错误的GPS位置产生“犬牙差互”的输出曲线。常用的防护手段卡方检验异常检测计算量测新息innovation如果超过阈值认为量测异常并拒绝更新。R自适应调整根据当前GPS的定位质量指标动态放大R比如单点定位时R放大RTK时缩小。量测限幅对位置/速度残差做限幅超过安全范围时只做部分或零更新。这些方法不冲突可以在一个项目里叠加使用。6.3 航向收敛慢或不收敛卡尔曼滤波对航向的可观测性依赖载体运动激励。静基座或直线运动时航向误差收敛很慢这属于系统可观性限制不是代码bug。解决办法让载体做机动比如S形行驶。测试时要设计合理的运动路径让航向误差在几分钟内被激励到可观。如果是车载产品建议在出厂测试说明里明确写出“产品安装后需进行一次包含转弯的校准行驶。”还有一种可能是陀螺Z轴零偏偏置较大卡尔曼滤波把航向误差和陀螺零偏混淆了导致航向收敛后又被拉偏。此时要检查滤波器的可观测性和状态模型必要时把陀螺零偏建模得更贴合实际。6.4 粗对准时航向差180度解析式粗对准偶尔会出现航向差了180度的情况根因是双矢量定姿时使用的矢量方向约定不一致或者两个参考矢量的叉乘顺序不对。比如把重力方向取成 ( g ) 而不是 ( -g )或者 ( v_1 \times v_2 ) 的叉乘顺序反了都会造成180度翻转。排查方法很简单对准完成后把计算出的姿态矩阵拿去计算“重力在b系的投影”跟加速度计实际输出对比方向一致则基本正确相反则说明定姿方向反了。6.5 组合后位置精度反而下降这种情况多半是时间不同步或坐标基准不统一。GNSS解算的坐标是WGS84或CGCS2000惯导的位置更新用的是NED系下的移动距离二者换算时椭球参数、投影带设置不一样位置就会长期偏离。另外检查GNSS输出的高度基准。有些设备默认输出椭球高有些输出海拔高混用会造成高度方向偏差并间接影响水平定位。6.6 一个完整的调试数据流建议最后分享一个我常用的调试套路先记录原始IMU数据做零偏估计和温度补偿验证。静基座跑一次对准观察对准收敛曲线和最终姿态角是否符合预期。用已知轨迹转台或车辆直线、转弯考验姿态更新检查圆锥补偿和划摇补偿是否生效。GPS/INS融合后先看残差序列再调Q/R矩阵不要一上来就动滤波器模型。每次改参数都保留日志文件对比前后结果。导航算法的调试本质上是“实验科学”数据日志是最重要的资产。我个人在实际项目里最深的体会是捷联惯导的算法每一个模块都不算难难的是把误差源一个个抠干净把坐标系、时间戳、杆臂这些东西理清楚。一个角度符号错了可能要花好几天才能从数据里定位到根因。所以做这套系统耐心和数据管理能力有时候比数学功底还重要。希望这篇总结能帮你少踩几个坑把从初始对准到组合导航这条路走得顺畅一些。