基础数学回顾（一）：向量与坐标系、旋转矩阵、欧拉角定义与万向锁问题
各位同学，欢迎来到第二章。
姿态控制算法，说白了就是研究「怎么让一个物体转成我们想要的角度」。你想想看，不管是四旋翼、固定翼，还是机械臂，核心问题都一样——我得知道它现在朝哪，然后告诉它该往哪转。这背后全是数学。
我个人习惯，在讲任何控制算法之前，先把数学工具磨锋利。这一章我们只干三件事：把坐标系和向量说透，把旋转矩阵讲明白，再把欧拉角和那个让人头疼的万向锁问题彻底掰扯清楚。
2.1 向量与坐标系：描述姿态的「尺子」
先问个问题：你怎么描述一个物体在空间里的朝向？
答案很简单——先定一把尺子，也就是坐标系。然后在这把尺子上，用向量表示方向。
核心概念：姿态描述的本质，就是将一个坐标系下的向量，映射到另一个坐标系下。
2.1.1 右手坐标系
飞控领域几乎只用右手坐标系。怎么判断？伸出右手，大拇指指向 X 轴，食指指向 Y 轴，中指指向 Z 轴。三个手指互相垂直，这就是右手系。
我在项目中遇到过有人把 Z 轴搞反了，结果飞机一解锁直接往地里钻。嗯，这种低级错误，犯一次就记住了。
2.1.2 常用坐标系
姿态控制里，我们主要跟两个坐标系打交道：
| 坐标系 | 符号 | 原点 | 用途 |
|---|---|---|---|
| 地球坐标系 | n 系 (导航系) | 通常取起飞点 | 定义期望姿态、高度、航向 |
| 机体坐标系 | b 系 | 飞行器质心 | 描述传感器测量值、控制力矩 |
说白了，控制器的任务就是：让机体坐标系的朝向，去对齐地球坐标系里我们想要的那个方向。
2.1.3 向量的点乘与叉乘
这两个运算在姿态解算里天天用，我建议你闭着眼睛都能写出来。
- 点乘：
a · b = |a||b|cosθ。结果是一个标量。用来算两个向量夹角的余弦，或者投影长度。比如，重力加速度在机体 Z 轴上的分量，就是点乘。 - 叉乘：
a × b = |a||b|sinθ · n。结果是一个向量，方向垂直于 a 和 b 所在的平面。这在控制里太重要了——两个姿态之间的误差，本质上就是一个叉乘。
我的小技巧：在代码里判断两个向量是否平行，别用点乘算角度，直接用叉乘的模长是否接近 0。叉乘在数值上更敏感，尤其当角度很小时。
2.2 旋转矩阵：姿态的「数学翻译官」
有了坐标系和向量，下一步就是怎么把一个向量从一个坐标系「翻译」到另一个坐标系。旋转矩阵就是干这个的。
2.2.1 定义与性质
一个 3x3 的旋转矩阵 R，满足两个性质：
- 正交性：
R^T · R = I。也就是说，矩阵的每一列都是单位向量，且两两正交。 - 行列式为 +1：保证是纯旋转，没有镜像或缩放。
为什么强调这个？因为在实际代码里，数值误差会让旋转矩阵慢慢「漂移」，不再满足正交性。我早期做飞控时，就吃过这个亏——飞着飞着姿态就歪了。后来每次更新完姿态，都得做一次正交化修正。
2.2.2 基本旋转矩阵
绕单轴的旋转矩阵是最基础的积木。绕 X 轴转 φ 角：
R_x(φ) = [1 0 0 ]
[0 cosφ -sinφ ]
[0 sinφ cosφ ]绕 Y 轴转 θ 角：
R_y(θ) = [ cosθ 0 sinθ ]
[ 0 1 0 ]
[-sinθ 0 cosθ ]绕 Z 轴转 ψ 角：
R_z(ψ) = [cosψ -sinψ 0 ]
[sinψ cosψ 0 ]
[ 0 0 1 ]注意符号约定：不同教材对旋转方向的定义可能相反。我建议你统一使用「右手定则」——大拇指指向旋转轴正方向，四指弯曲方向为正角度。整个项目保持一致，否则调试时会疯掉。
2.2.3 复合旋转
多个旋转组合起来，就是矩阵乘法。但顺序很重要！
假设我们先绕 Z 轴转 ψ，再绕 Y 轴转 θ，最后绕 X 轴转 φ。那么总的旋转矩阵是：
R = R_x(φ) · R_y(θ) · R_z(ψ)注意：矩阵乘法不满足交换律。先转 X 再转 Y，和先转 Y 再转 X，结果完全不同。我在调试飞控时，经常看到有人把乘法的左右顺序搞反，结果姿态解算出来的角度全是错的。
2.3 欧拉角：直观但危险的表达方式
旋转矩阵虽然数学上完美，但 9 个数字太不直观了。工程师喜欢用欧拉角——三个角度，一看就懂。
2.3.1 定义
欧拉角用三个依次的旋转来描述姿态。最常用的飞控约定是：
- 偏航角 ψ (Yaw)：绕 Z 轴旋转，范围 [-π, π]
- 俯仰角 θ (Pitch)：绕 Y 轴旋转，范围 [-π/2, π/2]
- 横滚角 φ (Roll)：绕 X 轴旋转，范围 [-π, π]
这个顺序叫 Z-Y-X 内旋。说白了，就是先看机头朝哪，再看抬头低头，最后看左右倾斜。
2.3.2 欧拉角与旋转矩阵的转换
从欧拉角到旋转矩阵：
R = R_z(ψ) · R_y(θ) · R_x(φ)从旋转矩阵到欧拉角，需要小心处理奇异情况：
φ = atan2(R[2][1], R[2][2])
θ = -asin(R[2][0])
ψ = atan2(R[1][0], R[0][0])经验之谈：在代码里用 atan2 而不是 atan。atan2 能自动处理象限判断，避免角度跳变。我见过有人用 atan 写姿态解算，结果在 90° 附近角度直接飞了。
2.4 万向锁问题：欧拉角的致命缺陷
好了，现在要讲一个所有飞控工程师都绕不开的坑——万向锁。
2.4.1 什么是万向锁？
当俯仰角 θ = ±90° 时，横滚和偏航的旋转轴会重合。此时，我们丢失了一个自由度。明明有三个角度，却只能控制两个方向的旋转。
为什么会这样？你想想看，当飞机机头垂直朝上时，你再怎么转横滚，其实都是在改变偏航。两个轴「锁」在一起了。
2.4.2 万向锁带来的问题
- 姿态解算奇异：从旋转矩阵反算欧拉角时，会出现除零。
- 控制失效：你给横滚通道发指令，飞机却在偏航方向转动。
- 内插异常：在两个姿态之间做线性插值时，路径会绕大圈。
我曾经踩过的坑：有一次做全姿态飞行演示，飞机做筋斗动作。俯仰拉到 90° 的瞬间，姿态解算直接发散，飞机失控炸机。后来查日志，就是万向锁导致的数值奇异。从那以后，我的飞控里只要涉及全姿态机动，一律用四元数，绝不用欧拉角做内部计算。
2.4.3 如何避免万向锁？
答案很简单：不要在内部计算中使用欧拉角。
欧拉角只适合做两件事：
- 给用户看（人机交互界面显示姿态）
- 作为遥控器输入（期望姿态的设定值）
在飞控的核心姿态解算和控制律里，请使用旋转矩阵或四元数。下一章我们会详细讲四元数，它没有奇点，计算效率也更高。
2.5 本章知识体系
下面这张图，把这一章的核心逻辑串起来了。我建议你保存下来，每次写代码前看一眼。
2.6 本章小结
这一章我们干了四件事：
- 搞清楚了坐标系和向量是描述姿态的「尺子」
- 学会了旋转矩阵这个「翻译官」怎么用
- 理解了欧拉角的直观定义，以及它和旋转矩阵的转换
- 最重要的是，认识了万向锁这个坑——以及为什么不能在内部计算里用欧拉角
一句话记住本章：姿态控制的核心是坐标系映射，旋转矩阵是数学上正确的工具，欧拉角只适合给人看。