去年冬天我在陆家嘴帮一个做物流无人车的朋友跑实车数据车刚进楼群密集区组合定位系统里的GNSS输出直接像喝醉了酒一样往外飘轨迹从左侧车道瞬移到右侧大楼里来回甩了十几米。做定位这行的人对这种感觉都不陌生——城市峡谷环境下多径、遮挡、卫星几何恶化一起来单纯靠GNSS根本顶不住。当时我们的救场方案就是INS/NHC/ODO组合导航用惯性导航做主力推算非完整约束加里程计在GNSS失锁期间顶住定位发散效果立竿见影。这篇文章我就把整套仿真过程完整拆开讲一遍基于PSINS这个惯导领域绕不开的开源工具箱手把手带你把组合导航闭环跑通。很多刚接触定位的人一搜“PSINS仿真”会撞到一堆ins镜像、ins期刊之类的无关热词绕了不少弯路。实际上PSINS是国内惯导圈使用最广的开源仿真平台之一用来验证SINS/GNSS组合、初始对准、标定算法都非常成熟唯一麻烦的是它没有现成的NHC/ODO里程约束demo。这篇文章就是把这块拼图补上从轨迹设计、IMU数据生成、滤波器初始化到NHC和ODO量测方程怎么嵌入卡尔曼滤波全部按实操经验来讲代码框架可以直接抄适合做组合导航算法验证、车辆自主定位、无人驾驶仿真的工程师和研究生参考。1. 城市峡谷定位漂移的根源为什么单独靠GNSS会翻车1.1 高楼遮蔽环境下GNSS到底在经历什么城市峡谷里的GNSS信号问题不是“信号弱”这么简单它是四种误差叠加在一起共同把定位结果推向漂移。第一个是多径效应。卫星信号经过玻璃幕墙和金属桥梁表面反射后反射路径比直射路径长接收机把反射信号和直射信号混合相关伪距测量会出现几米到几十米不等的正向偏差。第二个是卫星几何分布恶化也叫PDOP值增大。高楼把低仰角卫星全部挡住可用卫星集中在头顶狭小范围内同样的伪距误差被几何因子放大数倍定位精度跟着断崖式下跌。第三个问题是信号遮挡导致的失锁。车辆在高架桥下、地道里行驶时卫星信号可能短时间完全消失接收机从定位状态掉到速度保持甚至纯惯性推算状态这个切换瞬间的位置跳变和速度跳变非常吓人。第四个问题很容易被忽视——反射信号还在持续被锁定。多径信号不会让你完全失锁反而会让定位结果“稳定地错着”这种错误没有明显先兆最坑人。1.2 惯性导航为什么能补位又为什么不能单干惯性导航INS的核心优势在于完全不依赖外部信号靠IMU里的陀螺仪和加速度计积分出姿态、速度和位置更新频率高短期精度好不存在城市峡谷这种环境失效问题。但代价是误差随时间累积陀螺零偏引起姿态误差姿态误差又放大加速度积分的位置误差典型的“三次积分”发散特性——位置误差最终大致与时间的三次方成正比。这就是为什么INS不能长时间单干。组合导航的思路就是把GNSS、INS、ODO这些传感器放在卡尔曼滤波框架里做信息融合。GNSS在线的时候用GNSS修正INS的漂移GNSS失锁的时候用NHC和ODO去抑制INS的发散。NHC是车辆运动学上的两个约束正常行驶不打滑不跳起横向和垂向速度接近零ODO是里程计提供的纵向速度观测量。这两者组合起来等于一个“虚拟速度计”能让惯导在失去GNSS后依然维持可用级的位置精度。这个方案在城市峡谷、隧道、地库等GNSS盲区是工程上最成熟的兜底手段也是车规级组合导航的标准配置。2. 方案选型PSINS能给你什么有哪些替代方案2.1 为什么是PSINS而不是自己手写滤波器十五维状态量的惯导误差卡尔曼滤波状态方程涉及姿态误差、速度误差、位置误差、陀螺零偏、加计零偏的耦合传播推导过程非常劝退。如果从零开始写光是把状态转移矩阵推对、把离散化实现对再和捷联惯导解算正确拼装在一起没有几个星期很难调稳。PSINS最让人省心的是它把SINS解算和误差传播方程封装成了现成接口你只需要把量测方程按自己的传感器特性写进去就能搭出完整组合导航闭环。PSINS是国内惯导领域知名度最高的开源工具箱之一内置了完整的捷联惯导更新算法包括圆锥补偿、划船补偿、速度旋转补偿这些高精度项仿真结果能贴近真实解算器行为。它还带交互式轨迹生成器能方便地生成带指定运动过程的IMU数据省了从零造数据的功夫。更重要的是它有一批成熟的SINS/GNSS组合范例拿来对照可以快速确认自己的滤波框架是不是搭对了。NHC/ODO组合没有现成demo但基础模块都齐了缺少的正是本文要补的这部分。2.2 INS / NHC / ODO组合导航的整体架构先把这个组合方案的整体数据流理清楚。IMU以100Hz输出角增量和速度增量SINS解算模块负责把IMU数据积分成当前姿态、速度和位置。GNSS在有信号的时候以1Hz左右输出位置速度。ODO按车辆轮速脉冲换算成前向速度采样频率常见在10到50Hz。NHC属于运动学模型而不是传感器不直接产生数据它作为约束条件在滤波器量测更新时参与计算。在我的仿真框架里卡尔曼滤波器采用15维状态量包括姿态误差3维、速度误差3维、位置误差3维、陀螺零偏3维、加速度计零偏3维。GNSS可用时量测是SINS位置与GNSS位置的差GNSS失锁时量测切换为NHC/ODO速度约束的残差。滤波估计出的误差状态反馈修正SINS解算结果修正后滤波器状态清零防止误差重复累积。这套架构的好处是GNSS和NHC/ODO共用同一个滤波器状态量完全一致切换量测时不需要重置滤波器工程实现干净利落。2.3 PSINS版本差异与初始API确认PSINS从早期版本到现在接口发生过一些调整。老版本里惯导初始化通常是ins insinit([pos0; att0; vel0], ts)新版本可能是ins insinit(pos0, vel0, att0, ts)不同版本对欧拉角单位和顺序的约定也有差异。拿到新环境后第一件事是help insinit确认参数格式再跑一遍自带demo确认环境和工具箱路径没问题。这个认识看起来很小但能省下后面一整个晚上查bug的时间。滤波相关函数同理kfinit、kfupdate、insfeedback在不同版本里细节略有出入所有示例代码请以你本地版本为准。3. 动手实操从轨迹生成到滤波闭环跑通3.1 设计一条带城市峡谷特征的车辆轨迹仿真不能只验证算法正确的理想情况必须刻意设计一个GNSS失效区间才能看明白NHC/ODO的作用。我常用的做法是设计一条城市道路场景轨迹全程约三公里包含静止初始对准、直线加速、匀速行驶、直角转弯、掉头减速等典型工况。轨迹初始位置设在北纬34.2469度、东经108.9918度附近初始高度400米航向角90度也就是车头朝东。前60秒车辆静止用于模拟初始对准。随后加速到10米每秒并保持匀速直线行驶。进入峡谷段后有一个90度右转然后继续直线行驶再做一个180度掉头最后减速停车。GNSS信号设置成前60秒可用在进入峡谷段后中断约200秒车辆在这段时间内只能依靠INS/NHC/ODO推算位置。生成轨迹最省事的方式是使用PSINS自带的交互式轨迹生成工具在菜单里依次选择静止、加速、匀速、转弯等运动段工具会输出IMU数据和对应真值轨迹。如果希望完全自动化批量生成也可以自己写参数化的轨迹生成函数在运动段切换处用多项式曲线过渡角速度避免角速度突变导致仿真结果失真。轨迹设计直接决定仿真结果可信度转角速度不连续会引入额外误差源干扰对滤波算法的评价。我习惯在轨迹里故意加入一段10度左右的小角度蛇形运动用来检验车辆运动学约束在小转向下的表现。3.2 IMU参数与滤波器噪声矩阵的真实标定思路IMU仿真参数的设置要和目标传感器级别匹配。我用的参数是MEMS级别的典型值陀螺零偏10度每小时角度随机游走0.1度每根号小时加速度计零偏1毫克速度随机游走0.1米每秒每根号小时。仿真时PSINS生成的理想IMU数据需要自己叠加这些误差项模拟真实传感器输出。这里有个很容易犯的错误——把零偏当成常数叠加就结束了实际上真实零偏还受温度影响仿真里没有温度模型但至少要在滤波器过程噪声中留出裕量否则滤波会过度自信。滤波器的初始协方差矩阵P、过程噪声Q、量测噪声R三者决定了滤波器对状态估计的信任程度。P太大会让初始估计谨慎收敛慢P太小会让滤波器坚信初始对准结果量测来了也拉不动。Q反映IMU误差模型的发散速度Q太小意味着滤波器认为SINS模型非常准GNSS中断后误差发散时无法修正Q太大会让系统状态噪声过大定位结果抖动。R反映量测的信任程度R太大即量测不可信R太小则量测噪声会直接串进位置估计。实际操作中我习惯先给一组保守初值把整个流程跑通再根据新息序列的实际统计特性调整而不是一上来就精确标定。3.3 组合导航主循环NHC/ODO量测切换的核心代码下面这段是我整理后的核心仿真框架去掉了无关业务逻辑保留组合导航主循环的骨架。整个流程分成初始化、时间更新、量测更新三大块。% ---------- 初始化 ---------- clear; close all; clc; glvs; ts 0.01; % IMU采样周期 100Hz nt size(imu, 1); % 惯导初始值位置、速度、姿态 pos0 [34.2469; 108.9918; 400]; % 纬度(deg) 经度(deg) 高度(m) vel0 [0; 0; 0]; att0 [0; 0; 90]; % 横滚 俯仰 航向(deg) ins insinit(pos0, vel0, att0, ts); % 按本地版本确认参数顺序 % 滤波器初始化15维状态 kf kfinit(nt, 15); kf.Phikk_1 inserror0(ins, 15); kf.Pk diag([0.1; 0.1; 1] .* [att0err; att0err; att0err(3)/10] ... ... % 这里仅示意按实际单位与场景设定 ); kf.Qk diag([gyro_noise*ones(3,1); accel_noise*ones(3,1); zeros(9,1)].^2); kf.Rk diag([0.3; 0.1; 0.3].^2); % NHC/ODO量测噪声横向、前向、垂向 kf.Hk zeros(3, 15); kf.x zeros(15, 1); % ---------- 主循环 ---------- for k 1:nt % SINS解算输入一帧IMU数据 ins insupdate(ins, imu(k,:), ts); % 更新状态转移矩阵并执行时间更新 kf.Phikk_1 inserror0(ins, 15); kf kfupdate(kf); % GNSS量测有效位置观测 if gps_ok(k) kf.Hk [zeros(3,6), eye(3), zeros(3,6)]; kf.Rk diag(gps_pos_err.^2); z ins.pos - gps.pos(k,:); kf kfupdate(kf, z, kf.Hk, kf.Rk); ins insfeedback(kf, ins, 15); kf.x zeros(15, 1); % 反馈后必须清零误差状态 % GNSS失锁NHC/ODO速度约束 elseif odo_ok(k) vn ins.vn; Cnb ins.Cnb; % 注意PSINS中此变量是 b-n 的姿态阵 vb Cnb * vn; % 导航系速度转载体系新手容易在这里转置搞反 % NHC约束横向速度为0垂向速度为0ODO前向速度等于轮速 z [vb(1) - 0; vb(2) - vodo(k); vb(3) - 0]; % 量测矩阵姿态误差耦合项 速度误差项 H_att askew(vb); % 3x3 反对称阵 H_vel Cnb; % C_n^b kf.Hk [H_att, H_vel, zeros(3, 9)]; kf.Rk diag([0.3; 0.1; 0.3].^2); kf kfupdate(kf, z, kf.Hk, kf.Rk); ins insfeedback(kf, ins, 15); kf.x zeros(15, 1); end end主循环里最关键的是量测矩阵H的构造。NHC/ODO本质上观测的是载体系下的速度分量而滤波器状态里只有导航系速度误差和姿态误差所以必须通过一个坐标变换把载体系速度误差表示为状态量的线性组合。这个推导很多资料都跳过了我在下一节单独讲清楚。代码里askew(vb)把载体三维速度向量转成反对称矩阵正是姿态误差耦合项的来源。4. 核心原理拆解NHC和ODO到底怎么抑制漂移4.1 非完整约束的物理意义和数学表达NHC这条约束听起来简单物理背景却不简单。车辆在地面正常行驶时轮子沿着车身纵轴方向滚动垂直于行进方向没有速度分量也就是不发生侧滑车辆也没有离开地面所以沿车体垂直方向的速度也为零。这两个约束直接把载体系下的六维运动压缩到了四个自由度对惯导系统来说是极强的信息。用数学式子表达就是载体系速度的横向和垂向分量等于零。在滤波里我们把它作为量测方程代入载体系横向前向垂直三个速度分量中横向和垂直方向期望值为零前向期望值等于里程计测量值。这里的前提是轮子确实不打滑。湿滑路面、急转弯、过减速带瞬间假设被破坏量测会产生大的偏差直接进滤波器会污染状态估计。4.2 量测方程推导为什么H矩阵是态姿耦合项加速度项这一节稍微动点手帮大家把H矩阵的来龙去脉理清楚这样遇到问题能自己排查而不是只会抄代码。导航系速度记为v^n载体系速度记为v^b两者通过姿态矩阵联系v^b C_n^b * v^n。考虑滤波估计值有误差时令姿态失准角为phi导航系速度误差为delta_vn那么载体系速度的估计误差可以写成一阶近似形式。这里的关键结果是一个叉乘项姿态误差会通过载体速度向量耦合进速度观测量具体表现为askew(vb) * phi加上C_n^b * delta_vn。翻译成矩阵语言就是H矩阵的前三列是askew(vb)第4到6列是C_n^b后面位置和零偏列全是零。整行代码才变得容易理解。实际实现时有个细节要注意ins.Cnb在PSINS里表示载体系到导航系的姿态阵所以求C_n^b时要取转置。很多人在这个地方栽过跟头写出来量测矩阵每行都是错的滤波表现却是“半死不活”——误差不收敛也不发散很难定位。4.3 里程计刻度系数与安装误差影响约束精度的两只拦路虎里程计输出的轮速脉冲数换算成速度需要一个从脉冲频率到速度的比例系数这个比例系数由轮径、减速比、每转脉冲数共同决定。问题在于轮径会随胎压和磨损缓慢变化载重也会影响轮胎滚动半径。刻度系数如果有1%的误差在GNSS失锁100秒后引入的前向位置误差就能达到几十米量级。工程上解决刻度误差有两个层次。低层次做法是每次开车前用GNSS直线路段标定一次把刻度系数校准掉做法简单但对轮胎状态变化敏感。高层次做法是把刻度系数误差扩充进滤波器状态量在GNSS可用时持续在线估计GNSS失锁时使用校准后的值。这样滤波器状态量从15维扩展到16维量测方程里多一列与刻度系数误差对应的系数项实现成本不高收益非常明显。安装误差是另一个容易踩的坑。IMU装到车身上理想情况是IMU的坐标系和车体坐标系三轴完全对齐实际装出来会有几度偏差。NHC假设的是“车体系”下横向和垂向速度为零如果IMU和车体偏了一个小角度前向速度就会泄漏到横向和垂向量测上约束效果大打折扣。安装误差角可以通过一段直线行驶用GNSS速度做基准离线辨识也可以在滤波里扩充三个安装误差角状态在线估计车规产品普遍用后者。仿真阶段至少要明白这两个误差来源的影响才能解释为什么纯理论参数下滤波器能收敛得很好换到实际车辆上却效果变差。4.4 量测噪声矩阵R怎么调才能让约束既不生硬也不失效R矩阵本质上是告诉滤波器“这个量测值有多可信”。数值给得越小滤波器越相信量测修正力度越大给得越大量测越被轻视。NHC三条量测的噪声特性其实不同前向ODO速度相对可信噪声小一些横向速度约束在直行时确实准但在大转弯时侧偏会放大误差噪声应该给大一些垂向约束在良好路面上基本成立但过减速带瞬间会失效。经验值方面前向给0.05到0.2米每秒横向给0.1到0.5米每秒垂向给0.1到0.3米每秒具体根据车辆特性和路况调整。更稳妥的工程做法是动态调节R。实时监测滤波新息的滑动方差一旦发现量测残差明显偏离正常分布就临时调大对应R值甚至直接剔除这次量测。这个逻辑和GNSS领域的抗差估计类似用在新息上就是“离群值检测”。在仿真阶段可以不做得那么复杂但至少要把NHC约束在转弯处可能会失真这个性质牢牢记在脑子里。5. 常见问题与排查技巧实录5.1 仿真发散最让人头疼的问题往往出在三个地方遇到滤波器发散我的排查顺序是先检查各模块之间的时间基准是否统一再检查滤波器状态反馈逻辑最后检查协方差参数。时间基准问题最隐蔽轨迹生成的IMU时间戳、滤波主循环的迭代次数、ODO数据的对齐周期只要有一个错位相当于量测延迟误差就会被来回拉扯最终震荡发散。我踩过一次最离谱的坑是IMU数据频率从100Hz改成200Hz时主循环步长忘改滤波器时间更新和量测更新频次错位误差笔直飞掉。反馈逻辑问题次之。滤波估计出状态误差后通过insfeedback修正SINS解算结果修正完必须把kf.x清零或减掉已反馈的量。如果忘了清零下一轮时间更新会在这个基础上继续累积误差等于把一个误差估计了两遍。协方差参数问题里面最常见的是P矩阵初值给得太小滤波器从“误差很小”的假设出发量测修正力度被稀释GNSS失锁后的发散完全拉不回来。经验做法是把P初值相对对准精度放两三倍裕量宁可收敛慢一点不能自信过头。5.2 转弯时误差突变NHC假设被破坏时的应对手段直线路段滤波器收敛得很好一进转弯误差就突变这种情况我碰到不止一次。本质原因是转弯时车辆的实际运动不满足NHC假设车体会产生侧偏角横向速度不是严格为零同时大横摆角速度会让侧向加速度计读数出现明显变化相当于给NHC量测注入了一个模型偏差。最简单的处理是把横向速度约束的R值调大让滤波器在转弯时不过分信任NHC。更精细的做法是根据转向角速度动态调整R转得越急给横向约束的置信度越低。还有一条路上的经验——转弯时ODO前向速度也进入了大误差区间因为内外轮差速会让平均轮速和车体质心速度产生偏差前向速度观测也需要同步降权。如果仿真里发现掉头急弯误差峰值特别大可以把这段运动参数调整得更平滑比如在转弯段用正弦过渡角速度不要瞬时拉满横摆角速度。5.3 GNSS恢复后位置跳变切换量测时的平滑处理技巧GNSS中断几十秒后恢复SINS推算位置和GNSS真实位置可能已经差了十几米甚至几十米滤波器在量测更新时被一次拉回轨迹上直接出现一个明显台阶。这种情况在实车体验上很糟自动驾驶路径规划也会被瞬时冲击影响。工程上常用的处理方式是对量测置信度做渐近恢复。GNSS刚恢复的几秒内量测噪声R从较大的值逐渐缩小到正常值相当于让位置估计缓慢靠拢GNSS而不是一步跳到位。另一个做法是先恢复速度量测再恢复位置量测因为速度误差往往比位置误差小得多先用速度把滤波器状态拉平再引入位置量测冲击会小很多。仿真阶段建议把这种切换场景也做进去不然真车调试时第一次遇到切换跳变会非常慌。5.4 ODO数据跳变和丢数用新息卡方检验做野值剔除轮速传感器在工程环境里并不总像仿真数据那样干净电磁干扰、齿圈磨损、线束接触不良都可能导致单帧速度跳变量测一旦进入滤波器就把导航结果拉偏。仿真过程里体现不出来但值得提前把机制学习好。新息卡方检验的思路非常简单卡尔曼滤波每帧量测都能算出新息也就是量测残差。如果传感器工作正常新息应该服从均值为零、协方差为H*P*HR的高斯分布。如果新息向量模长明显超出门限就认为这次量测不可信直接跳过或者降权处理。这个逻辑在PSINS框架里实现起来非常直观本身也是组合导航系统抗野值最实用的手段之一。门限的选取有个经验值参考一般取5.99或者7.81对应卡方分布在95%置信度下的两个或三个自由度的阈值。实际运行中如果发现误删正常量测就把门限放宽一些。有了这一层保护即使ODO偶尔跳一个脉冲滤波也能稳定维持不至于把一次偶发错误放大成整个轨迹的偏移。6. 代码获取、后续扩展与个人心得完整可运行的demo工程包括轨迹生成、滤波闭环和画图评估脚本我已经打包整理好。想直接拿代码跑通再研究细节的评论区留言或者私信我都可以发你。如果你更倾向于自己动手搭那就按上面第三节的框架配合你本地的PSINS版本把接口对齐半天内跑通问题不大。拿到基础demo之后建议做三个方向的改动练习第一个是扩展16维状态量估计里程计刻度误差第二个是加入安装误差角在线估计第三个是把新息卡方检验加进去做完这三个练习基本就具备独立解决真实车辆定位问题的能力了。我自己做了这么多年惯导组合最大的体会是PSINS这种仿真工具的真正价值不在于把精度跑得多高而在于帮你建立起对误差传播的直觉。仿真发散时你会去推量测矩阵、去看协方差设置、去查坐标变换方向这些排查过程比任何教科书都让人印象深刻。等到你真的把一套组合导航系统搬到实车上会发现当年在仿真里踩过的每一个坑都变成了预判问题的本钱。关于NHC/ODO组合最后再分享一个经验如果你能在仿真里把GNSS失锁两分钟的位置误差控制在十几米量级那么实车上在相同时间尺度下大概率也不会太差因为环境误差的主要来源你已经在仿真里摸透了。