专业知识的应用03.惯导解算用了哪些知识

前言

在上一篇里我们说了卡尔曼在本项目里怎么用：它估计的是误差状态，用 GNSS 位置观测去修正惯导解算的结果。

那么惯导解算在本项目里做什么？一句话：在已知初始位置、速度、姿态的前提下，用 IMU 的角速度和加速度递推出当前时刻的位置、速度、姿态（名义状态）。

这些名义状态一方面是对外输出的「当前位姿」，另一方面也是卡尔曼预测步骤所需要的：误差状态的传播（状态转移矩阵 Φ、过程噪声驱动矩阵 G）都依赖当前惯导状态和 IMU 数据。

所以惯导解算是「组合导航」里和卡尔曼并列的另一条腿：惯导负责递推名义状态，供卡尔曼预测使用，卡尔曼负责估计误差并校正，同时名义状态也是组合系统对外输出的位置和姿态。

本篇不展开课本级的理论推导，只写在本项目里实际用到的知识点：坐标系怎么约定、姿态怎么表示和递推、地球模型在哪些公式里出现、我们采用了什么样的积分方式。

每个点只讲「是什么 + 在代码里对应什么」；

惯性系与非惯性系的详细推导、安装误差等复杂误差模型、高阶数值积分理论，本篇都不写，公式与推导请直接参考牛小骥老师的《惯性导航原理与算法设计》等教材。

惯导在本项目里的角色

惯导（Inertial Navigation System，INS）解算的核心是：

利用 IMU 测到的角速度和加速度，在给定初始状态的前提下，通过积分得到当前的位置、速度、姿态。

角速度用来更新姿态（载体相对导航系的朝向），加速度（比力）在扣除重力和哥式效应后用来更新速度，速度再积分得到位置。

因此惯导是一个积分系统：误差会随时间累积，这也是为什么需要 GNSS 等外部观测来定期校正。

在本项目中，惯导解算由 ins/pins.cpp 中的 Pins 类完成。

它对外提供 init(state, imu) 和 insUpdate(imu)：给定初始状态和上一帧 IMU 后，每次传入新的 IMU 数据，内部调用 propagate(dt)，依次更新速度、位置、姿态，得到当前时刻的 InsState（位置、速度、姿态）。这个 InsState 会被 pose_estimator 同步到名义状态，并传给滤波器的 predict()，用于构建 Φ 和 G。

所以惯导在本项目里的角色就是：用 IMU 递推名义状态，供卡尔曼预测使用；同时名义状态也是组合系统对外输出的位置和姿态。

实际用到的知识点

坐标系：NED 与 FRD

惯导公式和代码里会大量出现「在哪个坐标系下」的约定。本项目采用：

• 导航坐标系（n 系）：北东地（NED）——北为 X、东为 Y、地为 Z。位置用经纬高（LLH）表示，速度用 NED 下的三分量表示。
• 载体/IMU 坐标系（b 系）：前右下（FRD）——前为 X、右为 Y、下为 Z。

这两个约定和牛小骥老师《惯性导航原理与算法设计》中的定义一致。

统一约定的重要性在于：教材里的公式、地球模型（子午圈/卯酉圈半径、地球自转在 NED 下的投影等）、以及代码里的旋转矩阵、速度/位置增量的方向，都必须一致，否则符号错一个就会导致解算错误。

在代码里，位置存在 InsState::pos 中，是 [纬度, 经度, 高]（弧度、弧度、米）；速度 vel 是 NED 三分量；姿态四元数 quat 表示从载体系到导航系的旋转（b → n），即把矢量从 b 系变换到 n 系时用 quat.toRotationMatrix() * v_b。

姿态表示：四元数、欧拉角、旋转矩阵

姿态表示方法有很多种：欧拉角（滚转角、俯仰角、航向角）、旋转矩阵、四元数、旋转矢量等。

