旋转矩阵：从二维到三维，飞控中的几何基石

各位同学好，我是你们的嵌入式飞控算法讲师。今天我们来聊聊旋转矩阵——这个在姿态解算里绕不开的核心工具。

说实话，我刚入行那会儿，看到旋转矩阵就头疼。一堆数字排成方阵，到底在描述什么？后来在项目里摔过几次跟头，才真正理解它的妙处。今天我就把这点心得分享给你们。

一、二维旋转矩阵：先找找感觉

我们先从简单的开始。想象你面前有一个点P，坐标是(x, y)。现在我想让它绕原点逆时针转一个角度θ，转到P'的位置。新坐标怎么算？

其实很简单，用三角函数就能推出来：

x' = x·cosθ - y·sinθ
y' = x·sinθ + y·cosθ

写成矩阵形式就是：

[x']   [cosθ  -sinθ] [x]
[y'] = [sinθ   cosθ] [y]

这个2×2的矩阵，就是二维旋转矩阵R(θ)。

我个人习惯把旋转矩阵理解为「坐标系的搬家工具」。你想想看，它不只是在转一个点，更是在描述两个坐标系之间的映射关系。

二、三维旋转矩阵：飞控里的主角

到了三维空间，事情就变得更有意思了。无人机在空中翻滚，绕三个轴转来转去。我们需要一个3×3的矩阵来描述这种旋转。

绕三个轴的基本旋转矩阵长这样：

R_x(φ) = [1      0       0   ]
         [0    cosφ   -sinφ ]
         [0    sinφ    cosφ ]

R_y(θ) = [cosθ   0    sinθ ]
         [0      1      0   ]
         [-sinθ  0    cosθ ]

R_z(ψ) = [cosψ  -sinψ   0  ]
         [sinψ   cosψ   0  ]
         [0      0      1   ]

注意看，绕哪个轴旋转，那个轴对应的行和列就保持不动。这个规律记住了，写代码时不容易出错。

完整的姿态旋转矩阵，其实就是这三个基本旋转的乘积。不过顺序很重要——不同的旋转顺序会得到不同的结果。在飞控里，我们通常用Z-Y-X顺序（偏航→俯仰→滚转），也就是：

R = R_z(ψ) · R_y(θ) · R_x(φ)

展开后得到一个3×3的矩阵，里面全是三角函数。嗯，这里要注意，这个矩阵就是我们在飞控代码里频繁调用的那个「姿态矩阵」。

三、旋转矩阵的性质：为什么它这么特别？

旋转矩阵有两个非常重要的数学性质，直接决定了它在工程中的应用方式。

性质一：正交性

什么叫正交矩阵？就是矩阵的转置等于它的逆：

R^T = R^(-1)

换句话说，R · R^T = I（单位矩阵）。

这个性质太有用了。你想想看，在飞控里我们经常需要做坐标变换的反向操作——从机体坐标系转回地理坐标系。如果R是正交矩阵，那直接取转置就行，不用去算复杂的逆矩阵。这在嵌入式平台上能省下不少计算资源。

性质二：行列式为1

旋转矩阵的行列式值恒等于1。这个性质意味着什么？

说白了，旋转不改变物体的体积和形状。它只是「转了个方向」，没有拉伸、没有压缩、没有镜像。这在物理上对应着「刚体旋转」——无人机作为一个刚体，在空中翻滚时，机身的形状不会变。

如果某次计算出来的旋转矩阵行列式不等于1（或者非常接近1但有点偏差），那说明你的计算过程有误差累积。这时候就需要做「正交化修正」了。

四、知识体系总览

下面这张图是我自己整理的旋转矩阵知识框架，帮你理清思路：

五、代码实现要点

在嵌入式平台上实现旋转矩阵，我建议你注意以下几点：

1. 用查找表代替实时三角函数计算——STM32这类MCU算sin/cos挺慢的，预计算好角度对应的值存起来，能省不少时间。
2. 注意浮点精度——单精度float在长时间积分后误差会累积，定期做正交化修正很有必要。
3. 矩阵乘法顺序别搞反——R·v和v·R^T是完全不同的结果，我见过有人在这上面debug了一整天。

好了，这一章的内容就到这里。旋转矩阵是姿态解算的基石，后面的四元数、欧拉角、互补滤波，全都建立在这个基础之上。希望你能把今天的内容消化透，尤其是那两个性质——它们会在你写代码时反复出现。