简介捷联惯导初始对准是导航系统进入工作状态的前提而粗对准的姿态解算精度直接决定后续精对准与组合导航的可靠性。在静基座条件下利用重力与地球自转角速度构造矢量对的经典方法简单有效但载体一旦运动加速度计与陀螺的测量便混入运动加速度、角运动及外速度误差等污染导致传统解析法失效。工程上引入GNSS等外部速度参考通过比力方程积分构造姿态观测方程以速度全积分方法将微分噪声转化为累计误差实现姿态矩阵估计进一步结合滑动窗设计用有限窗口内的积分与多窗融合抑制误差随时间的发散。理解积分与滑动窗的本质差异平衡窗口长度、步长与重叠率是提升动基座粗对准鲁棒性的关键。这类方法在船载、车载等动态环境中具有重要应用价值助推惯性导航在复杂运动场景下的工程落地。 做惯导的人都知道粗对准是姿态解算的第一块地基。地基没打好后面精对准、组合导航做得再花哨也是空中楼阁。前几年做船载项目第一次在动基座条件下做SINS初始对准我以为教科书上的解析粗对准是万能钥匙结果一到海上静基座那套重力和地球自转角速度直接构造矢量对的办法直接被晃动和机动打回原形。后来把速度/位置全积分和速度滑动窗这两条路都走了一遍才把动基座粗对准这点事儿彻底理顺。这篇东西不是教科书复读是这些方法从公式推导、仿真验证到上设备实测的完整记录。适合正在做捷联惯导初始对准、尤其是动基座场景的同学参考也适合刚入行、想搞清楚粗对准到底在解决什么问题的朋友。下面我尽量用工程语言讲把每个为什么都拆开。1. 动基座粗对准的困境静基座的参考答案为什么失效1.1 静基座解析粗对准的套路先花两分钟把静基座粗对准过一遍因为后面所有动基座方法的出发点都是怎么把静基座能用的参考信息找回来。经典解析粗对准的思路非常朴素IMU固定不动时加速度计输出的是重力加速度在机体轴上的投影严格说是比力方向和重力相反陀螺输出的是地球自转角速度在机体轴上的投影。而重力和地球自转角速度在地理坐标系里的分量是精确已知的g^n [0; 0; -g]ω_ie^n [0; ω_ie·cos L; ω_ie·sin L]于是机体坐标系下有一组测量矢量f^b和ω_ib^b导航坐标系下有一组已知矢量g^n和ω_ie^n姿态矩阵C_b^n就是这两个坐标系之间的旋转关系。构造三组非共线矢量对用TRIAD或者直接矩阵求逆C_b^n [g^n, ω_ie^n, (g^n×ω_ie^n)] · [f^b, ω_ib^b, (f^b×ω_ib^b)]^(-1)三个轴向上的姿态角一次性解出来整个粗对准耗时不到一秒精度对粗对准来说绰绰有余。这套方法在静基座上没有任何问题绝大多数教材讲的都是它所以很多新手会觉得粗对准不是So Easy吗。1.2 动起来之后测量被三层污染叠加问题在于载体一旦运动上面所有等式全都不成立了。我把动基座对测量的污染拆成三层来说第一层是运动加速度。加速度计输出的比力变成f^b a^b - g^b多了一个运动加速度a^b。船的纵摇横摇、车的加减速转弯水平运动加速度轻松超过0.1g。而g的水平分量在水平姿态误差1°的时候才0.017g。也就是说如果你拿静基座公式硬算水平姿态误差的量级就是运动加速度/重力加速度好几个度到十几度都是可能的。第二层是角运动。陀螺输出的角速度变成ω_ib^b ω_ie^b ω_eb^b船在海上摇摆的角速度峰值每秒能到几度甚至几十度而地球自转角速度每小时才15度差了三个数量级。把摇摆角速度当成地球自转角速度去算航向结果完全是噪声。第三层是外速度的引入。动基座下我们引入了GNSS或DVL这样的外部速度参考但外速度本身有噪声、有延迟、和IMU之间有杆臂这些次级污染虽然不如前两层致命却会在积分类方法里随时间累积后面专门讲。所以动基座粗对准的第一个工程结论是瞬时测量值不能直接用来构造矢量对必须用积分、滤波、滑动窗这类手段把运动污染抑制掉。这也是本文两种方法的共同出发点。1.3 换个思路让外部速度参考当参考答案静基座能对准根本原因是有了两个已知方向重力方向和地球自转轴方向。动基座下这俩参考被运动污染了怎么办工程上的答案很直接引入新的外部参考最常见的就是GNSS速度。GNSS给出的速度是导航坐标系下的不受载体运动加速度和摇摆的影响。惯导自身输出的比力经过积分也能得到一个速度增量。这两个速度之间恰好通过姿态矩阵联系在一起。如果我们能把姿态矩阵从速度关系里解出来粗对准就完成了。全积分类和滑动窗类方法本质上都在做这一件事只是对速度观测量的利用方式不同。2. 速度/位置全积分粗对准把积分当时间放大器2.1 比力方程怎么变成姿态观测方程全积分方法的起点是捷联惯导最基础的比力方程v̇^n C_b^n f^b - (2ω_ie^n ω_en^n) × v^n g^n这个方程说的是导航系下的加速度等于比力经姿态矩阵投影到导航系再扣除哥氏加速度、向心加速度和重力。把姿态矩阵相关的项单独拎出来并且把v^n替换成外部参考速度v_r^n比如GNSS速度就得到C_b^n f^b(t) v̇_r^n(t) (2ω_ie^n ω_en^n) × v_r^n(t) - g^n右边这一整坨每一项都是已知量v_r是GNSS给的ω_ie是地球自转角速度精确已知ω_en可以通过GNSS位置速度算出来g^n也是已知的。所以我给它起名叫导航系合成比力a_syn^n(t)。问题来了右边第一项是v̇_r对GNSS速度求导噪声会被放大得没法用。GNSS速度本身有0.03~0.1m/s的噪声求导之后直接是几m/s²级别的毛刺比力方程根本没法瞬时求解。2.2 积分把微分噪声变成了累计误差解决微分噪声的办法就是积分。假设粗对准期间姿态矩阵C_b^n近似不变——这个假设在粗对准的几十秒内对中高精度惯导是成立的因为陀螺零漂很小、载体姿态变化可以靠后续补偿——对上面的等式两边同时积分C_b^n ∫(t0→t) f^b(τ) dτ ∫(t0→t) a_syn^n(τ) dτ左边是IMU比力在[t0, t]上的积分在硬件上就是速度增量完全可以由捷联积分获得。右边第一项变成了v_r^n(t) - v_r^n(t0)求导变成了差分噪声被大幅平滑。其余项也全是积分形式对噪声天然有抑制作用。我把左右两边分别记作矢量u_b(t)和u_n(t)u_b(t) ∫(t0→t) f^b(τ)dτ机体坐标系下比力积分矢量 u_n(t) v_r^n(t) - v_r^n(t0) ∫(t0→t) [(2ω_ie^n ω_en^n) × v_r^n(τ) - g^n]dτ导航坐标系下对应矢量于是姿态观测方程简化为C_b^n · u_b(t) u_n(t)。这是三个方程未知数是姿态矩阵的三个自由度理论上任意一个时刻t的矢量对只能提供两个独立约束因为矢量的方向只有两个自由度所以要解出完整的姿态矩阵至少需要两个不同时刻、且方向不共线的矢量对。工程上怎么取最简单的做法是取t1 T/2和t2 T两个时刻得到两组矢量对然后用TRIADC_b^n [u_n(t1), u_n(t2), u_n(t1)×u_n(t2)] · [u_b(t1), u_b(t2), u_b(t1)×u_b(t2)]^(-1)更稳妥的做法是取多个时刻的矢量对用最小二乘或者QUEST去拟合一个最优姿态矩阵。实测下来多时刻最小二乘比两组TRIAD抗噪能力强不少计算量也不大推荐直接用。2.3 位置全积分把速度再积一次位置全积分就是把上面的速度积分再积一次。左边变成比力的二次积分也就是位置增量在机体轴上的投影C_b^n ∫∫ f^b dt dt ∫∫ a_syn^n dt dt右边二次积分后第一项变成位置量p_r^n(t) - p_r^n(t0) - v_r^n(t0)·(t - t0)其余哥氏和重力项也全部二次积分。位置全积分的好处是二次积分对GNSS速度噪声的平滑作用更强高频噪声基本被磨平了。坏处也显而易见二次积分让加速度计零偏和GNSS速度常值误差变成了随时间二次增长的位置误差积分时间稍微一长误差就压不住了。所以位置全积分在实践中更适合外部速度噪声较大、但对准时间必须短比如30秒以内的场景。或者反过来和速度积分法联合使用把速度积分矢量对和位置积分矢量对全部堆进同一个最小二乘问题里相当于增加了观测量条件数更健康。2.4 全积分方案的三个软肋别急着高兴全积分方案有三个绕不开的坑第一个是姿态恒定假设。整个推导假设C_b^n在积分区间内是常数阵但载体在动姿态一定在变。陀螺零漂哪怕只有0.01°/h积分60秒姿态也就漂0.00017°可以忽略但在转弯机动时航向角每秒变化好几度C_b^n根本不是常数全积分结果会被明显拉偏。第二个是误差随积分时间线性积累。加速度计零偏和GNSS速度常值误差都会以斜坡形式累积到u_n或u_b里时间越长越偏。所以全积分不是时间越长越准而是时间太短看不出来、时间太长被误差淹没中间有个最优区间。第三个是重力方向变化量的可观测性问题。全积分方法本质是依靠重力矢量在地理系中的方向随时间缓慢变化地球自转引起来提供航向可观测性。积分时间越短重力方向变化越小矢量对之间越接近共线姿态解算的病态程度越高。船载环境如果只积10秒g的方向只转了不到0.04°航向的误差会很大。这也是为什么动基座粗对准普遍需要30秒以上外加速率的底层原因。3. 速度滑动窗粗对准给积分加一条遗忘曲线3.1 为什么非要用滑动窗全积分方法最大的矛盾点在于航向可观测性需要长时间积分而误差积累又不允许长时间积分。滑动窗就是冲着这个矛盾来的。滑动窗的做法很直观不用从t0一直积到现在而是用一个固定长度T的窗口窗口内照样做速度积分、构造矢量对、解算一个姿态然后把窗口向前滑一步重新做一次。每个窗口独立计算姿态估计随时间滚动更新。这样做的直接好处有三个一是每个窗口的积分时间被限制在T以内误差积累有上限不会像全积分那样无限增长二是窗口短了窗口内姿态恒定的假设比全积分更接近真实三是多个窗口给出了多个独立的姿态估计可以统计平均相当于增加了冗余观测。代价也清楚单个窗口内积分时间短重力方向变化量小矢量对接近共线单次解算的病态程度比全积分严重。所以滑动窗必须靠滑得多、平均得多来换精度本质上是把时间换精度变成了多窗口平均换精度。3.2 窗口怎么滑长度、步长、重叠率窗口参数设计是整个滑动窗方案最核心的工程问题。我直接给经验值窗口长度T船载环境下摇摆周期一般在6~15秒垂直方向的扰动周期更短。T至少要覆盖2个以上摇摆周期让窗口内的平均比力尽量接近纯重力真实运动的均值。船载一般取30~60秒车载取10~30秒。T太短矢量对共线程度高、姿态估计方差大T太长误差积累又回来了。滑动步长ΔT决定姿态输出的更新率和窗口间的重叠程度。ΔT T就是不重叠计算量最小但输出率低工程上更常用ΔT 1~5秒窗口之间高度重叠相邻窗口的姿态估计强相关平滑效果好但要注意重叠导致的信息冗余会让多窗口平均的降噪效果打折扣。重叠率我自己的经验是重叠率在50%~80%之间比较划算。超过80%相邻窗口差异太小计算浪费低于50%窗口间的独立性好但输出率低。窗口内的数据处理也有讲究。直接对原始比力积分之前最好先做一次带通滤波或者滑动平均把摇摆频段通常0.05~1Hz之外的高频振动和低频慢漂移先压一压这样窗口内的积分对噪声更鲁棒。我在船载数据上试验过积分前加一个0.01~2Hz的带通滤波航向误差能改善15%~20%。3.3 窗内姿态估计与多窗融合每个窗口内部的做法和全积分完全一样取窗口起点t_k作为积分起点对窗口内每个采样时刻计算u_b(t)和u_n(t)然后在窗口内取若干时刻的矢量对用最小二乘或QUEST解出一个姿态矩阵C_b^n(k)。有了每个窗口的姿态估计之后多窗口融合是滑动窗的精华所在。融合方式有几种最简单的是等权平均把K个窗口的姿态角直接做算术平均。但这样做有个问题——误差小的窗口和误差大的窗口贡献一样不合理。更合理的加权平均按窗口的积分时间给权重窗口越新、实际积分误差越小权重越高。我用过指数衰减权重衰减系数τ取窗口长度T的1~2倍效果比等权平均好一个档次。还有一种是矢量对层面的整体解法把当前窗口和之前若干个窗口的所有矢量对全部堆在一起做一个整体最小二乘。这种做法的好处是自然融入了所有历史信息坏处是计算量略大而且老的矢量对误差大需要在目标函数里按时间加权。我在嵌入式平台上测过200Hz采样、窗口60秒、叠加5个窗口的矢量对整体最小二乘单次求解也就几毫秒实时性完全没问题。3.4 和全积分方案的本质区别一句话总结全积分是一次性利用从起点到当前时刻的所有本文还有配套的精品资源点击获取