本项目内部递推采用四元数，原因主要有两点：一是无奇异，欧拉角在俯仰角接近 ±90° 时会遇到万向锁；二是递推形式简单，姿态微分方程可以写成四元数对时间的导数，离散化后用「旋转增量」对应的四元数左乘或右乘当前四元数即可，和 Sophus 的 SO3d::exp(旋转向量) 配合很方便。在代码里，当前姿态存在 InsState::quat（Eigen::Quaterniond），递推时用 Sophus::SO3d::exp(dtheta) 得到载体系旋转增量对应的四元数 qbb，再与导航系旋转 qnn 组合：currState_.quat = (qnn * prevState_.quat * qbb).normalized()（见 updateAttitudeByMid）。

欧拉角在项目里主要用于输入和输出：配置文件中的初始姿态是滚转、俯仰、航向（度），需要转换成四元数再参与解算；若要把结果输出成欧拉角（例如给可视化或日志），则用 utils/rotation.h 里的 quaternion2euler（内部先转成旋转矩阵再按 ZYX 顺序解算欧拉角）。

旋转矩阵则作为四元数的等价表示，在需要把矢量从载体系变到导航系时使用 quat.toRotationMatrix()，在滤波器里构建 Φ、G 时也会用到。

旋转矢量在误差状态里出现：姿态误差 attDelta 是三维旋转矢量，用 Sophus 的 SO3d::exp(attDelta) 可得到对应的旋转并作用于四元数（见 correctPose）。所以本项目实际用到了四元数（存储与递推）、欧拉角（配置与输出）、旋转矩阵（矢量变换与滤波）、旋转矢量（误差表示），各自用在不同的环节。

地球模型：WGS84 与导航参数

惯导解算中的速度、位置微分方程里会用到地球的形状和自转：子午圈曲率半径 Rm、卯酉圈曲率半径 Rn（与纬度、高度有关），地球自转角速度在导航系的投影 ω_ie_n，以及由于载体运动导致的运移角速度 ω_en_n。
这些参数在教材里统称为「导航系参数」，在本项目中由 utils/earth_model 提供。

• 椭球与重力：采用 WGS84 椭球（赤道半径、扁率等），与 GPS 常用的大地基准一致。重力加速度随纬度和高度变化，用 Somigliana 公式加高度修正计算，对应 EarthModel::getGravity(lat, h)。
• 曲率半径：子午圈半径 Rm、卯酉圈半径 Rn 由纬度和椭球参数计算；考虑高度后有 Rmh = Rm + h、Rnh = Rn + h。它们在位置微分方程里出现（NED 速度到经纬高变化率的转换），也在速度微分方程里出现（哥式力、曲率项）。对应 EarthModel::computeNavParam(lat, h, vn, ve) 返回的 NavEarthParam（Rm, Rn, Rmh, Rnh, wie_n, wen_n）。
• 地球自转与运移角速度：ω_ie_n 是地球自转角速度在 NED 下的投影，ω_en_n 是运移角速度（载体相对地心的角速度在 NED 下的投影）。它们出现在速度更新的哥式力项（−2(ω_ie + ω_en) × v）和姿态更新的导航系旋转 qnn 中。getQne、getQee、getQnn 等函数用于计算「导航系相对地心系」「地心系随时间」以及「导航系随时间」的旋转四元数，供位置和姿态更新使用。

因此，地球模型在代码里的角色就是：在速度、位置、姿态的微分方程（或离散化后的递推式）里，提供 Rm、Rn、重力、ω_ie_n、ω_en_n 等参数；具体对应 ins/pins.cpp 中调用 EarthModel::computeNavParam、getGravity、getQnn、getQne、NDE2LLhMatrix 等的位置。

公式的完整形式见教材，这里只强调「这些量在哪些步骤里被用到」。

可以看到，地球模型并不是初始化阶段的一次性计算，而是在每一帧 INS 递推中动态参与姿态、速度和位置的更新。

微分方程与积分：速度、位置、姿态

惯导解算的本质是对一组非线性微分方程做离散积分：

