先放一个画面高架桥下楼间距不到三十米导航里的小箭头过每个路口就往外跳十几米明明在主路走着定位却经常“穿墙”到旁边楼里。这不是某个地图App的bug而是城市峡谷里GNSS信号被遮挡、反射之后必然出现的物理现象。卫星信号在楼宇间来回弹射测距误差奔着几十米去任何地图软件都救不回来。但这时候如果车上有惯性导航和轮速传感器情况完全可以不一样——用惯导做短时推演用轮速和车体运动学约束去压住误差发散即使卫星彻底失锁也能把定位漂移控制在车道级以内。这篇文章要讲的就是用严恭敏老师开源的PSINS工具箱从零搭一套INS/NHC/ODO组合导航仿真。内容覆盖NHC非完整约束的数学本质、ODO里程计误差建模、卡尔曼滤波器设计、PSINS核心函数调用方式以及大量我从实际跑数据中总结出来的调参经验和坑。适合正在做车载组合导航的算法工程师、惯性导航方向的硕博研究生以及所有被“GNSS拒止环境定位漂移”折磨过的人。1. 城市峡谷的定位漂移困局为什么GNSS靠不住惯导又撑不久先说清楚我们到底在解决什么问题。城市峡谷从来不是简单的“信号变弱”而是多径效应和NLOS非视距接收叠加后的灾难现场。你收到的卫星信号很多是从玻璃幕墙上反弹过来的“假信号”测距误差不再是分米级而是直接到几十米甚至上百米。这时候GNSS/INS组合导航如果还按正常权重信任GNSS位置滤波器会被带偏得一塌糊涂。很多人以为RTK能解决实际在城市深峡谷里RTK的固定率经常掉到50%以下整周模糊度一解算失败定位直接跳回米级甚至十米级。1.1 高楼反射下GNSS定位有多离谱我实测过一段大约7公里的高架路两边是连续的高层商业楼GNSS单点定位的轨迹横跨整条路最大偏差超过80米过弯时定位点直接飞进旁边的小区。更麻烦的是这种误差不是单调的它会在真值附近来回摆——一会偏左一会偏右滤波器如果开了GNSS位置修正位置估计就会跟着来回抽动姿态和航向也被污染。相比而言惯导在这段时间内的短时递推是相当干净的几十秒上百秒内不发散这就是组合导航“以惯导为主体、其他传感器做校正”这一体系能成立的根本原因。1.2 纯惯导的发散速度数学上有多残酷纯惯导不是不能定位问题是它的误差随时间是发散的而且是积分叠加式发散。陀螺零偏会导致姿态误差姿态误差把重力加速度的一部分错误投影到水平方向这个等效水平加速度持续作用在速度积分和位置积分上位置误差大致按0.5·g·φ·t²的规律增长。举个例子初始失准角如果只有0.1度大约0.0017弧度100秒后位置误差就已经是0.5×9.8×0.0017×10000约83米。这还仅仅是失准角的贡献陀螺漂移、加速度计零偏带来的误差只会让情况更糟。换句话说车载纯惯导在低成本MEMS器件下有效定位时间可能只有几十秒一旦GNSS失锁超过这个时间位置基本不能看。1.3 为什么说单一传感器都是“瘸腿”的GNSS怕遮挡惯导怕发散那是不是只用里程计就好了也不行。里程计给出的是沿车辆前进方向的相对位移它没有绝对的航向参照弯一多就会累积巨大的横向误差。更关键的是轮速无法感知横向速度车辆转弯时如果只用里程计积分轨迹会往转弯内侧塌得非常厉害。所以这三者本质上是互补的惯导提供高频姿态和短时高精度递推ODO提供长期稳定的前向速度参考NHC提供侧向和天向的运动学约束。把它们放进同一个滤波器才能在城市峡谷这种恶劣场景下稳住定位。2. NHC与ODO的救场逻辑车辆运动学约束背后的数学原理在写代码之前一定要把NHC和ODO这两个东西的数学模型搞明白。很多人仿真失败不是代码写错而是坐标定义和约束方程从一开始就乱了。2.1 NHC非完整约束车不可以横着飞NHCNon-Holonomic Constraint翻译过来叫非完整约束本质是一个物理直觉在正常行驶条件下车辆不会发生侧向滑动也不会跳离地面。把车体坐标系b系定义为右前上坐标系x向右y向前z向上那么车辆在b系下的侧向速度x轴和天向速度z轴应该近似为零只有前向速度y轴是显著的。仿真时设一个很小的阈值比如侧向速度小于0.05 m/s天向速度小于0.05 m/s就认为约束成立。这个约束能发挥作用的关键在于惯导解算出的是导航系下的速度要使用NHC约束必须把导航系速度投影回车体系或者反过来把“b系下侧向/天向速度为0”这个条件等效成导航系下的速度观测。无论哪种方式都依赖当前姿态矩阵所以NHC对姿态误差尤其是横滚角和俯仰角有很强的可观性。这一点在实际仿真里体会特别明显加了NHC之后横滚角误差的收敛速度肉眼可见地变快。2.2 ODO里程计从轮速脉冲到前向速度车上轮速传感器输出的是脉冲计数要换算成速度需要知道每个脉冲对应的行驶距离这就是刻度系数单位通常是米每脉冲。问题在于这个系数不是固定不变的轮胎气压变化、载重变化、轮胎磨损、行驶速度变化都会让它偏移。实车标定一次只能标定某个工况下的值跑一段时间又会漂。所以组合导航滤波器里一定要把刻度系数误差设成待估状态让它在线估计、在线修正。仿真时里程计输出可以这样模拟从参考轨迹取真实前向速度加上一个常值刻度系数误差比如1.5%偏小再叠加一点高斯白噪声方差大约0.05 m/s。这样生成的ODO数据比较接近实际轮速信号。注意一点很多入门仿真直接把真实速度当ODO用这相当于白送一个完美观测滤波结果会好得不真实完全没有工程参考价值。2.3 组合架构选择为什么这里用速度观测ODO/NHC辅助惯导有两种常见架构。一种是紧组合把轮速脉冲原始信息放进滤波器对刻度系数、安装角做更精细的建模抗差性强但实现复杂。另一种是松组合先把轮速脉冲转换成前向速度结合NHC约束构造一个三维速度观测向量然后对惯导解算速度做校正。对于绝大多数车辆级应用松组合速度观测已经足够而且实现简洁、鲁棒性好PSINS框架下也最容易改造。在PSINS仿真里我们实际上构造的是这样的量测ODO给出b系前向速度v_odNHC约束侧向和天向速度为零所以b系下的完整速度观测为v_b [0; v_od; 0]右前上坐标系;通过姿态矩阵把v_b投影到导航系得到v_n_obs C_b^n * v_b惯导解算速度v_ins和v_n_obs之差就是卡尔曼滤波的量测残差。这个观测方程里姿态矩阵是核心桥梁姿态误差会直接体现在残差中所以NHC/ODO对姿态有很强的修正能力。里程计刻度系数误差则体现在v_od与真实速度的比例偏差上滤波器通过残差中的系统性偏差去估计它。3. PSINS工具箱的打开姿势环境配置与核心数据流要把上面的数学模型跑起来PSINS是目前最合适的选择。它不像商业软件是个黑盒子所有算法代码都摊开在面前每一行都可以读、可以改、可以验证。3.1 为什么选PSINS而不是MATLAB自带工具包MATLAB新版本的Navigation Toolbox也支持惯性导航仿真但一是授权不便宜二是算法细节封装太死做算法研究的人没法方便地修改状态方程、观测矩阵。PSINS是严恭敏老师实验室开源的十几年积累完整实现了捷联惯导的静基座对准、动基座对准、惯导机械编排、组合导航滤波以及大量轨迹仿真工具。最靠谱的一点是它的更新算法和误差模型与绝大多数论文、教材一致拿它跑出来的结果可以直接对标文献。3.2 安装与环境配置要点下载PSINS压缩包后解压到任意目录然后打开MATLAB在主界面执行addpath(genpath(‘psins目录路径’))把整个目录结构加入搜索路径。PSINS依赖的全局变量结构体glvs会在脚本运行时自动初始化。我建议安装后第一时间跑一遍自带的测试脚本比如test_SINS_GPS_153.m确认环境没问题。这个脚本是SINS/GPS组合导航的经典示例包含了轨迹生成、IMU误差添加、惯导解算、卡尔曼滤波、结果分析的全套流程。跑通它之后改造成INS/NHC/ODO组合导航就顺理成章了。3.3 理清数据流从轨迹生成到组合解算的完整闭环整个仿真数据流可以概括成这么几个环节用参考轨迹位置、速度、姿态随时间变化生成理想IMU数据给IMU数据叠加陀螺零偏、加速度计零偏、角度随机游走、速度随机游走从参考轨迹提取前向速度叠加刻度系数误差和噪声生成ODO数据惯导机械编排每次迭代向前推进一步得到位置速度姿态当ODO有数据时构造速度观测进行卡尔曼滤波修正把修正后的误差反馈给惯导解算状态循环往复。这个环路里的核心就是insupdate和kfupdate这两个函数。前者是捷联惯导更新后者是卡尔曼滤波一步更新。搞清楚这两个函数怎么调用、怎么传参整个仿真就通了。4. 手把手搭建INS/NHC/ODO仿真从轨迹生成到误差评估下面进入实操环节。我直接给出一套可以在PSINS框架下运行的MATLAB脚本框架每一步都解释为什么这样写。4.1 第一步生成参考轨迹与IMU原始数据仿真首先要有一条“真值轨迹”它决定了后续所有结果的对比基准。PSINS提供了轨迹生成工具可以按照多段运动拼接的方式生成直行、匀速、转弯、坡道、加减速都可以组合。以下示意代码是典型的做法% 参考轨迹初始化 glvs ts 0.005; % IMU采样间隔200Hz T 600; % 仿真时长600秒 avp0 [[0;0;0]*glv.deg; [0;0;0]; [34.2;108.9;400]]; % 初始姿态/速度/经纬高 % 定义多段运动并生成轨迹示意写法 trj trjsimu(ts, T, urban_route); % 从轨迹中提取理想IMU数据 imu trj.imu; % 添加惯导器件误差 imu_err imuadderr(imu, eb, db, web, wdb, ts); % 提取参考真值用于后续对比 trj_avp trj.avp;这里有几个细节值得注意。第一初始经纬高必须设置成实际场景的位置PSINS内部计算地球曲率半径、重力加速度等都依赖这个值。第二ts的选取要和实际硬件一致车载MEMS IMU常见的是100Hz到200Hz仿真设200Hz即可。第三imuadderr中的四个量分别是陀螺零偏、加速度计零偏、陀螺随机游走、加速度计随机游走具体数值可以参考你所用IMU的datasheet不要拍脑袋写。4.2 第二步构建NHC与ODO观测数据从参考轨迹里可以拿到每一时刻的真实速度和真实姿态。ODO数据不能直接用真实速度要人为注入误差。代码如下% 从参考轨迹提取前向速度右前上坐标系下前向对应速度的第二个分量 vn_true trj.avp(:, 4:6); Cnb_true q2mat(trj.avp(:, 1:4)); % 姿态四元数转矩阵 vb_true Cnb_true * vn_true; % 导航系速度投影到车体系 v_odo_true abs(vb_true(2, :)); % 前向速度 % 注入刻度系数误差和噪声 k_sf 1.015; % 刻度系数误差为1.5% v_odo v_odo_true / k_sf 0.05 * randn(size(v_odo_true));这一步特别容易踩坑。车体系坐标轴定义不同前向速度的索引就不同。很多人在PSINS默认的坐标系里直接用第一个分量结果NHC约束验证死活对不上。所以开始写代码前先确认坐标系定义再确认IMU安装方向再确认轨迹行进方向三个坐标系全部统一了再往下走。NHC观测在代码里的体现是当构造三维速度观测时v_b的侧向和天向分量强制设为零而不是用IMU解算出来的对应分量。很多人把这个细节忽略了写到最后变成“用带噪声的真实速度替代ODO”那NHC等于没有加。4.3 第三步卡尔曼滤波器设计与状态量选择状态量的设计直接决定滤波器能估计什么。基础15维状态量包括姿态误差3个、速度误差3个、位置误差3个、陀螺零偏3个、加速度计零偏3个。加入ODO后还需要把里程计刻度系数误差作为一个维度所以这里设计成16维状态量% 状态量定义16维 % 1:3 姿态误差 % 4:6 速度误差 % 7:9 位置误差 % 10:12 陀螺零偏 % 13:15 加速度计零偏 % 16 里程计刻度系数误差(b系前向)状态方程用PSINS线性化后的惯导误差方程离散化之后填充状态转移矩阵F。Q矩阵反映的是陀螺和加速度计随机游走强度R矩阵反映的是NHC/ODO观测噪声强度。这个不是随便设的后面专门讲怎么调。量测矩阵H的构造是核心。对于速度观测惯导解算速度和观测速度之差对应的H可以按下面这个框架写% 观测向量3x1惯导解算速度与NHC/ODO观测速度之差 Cbn q2mat(qk); vn_odo_obs Cbn * [0; v_odo(k); 0]; % 右前上坐标系 z vn_ins - vn_odo_obs; % H矩阵分块 Hk zeros(3, 16); Hk(1:3, 4:6) eye(3); % 速度误差直接可见 Hk(1:3, 1:3) -askew(vn_odo_obs); % 姿态误差通过投影影响观测 Hk(1:3, 16) -Cbn * [0; v_odo(k) / k_sf; 0]; % 刻度系数误差这三块分别对应三种状态量是如何进入量测的。速度误差直接就是惯导速度误差本身所以是单位阵姿态误差是通过姿态矩阵投影到导航系时产生的误差所以有反对称矩阵刻度系数误差是通过改变v_od大小进入系统的。H矩阵写对了滤波器才会正确地给各个状态量分配修正量。4.4 第四步主循环解算与结果存储主循环的核心就是惯导更新加滤波更新。思路如下for k 2:length(imu) % 惯导机械编排递推 ins insupdate(ins, imu(k, 1:6), ts); % 每步都进行ODO/NHC观测更新 if mod(k, odo_step) 0 % 构造量测向量和量测矩阵 [z, Hk] build_od_nhc_observation(ins, v_odo(k), k_sf_init); % 卡尔曼滤波更新 ins kfupdate(ins, z, Hk, Rk); % 反馈校正并重置滤波状态 ins kfupdate(ins); end % 存储结果 avp_ins(:, k) [ins.qnb; ins.vn; ins.pos]; end注意PSINS的kfupdate有两种调用方式带参数的是一次完整的量测更新不带参数的是一次时间更新。组合导航里的一般节奏是每个IMU周期先做时间更新有ODO观测时再做量测更新并把估计出的误差反馈给惯导状态。反馈之后要把滤波器状态向量清零否则误差会重复修正这就是kfupdate(ins)这行代码的作用。4.5 第五步误差评估与绘图仿真跑完不要只盯着轨迹图看“看起来差不多”要有量化指标。常用做法是把解算导航结果与真值轨迹做差统计位置误差的RMS和最大值看二维轨迹与真值轨迹的贴合程度。PSINS里可以有现成的绘图比较脚本% 计算位置和姿态误差 err avp_ins - trj_avp; figure; subplot(2,1,1); plot(trj_avp(:,7), trj_avp(:,8), b); hold on; plot(ins_pos(:,1), ins_pos(:,2), r); legend(参考轨迹, INS/NHC/ODO解算); xlabel(经度); ylabel(纬度); subplot(2,1,2); t (0:size(err,2)-1) * ts; plot(t, err(7:9, :)*1e3); xlabel(时间/s); ylabel(位置误差/m); legend({纬度误差,经度误差,高度误差});这里的位置误差单位要注意PSINS里位置通常用经纬度弧度表示直接乘1e3转成米再画图才直观更严格的做法是用初始位置的地球曲率半径换算。5. 仿真结果解读与调参直觉别只看轨迹漂没漂跑通第一次仿真后你大概率会看到两个结果一是轨迹基本贴合二是误差曲线有一些“锯齿”现象。这都正常关键是学会解读这些结果背后的算法行为。5.1 NHC带来的意外收益横滚角可观性改善我第一次对比“纯惯导ODO”和“惯导ODONHC”两组结果时发现横滚角误差的收敛速度完全不是一个量级。原因也想通了NHC把侧向速度强制设为零这个约束对姿态矩阵的横向投影特别敏感一旦横滚角有误差侧向投影出非零速度残差立刻变大滤波器马上修正。俯仰角同理坡道上尤其明显。航向角相对弱一些因为航向误差主要靠转弯时的加速度变化去激励仿真轨迹里如果全是直线航向误差会收敛得很慢。5.2 里程计刻度系数误差的在线估计16维状态量里的刻度系数误差是可观性很强的状态量只要车辆在跑前向速度激励一直存在滤波器就能持续估计它。我经常用这个收敛值来判断滤波器设计是否正确如果仿真注入1.5%的刻度系数误差估计收敛值应该非常接近这个数。如果收敛值偏差很大先检查观测矩阵Cbn那一列的符号再检查坐标系定义这两处是最容易出错的。5.3 Q/R矩阵的设参经验别凭感觉乱填Q矩阵是过程噪声矩阵反映你对IMU误差模型的信任程度R矩阵是量测噪声矩阵反映你对NHC/ODO观测的信任程度。这两个矩阵的比值决定滤波器的带宽和噪声水平比它们各自的绝对值重要得多。我个人调试的顺序是先把R设成0.05²的量级对应5cm/s的量测噪声然后用器件datasheet和Allan方差分析结果设Q再根据误差曲线微调。如果位置误差曲线噪声太大往往是Q/R比值偏大、观测权重不足调小Q或调大R即可如果误差曲线长时间不收敛则是Q设得过小滤波器“太自信”地认为惯导误差模型完全准确。5.4 NHC失效边界急刹、滑移、原地转向仿真时所有约束都是理想的但实车不是。NHC在以下场景会失效急刹车时车辆点头前向速度突跳路面光滑时侧滑侧向速度不再为零原地转向时轮速有效而GNSS大幅跨步。仿真阶段也要为这些边界情况保留处理逻辑最简单的做法是加一个“运动状态判断”当检测到IMU角速度或加速度突变量超过阈值时量测噪声R放大降低对观测的信任。这种策略在PSINS里很容易实现改一下Rk的数值就行。6. 代码获取与后续扩展从仿真到实车移植的关键一步到这里整套INS/NHC/ODO组合导航仿真已经跑通了。完整工程代码我整理成了一个仓库包含轨迹生成脚本、IMU误差注入模块、NHC/ODO观测生成模块、16维组合导航主循环、误差评估和绘图脚本全部基于PSINS框架拿下来改改IMU参数就能跑。你可以在GitHub搜索PSINS_NHC_ODO_Demo或者到同名博客后台回复“NHC_ODO”获取下载链接。6.1 代码里到底有什么仓库里最核心的是三个文件main_ins_nhc_odo.m主脚本、build_od_nhc_observation.m观测构造函数、plot_results.m结果可视化脚本。主脚本结构清晰每个关键步骤都有注释适合你在此基础上做各种扩展实验比如换成不同的轨迹、注入更大的IMU误差、改成紧组合架构。6.2 从仿真到实车移植的四个致命细节仿真跑得再漂亮不解决下面这四个问题上实车必翻车一是时间同步。IMU和ODO数据必须统一时间戳PSINS仿真里天然同步但实车需要做硬件同步或者软件插值时间不同步会导致量测残差出现系统性偏差。二是外参标定。ODO在车体系下的位置、IMU安装角、杆臂都会影响观测构建。安装角误差如果达到零点几度航向误差可能直接变成米级定位偏差这时候必须把安装角都加进滤波器估计。三是脉冲计数到距离的转换。实车ODO输出脉冲每个脉冲对应的距离需要精确标定。更麻烦的是倒车时脉冲方向和累计方式很多芯片的脉冲接口正反转处理方式不一样代码要区分清楚。四是车轮滑移和打滑。仿真里基本不考虑实车却逃不掉。急加速发动机会让车轮转速高于实际车速刹车ABS时又低于实际车速这种情况下要做滑移检测在观测端做保护。6.3 可以继续做的方向这套框架的扩展空间非常大。往深了做可以在16维状态量上加二维安装误差角变成19维组合可以在ODO基础上引入左右轮速差直接约束航向角更新量可以在GNSS信号恢复后加入GNSS位置观测实现GNSS/INS/NHC/ODO多源融合还可以把里程计替换成视觉里程计或激光里程计这套NHC/ODO的观测框架同样适用。逻辑是一样的凡是能提供运动学约束的传感器都能挂到这个滤波器上。我个人在实际项目里的体会是仿真阶段把NHC/ODO这类运动学约束想透到了实车调试阶段能省下大量时间。很多你以为的“硬件问题”最后查出来都是坐标系定义不一致或者观测矩阵符号写反了。先用仿真把算法逻辑磨干净再上实车做数据采集和参数标定这是最稳妥的路径。
基于PSINS的INS/NHC/ODO组合导航仿真：解决城市峡谷GNSS定位漂移
先放一个画面高架桥下楼间距不到三十米导航里的小箭头过每个路口就往外跳十几米明明在主路走着定位却经常“穿墙”到旁边楼里。这不是某个地图App的bug而是城市峡谷里GNSS信号被遮挡、反射之后必然出现的物理现象。卫星信号在楼宇间来回弹射测距误差奔着几十米去任何地图软件都救不回来。但这时候如果车上有惯性导航和轮速传感器情况完全可以不一样——用惯导做短时推演用轮速和车体运动学约束去压住误差发散即使卫星彻底失锁也能把定位漂移控制在车道级以内。这篇文章要讲的就是用严恭敏老师开源的PSINS工具箱从零搭一套INS/NHC/ODO组合导航仿真。内容覆盖NHC非完整约束的数学本质、ODO里程计误差建模、卡尔曼滤波器设计、PSINS核心函数调用方式以及大量我从实际跑数据中总结出来的调参经验和坑。适合正在做车载组合导航的算法工程师、惯性导航方向的硕博研究生以及所有被“GNSS拒止环境定位漂移”折磨过的人。1. 城市峡谷的定位漂移困局为什么GNSS靠不住惯导又撑不久先说清楚我们到底在解决什么问题。城市峡谷从来不是简单的“信号变弱”而是多径效应和NLOS非视距接收叠加后的灾难现场。你收到的卫星信号很多是从玻璃幕墙上反弹过来的“假信号”测距误差不再是分米级而是直接到几十米甚至上百米。这时候GNSS/INS组合导航如果还按正常权重信任GNSS位置滤波器会被带偏得一塌糊涂。很多人以为RTK能解决实际在城市深峡谷里RTK的固定率经常掉到50%以下整周模糊度一解算失败定位直接跳回米级甚至十米级。1.1 高楼反射下GNSS定位有多离谱我实测过一段大约7公里的高架路两边是连续的高层商业楼GNSS单点定位的轨迹横跨整条路最大偏差超过80米过弯时定位点直接飞进旁边的小区。更麻烦的是这种误差不是单调的它会在真值附近来回摆——一会偏左一会偏右滤波器如果开了GNSS位置修正位置估计就会跟着来回抽动姿态和航向也被污染。相比而言惯导在这段时间内的短时递推是相当干净的几十秒上百秒内不发散这就是组合导航“以惯导为主体、其他传感器做校正”这一体系能成立的根本原因。1.2 纯惯导的发散速度数学上有多残酷纯惯导不是不能定位问题是它的误差随时间是发散的而且是积分叠加式发散。陀螺零偏会导致姿态误差姿态误差把重力加速度的一部分错误投影到水平方向这个等效水平加速度持续作用在速度积分和位置积分上位置误差大致按0.5·g·φ·t²的规律增长。举个例子初始失准角如果只有0.1度大约0.0017弧度100秒后位置误差就已经是0.5×9.8×0.0017×10000约83米。这还仅仅是失准角的贡献陀螺漂移、加速度计零偏带来的误差只会让情况更糟。换句话说车载纯惯导在低成本MEMS器件下有效定位时间可能只有几十秒一旦GNSS失锁超过这个时间位置基本不能看。1.3 为什么说单一传感器都是“瘸腿”的GNSS怕遮挡惯导怕发散那是不是只用里程计就好了也不行。里程计给出的是沿车辆前进方向的相对位移它没有绝对的航向参照弯一多就会累积巨大的横向误差。更关键的是轮速无法感知横向速度车辆转弯时如果只用里程计积分轨迹会往转弯内侧塌得非常厉害。所以这三者本质上是互补的惯导提供高频姿态和短时高精度递推ODO提供长期稳定的前向速度参考NHC提供侧向和天向的运动学约束。把它们放进同一个滤波器才能在城市峡谷这种恶劣场景下稳住定位。2. NHC与ODO的救场逻辑车辆运动学约束背后的数学原理在写代码之前一定要把NHC和ODO这两个东西的数学模型搞明白。很多人仿真失败不是代码写错而是坐标定义和约束方程从一开始就乱了。2.1 NHC非完整约束车不可以横着飞NHCNon-Holonomic Constraint翻译过来叫非完整约束本质是一个物理直觉在正常行驶条件下车辆不会发生侧向滑动也不会跳离地面。把车体坐标系b系定义为右前上坐标系x向右y向前z向上那么车辆在b系下的侧向速度x轴和天向速度z轴应该近似为零只有前向速度y轴是显著的。仿真时设一个很小的阈值比如侧向速度小于0.05 m/s天向速度小于0.05 m/s就认为约束成立。这个约束能发挥作用的关键在于惯导解算出的是导航系下的速度要使用NHC约束必须把导航系速度投影回车体系或者反过来把“b系下侧向/天向速度为0”这个条件等效成导航系下的速度观测。无论哪种方式都依赖当前姿态矩阵所以NHC对姿态误差尤其是横滚角和俯仰角有很强的可观性。这一点在实际仿真里体会特别明显加了NHC之后横滚角误差的收敛速度肉眼可见地变快。2.2 ODO里程计从轮速脉冲到前向速度车上轮速传感器输出的是脉冲计数要换算成速度需要知道每个脉冲对应的行驶距离这就是刻度系数单位通常是米每脉冲。问题在于这个系数不是固定不变的轮胎气压变化、载重变化、轮胎磨损、行驶速度变化都会让它偏移。实车标定一次只能标定某个工况下的值跑一段时间又会漂。所以组合导航滤波器里一定要把刻度系数误差设成待估状态让它在线估计、在线修正。仿真时里程计输出可以这样模拟从参考轨迹取真实前向速度加上一个常值刻度系数误差比如1.5%偏小再叠加一点高斯白噪声方差大约0.05 m/s。这样生成的ODO数据比较接近实际轮速信号。注意一点很多入门仿真直接把真实速度当ODO用这相当于白送一个完美观测滤波结果会好得不真实完全没有工程参考价值。2.3 组合架构选择为什么这里用速度观测ODO/NHC辅助惯导有两种常见架构。一种是紧组合把轮速脉冲原始信息放进滤波器对刻度系数、安装角做更精细的建模抗差性强但实现复杂。另一种是松组合先把轮速脉冲转换成前向速度结合NHC约束构造一个三维速度观测向量然后对惯导解算速度做校正。对于绝大多数车辆级应用松组合速度观测已经足够而且实现简洁、鲁棒性好PSINS框架下也最容易改造。在PSINS仿真里我们实际上构造的是这样的量测ODO给出b系前向速度v_odNHC约束侧向和天向速度为零所以b系下的完整速度观测为v_b [0; v_od; 0]右前上坐标系;通过姿态矩阵把v_b投影到导航系得到v_n_obs C_b^n * v_b惯导解算速度v_ins和v_n_obs之差就是卡尔曼滤波的量测残差。这个观测方程里姿态矩阵是核心桥梁姿态误差会直接体现在残差中所以NHC/ODO对姿态有很强的修正能力。里程计刻度系数误差则体现在v_od与真实速度的比例偏差上滤波器通过残差中的系统性偏差去估计它。3. PSINS工具箱的打开姿势环境配置与核心数据流要把上面的数学模型跑起来PSINS是目前最合适的选择。它不像商业软件是个黑盒子所有算法代码都摊开在面前每一行都可以读、可以改、可以验证。3.1 为什么选PSINS而不是MATLAB自带工具包MATLAB新版本的Navigation Toolbox也支持惯性导航仿真但一是授权不便宜二是算法细节封装太死做算法研究的人没法方便地修改状态方程、观测矩阵。PSINS是严恭敏老师实验室开源的十几年积累完整实现了捷联惯导的静基座对准、动基座对准、惯导机械编排、组合导航滤波以及大量轨迹仿真工具。最靠谱的一点是它的更新算法和误差模型与绝大多数论文、教材一致拿它跑出来的结果可以直接对标文献。3.2 安装与环境配置要点下载PSINS压缩包后解压到任意目录然后打开MATLAB在主界面执行addpath(genpath(‘psins目录路径’))把整个目录结构加入搜索路径。PSINS依赖的全局变量结构体glvs会在脚本运行时自动初始化。我建议安装后第一时间跑一遍自带的测试脚本比如test_SINS_GPS_153.m确认环境没问题。这个脚本是SINS/GPS组合导航的经典示例包含了轨迹生成、IMU误差添加、惯导解算、卡尔曼滤波、结果分析的全套流程。跑通它之后改造成INS/NHC/ODO组合导航就顺理成章了。3.3 理清数据流从轨迹生成到组合解算的完整闭环整个仿真数据流可以概括成这么几个环节用参考轨迹位置、速度、姿态随时间变化生成理想IMU数据给IMU数据叠加陀螺零偏、加速度计零偏、角度随机游走、速度随机游走从参考轨迹提取前向速度叠加刻度系数误差和噪声生成ODO数据惯导机械编排每次迭代向前推进一步得到位置速度姿态当ODO有数据时构造速度观测进行卡尔曼滤波修正把修正后的误差反馈给惯导解算状态循环往复。这个环路里的核心就是insupdate和kfupdate这两个函数。前者是捷联惯导更新后者是卡尔曼滤波一步更新。搞清楚这两个函数怎么调用、怎么传参整个仿真就通了。4. 手把手搭建INS/NHC/ODO仿真从轨迹生成到误差评估下面进入实操环节。我直接给出一套可以在PSINS框架下运行的MATLAB脚本框架每一步都解释为什么这样写。4.1 第一步生成参考轨迹与IMU原始数据仿真首先要有一条“真值轨迹”它决定了后续所有结果的对比基准。PSINS提供了轨迹生成工具可以按照多段运动拼接的方式生成直行、匀速、转弯、坡道、加减速都可以组合。以下示意代码是典型的做法% 参考轨迹初始化 glvs ts 0.005; % IMU采样间隔200Hz T 600; % 仿真时长600秒 avp0 [[0;0;0]*glv.deg; [0;0;0]; [34.2;108.9;400]]; % 初始姿态/速度/经纬高 % 定义多段运动并生成轨迹示意写法 trj trjsimu(ts, T, urban_route); % 从轨迹中提取理想IMU数据 imu trj.imu; % 添加惯导器件误差 imu_err imuadderr(imu, eb, db, web, wdb, ts); % 提取参考真值用于后续对比 trj_avp trj.avp;这里有几个细节值得注意。第一初始经纬高必须设置成实际场景的位置PSINS内部计算地球曲率半径、重力加速度等都依赖这个值。第二ts的选取要和实际硬件一致车载MEMS IMU常见的是100Hz到200Hz仿真设200Hz即可。第三imuadderr中的四个量分别是陀螺零偏、加速度计零偏、陀螺随机游走、加速度计随机游走具体数值可以参考你所用IMU的datasheet不要拍脑袋写。4.2 第二步构建NHC与ODO观测数据从参考轨迹里可以拿到每一时刻的真实速度和真实姿态。ODO数据不能直接用真实速度要人为注入误差。代码如下% 从参考轨迹提取前向速度右前上坐标系下前向对应速度的第二个分量 vn_true trj.avp(:, 4:6); Cnb_true q2mat(trj.avp(:, 1:4)); % 姿态四元数转矩阵 vb_true Cnb_true * vn_true; % 导航系速度投影到车体系 v_odo_true abs(vb_true(2, :)); % 前向速度 % 注入刻度系数误差和噪声 k_sf 1.015; % 刻度系数误差为1.5% v_odo v_odo_true / k_sf 0.05 * randn(size(v_odo_true));这一步特别容易踩坑。车体系坐标轴定义不同前向速度的索引就不同。很多人在PSINS默认的坐标系里直接用第一个分量结果NHC约束验证死活对不上。所以开始写代码前先确认坐标系定义再确认IMU安装方向再确认轨迹行进方向三个坐标系全部统一了再往下走。NHC观测在代码里的体现是当构造三维速度观测时v_b的侧向和天向分量强制设为零而不是用IMU解算出来的对应分量。很多人把这个细节忽略了写到最后变成“用带噪声的真实速度替代ODO”那NHC等于没有加。4.3 第三步卡尔曼滤波器设计与状态量选择状态量的设计直接决定滤波器能估计什么。基础15维状态量包括姿态误差3个、速度误差3个、位置误差3个、陀螺零偏3个、加速度计零偏3个。加入ODO后还需要把里程计刻度系数误差作为一个维度所以这里设计成16维状态量% 状态量定义16维 % 1:3 姿态误差 % 4:6 速度误差 % 7:9 位置误差 % 10:12 陀螺零偏 % 13:15 加速度计零偏 % 16 里程计刻度系数误差(b系前向)状态方程用PSINS线性化后的惯导误差方程离散化之后填充状态转移矩阵F。Q矩阵反映的是陀螺和加速度计随机游走强度R矩阵反映的是NHC/ODO观测噪声强度。这个不是随便设的后面专门讲怎么调。量测矩阵H的构造是核心。对于速度观测惯导解算速度和观测速度之差对应的H可以按下面这个框架写% 观测向量3x1惯导解算速度与NHC/ODO观测速度之差 Cbn q2mat(qk); vn_odo_obs Cbn * [0; v_odo(k); 0]; % 右前上坐标系 z vn_ins - vn_odo_obs; % H矩阵分块 Hk zeros(3, 16); Hk(1:3, 4:6) eye(3); % 速度误差直接可见 Hk(1:3, 1:3) -askew(vn_odo_obs); % 姿态误差通过投影影响观测 Hk(1:3, 16) -Cbn * [0; v_odo(k) / k_sf; 0]; % 刻度系数误差这三块分别对应三种状态量是如何进入量测的。速度误差直接就是惯导速度误差本身所以是单位阵姿态误差是通过姿态矩阵投影到导航系时产生的误差所以有反对称矩阵刻度系数误差是通过改变v_od大小进入系统的。H矩阵写对了滤波器才会正确地给各个状态量分配修正量。4.4 第四步主循环解算与结果存储主循环的核心就是惯导更新加滤波更新。思路如下for k 2:length(imu) % 惯导机械编排递推 ins insupdate(ins, imu(k, 1:6), ts); % 每步都进行ODO/NHC观测更新 if mod(k, odo_step) 0 % 构造量测向量和量测矩阵 [z, Hk] build_od_nhc_observation(ins, v_odo(k), k_sf_init); % 卡尔曼滤波更新 ins kfupdate(ins, z, Hk, Rk); % 反馈校正并重置滤波状态 ins kfupdate(ins); end % 存储结果 avp_ins(:, k) [ins.qnb; ins.vn; ins.pos]; end注意PSINS的kfupdate有两种调用方式带参数的是一次完整的量测更新不带参数的是一次时间更新。组合导航里的一般节奏是每个IMU周期先做时间更新有ODO观测时再做量测更新并把估计出的误差反馈给惯导状态。反馈之后要把滤波器状态向量清零否则误差会重复修正这就是kfupdate(ins)这行代码的作用。4.5 第五步误差评估与绘图仿真跑完不要只盯着轨迹图看“看起来差不多”要有量化指标。常用做法是把解算导航结果与真值轨迹做差统计位置误差的RMS和最大值看二维轨迹与真值轨迹的贴合程度。PSINS里可以有现成的绘图比较脚本% 计算位置和姿态误差 err avp_ins - trj_avp; figure; subplot(2,1,1); plot(trj_avp(:,7), trj_avp(:,8), b); hold on; plot(ins_pos(:,1), ins_pos(:,2), r); legend(参考轨迹, INS/NHC/ODO解算); xlabel(经度); ylabel(纬度); subplot(2,1,2); t (0:size(err,2)-1) * ts; plot(t, err(7:9, :)*1e3); xlabel(时间/s); ylabel(位置误差/m); legend({纬度误差,经度误差,高度误差});这里的位置误差单位要注意PSINS里位置通常用经纬度弧度表示直接乘1e3转成米再画图才直观更严格的做法是用初始位置的地球曲率半径换算。5. 仿真结果解读与调参直觉别只看轨迹漂没漂跑通第一次仿真后你大概率会看到两个结果一是轨迹基本贴合二是误差曲线有一些“锯齿”现象。这都正常关键是学会解读这些结果背后的算法行为。5.1 NHC带来的意外收益横滚角可观性改善我第一次对比“纯惯导ODO”和“惯导ODONHC”两组结果时发现横滚角误差的收敛速度完全不是一个量级。原因也想通了NHC把侧向速度强制设为零这个约束对姿态矩阵的横向投影特别敏感一旦横滚角有误差侧向投影出非零速度残差立刻变大滤波器马上修正。俯仰角同理坡道上尤其明显。航向角相对弱一些因为航向误差主要靠转弯时的加速度变化去激励仿真轨迹里如果全是直线航向误差会收敛得很慢。5.2 里程计刻度系数误差的在线估计16维状态量里的刻度系数误差是可观性很强的状态量只要车辆在跑前向速度激励一直存在滤波器就能持续估计它。我经常用这个收敛值来判断滤波器设计是否正确如果仿真注入1.5%的刻度系数误差估计收敛值应该非常接近这个数。如果收敛值偏差很大先检查观测矩阵Cbn那一列的符号再检查坐标系定义这两处是最容易出错的。5.3 Q/R矩阵的设参经验别凭感觉乱填Q矩阵是过程噪声矩阵反映你对IMU误差模型的信任程度R矩阵是量测噪声矩阵反映你对NHC/ODO观测的信任程度。这两个矩阵的比值决定滤波器的带宽和噪声水平比它们各自的绝对值重要得多。我个人调试的顺序是先把R设成0.05²的量级对应5cm/s的量测噪声然后用器件datasheet和Allan方差分析结果设Q再根据误差曲线微调。如果位置误差曲线噪声太大往往是Q/R比值偏大、观测权重不足调小Q或调大R即可如果误差曲线长时间不收敛则是Q设得过小滤波器“太自信”地认为惯导误差模型完全准确。5.4 NHC失效边界急刹、滑移、原地转向仿真时所有约束都是理想的但实车不是。NHC在以下场景会失效急刹车时车辆点头前向速度突跳路面光滑时侧滑侧向速度不再为零原地转向时轮速有效而GNSS大幅跨步。仿真阶段也要为这些边界情况保留处理逻辑最简单的做法是加一个“运动状态判断”当检测到IMU角速度或加速度突变量超过阈值时量测噪声R放大降低对观测的信任。这种策略在PSINS里很容易实现改一下Rk的数值就行。6. 代码获取与后续扩展从仿真到实车移植的关键一步到这里整套INS/NHC/ODO组合导航仿真已经跑通了。完整工程代码我整理成了一个仓库包含轨迹生成脚本、IMU误差注入模块、NHC/ODO观测生成模块、16维组合导航主循环、误差评估和绘图脚本全部基于PSINS框架拿下来改改IMU参数就能跑。你可以在GitHub搜索PSINS_NHC_ODO_Demo或者到同名博客后台回复“NHC_ODO”获取下载链接。6.1 代码里到底有什么仓库里最核心的是三个文件main_ins_nhc_odo.m主脚本、build_od_nhc_observation.m观测构造函数、plot_results.m结果可视化脚本。主脚本结构清晰每个关键步骤都有注释适合你在此基础上做各种扩展实验比如换成不同的轨迹、注入更大的IMU误差、改成紧组合架构。6.2 从仿真到实车移植的四个致命细节仿真跑得再漂亮不解决下面这四个问题上实车必翻车一是时间同步。IMU和ODO数据必须统一时间戳PSINS仿真里天然同步但实车需要做硬件同步或者软件插值时间不同步会导致量测残差出现系统性偏差。二是外参标定。ODO在车体系下的位置、IMU安装角、杆臂都会影响观测构建。安装角误差如果达到零点几度航向误差可能直接变成米级定位偏差这时候必须把安装角都加进滤波器估计。三是脉冲计数到距离的转换。实车ODO输出脉冲每个脉冲对应的距离需要精确标定。更麻烦的是倒车时脉冲方向和累计方式很多芯片的脉冲接口正反转处理方式不一样代码要区分清楚。四是车轮滑移和打滑。仿真里基本不考虑实车却逃不掉。急加速发动机会让车轮转速高于实际车速刹车ABS时又低于实际车速这种情况下要做滑移检测在观测端做保护。6.3 可以继续做的方向这套框架的扩展空间非常大。往深了做可以在16维状态量上加二维安装误差角变成19维组合可以在ODO基础上引入左右轮速差直接约束航向角更新量可以在GNSS信号恢复后加入GNSS位置观测实现GNSS/INS/NHC/ODO多源融合还可以把里程计替换成视觉里程计或激光里程计这套NHC/ODO的观测框架同样适用。逻辑是一样的凡是能提供运动学约束的传感器都能挂到这个滤波器上。我个人在实际项目里的体会是仿真阶段把NHC/ODO这类运动学约束想透到了实车调试阶段能省下大量时间。很多你以为的“硬件问题”最后查出来都是坐标系定义不一致或者观测矩阵符号写反了。先用仿真把算法逻辑磨干净再上实车做数据采集和参数标定这是最稳妥的路径。
