第4章：INS机械编排——姿态、速度、位置更新与圆锥/划桨效应补偿

各位同学，欢迎来到INS机械编排这一章。说实话，这是整个紧耦合系统里最“硬核”的部分之一。你想想看，IMU输出的原始数据是角速度和比力，我们要靠这些算出姿态、速度、位置，这个过程就是机械编排。我当年刚接触时，觉得不就是积分嘛，有什么难的？后来发现，坑多着呢。

这一章，我会把姿态更新（四元数/等效旋转矢量）、速度更新、位置更新，还有圆锥效应和划桨效应补偿，掰开了讲清楚。嗯，咱们开始吧。

4.1 姿态更新：从角速度到旋转矩阵

姿态更新，说白了就是回答一个问题：载体转了多少？

IMU输出的角速度是载体坐标系相对于惯性系的旋转速率。我们要把它积分，得到当前时刻相对于上一时刻的姿态变化。常用的数学工具有两种：四元数和等效旋转矢量。

4.1.1 四元数法

四元数是个好东西。它没有欧拉角的万向锁问题，计算量也比方向余弦矩阵小。我个人习惯用四元数做姿态更新，因为它在工程实现上非常稳定。

四元数的微分方程长这样：

dq/dt = 0.5 * q ⊗ ω

其中ω是角速度向量，⊗表示四元数乘法。实际离散化时，我们通常用一阶或二阶龙格-库塔法。我建议用二阶，精度够用，计算量也不大。

举个例子，假设陀螺采样周期为dt，角增量为Δθ = ω * dt，那么四元数更新公式为：

q_new = q_old ⊗ [cos(|Δθ|/2), (Δθ/|Δθ|) * sin(|Δθ|/2)]

这里要注意，如果|Δθ|很小（比如小于1e-6），直接用一阶近似：

q_new = q_old ⊗ [1, Δθ/2]

4.1.2 等效旋转矢量法

等效旋转矢量法，是专门用来对付圆锥效应的。什么是圆锥效应？简单说，就是当载体同时绕两个轴做同频振动时，积分出来的姿态会多出一个虚假的旋转分量。这可不是数学游戏，实际IMU数据里真的会发生。

等效旋转矢量Φ的微分方程是：

dΦ/dt = ω + 0.5 * Φ × ω + (1/|Φ|²) * (1 - |Φ| * sin(|Φ|) / (2*(1-cos(|Φ|)))) * Φ × (Φ × ω)

看着复杂，但实际工程中常用简化形式。对于小角度，可以近似为：

Φ = Δθ₁ + Δθ₂ + (2/3) * (Δθ₁ × Δθ₂)

其中Δθ₁和Δθ₂是相邻两个半采样周期的角增量。这个公式就是著名的“二子样算法”。

4.2 速度更新：比力积分与重力补偿

速度更新，就是把加速度计测到的比力，去掉重力，再积分。公式很简单：

v_new = v_old + (f - g) * dt

但实际做起来，有几个坑要小心。

第一，比力坐标系转换。 加速度计测的是载体坐标系下的比力，但我们要在导航坐标系下积分。所以必须先做坐标变换：

f_n = C_b^n * f_b

其中C_b^n是姿态矩阵。这里有个鸡生蛋蛋生鸡的问题：姿态更新需要角速度，速度更新需要姿态矩阵。实际工程中，我们通常先做姿态更新，再用更新后的姿态做速度更新。

第二，重力模型。 重力不是常数。我建议用WGS84模型，考虑纬度和高度的影响。简单点可以用：

g = 9.780326 * (1 + 0.0053024 * sin²(lat) - 0.0000058 * sin²(2*lat)) - 0.000003086 * h

其中lat是纬度，h是高度。嗯，这个公式我背了十年了。

4.3 位置更新：速度积分与曲率补偿

位置更新，就是把速度积分得到位移。在平面导航中很简单：

p_new = p_old + v * dt

但在全球导航中，要考虑地球曲率。位置通常用经纬高表示，更新公式为：

lat_new = lat_old + v_n * dt / (R_m + h)
lon_new = lon_old + v_e * dt / ((R_t + h) * cos(lat))
h_new = h_old + v_d * dt

其中R_m是子午圈曲率半径，R_t是卯酉圈曲率半径。这两个值随纬度变化，公式如下：

R_m = a * (1 - e²) / (1 - e² * sin²(lat))^(3/2)
R_t = a / sqrt(1 - e² * sin²(lat))

a是地球长半轴，e是偏心率。WGS84模型下，a=6378137米，e²=0.00669437999014。

4.4 圆锥效应与划桨效应补偿

这两个效应，是INS机械编排里最容易被忽视，但影响最大的两个问题。

4.4.1 圆锥效应

圆锥效应发生在角运动上。当载体同时绕两个轴做同频正弦振动时，直接积分角速度会得到虚假的旋转。举个例子，假设载体绕x轴和y轴同时振动：

ω_x = A * sin(Ωt)
ω_y = A * cos(Ωt)

直接积分，你会得到一个绕z轴的虚假旋转。这就是圆锥效应。

补偿方法就是用等效旋转矢量法。前面提到的二子样算法就是最简单的补偿。更高阶的有多子样算法，比如三子样、四子样。我一般用二子样，性价比最高。

// 二子样圆锥补偿
Δθ = Δθ₁ + Δθ₂
Φ = Δθ + (2/3) * (Δθ₁ × Δθ₂)

4.4.2 划桨效应

划桨效应是圆锥效应的“平移版本”。它发生在比力积分中。当载体同时做线振动和角振动时，直接积分比力会得到虚假的速度增量。

想象一下，你坐在船上划桨。桨入水时船身倾斜，桨出水时船身回正。这个过程中，虽然桨的净位移为零，但船却前进了。这就是划桨效应的物理本质。

补偿公式与圆锥效应类似：

Δv_sculling = 0.5 * (Δθ₁ × Δv₂ + Δv₁ × Δθ₂)

其中Δv₁和Δv₂是相邻半采样周期的速度增量，Δθ₁和Δθ₂是角增量。

4.5 知识体系总览

为了让大家更直观地理解这一章的内容，我画了一张流程图。它展示了INS机械编排的核心逻辑：从IMU原始数据出发，经过姿态、速度、位置更新，最终得到导航解算结果。

从图中可以看到，IMU原始数据分为角速度和比力两条路径。角速度经过姿态更新（含圆锥补偿）得到姿态矩阵，比力经过速度更新（含划桨补偿）得到速度，速度再积分得到位置。姿态矩阵还会反馈给速度更新，用于比力的坐标系变换。这就是一个完整的机械编排闭环。

好了，这一章的内容就到这里。机械编排是INS的核心，也是紧耦合的基础。你把这些公式和补偿方法吃透了，后面做融合滤波就会轻松很多。

---