简介GNSS与INS组合导航是无人系统与高精度定位领域的核心方案GNSS提供长时间稳定的绝对位置INS以高频输出维持短时高精度二者通过卡尔曼滤波实现优势互补。工程实践中硬件成本与误差可控性成为学习门槛而软件仿真则能有效拆解算法链路。从传感器模型到机械编排从误差传播到滤波调参仿真工具可让开发者直观观察位置、速度、姿态的估计过程。其中科里奥利效应作为地球自转与载体运动产生的修正项对高速长航时场景影响显著常被初学者忽略。NaveGo作为轻量级开源仿真工具在Matlab/Octave环境下完整实现了GNSS/INS松耦合融合、IMU误差建模及Coriolis补偿是理解组合导航原理、验证滤波参数和进行误差分析的理想平台也为后续真实硬件接入提供了坚实铺垫。 最近整理资料时翻出一个早先下载的NaveGo-master.zip文件名还带着GNSS MASTER和coriolis的标记一看就是当年从某个下载站或者 fork 页面打包回来的。解压后顺手重跑了一遍发现这套 Matlab/Octave 环境下的 GNSS/INS 组合导航仿真工具到现在依然是入门惯导组合最顺手的开源项目之一。NaveGo 做的事可以简单概括为在纯软件环境里模拟 GNSS 接收机和 IMU 惯性测量单元的输出再用卡尔曼滤波把两者融合起来输出位置、速度和姿态的估计结果。它的代码里专门对 Coriolis科里奥利效应做了建模而这一项恰恰是惯性导航机械编排里最容易被忽略却又非常重要的修正。这篇文章从解压、跑 demo、再到拆解核心原理和调参经验完整写出来适合正在学组合导航、想快速理解 GNSS/INS 融合逻辑的开发者。1. NaveGo到底是干什么的GNSS/INS组合导航仿真的最佳入门工具1.1 GNSS和INS这对互补搭档GNSS 系统通过接收卫星信号获得接收机天线所在位置的绝对坐标和速度优点是没有累计误差长时间运行精度稳定信号好的时候位置精度可以达到米级甚至厘米级。但它有个天然短板更新率不高常见民品模组只有 1Hz 到 20Hz而且信号一旦被高楼、隧道、树冠遮挡定位立刻变差甚至丢失。实际项目里常见的 GNSS 天线和模组输出的 NMEA 数据格式比如 GGA、RMC 语句本质上都是接收机解算后的位置速度结果。INS 惯性导航系统则完全不同。它依靠三轴加速度计测量比力三轴陀螺仪测量角速度通过积分方式递推位置、速度和姿态。IMU 的输出频率可以到 100Hz 甚至更高完全自主不怕遮挡和干扰短时间内的相对精度非常高。但缺点是误差会随积分时间不断累积低精度 MEMS 器件几十秒就能飘出明显偏差。这两个系统天然互补所以工程上的主流方案是把它们组合起来GNSS 定期提供绝对位置速度来修正 INS 的累计误差INS 在 GNSS 信号中断或高动态场景下继续输出导航结果。NaveGo 这类仿真工具就是帮助我们把这个组合过程完整地跑通、看透。1.2 为什么需要仿真而不是直接买设备很多刚接触组合导航的人第一反应是买硬件一块工业级 IMU、一台 RTK GNSS 接收机、一根天线、一块采集板卡再算上供电和支架一套下来少则几千多则数万。硬件采购之后还有标定、时间同步、杆臂测量、坐标系对齐一堆麻烦事。如果目标只是学习松耦合滤波算法直接上硬件会浪费大量时间在排查接线和时钟问题上而不是算法本身。仿真工具的价值在于可以人为控制每一项误差源。你可以给加速度计注入 5mg 零偏也可以给陀螺加上 0.05°/√hr 的角度随机游走再精确重复同一组实验。这种“控制变量”的能力在算法开发阶段非常关键它能帮你快速搞清每个误差源对最终导航精度的影响这是真实硬件很难做到的。用 NaveGo 做仿真本质上就像搭了一座虚拟实验室轨迹可以设计传感器误差可以设定融合算法可以替换输出曲线可以对比。等仿真把算法边界摸透了再上硬件做验证效率会高很多。1.3 NaveGo的定位和优势NaveGo 的官方源码在 GitHub 上有仓库主要开发者是 Rodrigo González。整个项目体积不大核心功能就集中在ins、gnss、filters、demos这几个目录里。它不依赖 MATLAB 专用工具箱在 GNU Octave 下同样能跑这一点对预算有限的个人开发者非常友好。它提供的模块包括 IMU 传感器仿真、GNSS 接收机仿真、INS 机械编排、误差状态卡尔曼滤波、平滑滤波、以及误差评估与绘图工具。默认演示的是松耦合架构即 GNSS 把解算出的位置和速度作为量测与 INS 的预测状态做融合。代码模块之间耦合度低想研究紧耦合或深耦合的同学也可以在此基础上改造。我手头这个NaveGo-master.zip文件名里的circusjwz像是某个 fork 用户的名字内容主体和官方仓库一致。一般从非官方渠道下载的包需要先核对一些关键脚本的完整性建议后面使用时尽量拉取官方最新版避免带进别人的测试改动。2. 第一次运行从解压zip到画出第一条轨迹2.1 环境准备和目录结构运行 NaveGo 只需要预装 MATLAB 或者 GNU Octave我实测 Octave 9.x 也能跑通大部分 demo没有遇到复杂的工具箱依赖。解压后第一件事是浏览一下 README 和目录结构不要急着直接运行脚本。标准仓库里nav/目录下会有几个关键子目录通常包括core/ins/IMU 模型、INS 机械编排相关函数core/gnss/GNSS 模型、卫星几何与位置相关函数filters/卡尔曼滤波、扩展卡尔曼滤波、平滑器等demos/官方演示脚本入口在这里plotting/误差曲线和轨迹绘制工具tools/坐标转换、四元数运算等辅助工具我的习惯是先把整个nav目录加入 MATLAB/Octave 搜索路径命令很简单addpath(genpath(/your/path/nav))。这一部经常被忽略如果直接切换到某个子目录运行脚本脚本内部调用其他目录函数时就会报错“未定义函数”。首次运行演示脚本时建议在干净的工作区进行先执行一遍clear; close all; clc;。这样能避免上一次实验遗留变量干扰绘图和滤波初始化。2.2 推荐的demo入口NaveGo 的官方演示入口一般是nav_demo.m路径在nav/demos/下。运行它后脚本会先按照某种预设航迹生成一条参考轨迹比如包含加速直线段、转弯段和静止段的组合路径然后根据你设定的 IMU 和 GNSS 噪声参数生成对应的传感器测量数据最后调用机械编排和滤波器逐帧递推并把真实值与估计值对比。我拿到一个新版本代码时会先直接在nav根目录下运行nav_demo。如果报错提示找不到某个 m 文件先检查是否为路径问题如果出现在 Octave 的绘图函数上看看是否缺少octave-forge里的signal或statistics包安装对应扩展包即可。2.3 跑通后能看到的关键输出demo 跑完后屏幕上会打印每一帧或每几帧的位置、速度、姿态信息同时弹出图形窗口。图形通常包含四类内容参考轨迹与估计轨迹的俯视对比图能直观看到滤波结果是否跟住了真值位置误差随时间变化曲线通常是以米为单位的南北、东西、垂直误差速度误差曲线分为东向、北向、天向姿态误差曲线横滚、俯仰、航向角误差最值得看的是“纯 INS 解算”和“GNSS/INS 组合解算”的对比。NaveGo 会先执行一次不加 GNSS 修正的纯惯性递推你可以看到位置误差迅速发散紧接着 GNSS 量测加入后误差被拉回到稳定范围。这两条曲线的对比比任何教材里的长篇大论都直观。我在第一次跑通时最深的感受是误差发散的速度比想象中快而 GNSS 修正的效果也比想象中强。理解到这一点后续再学滤波参数的调整就有了抓手。3. 核心代码拆解INS机械编排与Coriolis项的来龙去脉3.1 ins_mechanization到底在算什么INS 机械编排mechanization是惯性导航的核心递推过程它把 IMU 输出的比力和角速度逐步转换成位置、速度和姿态。在 NaveGo 代码里对应模块的核心文件我印象里是ins_mechanization.m每次处理一步 IMU 测量。机械编排一般包含三个紧密相关的更新步骤姿态更新利用陀螺仪输出的角增量同时考虑地球自转角速度以及载体在地球表面运动导致的导航坐标系旋转更新姿态四元数或姿态矩阵。比力分解与速度更新把加速度计测量到的比力投影到导航坐标系减去科里奥利加速度和重力加速度然后积分得到速度变化。位置更新根据当前速度在参考椭球上计算纬度、经度和高度的变化率再积分得到新的位置。代码里的关键变量通常包括纬度lat、经度lon、高度h、北向速度vn、东向速度ve、天向速度vd以及姿态四元数q。每一步递推都依赖一个常数地球自转角速度Omega 7.292115e-5 rad/s以及随纬度和高度变化的子午圈曲率半径R_N和卯酉圈曲率半径R_E。简化的机械编排速度递推可以写成vn_dot(1) 比力_北向 omega相关项_北向 重力项_北向 vn_dot(2) 比力_东向 omega相关项_东向 重力项_东向 vn_dot(3) 比力_天向 omega相关项_天向 重力项_天向其中omega相关项就是标题里coriolis对应的地方。3.2 Coriolis项在导航方程里的物理意义地球本身在惯性空间中以角速度Omega自转而导航坐标系又随着载体在地球表面的移动而不断改变指向。我们是在地球表面这个旋转且非惯性坐标系里做导航解算因此不能直接忽略地球自转带来的虚拟加速度。在地理坐标系机械编排中需要补充的修正项一般由两部分组成一是地球自转引起的科里奥利项2 * Omega_ie_n × v_n二是载体在地球表面运动引起的运输率项Omega_en_n × v_n。两者合在一起常会被统称为 Coriolis 项。在 NaveGo 代码里这部分计算逻辑大致会是这样omega_en_n [ v_e / (R_E h); - v_n / (R_N h); - v_e * tan(lat) / (R_E h) ]; omega_ie_n [0; Omega * cos(lat); Omega * sin(lat)];然后用这两个角速度矢量去修正姿态更新并在速度更新时计算(2 * omega_ie_n omega_en_n) × v_n这一项。这里的×是叉乘。从物理上看这一修正在东向速度较大、纬度较高时会非常明显。我常用一个类比解释给同学听在旋转木马上沿径向走一步你会感觉自己被“甩”向一侧这个“甩”的感觉本质上就是参考系旋转带来的科里奥利效应。地球自转虽然转速很小但对高速度、长航时的载体效应会累积成不可忽略的导航误差。3.3 手动去掉Coriolis项直观对比实验NaveGo 这种开源代码最慷慨的地方在于你可以动手改然后立刻看到结果。我的建议是做一个很简单的对比实验在机械编排函数里把涉及omega_en_n和omega_ie_n的修正项临时注释掉也就是让程序完全忽略 Coriolis 效应然后重新运行同一个 demo。我实际做这个实验时结果非常明显。对于 demo 中那条包含直线高速段和转弯段的轨迹不考虑 Coriolis 的情况下位置误差从量级几米迅速增长到几十米级速度误差也有明显抬升。如果仿真时间再延长到 1 小时以上误差会彻底失控。这里可以做一个粗略量级估算假设载体以 300m/s 速度巡航纬度约 45°科里奥利修正项的水平分量约为2 * Omega * v * sin(lat) ≈ 0.03 m/s²。这个加速度看起来很小但持续 10 分钟就会在速度上累积出接近 20m/s 的误差位置误差甚至达到公里量级。所以凡是涉及高速运动、长时间飞行或航海的组合导航Coriolis 项绝对不能省。值得强调的是这个实验不是让你在实际代码里删掉修正确项而是帮助你建立直觉机械编排里的每一项修正都有它存在的理由看到误差曲线的那一刻你就不会再忽略这些“小而关键”的项了。4. 传感器误差与GNSS数据格式从真实模组的NMEA到仿真输入4.1 真实GNSS模组输出的NMEA与NaveGo仿真之间的差别真实的 GNSS 设备比如常见的 NEO-M8N、F9P 等模组上电后会通过串口输出 NMEA 0183 协议数据。最常用的语句包括$GPGGA包含经纬度、定位质量、卫星数和海拔$GPRMC包含经纬度、地速、航向和 UTC 时间$GPGSA和$GPGSV则是卫星几何与可见星信息。如果做高精度定位还可以输出 RTCM 数据或者 UBX 二进制数据但这些属于源数据需要配合对应的解算软件才能得到坐标结果。NaveGo 里的 GNSS 模型不是去解析这些 NMEA 语句而是直接“跳过接收机解算”在已知真实轨迹上叠加高斯白噪声生成位置和速度观测值。它的侧重点在组合导航算法而不在 GNSS 接收机内部处理链路。因此模型里的gnss结构体一般就是时间戳、纬度、经度、高度、北向速度、东向速度、天向速度这样的字段。理解这个区别很重要如果你用真实模组采样数据回来第一件事往往是写一个 NMEA 解析器把$GPGGA和$GPRMC转成位置速度。而用 NaveGo 做仿真时你相当于已经拿到了“解析后”的结果唯一要做的是把噪声方差改成符合实际接收机水平的数值。4.2 IMU误差模型怎么设置才贴近实际NaveGo 的 IMU 模型对误差源的刻画比较完整主要包含陀螺角度随机游走ARW单位一般是deg/sqrt(hr)陀螺零偏及其稳定性单位deg/hr加速度计速度随机游走VRW单位ug/sqrt(Hz)加速度计零偏单位mg刻度因子误差和安装误差下面是一组常见器件的参数参考我在仿真时经常用它们做初始设定器件等级陀螺零偏 (deg/hr)陀螺ARW (deg/sqrt(hr))加计零偏 (mg)加计VRW (ug/sqrt(Hz))低精度 MEMS30~1000.5~25~2050~200中精度 MEMS1~100.1~0.51~520~100战术级光纤0.1~10.01~0.10.1~15~30需要注意的是零偏并不只是启动时的固定常量它还会随温度漂移并且在每次开机时都不一样。NaveGo 里通常把固定零偏和随机游走分开建模这种处理方式也比较贴近工程实际。拿到厂家手册时需要把deg/h、ug/√Hz这些指标换算到代码里使用的单位我初期经常因为忘记换算弧度与角度导致仿真结果偏差很大。4.3 把真实采集数据接入NaveGo的完整思路如果你已经有一套真实的 IMU 和 GNSS 数据想输入 NaveGo 进行验证流程上可以按下面思路处理。第一步先把 IMU 数据整理成统一格式时间戳、三轴角速度rad/s、三轴加速度m/s²。很多采集板的原始输出是角增量或脉冲计数需要换算并去偏。第二步把 GNSS 数据整理成位置速度序列可以通过自己写 NMEA 解析也可以先用 RTKLIB 等工具做差分定位再导出经纬高和速度。仿真环境下可以把 GNSS 观测周期设置成 1s 或 0.5s真实系统也要对齐成固定间隔。第三步时间同步是最容易被低估的环节。GNSS 时间来自卫星接收机IMU 时间来自采集卡时钟两者启动时刻和时钟源都可能不一致。必须把两个时间轴统一到同一个 UTC 秒或者 GPS 时再做最近邻或线性插值生成与 GNSS 时间戳对齐的 IMU 测量序列。第四步注意杆臂效应。GNSS 天线相位中心和 IMU 测量中心不可能物理重合这个空间差在载体旋转时会被放大。NaveGo 仿真环境里通常可以默认杆臂为零但真实接入时必须设置一个常值或进行杆臂标定否则速度观测里会混入耦合的姿态误差。我自己的建议是没有十足的把握先把真实数据接入时不要直接拿整套系统做闭环实验先在 NaveGo 仿真里用同一组参数跑通流程确认滤波器能收敛再逐步替代为真实数据。5. 调参与排查让仿真结果更可信的实战经验5.1 调参顺序从默认参数到针对性实验跑通 NaveGo 的 demo 后很多人会急着把参数改成自己的场景结果一改就发散。我通常按固定的顺序来调整先保持 demo 原始参数确认整个链路没有错误只改运动轨迹观察滤波是否稳定最后再改传感器误差参数每次只改一个量。这样做的逻辑是滤波发散的原因可能是协方差初值、过程噪声或量测噪声的比值不对。一次性改多个参数出了问题根本定位不到源头。先确认“算法与轨迹匹配”再确认“噪声模型与真实器件匹配”是最快的路径。举个实际例子如果你把 GNSS 噪声设置得过小而 IMU 噪声设置得很大融合结果会过分信任 GNSS 量测可能出现位置曲线抖动明显、速度估计出现跳变。反过来如果 GNSS 噪声太大滤波器又会对观测值不敏感误差会接近纯 INS 发散水平。松耦合滤波器的调参本质就是在两个传感器的置信度之间找平衡。5.2 时间同步和坐标系转换的坑我在实际使用中碰到过两类高频问题几乎每个用 NaveGo 的朋友都会遇到。第一类是时间基准混乱。IMU 数据可能是 100HzGNSS 数据是 5HzNaveGo 内部的递推逻辑要求以 IMU 采样周期为步长在 GNSS 观测时刻进行量测更新。如果时间戳不是从 0 开始的统一秒表而是来自两个设备的本地时间滤波初值就会对不上误差曲线会出现周期性“台阶”。第二类是坐标系定义绕不清。NaveGo 默认使用当地地理坐标系做机械编排位置用经纬高表示。如果你从外部引入 ECEF地心地固系下的轨迹就需要先通过转换函数把 ECEF 转为经纬高再进入滤波器。别想着自己在外面转一半最后只改了 IF 语句就期望结果正确坐标系之间的旋转矩阵与四元数顺序一旦搞错误差可能直接到几十公里级。另外单位问题也要注意。演示代码里经纬度通常是弧度如果你从 NMEA 里读取的是十进制度数直接塞进结构体滤波器会瞬间“爆炸”。我的习惯是在接入数据的地方统一写一个deg2rad转换函数并在注释里标明输入单位。5.3 验证结果合理性的几个朴素办法仿真跑完你需要对结果“是否合理”有一个判断而不是只看曲线是否在图上。我推荐下面几个验证方法。第一个办法是纯 INS 与组合结果的对比。如果加入 GNSS 后误差没有显著优于纯 INS那说明滤波器可能没有真正工作检查一下量测更新是否进入如果组合结果比纯 INS 还差多半是滤波发散或量测方程写错了。第二个办法是看误差曲线是否收敛到合理水平。松耦合架构下位置误差的稳态 RMS 通常接近 GNSS 解的精度量级。单点定位大概几米差分定位则在分米级。如果位置误差远远大于 GNSS 量测噪声优先检查卡尔曼滤波的增益是否被压低。第三个办法是进行重复性实验。同一组参数、加入不同随机种子多次运行后的误差统计应基本一致。如果某一次仿真结果特别好或特别差很可能是初始化随机变量影响的偶然现象需要统计多次求 RMS。我记得自己刚上手时有段时间一直觉得组合结果“挺好看”后来发现是因为没有画纯 INS 对比曲线问题藏得很深。所以无论仿真结果看起来多顺都要保留一条“没有外部修正”的对照组这是判断组合算法真实作用的底线。结合一年多使用 NaveGo 做组合导航预研的经验我的体会是这类仿真工具最大的价值不是给你一个看起来完美的导航结果而是把整个 GNSS/INS 融合链路完全摊开让你能看到每项误差如何被建模、每一步修正如何被引入。尤其是 Coriolis 相关的那几行代码看起来不起眼实际却承载着地球自转对导航解算的全部影响。更细一环地说我习惯先用高噪声参数确认滤波器的鲁棒性再逐步降低噪声看精度上限这样的做法比直接上最佳参数更能够暴露算法短板。如果你正在为 GNSS/INS 仿真里的机械编排或者滤波器配置发愁建议先从 NaveGo 的 demo 入手跑通之后再做几个“关掉某个功能”的对比实验比如注释掉 Coriolis 修正、调大 GNSS 噪声、把 IMU 零偏翻倍误差曲线会告诉你答案。等你把这些问题都弄清楚再回归真实硬件会从容很多。本文还有配套的精品资源点击获取
简介GNSS与INS组合导航是无人系统与高精度定位领域的核心方案GNSS提供长时间稳定的绝对位置INS以高频输出维持短时高精度二者通过卡尔曼滤波实现优势互补。工程实践中硬件成本与误差可控性成为学习门槛而软件仿真则能有效拆解算法链路。从传感器模型到机械编排从误差传播到滤波调参仿真工具可让开发者直观观察位置、速度、姿态的估计过程。其中科里奥利效应作为地球自转与载体运动产生的修正项对高速长航时场景影响显著常被初学者忽略。NaveGo作为轻量级开源仿真工具在Matlab/Octave环境下完整实现了GNSS/INS松耦合融合、IMU误差建模及Coriolis补偿是理解组合导航原理、验证滤波参数和进行误差分析的理想平台也为后续真实硬件接入提供了坚实铺垫。 最近整理资料时翻出一个早先下载的NaveGo-master.zip文件名还带着GNSS MASTER和coriolis的标记一看就是当年从某个下载站或者 fork 页面打包回来的。解压后顺手重跑了一遍发现这套 Matlab/Octave 环境下的 GNSS/INS 组合导航仿真工具到现在依然是入门惯导组合最顺手的开源项目之一。NaveGo 做的事可以简单概括为在纯软件环境里模拟 GNSS 接收机和 IMU 惯性测量单元的输出再用卡尔曼滤波把两者融合起来输出位置、速度和姿态的估计结果。它的代码里专门对 Coriolis科里奥利效应做了建模而这一项恰恰是惯性导航机械编排里最容易被忽略却又非常重要的修正。这篇文章从解压、跑 demo、再到拆解核心原理和调参经验完整写出来适合正在学组合导航、想快速理解 GNSS/INS 融合逻辑的开发者。1. NaveGo到底是干什么的GNSS/INS组合导航仿真的最佳入门工具1.1 GNSS和INS这对互补搭档GNSS 系统通过接收卫星信号获得接收机天线所在位置的绝对坐标和速度优点是没有累计误差长时间运行精度稳定信号好的时候位置精度可以达到米级甚至厘米级。但它有个天然短板更新率不高常见民品模组只有 1Hz 到 20Hz而且信号一旦被高楼、隧道、树冠遮挡定位立刻变差甚至丢失。实际项目里常见的 GNSS 天线和模组输出的 NMEA 数据格式比如 GGA、RMC 语句本质上都是接收机解算后的位置速度结果。INS 惯性导航系统则完全不同。它依靠三轴加速度计测量比力三轴陀螺仪测量角速度通过积分方式递推位置、速度和姿态。IMU 的输出频率可以到 100Hz 甚至更高完全自主不怕遮挡和干扰短时间内的相对精度非常高。但缺点是误差会随积分时间不断累积低精度 MEMS 器件几十秒就能飘出明显偏差。这两个系统天然互补所以工程上的主流方案是把它们组合起来GNSS 定期提供绝对位置速度来修正 INS 的累计误差INS 在 GNSS 信号中断或高动态场景下继续输出导航结果。NaveGo 这类仿真工具就是帮助我们把这个组合过程完整地跑通、看透。1.2 为什么需要仿真而不是直接买设备很多刚接触组合导航的人第一反应是买硬件一块工业级 IMU、一台 RTK GNSS 接收机、一根天线、一块采集板卡再算上供电和支架一套下来少则几千多则数万。硬件采购之后还有标定、时间同步、杆臂测量、坐标系对齐一堆麻烦事。如果目标只是学习松耦合滤波算法直接上硬件会浪费大量时间在排查接线和时钟问题上而不是算法本身。仿真工具的价值在于可以人为控制每一项误差源。你可以给加速度计注入 5mg 零偏也可以给陀螺加上 0.05°/√hr 的角度随机游走再精确重复同一组实验。这种“控制变量”的能力在算法开发阶段非常关键它能帮你快速搞清每个误差源对最终导航精度的影响这是真实硬件很难做到的。用 NaveGo 做仿真本质上就像搭了一座虚拟实验室轨迹可以设计传感器误差可以设定融合算法可以替换输出曲线可以对比。等仿真把算法边界摸透了再上硬件做验证效率会高很多。1.3 NaveGo的定位和优势NaveGo 的官方源码在 GitHub 上有仓库主要开发者是 Rodrigo González。整个项目体积不大核心功能就集中在ins、gnss、filters、demos这几个目录里。它不依赖 MATLAB 专用工具箱在 GNU Octave 下同样能跑这一点对预算有限的个人开发者非常友好。它提供的模块包括 IMU 传感器仿真、GNSS 接收机仿真、INS 机械编排、误差状态卡尔曼滤波、平滑滤波、以及误差评估与绘图工具。默认演示的是松耦合架构即 GNSS 把解算出的位置和速度作为量测与 INS 的预测状态做融合。代码模块之间耦合度低想研究紧耦合或深耦合的同学也可以在此基础上改造。我手头这个NaveGo-master.zip文件名里的circusjwz像是某个 fork 用户的名字内容主体和官方仓库一致。一般从非官方渠道下载的包需要先核对一些关键脚本的完整性建议后面使用时尽量拉取官方最新版避免带进别人的测试改动。2. 第一次运行从解压zip到画出第一条轨迹2.1 环境准备和目录结构运行 NaveGo 只需要预装 MATLAB 或者 GNU Octave我实测 Octave 9.x 也能跑通大部分 demo没有遇到复杂的工具箱依赖。解压后第一件事是浏览一下 README 和目录结构不要急着直接运行脚本。标准仓库里nav/目录下会有几个关键子目录通常包括core/ins/IMU 模型、INS 机械编排相关函数core/gnss/GNSS 模型、卫星几何与位置相关函数filters/卡尔曼滤波、扩展卡尔曼滤波、平滑器等demos/官方演示脚本入口在这里plotting/误差曲线和轨迹绘制工具tools/坐标转换、四元数运算等辅助工具我的习惯是先把整个nav目录加入 MATLAB/Octave 搜索路径命令很简单addpath(genpath(/your/path/nav))。这一部经常被忽略如果直接切换到某个子目录运行脚本脚本内部调用其他目录函数时就会报错“未定义函数”。首次运行演示脚本时建议在干净的工作区进行先执行一遍clear; close all; clc;。这样能避免上一次实验遗留变量干扰绘图和滤波初始化。2.2 推荐的demo入口NaveGo 的官方演示入口一般是nav_demo.m路径在nav/demos/下。运行它后脚本会先按照某种预设航迹生成一条参考轨迹比如包含加速直线段、转弯段和静止段的组合路径然后根据你设定的 IMU 和 GNSS 噪声参数生成对应的传感器测量数据最后调用机械编排和滤波器逐帧递推并把真实值与估计值对比。我拿到一个新版本代码时会先直接在nav根目录下运行nav_demo。如果报错提示找不到某个 m 文件先检查是否为路径问题如果出现在 Octave 的绘图函数上看看是否缺少octave-forge里的signal或statistics包安装对应扩展包即可。2.3 跑通后能看到的关键输出demo 跑完后屏幕上会打印每一帧或每几帧的位置、速度、姿态信息同时弹出图形窗口。图形通常包含四类内容参考轨迹与估计轨迹的俯视对比图能直观看到滤波结果是否跟住了真值位置误差随时间变化曲线通常是以米为单位的南北、东西、垂直误差速度误差曲线分为东向、北向、天向姿态误差曲线横滚、俯仰、航向角误差最值得看的是“纯 INS 解算”和“GNSS/INS 组合解算”的对比。NaveGo 会先执行一次不加 GNSS 修正的纯惯性递推你可以看到位置误差迅速发散紧接着 GNSS 量测加入后误差被拉回到稳定范围。这两条曲线的对比比任何教材里的长篇大论都直观。我在第一次跑通时最深的感受是误差发散的速度比想象中快而 GNSS 修正的效果也比想象中强。理解到这一点后续再学滤波参数的调整就有了抓手。3. 核心代码拆解INS机械编排与Coriolis项的来龙去脉3.1 ins_mechanization到底在算什么INS 机械编排mechanization是惯性导航的核心递推过程它把 IMU 输出的比力和角速度逐步转换成位置、速度和姿态。在 NaveGo 代码里对应模块的核心文件我印象里是ins_mechanization.m每次处理一步 IMU 测量。机械编排一般包含三个紧密相关的更新步骤姿态更新利用陀螺仪输出的角增量同时考虑地球自转角速度以及载体在地球表面运动导致的导航坐标系旋转更新姿态四元数或姿态矩阵。比力分解与速度更新把加速度计测量到的比力投影到导航坐标系减去科里奥利加速度和重力加速度然后积分得到速度变化。位置更新根据当前速度在参考椭球上计算纬度、经度和高度的变化率再积分得到新的位置。代码里的关键变量通常包括纬度lat、经度lon、高度h、北向速度vn、东向速度ve、天向速度vd以及姿态四元数q。每一步递推都依赖一个常数地球自转角速度Omega 7.292115e-5 rad/s以及随纬度和高度变化的子午圈曲率半径R_N和卯酉圈曲率半径R_E。简化的机械编排速度递推可以写成vn_dot(1) 比力_北向 omega相关项_北向 重力项_北向 vn_dot(2) 比力_东向 omega相关项_东向 重力项_东向 vn_dot(3) 比力_天向 omega相关项_天向 重力项_天向其中omega相关项就是标题里coriolis对应的地方。3.2 Coriolis项在导航方程里的物理意义地球本身在惯性空间中以角速度Omega自转而导航坐标系又随着载体在地球表面的移动而不断改变指向。我们是在地球表面这个旋转且非惯性坐标系里做导航解算因此不能直接忽略地球自转带来的虚拟加速度。在地理坐标系机械编排中需要补充的修正项一般由两部分组成一是地球自转引起的科里奥利项2 * Omega_ie_n × v_n二是载体在地球表面运动引起的运输率项Omega_en_n × v_n。两者合在一起常会被统称为 Coriolis 项。在 NaveGo 代码里这部分计算逻辑大致会是这样omega_en_n [ v_e / (R_E h); - v_n / (R_N h); - v_e * tan(lat) / (R_E h) ]; omega_ie_n [0; Omega * cos(lat); Omega * sin(lat)];然后用这两个角速度矢量去修正姿态更新并在速度更新时计算(2 * omega_ie_n omega_en_n) × v_n这一项。这里的×是叉乘。从物理上看这一修正在东向速度较大、纬度较高时会非常明显。我常用一个类比解释给同学听在旋转木马上沿径向走一步你会感觉自己被“甩”向一侧这个“甩”的感觉本质上就是参考系旋转带来的科里奥利效应。地球自转虽然转速很小但对高速度、长航时的载体效应会累积成不可忽略的导航误差。3.3 手动去掉Coriolis项直观对比实验NaveGo 这种开源代码最慷慨的地方在于你可以动手改然后立刻看到结果。我的建议是做一个很简单的对比实验在机械编排函数里把涉及omega_en_n和omega_ie_n的修正项临时注释掉也就是让程序完全忽略 Coriolis 效应然后重新运行同一个 demo。我实际做这个实验时结果非常明显。对于 demo 中那条包含直线高速段和转弯段的轨迹不考虑 Coriolis 的情况下位置误差从量级几米迅速增长到几十米级速度误差也有明显抬升。如果仿真时间再延长到 1 小时以上误差会彻底失控。这里可以做一个粗略量级估算假设载体以 300m/s 速度巡航纬度约 45°科里奥利修正项的水平分量约为2 * Omega * v * sin(lat) ≈ 0.03 m/s²。这个加速度看起来很小但持续 10 分钟就会在速度上累积出接近 20m/s 的误差位置误差甚至达到公里量级。所以凡是涉及高速运动、长时间飞行或航海的组合导航Coriolis 项绝对不能省。值得强调的是这个实验不是让你在实际代码里删掉修正确项而是帮助你建立直觉机械编排里的每一项修正都有它存在的理由看到误差曲线的那一刻你就不会再忽略这些“小而关键”的项了。4. 传感器误差与GNSS数据格式从真实模组的NMEA到仿真输入4.1 真实GNSS模组输出的NMEA与NaveGo仿真之间的差别真实的 GNSS 设备比如常见的 NEO-M8N、F9P 等模组上电后会通过串口输出 NMEA 0183 协议数据。最常用的语句包括$GPGGA包含经纬度、定位质量、卫星数和海拔$GPRMC包含经纬度、地速、航向和 UTC 时间$GPGSA和$GPGSV则是卫星几何与可见星信息。如果做高精度定位还可以输出 RTCM 数据或者 UBX 二进制数据但这些属于源数据需要配合对应的解算软件才能得到坐标结果。NaveGo 里的 GNSS 模型不是去解析这些 NMEA 语句而是直接“跳过接收机解算”在已知真实轨迹上叠加高斯白噪声生成位置和速度观测值。它的侧重点在组合导航算法而不在 GNSS 接收机内部处理链路。因此模型里的gnss结构体一般就是时间戳、纬度、经度、高度、北向速度、东向速度、天向速度这样的字段。理解这个区别很重要如果你用真实模组采样数据回来第一件事往往是写一个 NMEA 解析器把$GPGGA和$GPRMC转成位置速度。而用 NaveGo 做仿真时你相当于已经拿到了“解析后”的结果唯一要做的是把噪声方差改成符合实际接收机水平的数值。4.2 IMU误差模型怎么设置才贴近实际NaveGo 的 IMU 模型对误差源的刻画比较完整主要包含陀螺角度随机游走ARW单位一般是deg/sqrt(hr)陀螺零偏及其稳定性单位deg/hr加速度计速度随机游走VRW单位ug/sqrt(Hz)加速度计零偏单位mg刻度因子误差和安装误差下面是一组常见器件的参数参考我在仿真时经常用它们做初始设定器件等级陀螺零偏 (deg/hr)陀螺ARW (deg/sqrt(hr))加计零偏 (mg)加计VRW (ug/sqrt(Hz))低精度 MEMS30~1000.5~25~2050~200中精度 MEMS1~100.1~0.51~520~100战术级光纤0.1~10.01~0.10.1~15~30需要注意的是零偏并不只是启动时的固定常量它还会随温度漂移并且在每次开机时都不一样。NaveGo 里通常把固定零偏和随机游走分开建模这种处理方式也比较贴近工程实际。拿到厂家手册时需要把deg/h、ug/√Hz这些指标换算到代码里使用的单位我初期经常因为忘记换算弧度与角度导致仿真结果偏差很大。4.3 把真实采集数据接入NaveGo的完整思路如果你已经有一套真实的 IMU 和 GNSS 数据想输入 NaveGo 进行验证流程上可以按下面思路处理。第一步先把 IMU 数据整理成统一格式时间戳、三轴角速度rad/s、三轴加速度m/s²。很多采集板的原始输出是角增量或脉冲计数需要换算并去偏。第二步把 GNSS 数据整理成位置速度序列可以通过自己写 NMEA 解析也可以先用 RTKLIB 等工具做差分定位再导出经纬高和速度。仿真环境下可以把 GNSS 观测周期设置成 1s 或 0.5s真实系统也要对齐成固定间隔。第三步时间同步是最容易被低估的环节。GNSS 时间来自卫星接收机IMU 时间来自采集卡时钟两者启动时刻和时钟源都可能不一致。必须把两个时间轴统一到同一个 UTC 秒或者 GPS 时再做最近邻或线性插值生成与 GNSS 时间戳对齐的 IMU 测量序列。第四步注意杆臂效应。GNSS 天线相位中心和 IMU 测量中心不可能物理重合这个空间差在载体旋转时会被放大。NaveGo 仿真环境里通常可以默认杆臂为零但真实接入时必须设置一个常值或进行杆臂标定否则速度观测里会混入耦合的姿态误差。我自己的建议是没有十足的把握先把真实数据接入时不要直接拿整套系统做闭环实验先在 NaveGo 仿真里用同一组参数跑通流程确认滤波器能收敛再逐步替代为真实数据。5. 调参与排查让仿真结果更可信的实战经验5.1 调参顺序从默认参数到针对性实验跑通 NaveGo 的 demo 后很多人会急着把参数改成自己的场景结果一改就发散。我通常按固定的顺序来调整先保持 demo 原始参数确认整个链路没有错误只改运动轨迹观察滤波是否稳定最后再改传感器误差参数每次只改一个量。这样做的逻辑是滤波发散的原因可能是协方差初值、过程噪声或量测噪声的比值不对。一次性改多个参数出了问题根本定位不到源头。先确认“算法与轨迹匹配”再确认“噪声模型与真实器件匹配”是最快的路径。举个实际例子如果你把 GNSS 噪声设置得过小而 IMU 噪声设置得很大融合结果会过分信任 GNSS 量测可能出现位置曲线抖动明显、速度估计出现跳变。反过来如果 GNSS 噪声太大滤波器又会对观测值不敏感误差会接近纯 INS 发散水平。松耦合滤波器的调参本质就是在两个传感器的置信度之间找平衡。5.2 时间同步和坐标系转换的坑我在实际使用中碰到过两类高频问题几乎每个用 NaveGo 的朋友都会遇到。第一类是时间基准混乱。IMU 数据可能是 100HzGNSS 数据是 5HzNaveGo 内部的递推逻辑要求以 IMU 采样周期为步长在 GNSS 观测时刻进行量测更新。如果时间戳不是从 0 开始的统一秒表而是来自两个设备的本地时间滤波初值就会对不上误差曲线会出现周期性“台阶”。第二类是坐标系定义绕不清。NaveGo 默认使用当地地理坐标系做机械编排位置用经纬高表示。如果你从外部引入 ECEF地心地固系下的轨迹就需要先通过转换函数把 ECEF 转为经纬高再进入滤波器。别想着自己在外面转一半最后只改了 IF 语句就期望结果正确坐标系之间的旋转矩阵与四元数顺序一旦搞错误差可能直接到几十公里级。另外单位问题也要注意。演示代码里经纬度通常是弧度如果你从 NMEA 里读取的是十进制度数直接塞进结构体滤波器会瞬间“爆炸”。我的习惯是在接入数据的地方统一写一个deg2rad转换函数并在注释里标明输入单位。5.3 验证结果合理性的几个朴素办法仿真跑完你需要对结果“是否合理”有一个判断而不是只看曲线是否在图上。我推荐下面几个验证方法。第一个办法是纯 INS 与组合结果的对比。如果加入 GNSS 后误差没有显著优于纯 INS那说明滤波器可能没有真正工作检查一下量测更新是否进入如果组合结果比纯 INS 还差多半是滤波发散或量测方程写错了。第二个办法是看误差曲线是否收敛到合理水平。松耦合架构下位置误差的稳态 RMS 通常接近 GNSS 解的精度量级。单点定位大概几米差分定位则在分米级。如果位置误差远远大于 GNSS 量测噪声优先检查卡尔曼滤波的增益是否被压低。第三个办法是进行重复性实验。同一组参数、加入不同随机种子多次运行后的误差统计应基本一致。如果某一次仿真结果特别好或特别差很可能是初始化随机变量影响的偶然现象需要统计多次求 RMS。我记得自己刚上手时有段时间一直觉得组合结果“挺好看”后来发现是因为没有画纯 INS 对比曲线问题藏得很深。所以无论仿真结果看起来多顺都要保留一条“没有外部修正”的对照组这是判断组合算法真实作用的底线。结合一年多使用 NaveGo 做组合导航预研的经验我的体会是这类仿真工具最大的价值不是给你一个看起来完美的导航结果而是把整个 GNSS/INS 融合链路完全摊开让你能看到每项误差如何被建模、每一步修正如何被引入。尤其是 Coriolis 相关的那几行代码看起来不起眼实际却承载着地球自转对导航解算的全部影响。更细一环地说我习惯先用高噪声参数确认滤波器的鲁棒性再逐步降低噪声看精度上限这样的做法比直接上最佳参数更能够暴露算法短板。如果你正在为 GNSS/INS 仿真里的机械编排或者滤波器配置发愁建议先从 NaveGo 的 demo 入手跑通之后再做几个“关掉某个功能”的对比实验比如注释掉 Coriolis 修正、调大 GNSS 噪声、把 IMU 零偏翻倍误差曲线会告诉你答案。等你把这些问题都弄清楚再回归真实硬件会从容很多。本文还有配套的精品资源点击获取
