2. 坐标系与运动学基础：地球坐标系、机体坐标系、欧拉角与四元数、无人机运动学模型
各位同学，欢迎来到《轨迹规划在无人机航迹中的实践》的第二讲。
今天我们要聊的，是无人机轨迹规划里最基础、也最绕不开的一块硬骨头——坐标系与运动学。说实话，我见过不少新手，算法写得飞起，结果一上真机就炸机。为什么？坐标系搞反了，姿态解算出错了。嗯，这玩意儿要是搞不清楚，后面所有的规划都是空中楼阁。
2.1 地球坐标系：我们到底在哪儿？
做轨迹规划，首先得知道飞机在哪儿。这个「在哪儿」，得有个统一的参考标准。
我个人习惯把地球坐标系分成两类：一类是给「大地测量」用的，另一类是给「导航控制」用的。
2.1.1 地心地固坐标系（ECEF）
这个坐标系的原点在地球质心。Z轴指向北极，X轴指向本初子午线与赤道的交点，Y轴按右手定则确定。说白了，它是一个随着地球自转一起转的坐标系。
我在做长航时无人机项目时，经常用ECEF来做GPS数据的预处理。因为GPS给出的经纬高（LLA）数据，最终都要转成ECEF下的笛卡尔坐标，才好做后续的路径插值。
2.1.2 北东地坐标系（NED）
这个更常用。原点在无人机起飞点或某个参考点。X轴指向北，Y轴指向东，Z轴指向地心（也就是向下）。
你想想看，我们做轨迹规划时，关心的往往是「相对于起飞点往北飞了多远，往东飞了多远，高度变化了多少」。NED坐标系正好满足这个需求。
2.2 机体坐标系：飞机自己的小世界
地球坐标系是「上帝视角」，而机体坐标系是「飞机视角」。它的原点在飞机质心，X轴指向机头，Y轴指向右翼，Z轴指向下（符合右手定则）。
为什么要搞个机体坐标系？因为传感器（IMU、磁力计）测量的数据，都是相对于飞机本身的。比如加速度计测的是「飞机在X轴方向上的加速度」，而不是「地球北向的加速度」。
这里有个坑，我曾经踩过：
2.3 欧拉角与四元数：姿态的两种「语言」
好了，现在有了两个坐标系，怎么把一个坐标系下的向量转到另一个坐标系下？这就涉及到姿态表示了。
2.3.1 欧拉角
欧拉角用三个角度来描述姿态：横滚角（Roll）、俯仰角（Pitch）、偏航角（Yaw）。
旋转顺序一般是：先偏航（绕Z轴），再俯仰（绕Y轴），最后横滚（绕X轴）。这个顺序很重要，搞反了姿态就全乱了。
| 角度 | 符号 | 范围 | 物理意义 |
|---|---|---|---|
| 横滚角 | φ | -π ~ π | 绕机头轴线旋转 |
| 俯仰角 | θ | -π/2 ~ π/2 | 绕右翼轴线旋转 |
| 偏航角 | ψ | -π ~ π | 绕垂直轴线旋转 |
欧拉角直观，但有个致命问题——万向锁。当俯仰角接近±90°时，横滚和偏航会耦合，丢失一个自由度。我在做特技飞行轨迹规划时，就因为这个吃过亏，飞机在做大角度俯仰时姿态解算直接崩了。
2.3.2 四元数
四元数用四个数来表示旋转：q = [w, x, y, z]。它没有万向锁问题，计算效率也高。
我建议所有做轨迹规划的同学，都尽量用四元数做姿态插值和旋转。虽然它不如欧拉角直观，但胜在稳定。
四元数转旋转矩阵的公式（C++代码示例）：
// 四元数转旋转矩阵
Eigen::Matrix3d quatToRotMat(double w, double x, double y, double z) {
Eigen::Matrix3d R;
R(0,0) = 1 - 2*y*y - 2*z*z; R(0,1) = 2*x*y - 2*w*z; R(0,2) = 2*x*z + 2*w*y;
R(1,0) = 2*x*y + 2*w*z; R(1,1) = 1 - 2*x*x - 2*z*z; R(1,2) = 2*y*z - 2*w*x;
R(2,0) = 2*x*z - 2*w*y; R(2,1) = 2*y*z + 2*w*x; R(2,2) = 1 - 2*x*x - 2*y*y;
return R;
}2.4 无人机运动学模型：飞机到底怎么动？
有了坐标系和姿态，我们终于可以聊聊运动学了。说白了，运动学模型就是描述「位置、速度、姿态」之间关系的数学方程。
2.4.1 位置运动学
在NED坐标系下，位置的变化率就是速度：
p_dot = v其中p是位置向量 [x, y, z]，v是速度向量 [vx, vy, vz]。
2.4.2 姿态运动学
姿态的变化率（角速度）与欧拉角速率之间的关系，是很多人的噩梦。这里我直接给出简化形式（小角度假设下）：
phi_dot = p + q * sin(phi) * tan(theta) + r * cos(phi) * tan(theta)
theta_dot = q * cos(phi) - r * sin(phi)
psi_dot = q * sin(phi) / cos(theta) + r * cos(phi) / cos(theta)其中p、q、r是机体坐标系下的角速度。
嗯，看着复杂对吧？实际工程中，我们很少直接用这个公式做轨迹规划。更常见的做法是：
- 用四元数微分方程来更新姿态
- 把角速度作为控制输入，直接规划角速度轨迹
2.4.3 完整的运动学模型
对于四旋翼无人机，我们常用的运动学模型是：
状态量: [x, y, z, vx, vy, vz, qw, qx, qy, qz]
控制量: [推力, 滚转角速度, 俯仰角速度, 偏航角速度]这个模型有10个状态量，4个控制量。在做轨迹规划时，我们通常会对这个模型做简化，比如假设姿态响应足够快，直接用加速度作为控制输入。
2.5 本章知识体系
为了让大家更直观地理解本章的知识结构，我画了一张图：
这张图把本章的核心逻辑串起来了：从坐标系定义，到姿态表示，再到运动学模型，最后落到工程实践。你想想看，每一步都是环环相扣的。
好了，这一章的内容就到这里。坐标系和运动学是后面所有章节的基础，一定要吃透。下一章我们会聊轨迹的数学表示，到时候见。
公众号:蓝海资料掘金营，微信deep3321