1. 初始对准从“找北”到“站稳”的第一步大家好我是老张一个在导航算法这行摸爬滚打了十来年的工程师。今天想和大家聊聊当我们拿到一个IMU惯性测量单元准备把它装到车上或者无人机上让它告诉我们“我在哪儿、头朝哪”时遇到的第一座大山——初始对准。这听起来挺学术但你可以把它想象成给一个刚睡醒、不知道自己面朝哪里的机器人“开机校准”。这一步要是没做好后面所有的导航、定位、路径规划全都会跑偏差之毫厘谬以千里。很多刚入行的朋友可能会觉得初始对准嘛不就是让设备静止一会儿让它自己算算姿态理论上是这样但工程实践里坑可太多了。比如你的IMU是随便装在车里的它本身的坐标系和车体坐标系根本不重合又比如你以为地面是静止的但实际上有细微的震动或者风在吹再比如你用的是成本较低的MEMS-IMU它的传感器噪声大、零偏不稳定静置十分钟算出来的航向角可能飘出去好几度。这些都是我们每天要面对的真实挑战。所以初始对准的核心目标就是在系统上电启动后快速、准确地确定载体坐标系b系与导航坐标系n系比如“东北天”之间的姿态转换矩阵。这个矩阵就是我们后续一切导航解算的“基石”。为了实现这个目标我们通常会把对准过程分为两步走粗对准和精对准。粗对准求一个“大概齐”速度快精对准则在这个基础上“精雕细琢”利用更复杂的模型和滤波算法把精度提上来。下面我就结合自己的踩坑经验详细拆解这两步。1.1 解析式粗对准快速抓住“重力”和“地球自转”粗对准的目标是在几秒到十几秒内给出一个姿态矩阵的初始估计。最经典的方法就是利用我们身边两个天然的、稳定的物理参考量重力加速度g和地球自转角速度ωie。理想很丰满在静止状态下IMU的加速度计测到的比力Specific Force就是重力加速度的反向即fb -Cnb * gn其中gn [0, 0, g]T。陀螺仪测到的角速度就是地球自转角速度在载体上的投影即ωb Cnb * ωien其中ωien [0, ωie cosL, ωie sinL]TL是当地纬度。你看这两个方程里都包含了我们想要的姿态矩阵Cnb。如果我们能精确测量出fb和ωb理论上解个方程就能得到Cnb。但现实很骨感。我拿一个消费级的MEMS-IMU做过测试在室内看似静止的桌面上加速度计的输出噪声能到几个mg陀螺仪的噪声就更大了地球自转角速度大约是15度/小时而一个低成本陀螺的零偏可能就有10度/小时噪声直接把信号淹没了。所以直接套用理想公式算出来的航向角可能完全随机。那怎么办呢工程上我们常用一种近似处理叫做双矢量定姿。我们选择测量相对更可靠的重力矢量作为主参考。具体操作时我们不会只用单点数据而是采集一小段时间比如1秒的数据进行平均以抑制随机噪声。然后我们构造两个矢量在导航系n下我们知道重力矢量v1n [0, 0, g]T和地球自转矢量v2n [0, ωie cosL, ωie sinL]T。在载体系b下我们用这段时间平均后的加速度计输出作为v1b的估计取反用平均后的陀螺仪输出作为v2b的估计。有了这两组矢量我们可以用“矢量叉积”的方法来构造姿态矩阵。原理是姿态矩阵的作用就是将一个坐标系下的矢量转换到另一个坐标系。如果我们有两个不共线的矢量在两个坐标系下的投影那么我们就可以唯一确定这两个坐标系之间的旋转关系当然这里忽略了正交化的细节。一个常用的求解公式是Cnb ≈ [v1b, (v1b × v2b), v1b × (v1b × v2b)] * [v1n, (v1n × v2n), v1n × (v1n × v2n)]^{-1}这个公式计算量小速度快能在秒级完成。但它有个致命弱点对陀螺仪测量地球自转的精度要求高。在低精度IMU或者有角晃动干扰比如车载发动机振动时v2b的估计误差会很大导致求出的姿态矩阵特别是方位角航向误差很大。实测下来水平姿态角俯仰、横滚靠重力矢量能对准到1度以内但航向角误差可能高达10度甚至更多。所以解析式粗对准通常只提供一个可用的水平姿态航向角要么靠其他传感器如磁力计给定要么留给后续的精对准来修正。1.2 精对准与卡尔曼滤波把“大概”变成“精确”粗对准给了我们一个不错的起点但离高精度导航的要求还差得远。精对准的任务就是把这个起点的精度再提升一两个数量级。这里卡尔曼滤波成了我们的核心工具。它的思想很巧妙我们不直接去“算”姿态而是去“估计”姿态的误差然后用估计出的误差去修正我们粗对准得到的姿态以及IMU传感器本身的误差如零偏。为什么这么做因为误差通常是小量其动态模型更容易建立。在静基座载体完全静止条件下精对准的模型非常经典。我们的状态向量通常包括失准角φ姿态误差、速度误差δv、位置误差δp、陀螺仪零偏ε、加速度计零偏∇。观测值是什么呢在静止状态下导航解算出来的速度理论上应该为零。所以我们可以把惯导解算的速度作为观测量理论值为0输入到卡尔曼滤波器中。滤波器的工作原理是这样的我们有一个基于惯性器件误差和力学编排方程推导出的状态方程它描述了失准角、速度误差等状态量如何随时间变化。同时我们有一个量测方程它建立了观测量速度与状态量主要是失准角和加速度计零偏之间的关系。滤波器不断迭代通过比较预测的速度和“观测到”的速度即零来反向推算出最有可能导致这个速度误差的失准角和传感器零偏是多少。这里有个很重要的概念叫可观测性。不是所有状态都能被很好地估计出来。在静基座下分析表明东向和北向的水平失准角、东向陀螺零偏、北向和天向加速度计零偏是可观测或弱可观测的。但是方位失准角航向误差和与地球自转相关的陀螺零偏是不可观测的。这意味着如果你让载体一动不动地对准滤波器永远无法精确修正航向角的误差。这是静基座对准的一个根本局限。为了解决这个问题工程师们想出了双位置对准法。方法很简单在精对准过程中让载体绕方位轴天向轴故意旋转180度。这个操作相当于给系统施加了一个已知的、大的激励。从滤波器的角度看系统的误差模型从一个“定常系统”变成了一个“时变系统”原来不可观测的方位失准角变得可观测了。我做过实验对于中等精度的光纤陀螺惯导静基座单位置对准的航向精度可能在0.1度左右而采用双位置对准精度可以提升到0.02度以下效果非常显著。当然转动时机、转动的平稳性都需要精心设计否则引入的晃动干扰又会带来新的误差。2. 多源融合当惯导不再孤单完成了初始对准我们的惯导系统算是“站稳了脚跟”可以开始独立工作了。但纯惯性导航有个致命问题误差会随时间累积。陀螺的零偏哪怕只有0.01度/小时积分几分钟后姿态误差就能积累到可观的程度加速度计的零偏积分两次就成了位置误差发散速度是时间的二次方。所以在高精度、长航时的应用里我们绝不会让惯导单打独斗。组合导航就是让惯导INS与其他有绝对参考但各有局限的传感器如GNSS、里程计、磁力计携手合作取长补短。2.1 与GNSS组合黄金搭档的磨合细节GNSS全球卫星导航系统如GPS、北斗能提供绝对的位置和速度信息且误差不随时间发散正好弥补惯导的短板。而惯导在GNSS信号丢失时如隧道、城市峡谷能提供短时高精度的导航信息并辅助GNSS抗干扰。INS/GNSS组合是导航领域的经典组合但想做好有几个工程细节必须抠死。第一个是杆臂误差。IMU和GNSS天线不可能安装在同一个点上它们之间有一个固定的空间偏移这个偏移就是杆臂。当载体有角运动时这个杆臂会导致IMU和天线处感受到的线速度不同。如果不补偿直接比较两者的速度或位置就会引入误差。补偿公式需要用到载体的角速度和杆臂矢量。假设杆臂在IMU坐标系b系下为l^b那么杆臂引起的速度补偿项为δv Cnb * (ωibb × l^b)其中ωibb是陀螺测量的载体旋转角速度。在车载应用中这个补偿至关重要因为车辆转弯时角速度不小。我遇到过因为杆臂测量不准差了几厘米导致组合滤波器在转弯时产生明显位置跳变的情况。第二个是时间同步误差。IMU的数据和GNSS的数据到达导航计算机的时间戳往往不是严格对齐的。GNSS数据从卫星信号接收、解算、到通过串口发送出来存在几十到上百毫秒的延迟。如果直接用收到GNSS数据时的IMU数据进行组合就等于把不同时刻的信息强行比对会引入误差。解决办法有两种一是进行时间延迟估计把它作为一个状态量加入到卡尔曼滤波器中一起估计二是在硬件或软件层面实现高精度时间同步比如使用GNSS的PPS秒脉冲信号来触发IMU采样或给IMU数据打上精确的时间戳。在实际工程中我们通常两者结合先尽量做好硬件同步再用滤波估计残差。在状态空间模型设计上一个典型的紧组合Tightly CoupledINS/GNSS滤波器状态量可能多达17维以上3维失准角、3维速度误差、3维位置误差、3维陀螺零偏、3维加速度计零偏、3维杆臂误差、2维时间同步误差钟差和钟漂等。观测量则是INS解算的位置/速度与GNSS原始伪距、伪距率之间的差值。通过滤波我们不仅能得到更优的导航结果还能实时估计出IMU的传感器误差这些估计值可以反馈回去校正IMU的原始数据形成一个闭环进一步提升纯惯性导航的精度。2.2 与里程计组合陆地行车的“脚踏实地”对于地面车辆里程计Odometer是一个极其重要的辅助传感器。它通过测量车轮转动的圈数来推算车辆行驶的距离提供的是车体坐标系下沿前进方向的速度。这个信息对于约束惯导在水平面内的误差发散特别是航向角误差有奇效。为什么这么说这涉及到航位推算DR的原理。假设车辆在平坦地面行驶没有打滑那么里程计测得的速度方向就是车头的指向。如果我们知道车体坐标系到导航坐标系的姿态矩阵这正是惯导提供的就能把里程计速度转换到导航系然后积分得到位置。这个位置解算过程只依赖于姿态和里程不直接依赖加速度计因此不受加速度计零偏长期积分的影响。更重要的是当车辆做直线运动时里程计提供的速度方向信息与GNSS提供的绝对速度方向信息进行对比可以非常有效地估计出惯导的航向角误差。这是对静基座对准不可观测方位角的一个极好补充。但是里程计组合的坑也不少。首先是安装误差角。里程计通常安装在非转向轮上它的测量坐标系m系和IMU的载体坐标系b系之间可能存在微小的安装偏差角俯仰、横滚、航向。这个角如果不标定准确会直接污染速度转换。我们通常把它作为状态量在滤波器中在线估计但前提是惯导本身的姿态精度要足够高否则“一个糊涂的向导无法校准另一个有偏差的尺子”。其次是刻度系数误差。轮胎气压、磨损都会影响轮胎周长导致里程计测得的脉冲数与实际行驶距离的比例因子发生变化。这个刻度系数误差也需要在线估计或定期标定。还有一个高级话题是增量组合模式。车辆行驶中难免遇到打滑、颠簸等不良路况此时里程计的测量不可信。一种聪明的做法是滤波器只在检测到车辆处于良好行驶状态如匀速、直线时才使用一段时间内的里程增量与惯导的对应增量进行组合比较。这样做的好处是即使中间有一段不良数据被剔除当车辆恢复良好状态时组合导航依然可以无缝衔接不受之前不良数据累积误差的影响系统的鲁棒性大大增强。3. 低成本方案的生存之道MEMS-IMU与多源融合前面讲的多是战术级甚至导航级IMU的应用。现在越来越多的消费级、工业级设备比如无人机、机器人、AR/VR头盔使用的是低成本的MEMS-IMU。这类传感器价格亲民但性能指标也“感人”陀螺零偏可能高达几十甚至上百度/小时加速度计零偏有几十个mg。用它们做纯惯性导航几十秒就飘得没影了。这时候多源深度融合和算法优化就成了救命稻草。3.1 简化模型与地磁辅助对于低成本MEMS-IMU地球自转角速度15°/h的信号完全淹没在陀螺噪声和零偏之下。因此在姿态更新算法中我们通常会忽略地球自转项和有害加速度中的哥氏项、向心项。姿态更新简化为只积分陀螺的角增量速度更新简化为将加速度计比力增量转换到导航系后直接积分。这虽然引入了理论误差但对于短时、低精度的应用这些误差远小于传感器自身噪声可以接受。初始对准时靠陀螺感知地球自转来找北是行不通了。我们回归最原始的方法用加速度计找水平用地磁计找北。在静止状态下加速度计的平均输出方向就是重力反方向由此可以解算出俯仰角和横滚角精度还不错1-2度内。但方位角是未知的。这时三轴磁力计就派上用场了。我们知道地磁场矢量在水平面的投影指向磁北。只要我们知道了当地的水平面由加速度计给出和地磁场在载体上的测量值由磁力计给出就能解算出载体相对于磁北的航向角。这里的关键在于磁干扰补偿。地磁场很弱很容易被周围的铁磁物质电机、电池、甚至螺丝干扰。这种干扰分为硬铁干扰固定磁场偏置和软铁干扰随载体姿态变化的磁场畸变。不处理好你的航向角可能会一直指向车里的某块电池。实践中我们通常需要做一个“八字形”或“球面”标定来估计出磁力计的误差参数。此外还需要知道当地的磁偏角地理北和磁北的夹角才能把磁航向转换成真北航向。这个值可以从地磁模型中在线获取或预先设置。3.2 动态条件下的“捷联式地平仪”思想低成本MEMS-IMU在动态下特别是匀速或低机动状态下比如无人机悬停、汽车匀速巡航有一个有趣的现象可以利用此时载体没有明显的线加速度加速度计的输出主要反映的是重力加速度在载体坐标系下的投影。如果姿态有误差那么根据错误姿态矩阵反算出来的重力矢量就会不对从而在水平通道上产生一个虚假的加速度输出。这个虚假的加速度可以被我们用来修正水平姿态角。这种思路被称为“捷联式地平仪”或“加速度计阻尼”。具体实现上我们可以将导航解算得到的水平速度理论上在匀速时应接近零与GNSS或其它传感器提供的水平速度进行比较其差值速度误差主要反映了水平失准角引起的重力投影误差。通过一个简单的比例-积分控制器可以将这个速度误差反馈回去微调姿态四元数从而抑制水平姿态角的发散。这种方法本质上是在动态中创造了一个“类静止”的观测条件。它不能修正航向角但对于稳定水平姿态、抑制俯仰和横滚角的漂移非常有效。在实际的MEMS-IMU/GNSS组合导航中我经常把这种速度阻尼环和卡尔曼滤波结合起来使用滤波负责估计和修正传感器零偏等低频误差而阻尼环则快速响应姿态的高频扰动系统整体稳定性和精度都有不错的表现。4. 工程实践中的调优与避坑指南理论模型是骨架工程实现才是血肉。在实际项目中把算法从仿真环境搬到真实的硬件和复杂的场景中会遇到无数纸上谈兵时想不到的问题。这里分享几个我踩过的坑和总结的经验。4.1 传感器标定是生命线再精巧的算法也救不了没标定好的传感器。对于组合导航系统以下几项标定必须做而且要做好IMU内部标定包括陀螺和加速度计的零偏、比例因子、非正交误差。这通常在出厂时完成但使用前最好在温箱里做一次温度补偿标定因为MEMS传感器的零偏随温度变化很大。安装关系标定IMU与车体b系之间的安装角、IMU与GNSS天线之间的杆臂、IMU与磁力计之间的安装角。杆臂最好用尺子精确测量安装角可以通过多位置静态标定或动态路径标定来估计。一个技巧是让车辆在开阔场地绕“8”字或跑方形轨迹同时记录GNSS、IMU和里程计数据事后处理时可以高精度地解算出这些安装参数。里程计刻度系数标定让车辆在已知长度的平直路面上匀速往返跑几次用GNSS测得的距离除以里程计输出的脉冲总数就能得到比较准确的刻度系数。4.2 滤波器参数调试没有银弹卡尔曼滤波器的性能极度依赖于过程噪声矩阵Q和量测噪声矩阵R的设定。这两个矩阵本质上是你对传感器噪声水平和模型信任程度的量化。Q矩阵反映了状态方程的不确定性。增大陀螺/加速度计零偏的噪声参数意味着你认为它们的零偏变化快滤波器会更信任观测值来修正它们减小则意味着你认为零偏很稳定滤波器会更依赖模型预测。R矩阵反映了观测值的噪声水平。GNSS在开阔天空下R可以设小些在高楼林立区域就要设大些告诉滤波器GNSS此刻不太可靠。调试没有固定公式需要结合传感器数据手册和实际测试。我常用的方法是先看滤波器的新息序列。新息是观测量的预测值与实际值之差。理想情况下新息序列应该是零均值、方差稳定的白噪声。如果新息出现明显的趋势或周期性说明模型有误或噪声参数设得不合适。通过反复调整Q和R使新息序列看起来“最白”通常就能得到不错的滤波效果。4.3 场景自适应与故障检测没有一种算法能通吃所有场景。好的导航系统必须具备场景自适应能力和故障检测机制。静止/运动检测初始对准和某些算法如速度阻尼需要知道载体是否静止。可以通过检查加速度计和陀螺仪输出的方差来判断。GNSS质量判断根据卫星数、PDOP值、载噪比等信息动态调整组合滤波器中GNSS观测量的权重即调整R矩阵。当GNSS信号失效时要能平滑地切换到纯惯性导航或INS/里程计组合模式。磁干扰检测实时计算地磁场矢量的模长。在无磁干扰区域模长应基本恒定。如果模长发生剧烈变化说明进入强磁干扰区此时应降低或完全舍弃磁力计航向的权重更多地依赖陀螺积分和GNSS航向。里程计打滑检测比较由IMU解算出的加速度与由里程计差分得到加速度。如果两者在水平方向上差异持续过大很可能发生了打滑或空转。这些逻辑判断和权重调整往往需要写成一个独立的状态机或专家系统与核心的导航算法模块协同工作。它们虽然不直接贡献导航解算却是系统在实际复杂环境中稳定、可靠运行的“守护神”。导航算法的调试是一个漫长且需要耐心的过程。多跑真实数据多分析滤波器内部状态多思考物理意义比单纯调参要有效得多。当你看到自己设计的系统在复杂的城市道路、在摇晃的无人机上依然能输出稳定、可靠的姿态、速度和位置信息时那种成就感就是对我们工程师最好的回报。