第4章姿态解算数学基础

4、姿态解算数学基础：向量叉积、矩阵运算、最小二乘法在姿态解算中的应用

各位同学，欢迎来到第四章。

说实话，姿态解算这个领域，数学是绕不开的坎。很多人一看到公式就头疼，我当年也一样。但后来我发现，搞懂这几个核心工具，比死记硬背一百个公式管用得多。

今天我们就来聊聊三个最常用的数学工具：向量叉积、矩阵运算、最小二乘法。它们不是孤立的，而是环环相扣的。你想想看，没有叉积，你怎么修正陀螺仪的漂移？没有矩阵，你怎么表示三维空间的旋转？没有最小二乘，你怎么从一堆噪声数据里提取出最靠谱的姿态？

嗯，咱们一个一个来。

先说说向量叉积。很多教材把它讲得很玄乎，什么右手定则、法向量之类的。但在飞控里，它的角色其实很单纯——衡量两个向量之间的“不对齐”程度。

举个例子。你有一个理想的参考向量（比如重力方向），还有一个传感器测出来的向量（比如加速度计测的）。如果飞机水平，这两个向量应该重合。但只要有倾斜，它们之间就有夹角。叉积的大小，就正比于这个夹角的正弦值。

我个人习惯用叉积来做误差修正。在互补滤波或者Mahony滤波里，核心就是这一行代码：

// 计算加速度计测量值与估计值之间的叉积误差
// g是估计的重力方向，a是加速度计测量值（归一化后）
error = cross(g, a);

为什么用叉积而不是点积？点积给出的是夹角余弦，在0°附近变化很平缓，灵敏度不够。而叉积在0°附近几乎是线性的，响应快。我在项目中遇到过一个问题：用点积做修正，飞机悬停时总有小幅晃动。换成叉积后，立马稳了。这就是细节。

小技巧： 叉积结果是一个向量，它的方向垂直于两个输入向量构成的平面。这个方向恰好就是旋转轴的方向。所以叉积不仅告诉你误差有多大，还告诉你该往哪个方向转。一石二鸟。

接下来是矩阵。说白了，矩阵就是用来描述“怎么转”的。

你想想看，一个物体在三维空间里旋转，你要怎么描述它？用欧拉角？直观但容易万向锁。用四元数？紧凑但不够直观。用旋转矩阵？计算方便，但参数多。

在实际工程中，我建议你把旋转矩阵当作“中间格式”。传感器数据进来，转成矩阵；控制指令出去，也转成矩阵。矩阵乘法天然支持连续旋转，而且可以跟向量叉积无缝衔接。

比如，你要把机体坐标系下的向量转换到世界坐标系：

// R是旋转矩阵，v_body是机体坐标系下的向量
v_world = R * v_body;

反过来，从世界坐标系到机体坐标系，就是转置矩阵：

v_body = R^T * v_world;

这里有个坑，我曾经踩过。旋转矩阵必须是正交矩阵，也就是说它的逆等于它的转置。但数值计算中，由于浮点误差，矩阵会慢慢偏离正交性。所以每隔一段时间，需要做一次正交化修正。不然你的姿态会越算越偏。

注意： 不要直接用矩阵求逆！对于旋转矩阵，转置就是逆。求逆计算量大，而且数值不稳定。记住：R^T = R^{-1}。

最后说说最小二乘法。这个工具在姿态解算里，主要用来做传感器融合。

你想想看，陀螺仪短期准但长期漂，加速度计长期稳但短期噪。怎么把两者结合起来？最小二乘法就是那个“裁判”——它帮你找到一组姿态参数，使得所有传感器的误差平方和最小。

举个最简单的例子。假设你有两个传感器对同一个角度θ的测量值：

最小二乘的最优估计就是加权平均：

θ_est = (θ_gyro / σ_g² + θ_acc / σ_a²) / (1/σ_g² + 1/σ_a²);

你看，方差小的传感器权重更大。这就是互补滤波的数学本质。我刚开始做飞控时，一直以为互补滤波是拍脑袋想出来的，后来才发现它背后就是最小二乘。

  核心思想： 最小二乘法不是万能的，但它给了你一个“最优”的基准。当你不知道该怎么融合数据时，先试试最小二乘，往往不会错。

好了，三个工具都讲完了。但它们不是孤立的。在实际的姿态解算算法中，它们是这样配合的：

下面这张图，是我自己画的一个简化流程，你可以对照着理解：

你看，整个流程就像一条流水线。每个数学工具负责一个环节，缺一不可。我个人觉得，理解了这个协同关系，比背下任何一个公式都重要。

避坑指南： 我曾经在最小二乘的权重选择上吃过亏。权重设得太小，修正太慢，飞机飘；权重设得太大，噪声全进来了，飞机抖。后来我总结了一个经验：先根据传感器手册的噪声方差定初始值，再在实际飞行中微调。别指望一次算对。

好了，这一章的内容就到这里。我们聊了三个数学工具：

它们不是孤立的，而是环环相扣的。你掌握了它们，就等于拿到了姿态解算的钥匙。下一章我们会把这些工具串起来，讲一个完整的姿态解算算法实现。

嗯，今天就到这里。有问题随时找我。