第4章姿态解算算法

姿态解算算法：毕卡逼近法、龙格-库塔法、等效旋转矢量法

姿态解算，说白了就是怎么把陀螺仪测到的角速度，变成我们想要的姿态角或者姿态矩阵。这步要是算不准，后面整个导航系统都是白搭。我个人习惯把这几种算法分成两类：一类是直接怼微分方程，另一类是玩点“花活”避开不可交换误差。今天咱们就把毕卡逼近法、龙格-库塔法和等效旋转矢量法掰开揉碎了讲清楚。

毕卡逼近法，本质上是一种逐次逼近的数学技巧。它不直接解微分方程，而是构造一个迭代序列，让这个序列收敛到真实解。嗯，这里要注意，它跟后面要讲的龙格-库塔法思路完全不同。

对于姿态矩阵的微分方程：

Ċ = C · [ω×]

其中[ω×]是角速度的反对称矩阵。毕卡逼近法的解可以写成：

C(t) = C(0) · exp(∫[ω×]dt)

把指数项展开成级数：

exp(∫[ω×]dt) = I + ∫[ω×]dt + (1/2!)(∫[ω×]dt)² + ...

实际工程中的简化

我在项目中遇到过，如果采样周期足够短（比如200Hz以上），取前两项就够用了：

C(t) ≈ C(0) · (I + [σ×])

其中[σ×]是角增量构成的反对称矩阵。这就是最常用的单子样算法。

核心结论：毕卡逼近法取到二阶项时，精度相当于四阶龙格-库塔法的一步计算。但要注意，它本质上假设了角速度在积分区间内方向不变——这在高动态环境下会出问题。

我的经验：毕卡逼近法最适合用在低动态、高采样率的场景。我曾经在一个无人机飞控项目里用过，采样率提到500Hz后，单子样算法的精度完全够用，而且计算量极小。

龙格-库塔法（RK法）是解微分方程的通用工具。对于姿态解算，我们通常用四阶RK法。你想想看，它每步要计算四个中间斜率，精度确实高，但计算量也上去了。

四阶RK法的标准形式：

k1 = f(tn, yn)
k2 = f(tn + h/2, yn + h·k1/2)
k3 = f(tn + h/2, yn + h·k2/2)
k4 = f(tn + h, yn + h·k3)
yn+1 = yn + (h/6)(k1 + 2k2 + 2k3 + k4)

应用到姿态解算中，f(t, C) = C · [ω(t)×]。这里有个坑——四元数更新时，必须保证更新后的四元数模长为1，否则姿态会漂移。

避坑指南：我曾经在车载导航项目里直接用RK4更新四元数，结果跑了10分钟姿态就歪了。后来发现是没做归一化。记住：每次更新后必须归一化！

RK法的优缺点：

我的建议：如果你做的是低动态系统（比如地面车辆、船舶），RK4完全够用。但要是做高动态飞行器，我建议你直接看等效旋转矢量法。

为什么要搞出等效旋转矢量法？说白了，前面两种方法都假设了角速度方向不变。但在高动态环境下（比如飞机剧烈机动），角速度方向在积分区间内会变化，这就产生了不可交换误差。

等效旋转矢量法的核心思想是：用旋转矢量Φ来描述从t到t+h的姿态变化，然后通过求解Φ的微分方程来补偿不可交换误差。

旋转矢量的微分方程：

Φ̇ = ω + (1/2)Φ × ω + (1/12)Φ × (Φ × ω) + ...

实际工程中，我们通常取前两项或前三项。最经典的是二子样算法：

Φ = Δθ1 + Δθ2 + (2/3)Δθ1 × Δθ2

其中Δθ1和Δθ2是半个采样周期内的角增量。

经验之谈：我在做某型导弹的捷联惯导时，一开始用四阶RK法，圆锥运动误差大到不可接受。换成三子样等效旋转矢量法后，误差直接降了一个数量级。嗯，这就是算法的力量。

常用子样算法对比：

算法	子样数	圆锥补偿精度	计算量	适用场景
单子样	1	无补偿	极低	低动态、高采样率
二子样	2	二阶	低	中等动态
三子样	3	四阶	中等	高动态
四子样	4	六阶	较高	极高动态

注意：子样数不是越多越好。子样数增加意味着需要更短的子采样间隔，对陀螺仪的带宽和采样率要求更高。我曾经见过有人用四子样算法，但陀螺仪采样率跟不上，结果精度反而比二子样还差。

说了这么多，到底该用哪个？我个人习惯按这个思路选：

我的最终建议：如果你刚开始做惯导，先用RK4跑起来，把整个系统调通。等遇到精度瓶颈了，再换成等效旋转矢量法。别一开始就追求最复杂的算法——我见过太多人把时间花在调算法上，结果发现问题是陀螺仪噪声太大。

好了，姿态解算的三种核心算法就讲到这里。下一章咱们聊聊初始对准——这可是惯导系统能不能正常工作的第一步，也是坑最多的一步。