• 姿态：微分方程描述四元数对时间的导数与角速度的关系；离散化后变成「用当前角速度（经零偏补偿后）积分得到旋转增量，再与上一时刻姿态组合」。本项目在简化版本（USE_EASY_PINS）里采用前一时刻参数计算导航系旋转 qnn，用当前角速度加 Coning 补偿（1/12·(ω_{k-1} × ω_k)）得到载体系旋转增量，再按 q_{b→n}k = qnn · q_{k-1} · qbb 更新姿态。Coning 补偿是为了在圆锥运动下减小一阶误差。
• 速度：微分方程包含比力在导航系的投影、重力、哥式力（−2(ω_ie + ω_en) × v）。离散化后，比力由加速度计测量值经Sculling 补偿（0.5·(ω × a)）后转到导航系，再加重力增量与哥式力增量。本项目简化版里用前一时刻的导航参数和姿态，计算 dvN、重力项、哥式力项，然后 v_k = v_{k-1} + dvN + gravity + coriolis。
• 位置：微分方程描述经纬高对时间的导数与 NED 速度、曲率半径的关系。离散化后用「中点速度」和 NED→LLH 的转换矩阵 T 更新：pos_k = pos_{k-1} + T · v_mid · dt，其中 T 由 EarthModel::NDE2LLhMatrix(lat, h) 给出。

本项目没有采用高阶多子步积分（如 RK4 或多子样圆锥/划桨补偿），而是采用简单的一阶/中点式积分和一阶 Coning/Sculling 补偿，在保证可读性和可维护性的前提下，满足练手与复习的目的。更高阶的积分格式和更完整的误差补偿，教材和工程实现里都有，这里不展开。

明确不写的内容

为避免写成课本，下面这些内容本篇不写，只列出名字，便于读者知道「存在但本项目未展开」：

• 惯性系与非惯性系的详细推导：为什么在导航系下要加哥式力、运移角速度从何而来等，请直接看教材。
• 复杂误差模型：安装误差（非正交、尺度因子）、高阶的圆锥/划桨补偿、零偏的复杂模型等，本项目均未实现，只建模了最简单的零偏（在滤波器里估计）。
• 高阶数值积分理论：多阶 Runge-Kutta、多子样算法等，本项目未采用，不展开推导。

代码对应小结

• 惯导解算：ins/pins.h、pins.cpp。Pins::propagate(dt) 内部根据 USE_EASY_PINS 调用 updateVelocityByMid、updatePositionByMid、updateAttitudeByMid（简化版）或 updateVelocity、updatePosition、updateAttitude（完整版）。状态存在 currState_（InsState：pos 纬经高、vel NED、quat 体到导航系）。
• 地球模型：utils/earth_model.h、earth_model.cpp。WGS84 椭球、computeNavParam（Rm, Rn, Rmh, Rnh, wie_n, wen_n）、getGravity、NDE2LLhMatrix/LLh2NEDMatrix、getQne/getQee/getQnn、getLLhByQne 等，被 pins.cpp 和滤波器 buildPhi 调用。
• 姿态与旋转：utils/rotation.h 提供欧拉角与旋转矩阵、四元数的转换（matrix2euler、quaternion2euler、euler2quaternion 等）；Sophus 的 SO3d::exp、unit_quaternion() 在 pins.cpp 和 pose_estimator.cpp 中用于旋转矢量与四元数的转换。
• 坐标系约定：README 与 ARCHITECTURE 中写明导航系 NED、载体系 FRD；位置为 LLH，速度为 NED，姿态四元数为 b→n，与代码中的存储一致。

总结

1. 惯导的角色：在本项目中，惯导用 IMU 角速度与加速度在给定初始状态下递推位置、速度、姿态（名义状态），供卡尔曼预测使用，并作为组合系统对外输出的位姿。
2. 用到的知识：坐标系（NED/FRD）统一约定；姿态用四元数递推，欧拉角用于配置与输出，旋转矩阵与旋转矢量在变换与误差表示中使用；地球模型（WGS84、Rm/Rn、重力、ω_ie_n、ω_en_n）在速度、位置、姿态微分方程及其离散化中出现；采用简单的一阶/中点式积分和一阶 Coning/Sculling 补偿，不展开高阶积分理论。
3. 不写什么：惯性系与非惯性系的详细推导、安装误差等复杂误差模型、高阶数值积分与多子样补偿的推导；公式与完整理论见牛小骥《惯性导航原理与算法设计》等教材。