企业官网建设流程全解析

去年冬天我在陆家嘴帮一个做物流无人车的朋友跑实车数据，车刚进楼群密集区，组合定位系统里的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，垂向速度为0；ODO：前向速度等于轮速 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*H'+R的高斯分布。如果新息向量模长明显超出门限，就认为这次量测不可信，直接跳过或者降权处理。这个逻辑在PSINS框架里实现起来非常直观，本身也是组合导航系统抗野值最实用的手段之一。

门限的选取有个经验值参考，一般取5.99或者7.81，对应卡方分布在95%置信度下的两个或三个自由度的阈值。实际运行中如果发现误删正常量测，就把门限放宽一些。有了这一层保护，即使ODO偶尔跳一个脉冲，滤波也能稳定维持，不至于把一次偶发错误放大成整个轨迹的偏移。

6. 代码获取、后续扩展与个人心得

完整可运行的demo工程，包括轨迹生成、滤波闭环和画图评估脚本，我已经打包整理好。想直接拿代码跑通再研究细节的，评论区留言或者私信我都可以发你。如果你更倾向于自己动手搭，那就按上面第三节的框架，配合你本地的PSINS版本把接口对齐，半天内跑通问题不大。拿到基础demo之后建议做三个方向的改动练习，第一个是扩展16维状态量估计里程计刻度误差，第二个是加入安装误差角在线估计，第三个是把新息卡方检验加进去，做完这三个练习基本就具备独立解决真实车辆定位问题的能力了。

我自己做了这么多年惯导组合，最大的体会是PSINS这种仿真工具的真正价值不在于把精度跑得多高，而在于帮你建立起对误差传播的直觉。仿真发散时你会去推量测矩阵、去看协方差设置、去查坐标变换方向，这些排查过程比任何教科书都让人印象深刻。等到你真的把一套组合导航系统搬到实车上，会发现当年在仿真里踩过的每一个坑都变成了预判问题的本钱。关于NHC/ODO组合，最后再分享一个经验：如果你能在仿真里把GNSS失锁两分钟的位置误差控制在十几米量级，那么实车上在相同时间尺度下大概率也不会太差，因为环境误差的主要来源你已经在仿真里摸透了。