第1章：刚体运动学基础——坐标系与姿态参数化

各位同学，欢迎来到《高动态姿态跟踪控制实战》。我是你们的老朋友，一个在飞控领域摸爬滚打了十几年的工程师。

今天咱们聊点最基础，但也最核心的东西——刚体运动学。说白了，就是搞清楚飞行器在空间里到底是怎么转的。你想想看，如果连它怎么转的都描述不清楚，那后面的控制算法就全是空中楼阁了。

我个人习惯，每次接手一个新项目，第一件事就是把坐标系和姿态表示方法理清楚。这一步省了，后面调试的时候准得哭。好，咱们开始。

1.1 坐标系定义：惯性系与体坐标系

描述运动，首先得有个参考。就像你说“我往前走了两步”，这个“前”是相对于谁？

惯性坐标系（Inertial Frame），我习惯用 {I} 表示。它是个静止不动的参考系，或者说是匀速直线运动的。在飞控里，我们通常用北-东-地（NED）坐标系。原点在地面某点，X轴指向北，Y轴指向东，Z轴指向地心。嗯，这里要注意，NED是右手系。

体坐标系（Body Frame），用 {B} 表示。它固连在飞行器上。原点在飞行器质心，X轴指向机头（前进方向），Y轴指向右翼，Z轴指向机腹。这也是右手系。

为什么要分这两个系？因为物理规律（比如牛顿第二定律）只在惯性系下成立，而传感器（比如IMU）测量的却是体坐标系下的数据。所以，我们得在两者之间来回切换。

1.2 欧拉角与旋转矩阵

欧拉角，大家应该不陌生。就是用三个角度来描述旋转：滚转角（Roll, φ）、俯仰角（Pitch, θ）、偏航角（Yaw, ψ）。

旋转顺序很重要。我见过太多新手因为顺序搞错，导致仿真结果一塌糊涂。在航空航天领域，最常用的是 Z-Y-X 顺序（先偏航，再俯仰，最后滚转）。

从体坐标系到惯性系的旋转矩阵 R 可以这样写：

R = Rz(ψ) * Ry(θ) * Rx(φ)

其中每个矩阵都是绕单轴旋转的标准形式。展开后是这样的：

R = | cosθ cosψ   sinφ sinθ cosψ - cosφ sinψ   cosφ sinθ cosψ + sinφ sinψ |
    | cosθ sinψ   sinφ sinθ sinψ + cosφ cosψ   cosφ sinθ sinψ - sinφ cosψ |
    | -sinθ       sinφ cosθ                     cosφ cosθ                |

这个矩阵可以把体坐标系下的向量 v_B 转换到惯性系：v_I = R * v_B。

欧拉角有个著名的缺陷——万向锁（Gimbal Lock）。当俯仰角接近 ±90° 时，滚转和偏航的旋转轴会重合，导致丢失一个自由度。这在全姿态机动中是致命的。

1.3 四元数基础

为了避开万向锁，我们引入了四元数。四元数可以理解为一种“超复数”，它用四个数来表示旋转：

q = [q0, q1, q2, q3]^T

其中 q0 是标量部分，[q1, q2, q3] 是矢量部分。它满足归一化条件：q0² + q1² + q2² + q3² = 1。

四元数最大的好处是：无奇异点，且计算效率高。它只需要四个参数，而旋转矩阵需要九个。在嵌入式系统里，这能省下不少计算资源。

四元数的乘法（表示两次旋转的复合）也很简洁：

q = q1 ⊗ q2

具体展开我就不写了，大家记住这个符号就行。实际编程时，很多库都提供了现成的乘法函数。

1.4 姿态参数化对比

好了，咱们来做个对比。我把三种常用姿态表示方法的优缺点列出来：

表示方法	参数数量	优点	缺点
旋转矩阵	9	无奇异点，易于向量旋转	冗余大，需正交化约束
欧拉角	3	直观，物理意义明确	有万向锁，计算慢（含三角函数）
四元数	4	无奇异点，计算快，易插值	不够直观，需归一化

我个人习惯：内部计算用四元数，人机交互用欧拉角，数学推导用旋转矩阵。三者之间可以互相转换，代码里准备好转换函数就行。

下面我用一张图来总结本章的知识体系：

嗯，这张图把本章的核心逻辑串起来了。从坐标系出发，到三种姿态表示方法，再到它们之间的转换关系。你把它记在脑子里，后面的章节就会顺畅很多。