3、姿态解算基础：角速度积分、圆锥效应与划桨效应、多子样算法、等效旋转矢量

各位同学，今天我们聊点硬核的。姿态解算，说白了就是怎么用陀螺仪的数据算出物体转了多少、转到哪去了。听起来简单？嗯，这里面的坑可不少。我当年刚入行时，以为不就是积分嘛，结果被现实狠狠教育了一顿。

3.1 角速度积分——最朴素的思路

陀螺仪输出的是角速度，单位是 °/s 或 rad/s。要得到角度，最直接的办法就是积分：

θ(t) = θ(0) + ∫ ω(t) dt

在离散系统中，我们通常用：

θ_k = θ_{k-1} + ω_k · Δt

这看起来人畜无害对吧？但实际用起来，问题就来了。

为什么会这样？因为积分会把低频误差无限放大。你想想看，陀螺仪零偏哪怕只有 0.01°/s，积分100秒就是1°，1000秒就是10°。所以纯积分只适合短时间使用。

3.2 圆锥效应与划桨效应——两个绕不开的坑

这两个效应，我当年学的时候觉得是理论家搞出来的东西，直到在振动台上做实验，才明白它们有多真实。

3.2.1 圆锥效应

圆锥效应发生在角速度积分时。简单说：当载体同时绕两个轴做同频正弦角振动时，即使每个轴的角速度积分都为零，但实际姿态却会慢慢漂移。

数学上，假设：

ω_x = A·sin(Ωt)
ω_y = A·cos(Ωt)
ω_z = 0

如果直接用欧拉角积分，你会发现俯仰和横滚角来回摆动，但航向角却出现了不可忽略的漂移。这就是圆锥效应。

我在做车载惯导时，遇到过车辆过减速带时的剧烈振动，圆锥效应导致航向角每秒钟漂移0.5度。如果不补偿，10秒就偏了5度，导航完全没法用。

3.2.2 划桨效应

划桨效应是圆锥效应的“平移版本”。它发生在加速度积分时。当载体同时沿两个轴做同频正弦线振动时，即使每个轴的加速度积分都为零，但速度却会慢慢漂移。

说白了，圆锥效应影响姿态，划桨效应影响速度。两者本质相同，都是因为积分过程中忽略了高阶耦合项。

3.3 多子样算法——工程上的解决方案

既然单次积分有问题，那我们就多采几个点，用更精细的算法来逼近真实旋转。这就是多子样算法的思路。

多子样算法的核心思想：在一个采样周期内，采集多个角速度样本，然后用多项式拟合角速度变化，再积分得到更精确的姿态变化。

双子样算法的公式长这样：

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

其中 Δθ₁ 和 Δθ₂ 是半个采样周期内的角增量。注意那个叉乘项，它就是圆锥补偿项。

3.4 等效旋转矢量——最优雅的数学工具

前面说了那么多问题，有没有一个统一的数学框架来解决？有，就是等效旋转矢量。

等效旋转矢量（Rotation Vector）的核心思想：任何有限时间的旋转，都可以用一个旋转轴和一个旋转角度来描述。这个旋转轴就是等效旋转矢量的方向，旋转角度就是它的模长。

数学上，等效旋转矢量 Φ 满足：

Φ = |Φ| · n

其中 n 是单位旋转轴，|Φ| 是旋转角度。

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

Φ̇ = ω + (1/2)·(Φ × ω) + (1/|Φ|²)·(1 - |Φ|·sin(|Φ|)/(2·(1-cos(|Φ|))))·(Φ × (Φ × ω))

看着复杂？嗯，实际工程中我们通常用近似形式：

Φ̇ ≈ ω + (1/2)·(Φ × ω)

这个近似在旋转角度小于30°时精度很高。

我个人的习惯是：在IMU预积分、视觉-惯性里程计（VIO）中，全部使用等效旋转矢量。虽然公式比欧拉角复杂，但鲁棒性高得多。

3.5 知识体系总览

下面这张图，是我自己总结的姿态解算知识体系。你看一遍，应该能理清思路：

从这张图你可以看到，所有的问题都归结到旋转不可交换性。多子样算法和等效旋转矢量，就是从不同角度解决这个问题的工具。

3.6 工程实践建议

最后，我结合自己的项目经验，给大家几条实在的建议：

• 采样率要够高： 对于圆锥效应明显的场景（比如无人机、机器人），陀螺仪采样率至少200Hz，建议400Hz以上。我见过有人用100Hz做姿态解算，结果圆锥误差大到离谱。
• 先做圆锥补偿，再做积分： 很多新手直接对原始角速度积分，然后才补偿。顺序反了，效果差很多。
• 等效旋转矢量是首选： 如果计算资源允许，直接用等效旋转矢量更新姿态。它比四元数更新更精确，而且天然补偿了圆锥效应。
• 别忘了划桨效应： 如果你做的是速度/位置估计（比如VIO、组合导航），划桨效应同样需要补偿。我曾经在车载导航中忽略了划桨效应，结果速度估计每10秒偏0.2m/s，位置误差累积很快。

好了，这一章的内容就到这里。姿态解算是个基础活，但基础不牢，地动山摇。希望大家在实际项目中，能把这些原理用起来，少走弯路。

可可精品资料库