INS/伪距组合导航原理与工程实践：从误差状态滤波到紧组合

1. 项目概述为什么第一篇文章就聊“伪距”做组合导航的人迟早会遇到一个绕不开的组合模式——INS/伪距组合。很多新手一开始接触的都是位置、速度松组合也就是把卫导解算出来的经纬高和速度直接塞进滤波器和惯导的位置速度做差。这套思路简单直观但实际工程里大家更愿意用伪距组合这里面有深层的技术逻辑。我最早接触这个项目是在一个高动态载体上做导航方案预研。当时手里有一套光纤惯导配上双频接收机需求是让组合系统在卫星信号被遮挡后又恢复的瞬间位置误差不要跳变而且速度精度要能顶得住短时失锁的考验。位置速度松组合在信号恢复瞬间经常出现位置跳变因为卫导定位解本身在被遮挡期间可能已经发散而起算位置又不准导致滤波器的量测更新被污染。后来切换到伪距组合情况明显好转。这个系列文章打算从伪距组合的基本原理讲起逐步深入到误差方程、量测方程、滤波器实现、粗差剔除、时间同步、几何观测性分析再到实测数据回放和调参经验。第一篇先把骨架搭起来讲清楚INS/伪距组合导航到底在做什么、为什么这么做、核心环节有哪些。适合谁来读如果你是刚接触组合导航的研究生、做飞控或者车载导航的工程师、想把手里的IMU和接收机数据真正融合起来做后处理的人这篇文章能帮你把概念理顺。即使你暂时不写代码光是把“伪距组合到底比位置组合强在哪”这个问题想明白后续做工程也会少走很多弯路。2. 内容整体设计与思路拆解为什么必须从“伪距”入手2.1 先搞清楚两位主角惯导和卫导各自的脾气惯导INS的优点是短时精度极高姿态、速度在几秒到几十秒内非常稳完全不依赖外部信号输出频率可以做到200Hz甚至更高。缺点是误差随时间累积尤其是加速度计零偏和陀螺漂移造成的速度、位置误差会以二次方、三次方的速度增长。你可以把它理解成一个记性极好但会逐渐“跑偏”的跑步者出发时每一步都很准跑得越久偏离真实路线越远。卫导GNSS则恰恰相反。单点定位精度在几米量级差分后可以做到厘米级而且误差是有界、不随时间发散的只要天上卫星几何够好每一秒给出的位置都是独立、稳定的。缺点是输出频率一般只有1Hz到10Hz在城市峡谷、树荫、隧道里容易失锁动态环境下多路径误差会明显增大。它就像地图上的路标每个路标本身是准的但路标和路标之间是断开的你不知道一个人在两个路标之间到底走了什么路线。这两个系统一个靠积分、一个靠观测误差特性完全互补。把两者融合在一起让惯导持续的短时高精度填补卫导低频和失锁的空白让卫导的有界误差周期性校正惯导的发散趋势这就是组合导航的基本盘。2.2 松组合和紧组合的分水岭很多人问组合导航到底“组”在哪里是按照卫导定位结果来校正惯导还是按照卫导原始观测值来校正惯导这就是松组合和紧组合的核心区别。松组合位置/速度组合的做法是卫导接收机先用最小二乘或卡尔曼滤波解算出位置速度然后把这些结果当作“量测值”和惯导推算出来的位置速度做差送入组合滤波器。结构清晰实现简单工程上非常成熟。但松组合有个隐患卫导定位解本身已经是滤波后的结果误差的时间相关性很强而且接收机内部的滤波器和组合滤波器之间存在“滤波套滤波”的问题。信号被遮挡后重新捕获、或者周跳发生时卫导解算结果会有一段异常收敛过程组合滤波器很难区分这是真实的运动还是卫导自身的收敛抖动。此外当可见卫星少于4颗时卫导根本无法输出定位结果松组合在这一瞬间就彻底失去量测更新。紧组合伪距/伪距率组合的做法是直接把接收机输出的伪距、伪距率、载波相位等原始观测值拿过来和“惯导推算出的接收机位置与卫星位置之间的几何距离”做差形成残差进入滤波器。这样就不要求接收机先解算出完整的位置哪怕只有1颗卫星可见依然能提供有效的量测约束。在信号部分遮挡的场景下紧组合的优势非常明显。2.3 伪距组合解决的三个痛点第一解决量测信息利用率的问题。松组合在可见星少于4颗时直接“罢工”而伪距组合只要有1颗卫星就能把惯导在某个方向上的误差投影到视线方向上做校正。虽然校正不完全但至少能抑制发散。第二解决量测噪声时间相关的问题。接收机内部解算出的位置误差往往在数十秒内是高度相关的这会让组合滤波器的量测噪声设置失真。伪距观测值经过差分、多路径抑制等处理后其残差特性更接近白噪声滤波器更容易“吃得动”。第三解决系统启动和信号恢复瞬间的瞬态问题。用位置组合时卫导重新定位的首次位置可能有几米的阶跃跳变组合滤波器会把这个跳变当成真实运动去校正惯导导致输出位置跟着跳。伪距组合里每个伪距残差是逐颗星独立进入的即使某颗星的伪距有偏差也可以通过残差检验把这一颗剔除掉不会污染整体状态估计。这几点合在一起决定了伪距组合成为工程上更“正式”的组合方式也是很多论文、教材在讲“深组合”之前必讲的一环。3. 核心细节解析与实操要点伪距组合导航到底在算什么3.1 系统状态你究竟在估计什么INS/伪距组合系统最常见的做法是采用误差状态error-state形式的扩展卡尔曼滤波。所谓误差状态就是不去直接估计位置、速度、姿态的绝对值而是估计惯导解算值与真实值之间的偏差。这背后的理由很实在惯导解算的轨迹本身已经比较接近真实轨迹误差是小量线性化误差可以被接受而直接用全状态滤波需要应对大范围非线性的问题数值稳定性反而难保证。典型的状态向量包含这些分量姿态误差3维惯导平台系与真实导航系之间的失准角用φ表示。速度误差3维惯导解算速度与真实速度之差。位置误差3维经度、纬度、高度误差。陀螺零偏3维沿载体轴系的陀螺漂移一般建模为一阶马尔可夫或随机常数。加速度计零偏3维同理。接收机钟差1维接收机时钟相对于卫星系统时间的偏差。接收机钟漂1维钟差的变化率。一共15维状态不含钟差钟漂是15维加上是17维具体看是否估计钟漂。为什么要把钟差、钟漂放到状态里因为伪距观测值里天然混入了接收机钟差如果不对它建模伪距残差里会残留一个共模偏差导致位置估计被系统性拉偏。3.2 量测方程伪距残差是怎么算出来的伪距的观测方程写出来是这样ρ_i |r_sat_i - r_rec| c·Δt_rec - c·Δt_sat_i I_i T_i ε_i其中ρ_i是接收机对第i颗卫星的伪距观测值r_sat_i是卫星位置r_rec是接收机位置c是光速Δt_rec是接收机钟差Δt_sat_i是卫星钟差可以通过导航电文修正I_i和T_i分别是电离层和对流层延迟ε_i是观测噪声。组合滤波器里的做法是把惯导推算出来的接收机位置r_INS当作接收机位置的参考值计算它与卫星位置的几何距离d_i |r_sat_i - r_INS|然后用观测伪距减去这个几何距离并修正卫星钟差、电离层、对流层得到的残差里包含的信息是惯导位置误差在卫星视线方向上的投影 接收机钟差 未完全修正的传播延迟。再把“真实位置 惯导位置 位置误差”代入做一阶泰勒展开就得到量测方程z_i ρ_i - d_i - (c·Δt_sat_i I_i T_i) H_i·δx c·Δt_rec ε_i这里的H_i是视线向量line-of-sight的负方向与接收机到卫星方向的单位向量有关具体写出来是三维视线单位向量投影到位置误差上的负号问题每个滤波器实现里符号要仔细核对。这里有一个新手容易踩的坑符号。不同的教材、不同的代码实现里视线向量定义、误差状态定义的正负号可能不一样直接抄别人的量测矩阵最后滤波器发散了都不知道是哪个符号写反了。最好的做法是自己从头推一遍然后用一个仿真场景去验证如果位置误差在某个方向是正残差应该是正是负做一次“符号走查”。3.3 卫星位置计算和钟差处理伪距残差的正确性高度依赖卫星位置的准确性。卫星位置由广播星历计算通常采用开普勒轨道参数递推。这里的细节很多GPS的星历参数和BDS的参数不完全一样Galileo也有细微差异计算的时候一定要按对应系统ICD里的公式来。工程上常用的做法是提前把星历缓存下来解析出轨道根数然后按参考时刻外推。外推时间不宜过长超过2小时的星历外推精度明显下降组合系统的残差里会掺入系统性偏差。卫星钟差通过星历里的钟差参数修正。广播星历给出的钟差参数是二阶多项式钟差修正公式为Δt_sat a0 a1·(t - t_oc) a2·(t - t_oc)^2需要注意的是这个修正值只适用于常规精度应用。如果要更高精度需要用到精密星历和精密钟差产品。在组合导航的实时场景里广播星历的钟差修正足够在后处理高精度场景里可以用精密产品替代。电离层延迟的修正方式取决于接收机是否双频。双频接收机可以直接用无电离层组合消除一阶电离层延迟单频接收机则需要用Klobuchar模型或格网产品来修正。在伪距组合中电离层残差如果不被修正会直接混入伪距残差影响滤波器的位置估计。实测中电离层活跃期的残差可以达到十几米这时候如果不处理组合导航的水平位置大概率会被拉偏几米。3.4 最小二乘/卡尔曼滤波的融合过程伪距组合滤波器的核心流程可以分成预测和更新两步。预测阶段使用IMU数据做惯导机械编排mechanization也就是更新位置、速度、姿态同时把状态误差协方差矩阵P按状态转移矩阵外推。这一步的精度取决于IMU的数据质量和机械编排算法的准确性。很多新手在跑仿真时发现滤波器发散往往不是量测的问题而是机械编排本身有bug比如姿态更新里的圆锥补偿没做、速度积分里的划桨补偿漏了这些误差会直接流进组合滤波器。更新阶段先计算每个可见卫星的伪距残差构架量测矩阵H然后按卡尔曼增益公式更新状态估计K P·H^T·(H·P·H^T R)^(-1) δx K·z P (I - K·H)·P这里的R矩阵是伪距观测噪声的方差一般设成1米到3米^2的量级。R的取值直接影响滤波器的响应速度R设小了滤波器相信量测、收敛快但容易受噪声干扰R设大了滤波器偏向惯导预测、平滑但反应迟钝。实际工程里R很少是固定值更常见的做法是和卫星仰角挂钩低仰角卫星的多路径和大气延迟误差大R相应放大高仰角卫星R可以放小。更新完之后把估计出的误差状态反馈到惯导解算结果里然后对误差状态做“重置”也就是把状态估计置零或者把误差加到导航状态后把误差状态清零。这一步看似简单但处理不好会引入新的问题比如反馈和协方差更新顺序反了导致状态已经被校正了、协方差却还停留在“误差已经被估计出来”的水平下一步预测就会过于乐观。4. 实操过程与核心环节实现从零搭一套伪距组合滤波器4.1 数据准备和坐标系对齐动手写代码以前必须先把数据准备好并且把坐标系之间的转换关系理清楚。伪距组合需要三种数据一是IMU数据包含三轴加速度、三轴角速度以及对应的时间戳。IMU数据要在载体坐标系b系下表达并且需要提前标定好零偏和尺度因子。如果陀螺和加速度计的数据没有做内参补偿组合滤波器的状态里估计出来的零偏项会去“吸收”这些误差结果就是位置精度看起来还行但零偏估计值严重偏离真实值。二是卫导原始观测数据包含伪距、载波相位、多普勒、信噪比C/N0、卫星星历。一般接收机输出的是RINEX格式或者厂商私有格式需要解析出每个观测值对应的卫星号、系统、观测时刻。三是时间基准信息。IMU时间戳和接收机时间戳必须统一到同一时间基准这个在后面的章节里要单独讲。坐标系上伪距观测方程是在地心地固系ECEF里表达的而惯导的机械编排通常是在当地水平坐标系比如ENU或NED里完成的。所以每做一个滤波周期都需要把惯导在导航坐标系下的位置转换成ECEF坐标再算和卫星的几何距离。这里的常见错误是忘了考虑地球曲率。用经纬高转ECEF的时候如果参考椭球参数用错了比如用了WGS-84但忘了考虑椭球扁率会产生百米量级的误差直接导致伪距残差巨大。4.2 惯导机械编排和误差状态协方差外推机械编排就是把IMU的角速度和比力积分为位置、速度、姿态的过程。对捷联惯导来说姿态更新的核心是姿态微分方程C_b^n C_b^n · (ω_nb^b ×)这里的ω_nb^b是载体相对导航坐标系的角速度在载体坐标系的投影需要从陀螺输出的角速度中扣除地球自转和导航系旋转的影响。地球自转分量在静止状态下也会产生明显的误差不补偿的话姿态会在数分钟内漂移。速度更新要处理引力、离心力、科里奥利力等项。加速度计输出的是比力specific force要加上重力加速度才能得到真正的加速度v_n C_b^n · f_b - (2ω_ie^n ω_en^n) × v_n g_n这里的符号细节很多每个量在哪个坐标系下表达、叉乘的顺序、正负号都需要按参考书逐一核对。在组合滤波器的预测阶段状态转移矩阵F可以由误差模型的线性化得到。很多实践者直接用连续时间F矩阵离散化用F·dt计算状态转移阵Φ。这里有一个精度问题如果IMU输出频率较低比如50Hz而载体的动态较大用简单的欧拉近似误差会比较大更稳妥的做法是用矩阵指数或者对F矩阵做分段恒定假设下的精确离散化。P矩阵的外推公式是P_k|k-1 Φ·P_{k-1}·Φ^T QQ矩阵是过程噪声矩阵代表陀螺和加速度计的随机游走强度、零偏时间演化的不确定性。Q的取值直接决定了滤波器对“惯导误差到底有多大”的相信程度也是最难调的参数之一。4.3 伪距残差计算里的细节残差计算是整个流程里最容易写错又最难排查的部分。我把需要注意的点列一下先看卫星位置的正确性。用广播星历算出来的卫星位置一定要把卫星天线相位中心偏移考虑进去。广播星历给出的位置是卫星质心的位置而伪距观测值对应的是卫星天线相位中心。两者之间的偏差可以达到1到2米对于要求高精度的组合导航来说不可忽略。再看相对论效应。GPS卫星处在高中轨道时钟受引力势影响比地面快这个效应已经被卫星钟出厂时的频率偏调修正了但轨道偏心率引起的周期性相对论效应需要用偏心率和偏近点角做周期修正。漏掉这一项伪距里会残留一个周期性误差峰值可以达到20米左右。我在早期实现里漏掉过这个修正回放数据时发现残差呈现正弦形状花了很长时间才定位到是相对论周期项的问题。再看接收机天线相位中心偏差。接收机观测值以天线相位中心为参考而惯导解算的位置一般指IMU测量中心。两者之间的杆臂lever arm在位置组合里要补偿在伪距组合里其实通过量测方程也能吸收掉一部分但更严格的做法是显式建模杆臂在把惯导位置转换到卫星几何距离时加上杆臂在ECEF下的投影。再看多路径。多路径是不太好建模的误差源。在静态环境里可以通过信噪比C/N0阈值筛选卫星低信噪比卫星的伪距残差权重降低。在动态城市环境里多路径往往表现为尖峰误差需要靠残差检验来剔除。后面的章节会专门讲RAIM方法。4.4 组合滤波器的初始化滤波器初始化是新手最容易忽视的环节。如果初始状态误差协方差矩阵P设得不合理滤波器要么收敛得很慢要么第一轮更新就把状态带偏。位置方差一般根据惯导初始对准精度设定。如果初始位置来自卫导单点定位水平位置方差可以设成5米^2高度方差因为卫导垂直精度较差可以设成10米^2。速度和初始对准相关水平速度方差设成0.1米/秒^2比较合理。姿态方差则要看初始对准的方式如果用了卫导测速辅助的动基座对准航向角初始不确定度可能还有0.5度到1度需要反映在P矩阵里。过程噪声Q的设定有个依赖关系Q设得越大滤波器越倾向于相信量测但Q过大会让滤波器对量测噪声过于敏感输出噪声变大。Q设得太小滤波器对惯导过度自信一旦惯导误差增长超过预测状态估计会滞后甚至发散。工程上标准做法是先离线跑一遍数据调到一个稳定收敛的Q再拿到实时环境去微调。4.5 反馈重置与协方差匹配误差状态卡尔曼滤波里“校正”不是直接给位置加一个修正量就完事还要处理协方差的匹配问题。具体操作流程是卡尔曼增益算出来后先用残差乘以增益得到误差状态估计δx然后把δx叠加到当前导航状态上得到校正后的位置速度姿态。最后把误差状态估计值清零同时将P矩阵做相应更新。这里有个细节P矩阵更新后描述的是“当前误差状态的不确定性”。因为已经把δx从状态里剥离出来了所以理论上更新后的P已经反映了残余不确定性。但如果实现的时候在反馈重置之后又额外对P做了一次处理或者把重置顺序搞错就会发现滤波器的协方差和真实误差明显失配。有一个快速验证方法保存事后处理结果统计真实位置误差与滤波器输出的1σ边界的匹配程度如果误差经常超过3σ边界说明协方差估计偏乐观需要调大Q或者调小R。5. 常见问题与排查技巧实录伪距组合里的“坑”都在哪5.1 滤波器发散先查符号和坐标系我见过太多组合导航的代码“一跑就炸”。通常第一反应是查代码但有一个更高效的思路先做符号和坐标系走查。准备一组静态数据让载体完全不动。如果滤波器正确伪距残差应该在零附近震荡误差状态的估计值应该趋近于零。如果残差呈现出持续增大或者明显的斜坡多半是符号或坐标系转换的问题。具体排查路径第一看IMU数据是否已经补偿了重力第二看机械编排输出的位置是否随时间漂移第三看ECEF转换的经纬高是否正确第四看卫星位置计算是否正确——可以用接收机广播的电离层参数逆向检查或者用公开的精密星历对比第五看量测矩阵H的视线向量方向是否正确可以先在“位置误差在某一方向1米”的情况下手工推演残差应该是什么符号再看代码是否一致。5.2 时间同步误差看起来不大杀伤力极强伪距组合里时间同步是一个经常被低估的问题。伪距观测值是接收机在某个历元时刻测得的惯导位置是IMU在另一个时刻的积分结果。如果两者之间的时间差没有对齐在载体运动时伪距残差里会引入一个与速度成比例的误差。举个例子载体以20米/秒的速度运动IMU和接收机时间差1毫秒位置偏差就是2厘米这个量级在伪距组合里还能忍。但如果时间差达到几十毫秒接收机输出延迟、串口缓冲、系统调度抖动都可能造成位置偏差就到几十厘米甚至米级了这个量级足以让滤波器的精度退化。解决思路有两条一是硬件/系统层面做时间同步比如用PPS脉冲对齐接收机观测时刻和IMU采样时刻二是软件层面做补偿在伪距残差计算时把惯导位置推算到接收机观测时刻或者在量测方程里加入时间同步偏差作为一个待估状态。第二种做法在一些论文里被称作“时间偏差估计”实现起来并不复杂但要注意可观测性问题如果载体长时间静止时间偏差和位置误差之间存在耦合可能无法同时估计需要引入额外的约束。5.3 粗差卫星的识别与剔除城市环境下伪距粗差很常见多路径、信号衍射、接收机通道异常都会造成某颗卫星的伪距突然偏差几十米甚至上百米。如果不做剔除卡尔曼滤波会把这些粗差当成真实量测导致位置估计被大幅拉偏。常用方法是基于残差的卡方检验或者“新息一致性”检验。具体做法是对该颗卫星计算其新息innovationv z - H·δx_pred并计算新息方差S H·P·H^T R。如果|v|超过某个阈值比如3倍或4倍的标准差就认为该卫星存在粗差本周期跳过这颗星的量测更新。这个方法的局限性在于如果同一时刻多颗卫星同时出现粗差或者粗差慢慢累积比如多路径在载体转向时逐步变化单星检验可能不够。更稳健的扩展是RAIMReceiver Autonomous Integrity Monitoring它对所有可见卫星的新息做一致性检测寻找“组合后残差最小的卫星子集”。在组合导航里可以把每颗星逐一剔除后检查整体一致性虽然计算量稍大但能有效解决多粗差问题。5.4 卫星几何不好滤波器怎么表现当可见卫星都集中在天空的某一小片区域时伪距组合对垂直方向的约束很弱。最极端的情况是只有一颗卫星那么滤波器只能约束“载体位置在卫星视线方向上的投影”其他方向完全靠惯导预测。这种情况下位置误差协方差在某些方向会持续增长这是正常现象不代表滤波器坏了。但如果卫星几何分布不好而且接收机钟差估计又不准伪距残差里会同时混入“位置误差投影”和“钟差偏差”这两个量在某些方向上不可区分。工程上通常用DOP精度因子来衡量这个问题的严重性PDOP超过5的时候即使滤波器不发散也要对输出精度保持警惕。实际操作中可以在滤波器里监控新息序列的均值和标准差。如果卫星几何变差新息的标准差可能仍然正常但某些状态方向上的协方差会拉大。一个实用的调参经验是在协方差的反馈中对垂直方向和接收机钟差方向设置合理的过程噪声不要为了追求窄边界而把Q压得过小否则几何变差时滤波器会“硬撑”输出结果表面平滑、实际偏差很大。5.5 常见问题速查表现象可能原因排查方法静态下位置误差缓慢漂移机械编排中未补偿地球自转或重力异常检查比力转换、重力模型参数伪距残差呈现正弦形状漏掉相对论周期项修正检查卫星位置计算中是否加入周期相对论修正滤波器持续收敛但始终有固定偏差卫星钟差修正不正确、天线相位中心未补偿逐颗核对星历参数、检查天线偏移模型城市环境下位置偶尔跳变多路径粗差进入量测更新引入残差卡方检验、RAIM剔除结合C/N0降权滤波器和真实误差不匹配Q、R设置不合理统计事后误差是否超过3σ按比例调整Q/R时间同步不准时动态定位误差大接收机和IMU时间未对齐使用PPS同步或估计时间偏差状态首次启动时滤波器剧烈收敛P矩阵初始化过小按卫导定位精度和惯导对准精度合理设P6. 实操总结与经验延伸写到这里第一篇的骨架算是立住了。回顾整个INS/伪距组合系统底层的逻辑并不复杂就是用一个误差状态滤波器把惯导的高频短时精度和卫星伪距的低频长期稳度有机拼在一起。真正难的地方全在细节里符号方向、时间同步、坐标系转换、误差补偿、粗差剔除、参数整定任何一环做得不干净最终精度都会被拖下来。我个人在实际操作中的一个深刻体会是伪距组合和位置组合的差距在卫星信号良好的开阔环境下并不明显但在信号遮挡、恢复、以及高动态的场景下会体现得非常清晰。伪距组合的“抗断”本质上来自它没有把“卫导结果”当成一个整体来消费而是把每一颗卫星的原始观测当成独立的约束来使用。这个思想认识到了后续做深组合、做矢量跟踪、做多传感器融合思路都是相通的。还有一个小技巧想分享给刚开始实践的人搭建完滤波器之后不要急着上实测数据。先用仿真器生成一组“完美”的数据比如直线运动、圆周运动、加噪声的伪距跑通整个链路。确认算法正确之后再用实测数据去迭代调参。这样可以极大缩短调试时间因为仿真环境下你掌握着真值任何一步出错都能立刻暴露而实测数据里的误差源太多出了问题往往不知从何下手。下一篇可以顺着这条线往下走讲伪距率多普勒如何加进来以及和伪距组合形成更完整的“伪距/伪距率组合”框架。伪距率相当于把卫星和接收机之间的径向速度引入滤波对速度精度的提升非常明显也对接收机钟漂的可观测性有帮助。有了这一篇的基础下一篇的动作会顺畅很多。