1. 无人机坐标系与运动学基础

做无人机控制这么多年，我最大的体会是——坐标系搞不清楚，后面全是坑。你想想看，飞控里算出来的角度、位置、速度，本质上都是在不同坐标系之间来回倒腾。今天我们就从最基础的坐标系说起。

1.1 地球坐标系

地球坐标系，也叫导航坐标系。说白了，就是给无人机一个「绝对位置」的参考。

我个人习惯用北-东-地（NED）坐标系：

• X轴：指向正北
• Y轴：指向正东
• Z轴：指向地心（向下）

为什么Z轴向下？嗯，这其实是个约定。我记得第一次做飞控时，看到Z轴向下总觉得别扭，后来习惯了反而觉得合理——因为重力加速度在Z轴上是正的，算起来方便。

1.2 机体坐标系

机体坐标系是固定在无人机身上的。想象你坐在飞机里，你的前后左右就是机体坐标系。

标准定义是这样的：

• X轴（Roll轴）：指向机头方向
• Y轴（Pitch轴）：指向机身右侧
• Z轴（Yaw轴）：指向机身下方

这里有个容易搞混的地方——机体坐标系的Z轴也是向下的。我曾经见过一个新手把Z轴设成向上，结果姿态解算全反了，飞机一解锁就往地上怼。嗯，血的教训。

1.3 欧拉角

欧拉角描述的是「机体坐标系相对于地球坐标系」的旋转。三个角度：

角度	符号	范围	说明
横滚角	φ (phi)	-180° ~ 180°	绕X轴旋转
俯仰角	θ (theta)	-90° ~ 90°	绕Y轴旋转
偏航角	ψ (psi)	-180° ~ 180°	绕Z轴旋转

旋转顺序是 Z → Y → X，也就是先偏航、再俯仰、最后横滚。为什么是这个顺序？你想想看，如果先横滚再偏航，那横滚后的「偏航」就不是绕地球的Z轴转了，而是绕机体自己的Z轴转——这就乱套了。

1.4 四元数

四元数是个数学工具，用来避免万向锁。它用四个数表示旋转：

q = [q0, q1, q2, q3]

其中：
q0 = cos(θ/2)
q1 = sin(θ/2) * cos(α)
q2 = sin(θ/2) * cos(β)
q3 = sin(θ/2) * cos(γ)

说白了，四元数就是「绕一个空间轴旋转θ角度」。α、β、γ是这个空间轴的方向余弦。

我刚开始学四元数时也觉得抽象，后来发现一个窍门——把它当成「带约束的复数」。复数绕平面转，四元数绕空间转，就这么简单。

1.5 旋转矩阵推导

旋转矩阵是连接地球坐标系和机体坐标系的桥梁。我们从三个基本旋转开始：

绕Z轴旋转（偏航）：

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

绕Y轴旋转（俯仰）：

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

绕X轴旋转（横滚）：

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

完整的旋转矩阵就是这三个矩阵的乘积，顺序是 Z → Y → X：

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θ            ]

这个矩阵的作用是：把机体坐标系下的向量转换到地球坐标系。反过来，它的转置矩阵就是把地球坐标系转到机体坐标系。

1.6 从四元数到旋转矩阵

四元数到旋转矩阵的转换公式：

R = [1-2(q2²+q3²)  2(q1q2-q0q3)    2(q1q3+q0q2)  ]
    [2(q1q2+q0q3)  1-2(q1²+q3²)    2(q2q3-q0q1)  ]
    [2(q1q3-q0q2)  2(q2q3+q0q1)    1-2(q1²+q2²)  ]

这个公式的好处是：没有三角函数，全是乘法和加法，计算效率高。而且不会出现万向锁。

我个人习惯在飞控里全程用四元数，只在最后输出给地面站时才转成欧拉角。这样既保证了计算稳定性，又方便人看。

1.7 运动学方程

有了坐标系和旋转矩阵，我们就可以描述无人机的运动了。

位置运动学：

P_dot = R * V_b

其中P_dot是地球坐标系下的速度，V_b是机体坐标系下的速度，R是旋转矩阵。

姿态运动学：

q_dot = 0.5 * q ⊗ ω_b

其中ω_b是机体角速度，⊗表示四元数乘法。

这两个方程是飞控算法的核心。你想想看，GPS给的是地球坐标系下的位置，IMU给的是机体坐标系下的加速度和角速度，怎么把它们融合起来？就是靠这些方程。

下一章我们会讲IMU的测量模型和姿态解算。到时候你会发现，今天这些坐标系和旋转的知识，就是整个飞控算法的地基。地基打不牢，楼盖得再高也得塌。