2. 多旋翼飞行器运动学建模

多旋翼的位置与姿态控制，本质上是在两个坐标系之间来回换算：一个是固定在地面上的惯性坐标系（也称世界系、导航系），另一个是固连在机体上的机体系。控制器读到的传感器数据、输出的电机指令，最终都要落到这两个坐标系的关系上。如果坐标系定义、旋转方向、欧拉角顺序没有统一，后面的姿态解算和位置控制一定会出现符号错误或耦合异常。因此本节先把“怎么定义、怎么变换、怎么建立运动学方程”讲清楚。

一、坐标系定义与约定

采用右手坐标系，并统一约定如下：

• 惯性系 O_eX_eY_eZ_e：原点取起飞点，X_e 指向东，Y_e 指向北，Z_e 垂直向上（ENU，东北天）。重力矢量在惯性系中为 [0, 0, −g]^T。
• 机体系 O_bX_bY_bZ_b：原点取机体几何中心（重心），X_b 指向机头，Y_b 指向机体右侧，Z_b 沿机体垂直向上。
• 姿态角：滚转角 φ 绕 X_b，俯仰角 θ 绕 Y_b，偏航角 ψ 绕 Z_b。正方向均按右手定则，即从轴的正向看去逆时针为正。

这套约定与 PX4、ArduPilot 常用的 ENU/FRD 体系在符号上略有差异，但只要全文一致，推导结果就自洽。后续所有公式均以本节约定为准。

二、旋转矩阵与坐标变换

绕单轴的旋转矩阵为：

R_x(φ) = [1, 0, 0; 0, cosφ, -sinφ; 0, sinφ, cosφ]
R_y(θ) = [cosθ, 0, sinθ; 0, 1, 0; -sinθ, 0, cosθ]
R_z(ψ) = [cosψ, -sinψ, 0; sinψ, cosψ, 0; 0, 0, 1]

从机体系到惯性系的旋转矩阵按 Z–Y–X 顺序复合：

R_be = R_z(ψ) · R_y(θ) · R_x(φ)

展开后可得：

R_be =
[ cψcθ,  cψsθsφ - sψcφ,  cψsθcφ + sψsφ ]
[ sψcθ,  sψsθsφ + cψcφ,  sψsθcφ - cψsφ ]
[ -sθ,   cθsφ,           cθcφ          ]

其中 c 表示 cos，s 表示 sin。该矩阵是正交矩阵，满足 R^TR = I，因此从惯性系到机体系的变换就是 R_eb = R_be^T。任意矢量 v 在两个坐标系间的换算为：

v_e = R_be · v_b
v_b = R_be^T · v_e

例如机体系下测得加速度 a_b = [0.2, 0.1, 9.7]^T m/s²，若当前 φ=5°、θ=3°、ψ=30°，代入 R_be 即可得到惯性系下的加速度分量，用于位置估计。

三、欧拉角与万向节死锁

欧拉角直观、便于人机交互，但存在两个问题：一是万向节死锁，当 θ = ±90° 时，φ 与 ψ 的旋转轴重合，姿态自由度退化；二是三角函数运算量大。因此欧拉角适合作为显示和输入接口，不适合作为内部姿态积分的主状态量。

欧拉角变化率与机体角速度 p、q、r 的关系为：

[φ̇]   [1, sinφ·tanθ, cosφ·tanθ] [p]
[θ̇] = [0, cosφ,      -sinφ     ] [q]
[ψ̇]   [0, sinφ/cosθ, cosφ/cosθ] [r]

可见当 θ 接近 ±90° 时 tanθ 与 1/cosθ 发散，这正是死锁在数学上的体现。

四、四元数姿态表示

四元数用四个参数 q = [q₀, q₁, q₂, q₃]^T 表示姿态，其中 q₀ 为标量部分，[q₁, q₂, q₃] 为矢量部分，满足单位约束 q₀² + q₁² + q₂² + q₃² = 1。其优势是无死锁、运算量小、便于插值。

由欧拉角构造四元数（Z–Y–X 顺序）：

q0 = c(φ/2)c(θ/2)c(ψ/2) + s(φ/2)s(θ/2)s(ψ/2)
q1 = s(φ/2)c(θ/2)c(ψ/2) - c(φ/2)s(θ/2)s(ψ/2)
q2 = c(φ/2)s(θ/2)c(ψ/2) + s(φ/2)c(θ/2)s(ψ/2)
q3 = c(φ/2)c(θ/2)s(ψ/2) - s(φ/2)s(θ/2)c(ψ/2)

四元数微分方程为：

q̇ = 0.5 · q ⊗ [0, p, q, r]^T

其中 ⊗ 表示四元数乘法。数值积分时通常采用一阶欧拉法或四阶龙格–库塔法，并在每步后重新归一化，抑制漂移。

五、位置与姿态运动学方程

设惯性系下位置为 P = [x, y, z]^T，速度为 V = [v_x, v_y, v_z]^T，机体系下速度为 V_b，则位置运动学为：

Ṗ = V = R_be · V_b

姿态运动学用四元数表示为：

q̇ = 0.5 · q ⊗ ω_b

其中 ω_b = [0, p, q, r]^T。这两组方程共同构成多旋翼的六自由度运动学模型：位置由速度积分得到，姿态由角速度积分得到，而速度与角速度来自动力学与控制器输出。运动学只描述几何关系，不涉及力和力矩，因此它是动力学模型的前置基础。

六、建模实践要点

在实际飞控代码中，姿态解算通常以四元数为内部状态，以欧拉角作为对外接口。一个典型的实现流程是：读取陀螺仪角速度 (p, q, r)，用四元数微分方程积分更新 q，归一化后转换为旋转矩阵 R_be，再用于将机体系加速度转换到惯性系进行位置积分。若需要显示姿态角，再由 q 反解 φ、θ、ψ。

需要注意的工程细节包括：

• 符号一致性：陀螺仪安装方向、旋转矩阵定义、欧拉角正方向必须与本文约定一致，否则会出现“滚转反向”或“偏航耦合”现象。
• 归一化频率：四元数积分每步都应归一化，否则数值误差累积会导致旋转矩阵不再正交。
• 死锁规避：当 θ 接近 ±90° 时，避免直接使用欧拉角微分方程，改用四元数或旋转矩阵更新。
• 单位统一：角速度用 rad/s，角度用 rad 参与运算，仅在显示时转换为度。

掌握以上坐标系定义、旋转矩阵、欧拉角与四元数的关系，以及位置与姿态运动学方程，就为后续的动力学建模、姿态控制器设计和位置控制器设计打下了统一的数学基础。下一阶段将在此基础上引入力与力矩，建立完整的六自由度动力学模型。