干组合导航的人应该都有过这种经历车开到高楼底下手机地图上的蓝色箭头开始乱跳明明在直行轨迹却画出了蛇形。这就是城市峡谷场景里典型的定位漂移问题。要解决它业内常用的路子之一就是用INS/NHC/ODO组合导航而PSINS这个开源工具箱能把整套方案用MATLAB快速仿真验证一遍非常适合学生、算法工程师和刚接触组合导航的研发人员用来做算法预研和参数摸底。这篇文章我会从一个做过多次车载组合导航仿真验证的工程视角出发把城市峡谷为什么定位会漂、INS/NHC/ODO三个传感器各自解决什么问题、PSINS仿真链路怎么搭以及核心代码里最容易踩的坑一次讲清楚。代码我会放在文末说明获取方式方便你直接复现跑通。1. 先搞清楚这套组合导航在解决什么问题1.1 城市峡谷里的漂移是怎么冒出来的城市峡谷这个词听起来有点玄其实就是高楼密集的城市街道。两边大楼一挡卫星信号会经过墙面反射、玻璃幕墙反射后才到达接收机形成多路径效应。更麻烦的是可见卫星数量会骤减卫星几何分布也会变差通俗讲就是PDOP值变大定位解算出来的位置误差会被几何因子放大好几倍。在开阔路段你可能感觉不到因为你手机里还有地图匹配、Wi-Fi指纹、基站辅助这些外部手段在兜底。但在真正的车载导航算法里如果GNSS信号一坏纯卫星定位的位置输出就会从一个点瞬间跳到另一个点甚至连续跳变反映在轨迹上就是“漂移”。我做过一个测试在两侧都是30层以上写字楼的路段上单点定位的误差能到几十米航向有时候直接反转。这时候如果旁边还有个惯性导航系统在持续解算GNSS一坏纯惯导的位置误差也会快速累积两三分钟就能漂出去一两百米。所以问题的本质是单一传感器在城市峡谷场景里都有软肋纯GNSS怕遮挡和反射纯INS怕长期漂移轮速计怕打滑但短时尺度上却极其稳定。组合导航的思路不是去赌哪个传感器绝对准而是把多个传感器的误差特性互补起来用滤波算法把“比较可信的信息”提取出来。1.2 为什么组合里要同时带上NHC和ODO很多人第一次看到INS/NHC/ODO这个组合会问既然已经有GNSS了为什么还要用NHC和ODO在地面车辆场景里GNSS在某些路段是完全失效的比如地下车库、隧道、连续高架桥下。这时候如果INS单独扛姿态误差和速度误差会互相激励位置发散得很快。而NHC和ODO恰好能在GNSS失效期间给惯导一个“止泻”的作用。NHC全称是Non-Holonomic Constraints翻译过来叫非完整约束。物理学上非完整约束代表系统状态变化受到某种不可积分的限制。放到轮式车辆上直观理解就是正常行驶的汽车正常情况下不会像螃蟹一样横着走也不会原地弹跳离开地面。也就是说在车身坐标系下侧向速度和垂向速度理论上应该是0或者说接近于0。这两个“伪量测”不需要额外硬件只要把车辆运动学常识写进滤波器就能持续约束惯导的姿态误差和速度误差。ODO就是轮速里程计它能直接给出车辆前进方向的速度通常安装在车轮或传动轴上。跟NHC配合起来看ODO负责提供“前向速度是真真实实存在的”这个信息NHC负责约束“侧向和垂向速度你别乱跑”。这样三个方向上都有速度约束惯导解算的发散速度就被大大抑制了。所以整套组合的定位思路是GPS能用时对位置/速度做修正GPS掉线时靠NHCODO撑着。2. PSINS选型与整体仿真链路设计2.1 为什么PSINS适合干这件事现在做惯性导航仿真选择其实不少自己从零写捷联解算、用MATLAB/Simulink搭模块、用商业软件比如惯性导航厂商提供的仿真工具或者用开源工具箱。我个人的经验是如果目标是验证“INS/NHC/ODO组合导航”的算法逻辑和参数敏感性而不是做工程产品PSINS是效率最高的选择。PSINSPortable SINS便携式捷联惯性导航系统最早来自严恭敏老师的团队在惯性导航和组合导航领域的名气非常大。它的核心优势有三点第一代码是开源MATLAB脚本姿态更新、速度更新、位置更新、姿态误差方程这些核心推导全部落地成了可以直接看、直接改的函数特别适合学习原理第二内置了完整的卡尔曼滤波框架SINS/GNSS组合、SINS/DR组合这些都有现成示例改造成NHCODO的组合只需要动量测方程就行第三它自带轨迹生成器可以生成包含静止、加速、转弯、爬坡等动作的航迹再反向生成IMU数据省去外场采集数据的麻烦。相比Simulink那种图形化建模PSINS的文本代码更贴近算法本身调试起来一眼就能看到状态量、量测矩阵、增益矩阵这些中间变量对理解滤波器的行为很有帮助。如果你后续要移植到C代码PSINS里的公式推导和变量定义也能直接当参考。2.2 仿真的整体流程与数据流向PSINS仿真整套链路可以分成四个环节轨迹生成、IMU数据仿真、惯性解算、组合滤波。数据流向大致是先由轨迹生成器确定真值轨迹位置、速度、姿态再由真值轨迹反推出陀螺和加速度计的理论输出叠加上零偏、噪声、刻度因子误差等得到带误差的IMU测量值惯导解算模块把这些IMU数据做捷联积分得到含误差的导航结果最后组合滤波模块读取惯导输出、NHC虚拟量测、ODO速度信息用卡尔曼滤波估计姿态误差、速度误差、位置误差以及传感器零偏反馈校正惯导解算。在PSINS的框架里每一步都有对应的函数。轨迹生成通常在trjsimu里完成IMU数据仿真在sinsimu或者手动构造惯导解算是sins组合滤波是kfupdate。这套数据流的好处是真值轨迹一直在手里攥着全程都能随时和滤波结果对比误差曲线可以直接画出来仿真结论非常有说服力。3. 三个关键模型的原理一次性讲透3.1 INS捷联解算到底在算什么捷联惯导的核心思想是“固连在载体上的陀螺仪和加速度计直接测量角速度和比力然后在计算机里虚拟出一个数学平台”。姿态更新用的是陀螺输出的角增量或角速度通过四元数或方向余弦矩阵把载体坐标系和导航坐标系联系起来速度更新用的是比力方程把加速度计测到的比力投影到导航坐标系扣除重力加速度和哥里奥利项得到速度变化位置更新就是速度积分。纯惯导的误差传播规律是工程上必须记住的陀螺零偏一上来姿态误差会持续累积然后姿态误差会“串”到速度误差里速度误差积分又变成位置误差。如果不加外部修正位置误差会随运行时间呈现三次方增长。这就是为什么纯惯导单独拿去做地面导航几分钟就不可用。理解这条误差链才能明白NHC和ODO到底在打断哪一环。3.2 NHC约束的数学打开方式NHC约束的数学形式非常简洁。假设惯导解算得到了载体坐标系下的速度记作$v^b [v_x^b, v_y^b, v_z^b]^T$其中$v_x^b$是前向速度$v_y^b$是侧向速度$v_z^b$是垂向速度。车辆正常直线行驶时$v_y^b$和$v_z^b$趋近于0所以可以把这两项当作零均值的伪量测。实际操作中NHC伪量测不是直接把侧向和垂向速度设成0应用而是要通过滤波器的量测方程把“惯导解算出来的侧向/垂向速度偏差”作为量测残差。量测矩阵H里对应侧向和垂向速度误差的位置写1量测噪声方差根据路面条件设置。路面颠簸时垂向速度噪声要适当放大转弯时侧向约束的噪声也要相应调整不然误差模型太硬反而容易把滤波器带歪。3.3 ODO里程计刻度因子怎么进状态方程轮速计输出的是轮子转速单位通常是脉冲数每秒或者rpm。要变成速度需要乘一个刻度因子这个刻度因子理论上等于轮子周长除以编码器线数。问题在于胎压变化、轮胎磨损、载荷变化都会让有效车轮半径发生变化刻度因子不是固定不变的。所以在组合导航的状态向量里除了姿态误差、速度误差、位置误差和IMU零偏我还会把ODO刻度因子误差也放进去。状态向量取17维多出来的一个维度就是这个刻度因子误差。它通过状态转移矩阵和前向速度关联起来由卡尔曼滤波在线估计。如果不估计刻度因子误差长距离行驶后前向速度会存在一个非常讨厌的比例偏差位置误差会随时间线性增长看起来像缓慢发散实际是刻度因子没校准到。3.4 卡尔曼滤波器里怎么把这几个量测揉在一起这套组合导航用的是典型的松组合卡尔曼滤波框架更准确说是位置/速度组合加伪量测组合。GNSS信号有效时量测向量包含GNSS与惯导的位置差、速度差GNSS信号中断时切换到NHCODO量测量测向量由惯导解算的侧向速度、垂向速度以及ODO前向速度和惯导前向速度之差组成。滤波器的执行节奏是典型的预测-更新循环。预测阶段用IMU数据驱动状态外推更新阶段根据量测残差、量测噪声矩阵R和状态协方差矩阵P计算卡尔曼增益进而修正状态估计。反馈方式我建议采用闭环反馈也就是滤波估计出的姿态误差、速度误差、位置误差和零偏误差直接反馈给惯导解算结果避免误差在线性化点附近累积过大。PSINS里kfupdate函数的参数选择上要特别注意phi这个参数它控制的是反馈全量还是反馈部分量。注意NHC量测本质上是“约束量测”不是真实传感器的量测所以量测噪声矩阵R不能设置得太小。我见过很多新手把R设成1e-6结果滤波器增益被NHC主导速度估计反而被“硬拽”出锯齿状波动。4. 基于PSINS的完整仿真实操附代码4.1 第一步生成仿真轨迹和IMU数据PSINS里生成轨迹和IMU数据非常方便。我先设定一个包含多种运动状态的城市道路测试轨迹然后反向生成IMU数据。下面是一段我在工程里常用的脚本骨架你可以根据测试需求改动作参数。% 设置初始参数 fs 100; % IMU数据频率 100Hz % 轨迹生成包含匀速、加速、左转、右转、减速 trj trjsimu(fs); % 手动添加动作段 trj waypoint_add(trj, 匀速段, 30); trj waypoint_add(trj, 加速段, 10); trj waypoint_add(trj, 右转段, 8); trj waypoint_add(trj, 直线段, 20); trj waypoint_add(trj, 左转段, 8); trj waypoint_add(trj, 减速段, 10);需要注意trjsimu生成的IMU数据是理想值仿真实验需要在理想值上叠加误差才能模拟真实传感器。我习惯叠加四类误差固定零偏、零偏稳定性、角度随机游走和速度随机游走。仿真时把这几项参数按实际器件指标填进去比如陀螺零偏稳定性取5°/h加速度计零偏稳定性取0.5mg这样仿真的结果才更有参考意义。4.2 第二步初始化导航与组合滤波参数初始化是整个仿真里最容易被忽略、却最容易出错的地方。初始姿态误差、初始位置误差、初始协方差矩阵P0、系统噪声协方差矩阵Q、量测噪声协方差矩阵R每一个参数都会直接影响滤波收敛速度和稳态精度。我在设P0和Q时的经验是P0体现“你对初始状态有多不确定”Q体现“你对IMU误差模型的信任程度”。初始航向误差如果估计有10°P0的航向误差方差就不要写1e-10这样滤波器在启动时根本没法收敛。下面的参数是我做车载实验时常用的基准实际使用时要根据你的IMU器件型号调整。% 初始状态误差 q0 a2qua([0;0;0]*glv.deg); % 初始四元数 pos0 trj.pos(1,:); % 初始位置 vn0 trj.vn(1,:); % 初始速度 % 滤波器状态位置误差3维速度误差3维姿态误差3维 % 陀螺零偏3维加速度计零偏3维ODO刻度因子1维 % 状态向量维度为17 kf kfinit(17); kf.Pmin [zeros(9,1); [10;10;10]*glv.dpsh; [1;1;1]*10*1e-6; 0.01].^2; kf.Pmax [1000*ones(3,1); 100*ones(3,1); 10*ones(3,1); ... 100*glv.dpsh*ones(3,1); 10*ones(3,1)*1e-3; 0.1].^2; kf.P0 kf.Pmax; % 系统噪声协方差 kf.Qk diag([0.01*ones(3,1); 0.01*ones(3,1); 0.01*ones(3,1); ... 5*glv.dpsh*ones(3,1); 0.5*ones(3,1)*1e-6; 1e-6])^2;这段代码可能和PSINS具体版本的写法有差异但核心意图是明确的你要给滤波器一个合理的“不确定度范围”。P0不能给无穷大也不能给零而是应该结合IMU指标估一个不保守、不激进的值。4.3 第三步跑组合导航主循环主循环是整个仿真最核心的部分。下面我给出一段简化的循环逻辑完整代码我整理到了工程仓库里这里先把最关键的骨架列出来。% 角速度和比力增量 [wm, vm] cgnuart(...); % 从IMU数据中提取角增量和速度增量 % 惯导解算 [sins, ins] insupdate(sins, wm, vm); % 量测构造GNSS有效或使用NHCODO if gnss_valid % GNSS量测位置和速度差 zk [ins.pos - gnss.pos; ins.vn - gnss.vn]; Hk [zeros(3), eye(3), zeros(3,9), zeros(3,1); eye(3), zeros(3,3), zeros(3,9), zeros(3,1)]; Rk diag([pos_noise; vel_noise]); else % NHCODO量测 % 前向速度差 ins前向速度 - odo前向速度 % 侧向速度 ins侧向速度 - 0 % 垂向速度 ins垂向速度 - 0 Cnb q2mat(ins.qnb); vn_b Cnb * ins.vn; % 投影到车体系 zk [vn_b(2) - 0; vn_b(3) - 0; vn_b(1) - odo_v]; Hk ...; % 根据误差模型推导 Rk diag([lateral_noise; vertical_noise; forward_noise]); end % 卡尔曼更新 kf kfupdate(kf, zk, Hk, Rk, f); % f表示全反馈这段代码足以说明整体流程但我要特别强调几个容易错的地方。第一NHC量测的H矩阵不能拍脑袋写需要从速度误差方程推导。如果你把惯导解算速度投影到车体系约束的是车体系侧向和垂向速度H矩阵要考虑姿态误差的影响。第二ODO前向速度不是直接拿惯导前向速度减因为ODO速度是脉冲速率乘以刻度因子它带有刻度因子误差量测方程里必须体现状态向量里那个刻度因子误差量的作用。这是我见过最多的两个出错点仿真结果表现为滤波发散或误差曲线剧烈振荡。4.4 第四步结果对比与误差分析仿真的输出最好直接画三组对比图纯惯导的位置误差、INSNHCODO组合的位置误差、以及GNSS失效全程的组合导航轨迹。通常你会看到纯惯导位置误差很快突破几百米而加了NHCODO之后位置误差被“锁”在几十米到几米以内取决于场景长度、滤波参数和传感器精度。画图我一般直接用PSINS自带的绘图工具再叠加Matlab的plot微调。重点看速度误差曲线不要只看位置误差。因为NHCODO对速度的约束是即时的速度误差应该被限制在一个小范围内波动如果速度误差曲线发散或者呈现高频振荡大概率是量测噪声设置不合理或者H矩阵推导有误。5. 调参经验与常见坑排查实录5.1 仿真发散、轨迹飞掉的三个原因搞组合导航仿真的人几乎每个人都经历过“轨迹突然飞了”的时刻。滤波器在某一时刻突然输出一个离谱的位置或速度然后误差像滚雪球一样越来越大。我总结下来城市峡谷场景下仿真发散有三个最典型原因。第一个原因是初始航向误差太大。组合滤波本质是对误差做局部线性化初始误差太大会直接超出线性化范围滤波器在那个点上根本不收敛。你看到的现象往往是前几秒钟位置误差和速度误差剧烈振荡然后直接发散。解决办法是调整初始姿态误差到实际水平或者先做一段静止对准。第二个原因是量测噪声R给得太小NHC伪量测变得过于“强势”导致速度估计被约束得毫无弹性出现锯齿状跳变同时位置误差振荡也变大。第三个原因是状态转移矩阵或H矩阵拼接错误比如维度对不上、符号写反这类问题通常表现为第一帧更新后误差就变成NaN。遇到NaN优先检查矩阵维度一致性和kfupdate的调用格式。5.2 量测噪声矩阵R怎么定量测噪声矩阵R是整套滤波里最需要根据经验反复调的参数。GNSS量测的R可以根据接收机标称精度来给比如水平位置噪声1米速度噪声0.1米/秒。NHC量测的R则要更“谦虚”一点因为“侧向速度为0”这个约束在真实转弯场景里并不严格成立。我给出的参考做法是把NHC量测的R设成跟GNSS速度量测同一量级或者稍大一点比如0.1到0.5米/秒的标准差。如果车辆在砂石路、颠簸路面上测试垂向速度约束的R还要再放宽到1米/秒以上。ODO前向速度的R主要取决于轮速计的测速精度一般可以给0.05到0.3米/秒。这些数值没有标准答案需要结合仿真轨迹里的转弯幅度、路面激励强弱来微调。5.3 城市峡谷场景里的额外细节如果只是做算法验证用标准仿真轨迹就够了。但如果你想模拟更真实的城市峡谷建议在轨迹生成和量测构造上做几件事在轨迹中增加频繁的停车和起步模拟路口等红灯在GNSS量测中人为加入遮挡时段每隔一段时间就置gnss_valid false模拟隧道和高架桥下丢失信号还可以在GNSS有效时段加入较大的位置跳变噪声模拟多路径效应被接收机解算出的“飞点”。我在验证算法时会刻意让GNSS在中间某段连续失效180秒。结果是纯惯导漂了大约200米而NHCODO组合把最终位置误差控制在30米以内。如果ODO刻度因子被滤波估计得很准这个误差还能进一步缩小到10米左右。这就是这套组合导航的实际价值——不是让你在开阔天空下跟GNSS比精度而是让GNSS掉线的那几分钟位置不发散到完全没法看。6. 一些容易忽略的代码细节与工程建议6.1 PSINS版本和MATLAB环境的兼容性PSINS本身是开源维护的但不同版本之间函数名和接口会有调整。比如早期版本里kfinit、kfupdate的参数个数可能不太一样后来版本里加入了更多组合导航示例函数签名也有所变化。建议先跑一下工具箱自带的test_SINS_DR.m或test_SINS_GPS.m确认你的MATLAB环境下PSINS能正常执行再跑我给的这段组合逻辑。很多所谓“代码跑不通”的问题其实都是版本不匹配导致的。如果你用的是MATLAB R2020b以上的版本遇到类似“function or variable may be unsafe”的C语言编译类报错多半是PSINS里某些mex文件没有重新编译。这时候只要用mex命令重新编译对应文件或者改用纯MATLAB版本函数问题就能解决。这个坑在64位Windows和Linux环境里都会遇到。6.2 从仿真到实车移植的思路仿真跑通之后如果要往实车平台上搬有几个点要提前想清楚。第一PSINS里的姿态更新、速度更新算法可以直接用但数据链路要改成实时读取IMU和轮速注意时间同步误差哪怕差10毫秒对速度约束都会产生不可忽视的影响。第二NHC约束要增加“车辆的侧向滑动判别”不是所有时刻都适合用NHC比如车辆原地漂移、急转向时侧向速度约束的噪声要动态放大否则会把滤波器带偏。第三ODO刻度因子在实车上建议每隔一段时间重新估计或者引入温度和胎压补偿模型。从工程角度看仿真最大的价值是帮你把滤波维度和量测方程验证清楚。我在实际项目中就是先在PSINS里把17维状态量、NHCODO量测方程、R矩阵参数摸透了再搬到嵌入式平台上调试整体收敛速度快了很多。7. 结尾前再分享一个我自己的习惯最后分享一个我个人的小习惯不管仿真结果多漂亮我都会故意把初始航向误差设成5度、把ODO刻度因子初始误差设成2%再跑一遍整套流程。这样做的目的是检验滤波器在“不那么理想”的条件下的鲁棒性。很多时候教科书参数下收敛漂亮的滤波器只要把初始误差稍微调大一点或者量测噪声按真实器件指标设宽一点立刻就会露馅。做组合导航算法提前暴露问题比现场炸机后再排查要好得多。整套demo脚本我整理了一下包含轨迹生成、IMU数据仿真、17维状态量组合滤波主循环、绘图脚本和参数配置文件需要的朋友直接到我仓库里拿。运行环境我用的MATLAB R2022b配PSINS 1.5系列其余版本只要把函数适配好也都能跑通。希望你用这套仿真能少走点我当年走过的弯路。