为什么IC-GVINS要补偿地球自转高精度IMU预积分的数学原理与实现深度剖析【免费下载链接】IC-GVINSA Robust, Real-time, INS-Centric GNSS-Visual-Inertial Navigation System项目地址: https://gitcode.com/gh_mirrors/ic/IC-GVINSIC-GVINS 是武汉大学 i2Nav 团队开源的以惯性导航INS为核心的 GNSS-视觉-惯性融合导航系统。它最大的技术亮点之一就是在 INS 的IMU 预积分中显式补偿了地球自转从而充分发挥高精度惯性测量单元IMU的精度潜力。本文带你从为什么讲到怎么做先看地球自转带来的系统误差有多大再剖析旋转坐标系下的科里奥利修正与二阶位置补偿的数学原理最后对照源码看懂 IC-GVINS 的四类预积分器是如何落地的。一、IC-GVINS把 INS 放在C 位的融合导航系统大多数视觉惯性系统VIO/VSLAM以视觉为主、IMU 为辅而 IC-GVINS 反其道而行之——惯性导航系统INS不受外部环境干扰视觉容易在光照变化、动态场景中失效而高精度 IMU 的短程积分精度极高。因此系统的设计思路是INS 为主干IMU 预积分作为骨架GNSS/RTK 提供 1 Hz 的位置基准进行校正紧耦合因子图IMU 预积分、GNSS、视觉重投影残差统一在 Ceres 因子图中优化状态定义在统一的世界坐标系WGS-84中GNSS 辅助初始化利用 GNSS/RTK 快速确定初始位姿再做因子图优化。在这种INS 中心化架构下IMU 预积分的精度直接决定整个系统的精度上限。对 MEMS 级 IMU 来说很多二阶效应可以忽略但对高精度 IMU地球自转这一隐藏玩家就必须被认真对待。二、为什么必须补偿地球自转2.1 地球自转有多快地球自转角速度约为7.2921151467×10⁻⁵ rad/s即每 1 小时旋转15°每秒约0.0042°。在 earth.h 中IC-GVINS 直接定义了 WGS-84 标准参数WGS84_WIE地球自转角速度 7.2921151467E-5 rad/sWGS84_RA/WGS84_RBWGS-84 长、短半轴WGS84_E1第一偏心率平方2.2 它比高精度 IMU 的噪声还大关键矛盾在这里。IC-GVINS 默认 IMU 噪声参数gvins.yaml参数典型值高精度 MEMS 档说明陀螺随机Walkarw0.1 °/√hr每 1 秒约 0.0017°陀螺零偏gbstd50 °/hr与地球自转 15°/hr 同量级地球自转15 °/hr固定、不可标定掉的系统项对比一下地球每 1 秒让本地导航坐标系n 系转过0.0042°而同级别陀螺每 1 秒的随机游走噪声只有约0.0017°——地球自转引起的坐标系旋转是陀螺噪声的 2.5 倍以上。对 MEMS 级 IMU零偏数百 °/hr来说15°/hr 被淹没在噪声里但对零偏远小于 15°/hr 的高精度 IMU这个旋转无法被零偏估计完全吸收它随载体姿态变化而投影变化是方向相关的系统量只能显式补偿否则会以伪零偏 姿态漂移的形式污染整个因子图。2.3 误差如何累积若忽略地球自转n 系相对惯性系的旋转被错误地归给了载体误差随时间系统性累积姿态误差近似 ∝ ω·t1 小时后达 15°1 秒约 0.0042°速度科里奥利项 2(ω_ie × v) 长期漏积v 10 m/s 时该项约1.5×10⁻³ m/s²是恒定偏置而非噪声无法靠平均消除位置随积分时间二次甚至三次增长GNSS 间隔越长1 Hz 间隔内 INS 自主积分累积越明显。这正是 README 中那句话的由来The Earth rotation has been compensated in the INS to improve the accuracy of high-grade IMUs.在 INS 中补偿地球自转以提升高精度 IMU 的精度。三、IMU 预积分的数学原理旋转坐标系下的修正项预积分的本质把两次姿态更新之间的 IMU 增量提前积分好优化时只需轻量校正。当地理坐标系n 系ENU 北东地本身在以 ω_ie 旋转时经典 VIO 预积分公式必须加入三类修正。以下记 ω ω_ie^nn 系下的地球自转角速度IC-GVINS 中即iewn_C 表示姿态旋转矩阵。3.1 姿态两个会动的坐标系之间的相对旋转经典预积分中n→b 的姿态更新只含 IMU 角增量 Δθ。但起点 n₀ 与终点 n₁ 是两个都随地球转动的坐标系二者之间还差一个相对旋转 q_nndnn -ω·Δt 旋转矢量 q_nn exp(dnn) 起止导航系间相对旋转 姿态更新: q q_nn · q · exp(Δθ)在 preintegration_earth.cc 中qnn既用于连续状态积分的姿态更新也用于速度项的平均旋转0.5·(I C(qnn))相当于把 b 系增量速度投影到区间中间的坐标系消除区间两端坐标系不一致带来的一阶误差。预积分残差里的相对姿态也随之修正为q_b0b1 q1⁻¹ · q_nn · q0evaluate 函数保证两端状态与预积分量在同一旋转基准下比较。3.2 速度科里奥利修正项旋转坐标系下的速度微分方程为ṽ C_nb·f g - (2ω_ie^n ω_en^n) × v2(ω_ie × v)科里奥利项必须补偿ω_en地球曲率运输项对低速短基线属二阶小量IC-GVINS 直接舍弃——源码注释写得很直白地球自转补偿项, 省去了enwn项integrationProcess。对应实现是(gravity_ - 2.0 * iewn_.cross(current_state_.v)) * dt。同时IMU 测得的比力增量 dvfb 要先乘以平均旋转再转入 n 系与 3.1 的qnn机制一致。3.3 位置与残差二阶补偿项地球自转补偿并非只在积分循环里做文章残差计算中还有两个容易被忽略的修正evaluate补偿项形式物理含义位置补偿 p_cor2·[ω]× · Σ (pₖ - p₀)·Δt沿轨迹各点地球自转场相对起点的差异二阶量用缓存的位置序列逐段累加速度补偿 v_cor2·[ω]× · (p₁ - p₀)终点与起点的自转场差修正速度残差的基准系旋转其中[ω]×是iewn_skew_斜对称矩阵。积分时每个 IMU 历元的位置都会存入pn_缓存preintegration_earth.cc 第236行评估残差时统一回放累加。位置残差的雅可比也随之带上-2·C_nb0·[ω]×·Δt的修正项residualJacobianPose0雅可比与协方差传播矩阵 φ 中同样引入cbb0 -(q0⁻¹·q_nn·q0·Δq)使预积分量的协方差也建立在旋转坐标系基准上updateJacobianAndCovariance。 一句话总结姿态加了 q_nn、速度加了科里奥利项、位置加了二阶场差补偿、雅可比与协方差同步修正——四件套缺一不可这就是高精度 IMU 预积分与普通 VIO 预积分的本质区别。四、源码实现四类预积分器如何切换 ️IC-GVINS 把预积分抽象为基类 PreintegrationBase派生出 4 个具体实现preintegration.h预积分器文件地球自转补偿里程计状态PreintegrationNormalpreintegration_normal.cc❌❌PreintegrationOdopreintegration_odo.cc❌✅PreintegrationEarthpreintegration_earth.cc✅❌PreintegrationEarthOdopreintegration_earth_odo.cc✅✅选择逻辑极其简洁iswithearth与isuseodo两个布尔位组合出 4 种选项getOptions。配置入口只有一个开关——config/gvins.yaml 中的iswithearth: true系统初始化时会用它计算iewn并在 GNSS/INS 初始化后重算ic_gvins.cc。几个实现细节值得注意iewn_的取法地球自转角速度投影到 n 系为{ω·cosφ, 0, -ω·sinφ}φ 为纬度见 Earth::iewn。预积分器重置状态时近似取预积分起点位置计算一次预积分段内保持不变resetState——因为单个积分段内位置变化很小这一近似完全足够状态维度不变PreintegrationEarth的状态仍是 15 维位置 3 速度 3 姿态 3 陀螺零偏 3 加速度计零偏 3与Normal版一致差别全部体现在积分递推与残差/雅可比的修正项中工程侵入性很低⚙️里程计版本同构PreintegrationEarthOdo在PreintegrationEarth基础上增加里程增量状态s及其零偏sodo补偿项完全同构方便机器人等带轮速/里程计的平台复用同一套地球自转补偿框架。五、实践要点开启与验证 配置在 config/gvins.yaml 中确认iswithearth: trueIMU 数据须为前-右-下FRD坐标系采样率如 200 Hz须与imudatarate一致IMU 噪声参数要真实arw/vrw/gbstd/abstd应来自 IMU 出厂标定值。噪声参数虚高会放大预积分协方差让 GNSS 与视觉约束抢话语权补偿地球自转的收益也就体现不出来验证方法同一 bag 分别设置iswithearth: true/false跑两遍对比长时≥30 min轨迹的横向位置漂移与姿态稳定性。典型现象是关闭补偿后漂移呈缓慢单向趋势系统偏置特征而非随机抖动适用判断IMU 零偏 10°/hr 级别战术级/光纤级强烈建议开启消费级 MEMS零偏数百 °/hr开关影响有限但开启无害。六、写在最后地球自转补偿是不起眼但决定上限的细节 15°/hr 的自转比高精度 IMU 的噪声还大而 IC-GVINS 用一套干净利落的四件套——姿态相对旋转 q_nn、科里奥利项、二阶位置场差补偿、同步修正的雅可比与协方差——把这一系统误差从 INS 骨架中剔除让 GNSS/RTK 的 1 Hz 基准真正校得准。想进一步阅读建议按此顺序 预积分基类与状态定义preintegration_base.h 地球自转补偿核心实现preintegration_earth.cc WGS-84 大地测量工具earth.h 主流程与初始化ic_gvins.cc 配套论文《Impact of the Earth Rotation Compensation on MEMS-IMU Preintegration of Factor Graph Optimization》IEEE Sensors Journal, 2022掌握这套机制后你再去审视任何INS 中心化的融合导航系统都不会再被这个旋转的地球坑到。【免费下载链接】IC-GVINSA Robust, Real-time, INS-Centric GNSS-Visual-Inertial Navigation System项目地址: https://gitcode.com/gh_mirrors/ic/IC-GVINS创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考 为什么IC-GVINS要补偿地球自转？高精度IMU预积分的数学原理与实现深度剖析
为什么IC-GVINS要补偿地球自转高精度IMU预积分的数学原理与实现深度剖析【免费下载链接】IC-GVINSA Robust, Real-time, INS-Centric GNSS-Visual-Inertial Navigation System项目地址: https://gitcode.com/gh_mirrors/ic/IC-GVINSIC-GVINS 是武汉大学 i2Nav 团队开源的以惯性导航INS为核心的 GNSS-视觉-惯性融合导航系统。它最大的技术亮点之一就是在 INS 的IMU 预积分中显式补偿了地球自转从而充分发挥高精度惯性测量单元IMU的精度潜力。本文带你从为什么讲到怎么做先看地球自转带来的系统误差有多大再剖析旋转坐标系下的科里奥利修正与二阶位置补偿的数学原理最后对照源码看懂 IC-GVINS 的四类预积分器是如何落地的。一、IC-GVINS把 INS 放在C 位的融合导航系统大多数视觉惯性系统VIO/VSLAM以视觉为主、IMU 为辅而 IC-GVINS 反其道而行之——惯性导航系统INS不受外部环境干扰视觉容易在光照变化、动态场景中失效而高精度 IMU 的短程积分精度极高。因此系统的设计思路是INS 为主干IMU 预积分作为骨架GNSS/RTK 提供 1 Hz 的位置基准进行校正紧耦合因子图IMU 预积分、GNSS、视觉重投影残差统一在 Ceres 因子图中优化状态定义在统一的世界坐标系WGS-84中GNSS 辅助初始化利用 GNSS/RTK 快速确定初始位姿再做因子图优化。在这种INS 中心化架构下IMU 预积分的精度直接决定整个系统的精度上限。对 MEMS 级 IMU 来说很多二阶效应可以忽略但对高精度 IMU地球自转这一隐藏玩家就必须被认真对待。二、为什么必须补偿地球自转2.1 地球自转有多快地球自转角速度约为7.2921151467×10⁻⁵ rad/s即每 1 小时旋转15°每秒约0.0042°。在 earth.h 中IC-GVINS 直接定义了 WGS-84 标准参数WGS84_WIE地球自转角速度 7.2921151467E-5 rad/sWGS84_RA/WGS84_RBWGS-84 长、短半轴WGS84_E1第一偏心率平方2.2 它比高精度 IMU 的噪声还大关键矛盾在这里。IC-GVINS 默认 IMU 噪声参数gvins.yaml参数典型值高精度 MEMS 档说明陀螺随机Walkarw0.1 °/√hr每 1 秒约 0.0017°陀螺零偏gbstd50 °/hr与地球自转 15°/hr 同量级地球自转15 °/hr固定、不可标定掉的系统项对比一下地球每 1 秒让本地导航坐标系n 系转过0.0042°而同级别陀螺每 1 秒的随机游走噪声只有约0.0017°——地球自转引起的坐标系旋转是陀螺噪声的 2.5 倍以上。对 MEMS 级 IMU零偏数百 °/hr来说15°/hr 被淹没在噪声里但对零偏远小于 15°/hr 的高精度 IMU这个旋转无法被零偏估计完全吸收它随载体姿态变化而投影变化是方向相关的系统量只能显式补偿否则会以伪零偏 姿态漂移的形式污染整个因子图。2.3 误差如何累积若忽略地球自转n 系相对惯性系的旋转被错误地归给了载体误差随时间系统性累积姿态误差近似 ∝ ω·t1 小时后达 15°1 秒约 0.0042°速度科里奥利项 2(ω_ie × v) 长期漏积v 10 m/s 时该项约1.5×10⁻³ m/s²是恒定偏置而非噪声无法靠平均消除位置随积分时间二次甚至三次增长GNSS 间隔越长1 Hz 间隔内 INS 自主积分累积越明显。这正是 README 中那句话的由来The Earth rotation has been compensated in the INS to improve the accuracy of high-grade IMUs.在 INS 中补偿地球自转以提升高精度 IMU 的精度。三、IMU 预积分的数学原理旋转坐标系下的修正项预积分的本质把两次姿态更新之间的 IMU 增量提前积分好优化时只需轻量校正。当地理坐标系n 系ENU 北东地本身在以 ω_ie 旋转时经典 VIO 预积分公式必须加入三类修正。以下记 ω ω_ie^nn 系下的地球自转角速度IC-GVINS 中即iewn_C 表示姿态旋转矩阵。3.1 姿态两个会动的坐标系之间的相对旋转经典预积分中n→b 的姿态更新只含 IMU 角增量 Δθ。但起点 n₀ 与终点 n₁ 是两个都随地球转动的坐标系二者之间还差一个相对旋转 q_nndnn -ω·Δt 旋转矢量 q_nn exp(dnn) 起止导航系间相对旋转 姿态更新: q q_nn · q · exp(Δθ)在 preintegration_earth.cc 中qnn既用于连续状态积分的姿态更新也用于速度项的平均旋转0.5·(I C(qnn))相当于把 b 系增量速度投影到区间中间的坐标系消除区间两端坐标系不一致带来的一阶误差。预积分残差里的相对姿态也随之修正为q_b0b1 q1⁻¹ · q_nn · q0evaluate 函数保证两端状态与预积分量在同一旋转基准下比较。3.2 速度科里奥利修正项旋转坐标系下的速度微分方程为ṽ C_nb·f g - (2ω_ie^n ω_en^n) × v2(ω_ie × v)科里奥利项必须补偿ω_en地球曲率运输项对低速短基线属二阶小量IC-GVINS 直接舍弃——源码注释写得很直白地球自转补偿项, 省去了enwn项integrationProcess。对应实现是(gravity_ - 2.0 * iewn_.cross(current_state_.v)) * dt。同时IMU 测得的比力增量 dvfb 要先乘以平均旋转再转入 n 系与 3.1 的qnn机制一致。3.3 位置与残差二阶补偿项地球自转补偿并非只在积分循环里做文章残差计算中还有两个容易被忽略的修正evaluate补偿项形式物理含义位置补偿 p_cor2·[ω]× · Σ (pₖ - p₀)·Δt沿轨迹各点地球自转场相对起点的差异二阶量用缓存的位置序列逐段累加速度补偿 v_cor2·[ω]× · (p₁ - p₀)终点与起点的自转场差修正速度残差的基准系旋转其中[ω]×是iewn_skew_斜对称矩阵。积分时每个 IMU 历元的位置都会存入pn_缓存preintegration_earth.cc 第236行评估残差时统一回放累加。位置残差的雅可比也随之带上-2·C_nb0·[ω]×·Δt的修正项residualJacobianPose0雅可比与协方差传播矩阵 φ 中同样引入cbb0 -(q0⁻¹·q_nn·q0·Δq)使预积分量的协方差也建立在旋转坐标系基准上updateJacobianAndCovariance。 一句话总结姿态加了 q_nn、速度加了科里奥利项、位置加了二阶场差补偿、雅可比与协方差同步修正——四件套缺一不可这就是高精度 IMU 预积分与普通 VIO 预积分的本质区别。四、源码实现四类预积分器如何切换 ️IC-GVINS 把预积分抽象为基类 PreintegrationBase派生出 4 个具体实现preintegration.h预积分器文件地球自转补偿里程计状态PreintegrationNormalpreintegration_normal.cc❌❌PreintegrationOdopreintegration_odo.cc❌✅PreintegrationEarthpreintegration_earth.cc✅❌PreintegrationEarthOdopreintegration_earth_odo.cc✅✅选择逻辑极其简洁iswithearth与isuseodo两个布尔位组合出 4 种选项getOptions。配置入口只有一个开关——config/gvins.yaml 中的iswithearth: true系统初始化时会用它计算iewn并在 GNSS/INS 初始化后重算ic_gvins.cc。几个实现细节值得注意iewn_的取法地球自转角速度投影到 n 系为{ω·cosφ, 0, -ω·sinφ}φ 为纬度见 Earth::iewn。预积分器重置状态时近似取预积分起点位置计算一次预积分段内保持不变resetState——因为单个积分段内位置变化很小这一近似完全足够状态维度不变PreintegrationEarth的状态仍是 15 维位置 3 速度 3 姿态 3 陀螺零偏 3 加速度计零偏 3与Normal版一致差别全部体现在积分递推与残差/雅可比的修正项中工程侵入性很低⚙️里程计版本同构PreintegrationEarthOdo在PreintegrationEarth基础上增加里程增量状态s及其零偏sodo补偿项完全同构方便机器人等带轮速/里程计的平台复用同一套地球自转补偿框架。五、实践要点开启与验证 配置在 config/gvins.yaml 中确认iswithearth: trueIMU 数据须为前-右-下FRD坐标系采样率如 200 Hz须与imudatarate一致IMU 噪声参数要真实arw/vrw/gbstd/abstd应来自 IMU 出厂标定值。噪声参数虚高会放大预积分协方差让 GNSS 与视觉约束抢话语权补偿地球自转的收益也就体现不出来验证方法同一 bag 分别设置iswithearth: true/false跑两遍对比长时≥30 min轨迹的横向位置漂移与姿态稳定性。典型现象是关闭补偿后漂移呈缓慢单向趋势系统偏置特征而非随机抖动适用判断IMU 零偏 10°/hr 级别战术级/光纤级强烈建议开启消费级 MEMS零偏数百 °/hr开关影响有限但开启无害。六、写在最后地球自转补偿是不起眼但决定上限的细节 15°/hr 的自转比高精度 IMU 的噪声还大而 IC-GVINS 用一套干净利落的四件套——姿态相对旋转 q_nn、科里奥利项、二阶位置场差补偿、同步修正的雅可比与协方差——把这一系统误差从 INS 骨架中剔除让 GNSS/RTK 的 1 Hz 基准真正校得准。想进一步阅读建议按此顺序 预积分基类与状态定义preintegration_base.h 地球自转补偿核心实现preintegration_earth.cc WGS-84 大地测量工具earth.h 主流程与初始化ic_gvins.cc 配套论文《Impact of the Earth Rotation Compensation on MEMS-IMU Preintegration of Factor Graph Optimization》IEEE Sensors Journal, 2022掌握这套机制后你再去审视任何INS 中心化的融合导航系统都不会再被这个旋转的地球坑到。【免费下载链接】IC-GVINSA Robust, Real-time, INS-Centric GNSS-Visual-Inertial Navigation System项目地址: https://gitcode.com/gh_mirrors/ic/IC-GVINS创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考