2、坐标系与运动学：地球坐标系、机体坐标系、欧拉角与四元数、运动学方程推导
各位同学，欢迎来到第二章。这一章是后面所有控制算法的基础，说白了就是搞清楚「无人机在哪儿、朝哪儿飞、怎么描述它的姿态」。我当年刚入行时，觉得坐标系这东西太简单了，结果第一次调飞控就栽了大跟头——飞机在天上乱转，日志里数据全对，就是飞不稳。后来才发现，是坐标系定义搞混了。嗯，咱们今天就把这个坑填上。
2.1 地球坐标系与机体坐标系
先说说两个最常用的坐标系。
地球坐标系，也叫导航坐标系或惯性坐标系（NED系）。我习惯用NED：北-东-地。X轴指向北，Y轴指向东，Z轴指向地心。这个坐标系是固定的，我们用它来描述无人机的位置和期望轨迹。
机体坐标系，固定在无人机上。原点在重心，X轴指向机头，Y轴指向右侧，Z轴指向下方（符合右手定则）。传感器（IMU、磁力计）测量的数据，默认都是机体坐标系下的。
为什么要区分这两个坐标系？
- GPS给的是地球坐标系下的位置
- 加速度计测的是机体坐标系下的加速度
- 控制指令（比如「往北飞」）是在地球坐标系下给出的
所以，你得学会在两者之间来回切换。我在项目里见过有人直接把机体加速度当成地球加速度去积分，结果位置估计直接飘到天上去。避坑指南：任何传感器数据，先确认坐标系，再使用。
2.2 欧拉角：直观但危险
欧拉角是最直观的姿态表示法。三个角度：滚转角（φ）、俯仰角（θ）、偏航角（ψ）。旋转顺序我习惯用Z-Y-X（偏航→俯仰→滚转），这也是航空领域的标准。
欧拉角的好处是直观，看一眼就知道飞机歪了多少。但坏处也很明显——万向锁。当俯仰角接近±90°时，滚转和偏航会耦合，丢失一个自由度。你想想看，这时候你给滚转指令，飞机可能偏航了，控制逻辑直接乱套。
我曾经在测试特技飞行时遇到过这个问题。飞机翻到90°俯仰，姿态解算直接炸了，角度跳变，飞控紧急切回手动模式才救回来。所以，如果你做全姿态飞行（比如穿越机、特技无人机），别用欧拉角做内部姿态表示。
2.3 四元数：优雅的数学工具
四元数是什么？说白了就是一个四维复数：q = w + xi + yj + zk，其中w是实部，x、y、z是虚部。它用来表示三维空间中的旋转，没有万向锁问题，而且计算效率高。
我个人建议：飞控内部所有姿态运算，都用四元数。欧拉角只在最后输出给地面站时转换一下。
四元数的几个关键操作：
- 归一化：四元数必须保持单位长度，否则旋转会变形。每次更新后都要归一化。
- 乘法：q1 * q2 表示先做q2旋转，再做q1旋转。注意顺序不可交换。
- 旋转向量：用四元数旋转一个三维向量 v：v' = q * v * q-1
代码示例：四元数归一化（C语言风格）
void quaternion_normalize(float q[4]) {
float norm = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
if (norm < 1e-10f) {
q[0] = 1.0f; // 单位四元数
q[1] = q[2] = q[3] = 0.0f;
return;
}
float inv_norm = 1.0f / norm;
q[0] *= inv_norm;
q[1] *= inv_norm;
q[2] *= inv_norm;
q[3] *= inv_norm;
}2.4 欧拉角与四元数的转换
实际开发中，你经常需要在两者之间来回转换。比如从姿态解算拿到四元数，但地面站要显示欧拉角。或者遥控器输入的是欧拉角指令，你要转成四元数去更新姿态。
四元数 → 欧拉角（Z-Y-X顺序）
float roll = atan2(2*(q[0]*q[1] + q[2]*q[3]), 1 - 2*(q[1]*q[1] + q[2]*q[2]));
float pitch = asin(2*(q[0]*q[2] - q[3]*q[1]));
float yaw = atan2(2*(q[0]*q[3] + q[1]*q[2]), 1 - 2*(q[2]*q[2] + q[3]*q[3]));欧拉角 → 四元数
float cy = cos(yaw * 0.5f);
float sy = sin(yaw * 0.5f);
float cp = cos(pitch * 0.5f);
float sp = sin(pitch * 0.5f);
float cr = cos(roll * 0.5f);
float sr = sin(roll * 0.5f);
q[0] = cr * cp * cy + sr * sp * sy;
q[1] = sr * cp * cy - cr * sp * sy;
q[2] = cr * sp * cy + sr * cp * sy;
q[3] = cr * cp * sy - sr * sp * cy;2.5 运动学方程推导
运动学方程，说白了就是描述无人机位置和姿态随时间变化的规律。我们不考虑力，只考虑几何关系。
位置运动学
很简单：位置的变化率等于速度。
p_dot = v其中p是地球坐标系下的位置，v是地球坐标系下的速度。注意，如果速度是机体坐标系下测得的，要先旋转到地球坐标系。
姿态运动学
这里稍微复杂一点。姿态的变化率（角速度）和欧拉角导数之间的关系是：
[φ_dot] [1 sinφ*tanθ cosφ*tanθ] [p]
[θ_dot] = [0 cosφ -sinφ ] [q]
[ψ_dot] [0 sinφ/cosθ cosφ/cosθ ] [r]其中p、q、r是机体坐标系下的角速度（陀螺仪测量值）。
你想想看，当θ接近±90°时，tanθ和1/cosθ会趋于无穷大。这就是欧拉角运动学方程在万向锁处失效的原因。
四元数运动学就优雅多了：
q_dot = 0.5 * q * ω其中ω是四元数形式的角速度（虚部为p、q、r，实部为0）。展开成标量形式：
q0_dot = -0.5 * (q1*p + q2*q + q3*r)
q1_dot = 0.5 * (q0*p + q2*r - q3*q)
q2_dot = 0.5 * (q0*q + q3*p - q1*r)
q3_dot = 0.5 * (q0*r + q1*q - q2*p)没有奇异点，没有三角函数，只有乘法和加法。这就是为什么飞控内部都用四元数做姿态更新。
2.6 本章小结
- 地球坐标系：固定，用于导航和位置控制
- 机体坐标系：随飞机运动，传感器数据都在这个系下
- 欧拉角：直观，但有万向锁，适合人机交互和小角度
- 四元数：无奇异，计算高效，飞控内部姿态表示的首选
- 运动学方程：四元数形式没有奇异点，是工程实现的标准方法
下一章我们会讲动力学建模，到时候这些坐标系和姿态表示的知识全都会用上。嗯，打好基础，后面才飞得稳。