给一台园区巡检小车做定位改造是我第一次真切体会到卫星定位和惯性导航之间的脾气差异。那台车在开阔场地跑得挺稳一钻进树荫和楼缝卫星定位输出就开始跳十几米的漂移说来就来我当时的想法很简单信号不好就切惯性推算结果转弯的时候轨迹开始画圈停车之后速度竟然还不归零。后来花了大半个月重做时间同步、补杆臂、重新估零偏才把这条链路跑通。这篇内容就是把这套东西从小白视角重新捋一遍卫星定位GNSS和惯性导航IMU/INS各自到底在测什么、误差长什么脾气、融合的时候状态量怎么定、实测里最常见的几类异常怎么排查。它适合刚接触导航定位的在校同学、做机器人或车载设备的工程师也适合想快速判断“我这套方案到底够不够用”的产品侧同学。不需要你事先懂卡尔曼滤波但需要你对三维坐标和基本物理量不排斥。1. 一台巡检小车教会我的事两套系统的分工不是主备关系很多人第一次接触这两套东西脑子里会形成一个很自然的印象卫星定位负责给绝对位置惯性导航负责在没信号的时候顶上去。这个理解不能算错但它会把你带到一个很别扭的工程实现上——你会不自觉地把两者做成“开关切换”的关系好用时用AA不行了切B。我当初就是这么干的结果就是在切换点附近出现位置跳变控制器直接报警。真正跑得通的组合方式是让两者同时在线、持续互相修正而不是轮流上岗。这个差别值得展开讲因为它决定了你后面所有代码的组织方式。1.1 GNSS解算的不是“位置”而是一串信号的传播时间卫星定位的接收机最终确实会给你输出经纬高但它在底层真正测量到的物理量是信号从卫星天线发出到进入接收机天线这段时间。光速乘上这个时间就是所谓的伪距。之所以叫“伪”是因为接收机的本地时钟不可能和卫星上的原子钟完全对齐这个钟差会直接折算成距离误差而且量级不小——一微秒的钟差就是三百米。所以接收机至少要同时看到四颗卫星三颗用来解三维位置第四颗用来把接收机钟差这个未知量一起解出来。这也是为什么你在开阔场地看到七八颗卫星觉得“很多”实际上真正决定解算质量的是那几颗几何分布好的卫星而不是数量本身。伪距里混进去的误差项可以列一长串卫星钟差、电离层延迟、对流层延迟、多路径反射、接收机噪声。单点定位把这些误差都留着水平精度大概在几米这个量级差分和增强系统能把公共误差消掉一部分做到亚米级载波相位做整周模糊度固定之后能进到厘米级。这三档对应的硬件成本、使用条件、初始化时间完全不是一个概念后面选型章节会细说。这里先记住一个关键点卫星定位输出的是一个“绝对但有噪声”的位置噪声的大小和卫星几何构型强相关。同一套硬件在高楼夹缝里和在一望无际的农田里表现能差出两个数量级。1.2 IMU输出的不是“姿态”而是加速度和角速度对时间的积分惯性测量单元里面最核心的是两种器件陀螺仪测角速度加速度计测比力。注意是比力不是纯粹的加速度——静止放在桌面上加速度计的输出是一个g因为它测的是“支撑力对你的作用”重力本身也被算进去了。姿态是怎么来的把陀螺仪输出的角速度对时间做积分。位置是怎么来的先把加速度计输出里的重力分量按姿态扣掉得到运动加速度再积分一次得速度再积分一次得位置。两次积分意味着误差会被时间放大两次而姿态误差又会以重力为杠杆耦合进加速度通道所以实际是三阶增长。这就解释了一个很反直觉的现象一颗零偏看起来很小的陀螺短时间用起来“挺准”但只要断掉外部观测位置误差会以一种非常凶的速度扩散。我在小车项目上第一次纯惯导跑六十秒位置飘出去将近两百米当时还以为是坐标系搞错了后来一算零偏量级发现完全对得上。1.3 “GPS没了就切IMU”这种土办法撑不过十秒把上面两段合起来看就明白了。卫星定位的问题是“有噪声偶尔还会跳”惯性导航的问题是“平滑但会持续发散”。如果做成切换你在切换的那一刻会同时继承两个缺点既没有外部观测来压住发散又要面对切换瞬间的状态不连续。更合理的思路是把它们当成一个估计问题用惯性导航做高频的预测用卫星定位做低频的校正中间由一个滤波器持续地权衡“我更信谁”。这也是为什么几乎所有成熟的组合导航实现核心都是一个卡尔曼滤波器或者它的变体而不是一堆if-else。2. 误差的两种脾气一个有界会跳一个顺滑却发散理解了分工下一步必须把两种误差的“性格”摸清楚。这一步不做后面滤波器参数你只能靠试试出来的结果也没法解释。2.1 GNSS误差的边界来自几何构型和多路径卫星定位的误差有一个很好的性质它是有界的。只要卫星几何构型不离谱误差不会随着时间累积下一秒的误差和这一秒无关。这个性质对滤波器来说非常友好因为它意味着你可以用一个相对稳定的观测噪声模型去描述它。影响误差大小的第一个因素是几何精度因子。简单说卫星在天上分布得越散位置解算对测距误差的放大倍数就越小如果几颗卫星挤在同一片天空解算出来的位置就对测距误差极其敏感稍微有点扰动就跳。你可以把接收机输出的精度因子当成一个“当前解算可信度”的指示器超过某个阈值就直接降权或者丢弃这一帧。第二个因素是多路径。信号打到玻璃幕墙、水面、金属车顶再反射进天线走的路径比直线长折算成距离就是正偏差。多路径的典型特征是误差不服从零均值分布它偏向一边而且会随着载体移动缓慢变化。在楼宇密集区伪距多路径误差做到十几米很常见这也是为什么单点定位在高架下面会突然“跑偏”。第三个容易被忽视的是天线。天线的相位中心不等于几何中心而且会随信号入射角变化。如果你做的是厘米级应用天线相位中心偏差必须建标定否则精度上限就被它卡死了。2.2 IMU误差的爆炸式增长这笔账可以直接算惯性导航的误差增长可以写成一个很干净的公式。假设水平方向存在一个常值陀螺零偏 b单位是弧度每秒那么姿态误差随时间线性增长$$\theta(t) b \cdot t$$姿态误差乘以重力加速度就成了水平加速度通道里的虚假输入$$\delta a(t) \approx g \cdot b \cdot t$$再积分两次位置误差$$\delta p(t) \frac{1}{6} g b t^3$$拿具体数字算一遍就很直观了。一弧度每秒对应的是一小时转一圈而工程上常说的度每小时要除以五十七点三一颗零偏 1 度每小时的陀螺单位换算后是 4.85e-6 弧度每秒。断掉外部观测六十秒位置误差约 1.7 米。同一颗陀螺断掉三百秒位置误差约 214 米。再看加速度计这一路。加速度计零偏的影响是二阶的$$\delta p(t) \frac{1}{2} b_a t^2$$一个 1 毫g 的加速度计零偏六十秒就是约 17.6 米。所以加速度计的零偏同样不能放任不管尤其是那些靠“静止时读一个数就扣掉”的土办法温漂一起来就不准了。把不同等级器件的数量级整理成一张表会更清楚器件等级陀螺零偏稳定性典型量级纯惯导 60 秒位置误差常见用途消费级 MEMS10 到 100 度每小时17 到 171 米手机、玩具、低端可穿戴工业级 MEMS1 到 10 度每小时1.7 到 17 米机器人、无人机、车载辅助战术级0.1 到 1 度每小时0.17 到 1.7 米专业测绘、稳定平台导航级0.01 度每小时以下厘米级航空、航海、高精度测绘这张表最大的用处不是让你背参数而是让你在方案阶段就能算出“我这两秒一次卫星观测够不够”。如果器件等级对应的发散速度在你两次观测间隔内已经超过可接受误差那方案就得改换器件或者提高观测频率没有第三条路。2.3 互补性不是玄学滤波器在做的其实是加权平均很多人第一次看卡尔曼滤波的五个方程会觉得抽象但它的直觉非常朴素你有两个来源的信息都不完全可信那就按各自的不确定度做加权平均然后把结果的不确定度也更新一遍。具体到组合导航预测步骤用的是惯性导航的力学模型知道上一时刻的位置速度姿态加上这一时刻陀螺和加速度计的测量推算出当前时刻应该在哪儿。这一步会同时把状态协方差往前推也就是“我对自己的估计越来越不确定了”这个协方差增长的速度直接由器件的噪声参数决定。更新步骤用的是卫星定位给出的观测接收机说我在这个位置还有一个标称精度。滤波器就会算一个增益把预测值和观测值按方差比例融合。卫星观测越差精度因子大、卫星数少增益越小滤波器就越倾向于相信惯性推算反过来静止或低速时卫星解算质量好增益就大能把惯性积累的误差一次性纠回来。这个机制还带来一个额外的好处卫星定位不仅校正了位置还通过观测模型把陀螺零偏和加速度计零偏一起估出来了。因为位置误差里含有零偏的信息滤波器能看到“预测的位置一直在往一个方向偏”就能反推出是零偏在作祟从而持续修正。这是组合导航能长时间保持精度的根本原因。3. 动手前必须钉死的三件事量程、坐标系、时间戳我见过不少项目算法照着一篇论文敲完了数据一跑结果全乱最后发现是时间戳。这三件事看起来基础但每一件都能让你多花一周时间。3.1 选器件别只看“几轴几通道”要盯住四个硬指标市面上很多模块标着九轴、十轴其实把加速度计、陀螺仪、磁力计甚至气压计打包在一起轴数多不代表精度高。真正要看的是下面这几个零偏稳定性衡量常值误差随时间漂移的程度单位一般是度每小时。这个指标直接决定纯惯导能撑多久是选型第一优先级。角度随机游走描述噪声的统计特性单位是度每根号小时。它决定滤波器里过程噪声协方差该设多大设小了滤波器过度自信设大了就跟不住。量程陀螺和加速度计都得覆盖你的实际动态。做车载的要考虑颠簸冲击做机械臂的要考虑快速启停。量程选小了会削顶削顶的后果比噪声严重得多因为它是非线性失真滤波器完全无法建模。带宽与采样率一般 IMU 采样率给到一百到两百赫兹就够用但要注意内部滤波器的截止频率。有些模块默认开了很强的低通动态响应会变慢做高频振动分析就不合适了。还有一点很实用温度漂移必须在实际工作温度范围内标定。出厂标定通常在常温下做装到设备里主板发热、外壳散热条件不同零偏可能整体偏移一截。我一般会在样机阶段做一次简易的温漂测试把设备跑热之后静置记录零偏随温度的变化。3.2 坐标链路每一环都要能说清来路坐标系统这块新手最容易在几个地方翻车地心地固坐标系是卫星定位解算的天然输出原点在地心随地球自转。东北天坐标系是以载体所在点为原点的局部坐标做控制和显示更直观。机体坐标系跟着载体走通常定义成右前上或者前右下取决于你的器件安装方向。从卫星定位到控制输出一般要经过地心地固到东北天东北天到机体最后到具体的传感器安装坐标系。每一环都有旋转矩阵每一环的旋转顺序都不能随意改。欧拉角有十二种旋转顺序你用的到底是哪一种必须和提供数据的那一方对齐。我吃过的一次亏是惯性器件和卫星天线的安装基准面不一致代码里默认两者同向实际上差了大概五度。静止时看不出来一转弯轨迹就开始画圈排查了整整两天才发现是支架装歪了。从那次之后我要求在装配图上明确标出每个传感器的 X 轴指向并且在开机自检里加一条“静止时航向是否与已知基准一致”的检查。另外提醒一句姿态用欧拉角做长时间积分会遇到万向锁九十度附近数值会爆。工程上内部一律用四元数或者旋转矩阵只在输出给人看的时候才转成欧拉角。3.3 时间戳对不齐后面所有融合都是白算这是最隐蔽的一类问题。卫星接收机一般是一赫兹到十赫兹输出惯性器件是一百赫兹以上两路数据来自不同的硬件接口到达处理器的时间有先有后更麻烦的是每一路自己的时间基准可能都不一样。假设卫星定位的观测比惯性数据晚了五十毫秒而载体正以十米每秒运动那就是零点五米的系统性位置偏差。滤波器会把这点偏差当成惯性漂移去修正结果就是把零偏估歪反过来污染后面的所有预测。处理办法有几层第一层硬件同步。能用秒脉冲信号同步的尽量用让接收机在每个整秒输出一个电平跳变惯性采集卡收到这个跳变就把计数器清零。这一层做到了时间对齐误差能压到微秒级。第二层软件时间戳。如果硬件同步做不到至少保证两路数据进到同一个时钟域。我的做法是在数据采集的最前端统一打时间戳而不是在各个模块内部各打各的。第三层插值对齐。融合的时候把惯性数据按时间插值到卫星观测的时刻或者反过来。插值阶数按你的动态来定低速场景线性插值够用高速高动态建议用二次或者三次。提示排查时间同步问题时一个很有效的办法是让设备做一次匀速直线运动再急停观察卫星输出的速度和惯性积分出来的速度在时间轴上的错位。如果两条曲线形状一致但有水平平移那就是时间戳问题如果形状都不一样那要往坐标系或者标定上找。4. 从零搭一套组合导航状态量怎么定量测怎么喂到这里基础就打完了可以进到实现层面。我不打算给一份完整的可运行代码因为不同平台的接口差异太大给了也跑不起来。更有价值的是把结构讲清楚让你知道每一块该填什么。4.1 十五维状态向量是怎么来的组合导航里最常用的是十五维误差状态卡尔曼滤波。为什么是十五可以从“你想估什么”倒推位置误差三个方向速度误差三个方向姿态误差三个方向陀螺零偏三个方向加速度计零偏三个方向加起来十五。注意这里估的是误差量而不是绝对量滤波输出的是对当前惯导推算结果的修正量这样线性化误差小数值也更稳定。每次滤波更新后把误差量反馈回去修正惯导状态然后把误差状态清零这叫反馈校正。状态转移矩阵的核心是惯导的误差传播方程。手工推一遍会很长但结构上可以拆成几块位置对速度的耦合、速度对姿态的耦合这一块带重力、姿态对陀螺零偏的耦合剩下大部分是零。实际写代码的时候我一般先用解析形式搭出骨架再用数值雅可比做验证两边对上了才敢往下走。过程噪声矩阵决定滤波器有多“相信”惯导模型。它的对角元素直接由器件的角度随机游走和速度随机游走换算不要凭感觉填。这一块填错的典型症状是静止时滤波器收敛很慢或者动态时明显滞后。4.2 松耦合起步最稳别一上来就紧耦合按卫星观测进入滤波器的方式组合导航分三个层次松耦合接收机自己先解算出位置和速度把这个结果连同精度指标一起送给滤波器当观测。优点是实现简单接收机可以当黑盒用换型号不用改算法。缺点是在卫星数少于四颗时接收机自己就解不出位置组合也就断了。另外接收机内部的滤波会引入时间相关性严格来说不完全符合卡尔曼滤波的观测独立假设工程上一般忽略。紧耦合把伪距和伪距率直接送进滤波器卫星观测和惯性状态在原始观测层面融合。好处是哪怕只剩一两颗卫星也能提供部分约束在城市峡谷和隧道口这种场景优势明显。代价是你要能拿到接收机的原始观测还要处理钟差状态实现复杂度上一个台阶。深耦合进一步把惯性信息送进接收机的信号跟踪环路用惯性预测的多普勒频移去辅助载波环弱信号下能显著改善跟踪性能。这一层基本是接收机厂商做的事普通开发者接触不到。我的建议很明确第一版一定做松耦合。它能让你把坐标系、时间同步、零偏估计这些基础问题全部暴露并解决掉而且性能在开阔场景下完全够用。等你把松耦合跑稳了再考虑要不要往紧耦合走。松耦合的观测模型很简单位置和速度观测直接对应状态里的位置速度分量观测矩阵就是一个分块对角矩阵import numpy as np # 15维误差状态: [dp(3), dv(3), dtheta(3), bg(3), ba(3)] H np.zeros((6, 15)) H[0:3, 0:3] np.eye(3) # 位置观测对应位置误差 H[3:6, 3:6] np.eye(3) # 速度观测对应速度误差 # 观测残差 GNSS结果 - 惯导推算结果(已做杆臂补偿) # 观测噪声协方差按接收机给出的精度指标构造, 水平与垂直分开设 R np.diag([sig_px**2, sig_py**2, sig_pz**2, sig_vx**2, sig_vy**2, sig_vz**2])要注意R里的噪声不能填成常数。接收机在不同环境下的实际精度差异很大我一般会拿卫星数、精度因子、差分状态这几个量做一个简单的分段映射环境差的时候把噪声放大几倍让滤波器自动降低对这一帧的信任。4.3 一份能跑通的最小闭环流程把整个链路按执行顺序列一遍你照这个顺序搭基本不会漏东西传感器采集层惯性数据按固定周期进环形缓冲区卫星数据进另一个缓冲区两路都在入口处打统一时间戳。初始对准静止若干秒用加速度计输出估计横滚和俯仰用卫星速度方向或者磁力计估计航向。这一步的精度直接决定后面收敛速度能多等几秒就多等几秒。惯导机械编排按上一个时刻的姿态、速度、位置加上这一时刻的角速度和比力推算新状态。姿态用四元数更新注意用等效旋转矢量做圆锥补偿高速旋转时不做补偿会有明显误差。时间对齐查卫星缓冲区里有没有落在当前惯导周期附近的观测有就取出并做插值对齐。滤波预测状态转移矩阵按时间间隔更新协方差往前推。滤波更新有卫星观测就做更新算增益、修正状态、修正协方差。反馈校正把误差状态加回惯导的绝对状态误差状态清零。输出按需要转成欧拉角和东北天坐标送给上层。整个循环里最容易出问题的是第 3 步和第 4 步。第 3 步的圆锥补偿如果漏了载体做高频摆动时姿态会有系统性偏差第 4 步如果对齐逻辑写错比如取到了未来时刻的数据滤波器会直接发散而且报错信息完全不指向问题根源。5. 实测里最耗时间的几类异常数据都对结果不对算法跑起来只是开始真正花时间的是把异常一个个揪出来。下面这几类是我踩过或者帮别人排查过的按出现频率排序。5.1 静止时速度不为零八成是零偏没估对这个现象很典型设备放在桌上不动输出的速度却在缓慢漂移几分钟能漂到零点几米每秒。原因通常是加速度计的零偏没有被正确估计或者零偏估出来了但符号搞反了。排查顺序我一般这么走先看原始加速度计三轴输出静止时应该是稳定的常值如果看到明显漂移那就是器件温漂或者供电不稳再看滤波器的零偏状态估计值如果它一直在往一个方向跑而且不发散说明观测信息不足以约束它最后检查量测更新时用的速度观测静止时卫星输出的速度本身就有噪声如果噪声设得太小滤波器会被这些噪声带着走。一个很实用的工程手段是加零速修正检测到载体静止加速度方差小、角速度小、卫星速度小三个条件同时满足时直接构造一个速度为零的伪观测送进滤波器。这一招在车载和行人导航里效果非常好能把静止段的漂移压到几乎为零。5.2 转弯画圈与杆臂补偿轨迹在转弯时突然向外扩出一圈转完又回来这个现象我在开头提到过根因是卫星天线和惯性单元不在同一个物理位置。载体转动时两个点的速度不一样差值是角速度叉乘杆臂向量$$\vec{v}{ant} \vec{v}{imu} \vec{\omega} \times \vec{r}$$其中 r 是从惯性单元指向天线的向量在机体坐标系里是个常值。转弯角速度大的时候这个差值可能达到每秒几十厘米如果没补偿滤波器会把它当成速度观测误差进而污染零偏估计。补偿的做法很简单在构造观测残差之前先把惯导推算出的速度用当前角速度做一次杆臂修正再和卫星速度相减。前提是你得把 r 量准我一般要求装配时用卡尺量三个方向的距离误差控制在厘米级别靠目测。同理如果设备上还装了轮速计或者视觉传感器它们和惯性单元之间的杆臂也都要补。5.3 城市环境下的多路径与卫星数骤降进到楼宇密集区你会看到两个现象同时发生卫星数从十几颗掉到五六颗精度因子迅速变大同时位置开始出现几米的系统性偏移。这时候如果滤波器还按开阔地的噪声模型去信任这一帧位置就会被带跑。处理思路分三层。第一层是观测质量控制用卫星数、精度因子、载噪比做一个综合评分低于阈值直接不进滤波。第二层是自适应噪声把观测噪声协方差按评分放大让滤波器自己降低权重。第三层是引入额外的约束比如载体运动学约束车轮不会侧滑、行人步速有上限在卫星观测不可信的时候顶上去。还有一个容易忽略的点卫星失锁再恢复的瞬间不要立刻全信。接收机重新捕获后前几帧的解算质量往往不稳我一般会设一个短时间的降权窗口等解算稳定了再恢复正常权重。6. 不同场景的配置思路同一套算法参数差很远组合导航的算法框架是通用的但参数和辅助手段必须按场景调。这一节按我实际接触过的几类场景说一下差异。6.1 低速地面机器人这类场景的特点是速度低、加速度小、转弯频繁。器件选工业级 MEMS 就够陀螺零偏在几度每小时这个量级配合一秒一次的卫星观测开阔场景下位置误差能稳定在分米级。重点要处理的是低速段的航向观测。速度低于每秒零点五米时卫星输出的速度方向噪声很大用它来校正航向反而会把航向带偏。我的做法是在低速段把速度观测的权重降下来同时依赖磁力计或者双天线定向来提供航向约束。如果场地里有金属结构干扰磁力计那双天线定向基本是必选项。另一个实用技巧是把轮式里程计接进来。里程计给出的速度虽然尺度因子不准但短期一致性很好做非完整性约束车体侧向速度为零非常有效能显著改善卫星短时失锁时的表现。6.2 车载与户外移动平台车载的动态范围大得多急加速、急刹车、过减速带都会带来冲击。器件量程要留足余量加速度计建议至少正负八g陀螺至少正负三百度每秒。车载场景最大的优势是有比较强的运动约束可用正常情况下车辆不会侧滑垂向速度接近零这些都可以做成伪观测。另外一个很有效的做法是用车辆的运动模型去约束航向变化率尤其在隧道里卫星完全失锁的时候光靠陀螺积分航向一分钟就能漂出去好几度加上转向约束会好很多。隧道场景还要注意一点出隧道瞬间卫星会一次性恢复很多颗如果直接全部接收位置会有一个跳变。建议做一个渐进的权重恢复策略几百毫秒内逐步提高信任度。6.3 手持与可穿戴设备这一类对体积和功耗极其敏感用的基本都是消费级器件陀螺零偏几十度每小时很常见。纯靠惯性加卫星断掉卫星几秒钟位置就飘得没法看。所以这类产品几乎一定要引入额外的传感器做辅助磁力计提供绝对航向气压计提供高度变化甚至运动分类器识别走路、跑步、静止等状态不同状态下用不同的过程噪声。行人导航里还有一套专门的步态推算方法用步频和步长模型去推位移和惯性积分互补。我个人的经验是消费级器件做组合导航把精力花在状态检测和自适应噪声上收益远大于去抠滤波器的数学细节。器件本身的噪声水平摆在那儿算法再精巧也变不出精度但一个准确的静止检测能让静止段的漂移直接归零。7. 几个我反复验证过的小经验最后说几条不太适合写进教科书但确实管用的东西。第一先做数据回放再做在线调试。把所有传感器数据录成文件离线反复跑同一段数据改一次参数跑一次比对结果。在线调试的时候你只能看到当前几秒的表现很多慢性问题根本暴露不出来。我现在的习惯是每换一个场地就录一段攒成一个测试集每次改算法都拿它回归一遍。第二滤波器发散先看协方差不要先改噪声参数。协方差矩阵如果出现负定或者数值爆炸通常是状态转移矩阵写错或者时间间隔用了负数跟噪声参数没关系。加一个对称性检查和正定性检查能在早期就拦住这类问题。第三初始对准的时间不要省。很多产品为了用户体验开机一秒就要求出结果代价是初始航向误差好几度后面要靠卫星慢慢纠前几分钟的轨迹都是歪的。如果场景允许宁可多等五秒。第四把中间变量都打日志。卫星观测残差、滤波增益、零偏估计值、协方差对角元这些量在正常运行时应该是什么范围你心里要有个数。出问题的时候看一眼残差是不是突然变大就能判断是观测的问题还是预测的问题排查效率差好几倍。第五别迷信精度指标。厂商标称的精度都是在特定条件下测的实际装到你的结构上、走你的线束、在你的温度环境里跑结果可能差不少。样机阶段的实测数据比任何规格书都可靠这也是为什么我坚持每个项目都要做一轮完整的静态和动态标定哪怕看起来是浪费时间。 
长河君 · 2026/10/29 16:12:58 · 阅读 1.2k GNSS与IMU组合导航：原理、状态量设计与实测异常排查
给一台园区巡检小车做定位改造是我第一次真切体会到卫星定位和惯性导航之间的脾气差异。那台车在开阔场地跑得挺稳一钻进树荫和楼缝卫星定位输出就开始跳十几米的漂移说来就来我当时的想法很简单信号不好就切惯性推算结果转弯的时候轨迹开始画圈停车之后速度竟然还不归零。后来花了大半个月重做时间同步、补杆臂、重新估零偏才把这条链路跑通。这篇内容就是把这套东西从小白视角重新捋一遍卫星定位GNSS和惯性导航IMU/INS各自到底在测什么、误差长什么脾气、融合的时候状态量怎么定、实测里最常见的几类异常怎么排查。它适合刚接触导航定位的在校同学、做机器人或车载设备的工程师也适合想快速判断“我这套方案到底够不够用”的产品侧同学。不需要你事先懂卡尔曼滤波但需要你对三维坐标和基本物理量不排斥。1. 一台巡检小车教会我的事两套系统的分工不是主备关系很多人第一次接触这两套东西脑子里会形成一个很自然的印象卫星定位负责给绝对位置惯性导航负责在没信号的时候顶上去。这个理解不能算错但它会把你带到一个很别扭的工程实现上——你会不自觉地把两者做成“开关切换”的关系好用时用AA不行了切B。我当初就是这么干的结果就是在切换点附近出现位置跳变控制器直接报警。真正跑得通的组合方式是让两者同时在线、持续互相修正而不是轮流上岗。这个差别值得展开讲因为它决定了你后面所有代码的组织方式。1.1 GNSS解算的不是“位置”而是一串信号的传播时间卫星定位的接收机最终确实会给你输出经纬高但它在底层真正测量到的物理量是信号从卫星天线发出到进入接收机天线这段时间。光速乘上这个时间就是所谓的伪距。之所以叫“伪”是因为接收机的本地时钟不可能和卫星上的原子钟完全对齐这个钟差会直接折算成距离误差而且量级不小——一微秒的钟差就是三百米。所以接收机至少要同时看到四颗卫星三颗用来解三维位置第四颗用来把接收机钟差这个未知量一起解出来。这也是为什么你在开阔场地看到七八颗卫星觉得“很多”实际上真正决定解算质量的是那几颗几何分布好的卫星而不是数量本身。伪距里混进去的误差项可以列一长串卫星钟差、电离层延迟、对流层延迟、多路径反射、接收机噪声。单点定位把这些误差都留着水平精度大概在几米这个量级差分和增强系统能把公共误差消掉一部分做到亚米级载波相位做整周模糊度固定之后能进到厘米级。这三档对应的硬件成本、使用条件、初始化时间完全不是一个概念后面选型章节会细说。这里先记住一个关键点卫星定位输出的是一个“绝对但有噪声”的位置噪声的大小和卫星几何构型强相关。同一套硬件在高楼夹缝里和在一望无际的农田里表现能差出两个数量级。1.2 IMU输出的不是“姿态”而是加速度和角速度对时间的积分惯性测量单元里面最核心的是两种器件陀螺仪测角速度加速度计测比力。注意是比力不是纯粹的加速度——静止放在桌面上加速度计的输出是一个g因为它测的是“支撑力对你的作用”重力本身也被算进去了。姿态是怎么来的把陀螺仪输出的角速度对时间做积分。位置是怎么来的先把加速度计输出里的重力分量按姿态扣掉得到运动加速度再积分一次得速度再积分一次得位置。两次积分意味着误差会被时间放大两次而姿态误差又会以重力为杠杆耦合进加速度通道所以实际是三阶增长。这就解释了一个很反直觉的现象一颗零偏看起来很小的陀螺短时间用起来“挺准”但只要断掉外部观测位置误差会以一种非常凶的速度扩散。我在小车项目上第一次纯惯导跑六十秒位置飘出去将近两百米当时还以为是坐标系搞错了后来一算零偏量级发现完全对得上。1.3 “GPS没了就切IMU”这种土办法撑不过十秒把上面两段合起来看就明白了。卫星定位的问题是“有噪声偶尔还会跳”惯性导航的问题是“平滑但会持续发散”。如果做成切换你在切换的那一刻会同时继承两个缺点既没有外部观测来压住发散又要面对切换瞬间的状态不连续。更合理的思路是把它们当成一个估计问题用惯性导航做高频的预测用卫星定位做低频的校正中间由一个滤波器持续地权衡“我更信谁”。这也是为什么几乎所有成熟的组合导航实现核心都是一个卡尔曼滤波器或者它的变体而不是一堆if-else。2. 误差的两种脾气一个有界会跳一个顺滑却发散理解了分工下一步必须把两种误差的“性格”摸清楚。这一步不做后面滤波器参数你只能靠试试出来的结果也没法解释。2.1 GNSS误差的边界来自几何构型和多路径卫星定位的误差有一个很好的性质它是有界的。只要卫星几何构型不离谱误差不会随着时间累积下一秒的误差和这一秒无关。这个性质对滤波器来说非常友好因为它意味着你可以用一个相对稳定的观测噪声模型去描述它。影响误差大小的第一个因素是几何精度因子。简单说卫星在天上分布得越散位置解算对测距误差的放大倍数就越小如果几颗卫星挤在同一片天空解算出来的位置就对测距误差极其敏感稍微有点扰动就跳。你可以把接收机输出的精度因子当成一个“当前解算可信度”的指示器超过某个阈值就直接降权或者丢弃这一帧。第二个因素是多路径。信号打到玻璃幕墙、水面、金属车顶再反射进天线走的路径比直线长折算成距离就是正偏差。多路径的典型特征是误差不服从零均值分布它偏向一边而且会随着载体移动缓慢变化。在楼宇密集区伪距多路径误差做到十几米很常见这也是为什么单点定位在高架下面会突然“跑偏”。第三个容易被忽视的是天线。天线的相位中心不等于几何中心而且会随信号入射角变化。如果你做的是厘米级应用天线相位中心偏差必须建标定否则精度上限就被它卡死了。2.2 IMU误差的爆炸式增长这笔账可以直接算惯性导航的误差增长可以写成一个很干净的公式。假设水平方向存在一个常值陀螺零偏 b单位是弧度每秒那么姿态误差随时间线性增长$$\theta(t) b \cdot t$$姿态误差乘以重力加速度就成了水平加速度通道里的虚假输入$$\delta a(t) \approx g \cdot b \cdot t$$再积分两次位置误差$$\delta p(t) \frac{1}{6} g b t^3$$拿具体数字算一遍就很直观了。一弧度每秒对应的是一小时转一圈而工程上常说的度每小时要除以五十七点三一颗零偏 1 度每小时的陀螺单位换算后是 4.85e-6 弧度每秒。断掉外部观测六十秒位置误差约 1.7 米。同一颗陀螺断掉三百秒位置误差约 214 米。再看加速度计这一路。加速度计零偏的影响是二阶的$$\delta p(t) \frac{1}{2} b_a t^2$$一个 1 毫g 的加速度计零偏六十秒就是约 17.6 米。所以加速度计的零偏同样不能放任不管尤其是那些靠“静止时读一个数就扣掉”的土办法温漂一起来就不准了。把不同等级器件的数量级整理成一张表会更清楚器件等级陀螺零偏稳定性典型量级纯惯导 60 秒位置误差常见用途消费级 MEMS10 到 100 度每小时17 到 171 米手机、玩具、低端可穿戴工业级 MEMS1 到 10 度每小时1.7 到 17 米机器人、无人机、车载辅助战术级0.1 到 1 度每小时0.17 到 1.7 米专业测绘、稳定平台导航级0.01 度每小时以下厘米级航空、航海、高精度测绘这张表最大的用处不是让你背参数而是让你在方案阶段就能算出“我这两秒一次卫星观测够不够”。如果器件等级对应的发散速度在你两次观测间隔内已经超过可接受误差那方案就得改换器件或者提高观测频率没有第三条路。2.3 互补性不是玄学滤波器在做的其实是加权平均很多人第一次看卡尔曼滤波的五个方程会觉得抽象但它的直觉非常朴素你有两个来源的信息都不完全可信那就按各自的不确定度做加权平均然后把结果的不确定度也更新一遍。具体到组合导航预测步骤用的是惯性导航的力学模型知道上一时刻的位置速度姿态加上这一时刻陀螺和加速度计的测量推算出当前时刻应该在哪儿。这一步会同时把状态协方差往前推也就是“我对自己的估计越来越不确定了”这个协方差增长的速度直接由器件的噪声参数决定。更新步骤用的是卫星定位给出的观测接收机说我在这个位置还有一个标称精度。滤波器就会算一个增益把预测值和观测值按方差比例融合。卫星观测越差精度因子大、卫星数少增益越小滤波器就越倾向于相信惯性推算反过来静止或低速时卫星解算质量好增益就大能把惯性积累的误差一次性纠回来。这个机制还带来一个额外的好处卫星定位不仅校正了位置还通过观测模型把陀螺零偏和加速度计零偏一起估出来了。因为位置误差里含有零偏的信息滤波器能看到“预测的位置一直在往一个方向偏”就能反推出是零偏在作祟从而持续修正。这是组合导航能长时间保持精度的根本原因。3. 动手前必须钉死的三件事量程、坐标系、时间戳我见过不少项目算法照着一篇论文敲完了数据一跑结果全乱最后发现是时间戳。这三件事看起来基础但每一件都能让你多花一周时间。3.1 选器件别只看“几轴几通道”要盯住四个硬指标市面上很多模块标着九轴、十轴其实把加速度计、陀螺仪、磁力计甚至气压计打包在一起轴数多不代表精度高。真正要看的是下面这几个零偏稳定性衡量常值误差随时间漂移的程度单位一般是度每小时。这个指标直接决定纯惯导能撑多久是选型第一优先级。角度随机游走描述噪声的统计特性单位是度每根号小时。它决定滤波器里过程噪声协方差该设多大设小了滤波器过度自信设大了就跟不住。量程陀螺和加速度计都得覆盖你的实际动态。做车载的要考虑颠簸冲击做机械臂的要考虑快速启停。量程选小了会削顶削顶的后果比噪声严重得多因为它是非线性失真滤波器完全无法建模。带宽与采样率一般 IMU 采样率给到一百到两百赫兹就够用但要注意内部滤波器的截止频率。有些模块默认开了很强的低通动态响应会变慢做高频振动分析就不合适了。还有一点很实用温度漂移必须在实际工作温度范围内标定。出厂标定通常在常温下做装到设备里主板发热、外壳散热条件不同零偏可能整体偏移一截。我一般会在样机阶段做一次简易的温漂测试把设备跑热之后静置记录零偏随温度的变化。3.2 坐标链路每一环都要能说清来路坐标系统这块新手最容易在几个地方翻车地心地固坐标系是卫星定位解算的天然输出原点在地心随地球自转。东北天坐标系是以载体所在点为原点的局部坐标做控制和显示更直观。机体坐标系跟着载体走通常定义成右前上或者前右下取决于你的器件安装方向。从卫星定位到控制输出一般要经过地心地固到东北天东北天到机体最后到具体的传感器安装坐标系。每一环都有旋转矩阵每一环的旋转顺序都不能随意改。欧拉角有十二种旋转顺序你用的到底是哪一种必须和提供数据的那一方对齐。我吃过的一次亏是惯性器件和卫星天线的安装基准面不一致代码里默认两者同向实际上差了大概五度。静止时看不出来一转弯轨迹就开始画圈排查了整整两天才发现是支架装歪了。从那次之后我要求在装配图上明确标出每个传感器的 X 轴指向并且在开机自检里加一条“静止时航向是否与已知基准一致”的检查。另外提醒一句姿态用欧拉角做长时间积分会遇到万向锁九十度附近数值会爆。工程上内部一律用四元数或者旋转矩阵只在输出给人看的时候才转成欧拉角。3.3 时间戳对不齐后面所有融合都是白算这是最隐蔽的一类问题。卫星接收机一般是一赫兹到十赫兹输出惯性器件是一百赫兹以上两路数据来自不同的硬件接口到达处理器的时间有先有后更麻烦的是每一路自己的时间基准可能都不一样。假设卫星定位的观测比惯性数据晚了五十毫秒而载体正以十米每秒运动那就是零点五米的系统性位置偏差。滤波器会把这点偏差当成惯性漂移去修正结果就是把零偏估歪反过来污染后面的所有预测。处理办法有几层第一层硬件同步。能用秒脉冲信号同步的尽量用让接收机在每个整秒输出一个电平跳变惯性采集卡收到这个跳变就把计数器清零。这一层做到了时间对齐误差能压到微秒级。第二层软件时间戳。如果硬件同步做不到至少保证两路数据进到同一个时钟域。我的做法是在数据采集的最前端统一打时间戳而不是在各个模块内部各打各的。第三层插值对齐。融合的时候把惯性数据按时间插值到卫星观测的时刻或者反过来。插值阶数按你的动态来定低速场景线性插值够用高速高动态建议用二次或者三次。提示排查时间同步问题时一个很有效的办法是让设备做一次匀速直线运动再急停观察卫星输出的速度和惯性积分出来的速度在时间轴上的错位。如果两条曲线形状一致但有水平平移那就是时间戳问题如果形状都不一样那要往坐标系或者标定上找。4. 从零搭一套组合导航状态量怎么定量测怎么喂到这里基础就打完了可以进到实现层面。我不打算给一份完整的可运行代码因为不同平台的接口差异太大给了也跑不起来。更有价值的是把结构讲清楚让你知道每一块该填什么。4.1 十五维状态向量是怎么来的组合导航里最常用的是十五维误差状态卡尔曼滤波。为什么是十五可以从“你想估什么”倒推位置误差三个方向速度误差三个方向姿态误差三个方向陀螺零偏三个方向加速度计零偏三个方向加起来十五。注意这里估的是误差量而不是绝对量滤波输出的是对当前惯导推算结果的修正量这样线性化误差小数值也更稳定。每次滤波更新后把误差量反馈回去修正惯导状态然后把误差状态清零这叫反馈校正。状态转移矩阵的核心是惯导的误差传播方程。手工推一遍会很长但结构上可以拆成几块位置对速度的耦合、速度对姿态的耦合这一块带重力、姿态对陀螺零偏的耦合剩下大部分是零。实际写代码的时候我一般先用解析形式搭出骨架再用数值雅可比做验证两边对上了才敢往下走。过程噪声矩阵决定滤波器有多“相信”惯导模型。它的对角元素直接由器件的角度随机游走和速度随机游走换算不要凭感觉填。这一块填错的典型症状是静止时滤波器收敛很慢或者动态时明显滞后。4.2 松耦合起步最稳别一上来就紧耦合按卫星观测进入滤波器的方式组合导航分三个层次松耦合接收机自己先解算出位置和速度把这个结果连同精度指标一起送给滤波器当观测。优点是实现简单接收机可以当黑盒用换型号不用改算法。缺点是在卫星数少于四颗时接收机自己就解不出位置组合也就断了。另外接收机内部的滤波会引入时间相关性严格来说不完全符合卡尔曼滤波的观测独立假设工程上一般忽略。紧耦合把伪距和伪距率直接送进滤波器卫星观测和惯性状态在原始观测层面融合。好处是哪怕只剩一两颗卫星也能提供部分约束在城市峡谷和隧道口这种场景优势明显。代价是你要能拿到接收机的原始观测还要处理钟差状态实现复杂度上一个台阶。深耦合进一步把惯性信息送进接收机的信号跟踪环路用惯性预测的多普勒频移去辅助载波环弱信号下能显著改善跟踪性能。这一层基本是接收机厂商做的事普通开发者接触不到。我的建议很明确第一版一定做松耦合。它能让你把坐标系、时间同步、零偏估计这些基础问题全部暴露并解决掉而且性能在开阔场景下完全够用。等你把松耦合跑稳了再考虑要不要往紧耦合走。松耦合的观测模型很简单位置和速度观测直接对应状态里的位置速度分量观测矩阵就是一个分块对角矩阵import numpy as np # 15维误差状态: [dp(3), dv(3), dtheta(3), bg(3), ba(3)] H np.zeros((6, 15)) H[0:3, 0:3] np.eye(3) # 位置观测对应位置误差 H[3:6, 3:6] np.eye(3) # 速度观测对应速度误差 # 观测残差 GNSS结果 - 惯导推算结果(已做杆臂补偿) # 观测噪声协方差按接收机给出的精度指标构造, 水平与垂直分开设 R np.diag([sig_px**2, sig_py**2, sig_pz**2, sig_vx**2, sig_vy**2, sig_vz**2])要注意R里的噪声不能填成常数。接收机在不同环境下的实际精度差异很大我一般会拿卫星数、精度因子、差分状态这几个量做一个简单的分段映射环境差的时候把噪声放大几倍让滤波器自动降低对这一帧的信任。4.3 一份能跑通的最小闭环流程把整个链路按执行顺序列一遍你照这个顺序搭基本不会漏东西传感器采集层惯性数据按固定周期进环形缓冲区卫星数据进另一个缓冲区两路都在入口处打统一时间戳。初始对准静止若干秒用加速度计输出估计横滚和俯仰用卫星速度方向或者磁力计估计航向。这一步的精度直接决定后面收敛速度能多等几秒就多等几秒。惯导机械编排按上一个时刻的姿态、速度、位置加上这一时刻的角速度和比力推算新状态。姿态用四元数更新注意用等效旋转矢量做圆锥补偿高速旋转时不做补偿会有明显误差。时间对齐查卫星缓冲区里有没有落在当前惯导周期附近的观测有就取出并做插值对齐。滤波预测状态转移矩阵按时间间隔更新协方差往前推。滤波更新有卫星观测就做更新算增益、修正状态、修正协方差。反馈校正把误差状态加回惯导的绝对状态误差状态清零。输出按需要转成欧拉角和东北天坐标送给上层。整个循环里最容易出问题的是第 3 步和第 4 步。第 3 步的圆锥补偿如果漏了载体做高频摆动时姿态会有系统性偏差第 4 步如果对齐逻辑写错比如取到了未来时刻的数据滤波器会直接发散而且报错信息完全不指向问题根源。5. 实测里最耗时间的几类异常数据都对结果不对算法跑起来只是开始真正花时间的是把异常一个个揪出来。下面这几类是我踩过或者帮别人排查过的按出现频率排序。5.1 静止时速度不为零八成是零偏没估对这个现象很典型设备放在桌上不动输出的速度却在缓慢漂移几分钟能漂到零点几米每秒。原因通常是加速度计的零偏没有被正确估计或者零偏估出来了但符号搞反了。排查顺序我一般这么走先看原始加速度计三轴输出静止时应该是稳定的常值如果看到明显漂移那就是器件温漂或者供电不稳再看滤波器的零偏状态估计值如果它一直在往一个方向跑而且不发散说明观测信息不足以约束它最后检查量测更新时用的速度观测静止时卫星输出的速度本身就有噪声如果噪声设得太小滤波器会被这些噪声带着走。一个很实用的工程手段是加零速修正检测到载体静止加速度方差小、角速度小、卫星速度小三个条件同时满足时直接构造一个速度为零的伪观测送进滤波器。这一招在车载和行人导航里效果非常好能把静止段的漂移压到几乎为零。5.2 转弯画圈与杆臂补偿轨迹在转弯时突然向外扩出一圈转完又回来这个现象我在开头提到过根因是卫星天线和惯性单元不在同一个物理位置。载体转动时两个点的速度不一样差值是角速度叉乘杆臂向量$$\vec{v}{ant} \vec{v}{imu} \vec{\omega} \times \vec{r}$$其中 r 是从惯性单元指向天线的向量在机体坐标系里是个常值。转弯角速度大的时候这个差值可能达到每秒几十厘米如果没补偿滤波器会把它当成速度观测误差进而污染零偏估计。补偿的做法很简单在构造观测残差之前先把惯导推算出的速度用当前角速度做一次杆臂修正再和卫星速度相减。前提是你得把 r 量准我一般要求装配时用卡尺量三个方向的距离误差控制在厘米级别靠目测。同理如果设备上还装了轮速计或者视觉传感器它们和惯性单元之间的杆臂也都要补。5.3 城市环境下的多路径与卫星数骤降进到楼宇密集区你会看到两个现象同时发生卫星数从十几颗掉到五六颗精度因子迅速变大同时位置开始出现几米的系统性偏移。这时候如果滤波器还按开阔地的噪声模型去信任这一帧位置就会被带跑。处理思路分三层。第一层是观测质量控制用卫星数、精度因子、载噪比做一个综合评分低于阈值直接不进滤波。第二层是自适应噪声把观测噪声协方差按评分放大让滤波器自己降低权重。第三层是引入额外的约束比如载体运动学约束车轮不会侧滑、行人步速有上限在卫星观测不可信的时候顶上去。还有一个容易忽略的点卫星失锁再恢复的瞬间不要立刻全信。接收机重新捕获后前几帧的解算质量往往不稳我一般会设一个短时间的降权窗口等解算稳定了再恢复正常权重。6. 不同场景的配置思路同一套算法参数差很远组合导航的算法框架是通用的但参数和辅助手段必须按场景调。这一节按我实际接触过的几类场景说一下差异。6.1 低速地面机器人这类场景的特点是速度低、加速度小、转弯频繁。器件选工业级 MEMS 就够陀螺零偏在几度每小时这个量级配合一秒一次的卫星观测开阔场景下位置误差能稳定在分米级。重点要处理的是低速段的航向观测。速度低于每秒零点五米时卫星输出的速度方向噪声很大用它来校正航向反而会把航向带偏。我的做法是在低速段把速度观测的权重降下来同时依赖磁力计或者双天线定向来提供航向约束。如果场地里有金属结构干扰磁力计那双天线定向基本是必选项。另一个实用技巧是把轮式里程计接进来。里程计给出的速度虽然尺度因子不准但短期一致性很好做非完整性约束车体侧向速度为零非常有效能显著改善卫星短时失锁时的表现。6.2 车载与户外移动平台车载的动态范围大得多急加速、急刹车、过减速带都会带来冲击。器件量程要留足余量加速度计建议至少正负八g陀螺至少正负三百度每秒。车载场景最大的优势是有比较强的运动约束可用正常情况下车辆不会侧滑垂向速度接近零这些都可以做成伪观测。另外一个很有效的做法是用车辆的运动模型去约束航向变化率尤其在隧道里卫星完全失锁的时候光靠陀螺积分航向一分钟就能漂出去好几度加上转向约束会好很多。隧道场景还要注意一点出隧道瞬间卫星会一次性恢复很多颗如果直接全部接收位置会有一个跳变。建议做一个渐进的权重恢复策略几百毫秒内逐步提高信任度。6.3 手持与可穿戴设备这一类对体积和功耗极其敏感用的基本都是消费级器件陀螺零偏几十度每小时很常见。纯靠惯性加卫星断掉卫星几秒钟位置就飘得没法看。所以这类产品几乎一定要引入额外的传感器做辅助磁力计提供绝对航向气压计提供高度变化甚至运动分类器识别走路、跑步、静止等状态不同状态下用不同的过程噪声。行人导航里还有一套专门的步态推算方法用步频和步长模型去推位移和惯性积分互补。我个人的经验是消费级器件做组合导航把精力花在状态检测和自适应噪声上收益远大于去抠滤波器的数学细节。器件本身的噪声水平摆在那儿算法再精巧也变不出精度但一个准确的静止检测能让静止段的漂移直接归零。7. 几个我反复验证过的小经验最后说几条不太适合写进教科书但确实管用的东西。第一先做数据回放再做在线调试。把所有传感器数据录成文件离线反复跑同一段数据改一次参数跑一次比对结果。在线调试的时候你只能看到当前几秒的表现很多慢性问题根本暴露不出来。我现在的习惯是每换一个场地就录一段攒成一个测试集每次改算法都拿它回归一遍。第二滤波器发散先看协方差不要先改噪声参数。协方差矩阵如果出现负定或者数值爆炸通常是状态转移矩阵写错或者时间间隔用了负数跟噪声参数没关系。加一个对称性检查和正定性检查能在早期就拦住这类问题。第三初始对准的时间不要省。很多产品为了用户体验开机一秒就要求出结果代价是初始航向误差好几度后面要靠卫星慢慢纠前几分钟的轨迹都是歪的。如果场景允许宁可多等五秒。第四把中间变量都打日志。卫星观测残差、滤波增益、零偏估计值、协方差对角元这些量在正常运行时应该是什么范围你心里要有个数。出问题的时候看一眼残差是不是突然变大就能判断是观测的问题还是预测的问题排查效率差好几倍。第五别迷信精度指标。厂商标称的精度都是在特定条件下测的实际装到你的结构上、走你的线束、在你的温度环境里跑结果可能差不少。样机阶段的实测数据比任何规格书都可靠这也是为什么我坚持每个项目都要做一轮完整的静态和动态标定哪怕看起来是浪费时间。