3. 运动学建模：位置、速度、加速度在惯性系与机体系间的转换，旋转矩阵、四元数微分方程
四旋翼飞行器的运动学建模是连接“力与力矩”和“飞行轨迹”的桥梁。与动力学关注“为何运动”不同，运动学只研究“如何描述运动”，即位置、速度、加速度在不同坐标系下的表达与转换关系。对于四旋翼而言，核心问题在于：机载传感器（IMU、GPS）测量值分别定义在机体系（Body Frame）和惯性系（Inertial Frame）中，而飞控算法（如PID、LQR）往往需要在特定坐标系下进行解算。因此，理解旋转矩阵与四元数微分方程，是进行状态估计、姿态解算和轨迹控制的前提。本章将系统梳理坐标系定义、姿态描述方法、微分方程推导以及工程实现中的关键陷阱。
上图展示了本章的核心逻辑框架：惯性系与机体系之间的转换通过旋转矩阵 R_AB 实现，而姿态的更新则依赖四元数微分方程或旋转矩阵微分方程。下面我们将逐一拆解这些数学工具的物理意义与工程实现路径。
3.1 坐标系定义与姿态描述方法
核心原理：四旋翼运动学中，最常用的两个坐标系是地面惯性系（通常取NED：北-东-地）和机体坐标系（FRD：前-右-下）。惯性系为牛顿第二定律提供基准，机体系则直接对应IMU的安装方向。姿态描述有三种主流方法：欧拉角、旋转矩阵和四元数。欧拉角（横滚φ、俯仰θ、偏航ψ）直观但存在万向节锁死问题；旋转矩阵（3×3正交阵）无奇异但参数冗余；四元数（4参数）兼具无奇异和计算高效的优势，是飞控中的主流选择。
| 描述方法 | 参数数量 | 优点 | 缺点 | 典型应用 |
|---|---|---|---|---|
| 欧拉角 | 3 | 物理意义直观 | 万向节锁死、三角函数计算量大 | 遥控器显示、姿态初始化 |
| 旋转矩阵 | 9 | 无奇异、直接用于向量旋转 | 冗余、需正交化修正 | 坐标变换、外环控制 |
| 四元数 | 4 | 无奇异、计算量小、易插值 | 物理意义不直观 | 姿态解算、EKF状态量 |
判断方法：在工程中，选择哪种描述方式取决于应用场景。若仅用于地面站显示姿态，欧拉角足够；若进行惯性导航解算，必须使用四元数或旋转矩阵。值得注意的是，从四元数转换到欧拉角时，需注意象限判断和奇异值处理（如θ接近±90°时）。
3.2 旋转矩阵：向量在不同坐标系间的投影
核心原理：旋转矩阵 R_AB 将机体系中的向量投影到惯性系。其列向量为机体系各轴在惯性系中的方向余弦。例如，若机体系X轴在惯性系中的投影为 [r11, r21, r31]^T，则 R_AB 的第一列即为该向量。旋转矩阵是正交矩阵，满足 R^T = R^{-1}，且 det(R)=1。
操作流程：在实际代码中，通常使用 Z-Y-X 欧拉角顺序构建旋转矩阵：
// 由欧拉角构建旋转矩阵 (Z-Y-X顺序)
R = Rz(yaw) * Ry(pitch) * Rx(roll)
其中：
Rz(yaw) = [cosψ, -sinψ, 0; sinψ, cosψ, 0; 0, 0, 1]
Ry(pitch) = [cosθ, 0, sinθ; 0, 1, 0; -sinθ, 0, cosθ]
Rx(roll) = [1, 0, 0; 0, cosφ, -sinφ; 0, sinφ, cosφ]案例拆解：假设无人机悬停时机头朝北（ψ=0），俯仰角θ=10°（抬头），横滚角φ=0。此时机体系X轴在惯性系中的投影为 [cos10°, 0, -sin10°]^T ≈ [0.985, 0, -0.174]^T。若机体系前向速度为 5 m/s，则惯性系中的北向速度为 4.925 m/s，垂直向下速度为 -0.87 m/s（即上升）。这个例子说明，即使机体水平速度不为零，若存在俯仰角，垂直方向也会产生速度分量，这正是运动学耦合的体现。
3.3 四元数微分方程：姿态更新的核心引擎
核心原理：四元数 q = [q0, q1, q2, q3]^T 描述刚体姿态，其中 q0 为标量部分。其微分方程为：
q̇ = 0.5 * q ⊗ ω_B
其中 ω_B = [0, p, q, r]^T 为机体系角速度的四元数形式，⊗ 表示四元数乘法。展开后得到矩阵形式：
[q̇0] [ 0 -p -q -r ] [q0]
[q̇1] = [ p 0 r -q ] [q1]
[q̇2] [ q -r 0 p ] [q2]
[q̇3] [ r q -p 0 ] [q3]操作流程：在飞控中，陀螺仪以固定频率（如1kHz）输出角速度，通过四阶龙格-库塔法或简单的毕卡积分更新四元数。每次更新后需对四元数做归一化处理，防止累积误差导致模长偏离1。
案例拆解：某四旋翼以角速度 ω_B = [0, 0.2, 0, 0]^T rad/s（即绕机体系X轴以0.2 rad/s滚转）飞行。初始姿态为水平（q=[1,0,0,0]）。经过0.1秒后，采用一阶欧拉积分：q_new = q + q̇ * dt。计算得 q̇ = 0.5 * [0, 0.2, 0, 0] ⊗ [1,0,0,0] = [0, 0.1, 0, 0]。因此 q_new = [1, 0.01, 0, 0]，归一化后约为 [0.99995, 0.01, 0, 0]。对应滚转角约为 2*atan2(0.01, 0.99995) ≈ 0.02 rad，与理论值 0.2*0.1=0.02 rad 一致。若采用更高阶积分，精度会进一步提升。
3.4 位置、速度、加速度的完整转换链
核心原理：在惯性系中，位置 P_A 的导数为速度 V_A，速度的导数为加速度 a_A。而机载加速度计测量的是比力 f_B（单位质量所受的合力减去重力），即 f_B = a_B - g_B。因此，从传感器到导航信息的完整链路为：
- 加速度计输出比力 f_B（机体系）；
- 利用旋转矩阵将比力转换到惯性系：f_A = R_AB · f_B；
- 在惯性系中补偿重力：a_A = f_A + g_A（NED系中 g_A = [0,0,9.81]^T）；
- 对 a_A 积分得到速度 V_A，再积分得到位置 P_A。
判断方法：上述开环积分会因噪声和零偏迅速发散，因此实际系统中必须引入外部测量（GPS、光流、气压计）进行闭环修正。例如，GPS提供位置和速度观测，通过卡尔曼滤波器估计并补偿加速度计零偏。
| 物理量 | 机体系表示 | 惯性系表示 | 转换关系 |
|---|---|---|---|
| 位置 | — | P_A | 通常直接在惯性系中定义 |
| 速度 | V_B | V_A | V_A = R_AB · V_B |
| 加速度 | f_B (比力) | a_A | a_A = R_AB · f_B + g_A |
| 角速度 | ω_B | — | 直接测量，用于姿态更新 |
实战场景：在PX4或ArduPilot飞控中，姿态估计器（如EKF2）内部维护一个四元数状态量，利用陀螺仪进行时间更新，利用加速度计和磁力计进行量测更新。位置估计器则融合GPS、气压计和加速度计数据。理解上述转换链是调试“位置漂移”或“高度漂移”问题的基础。例如，若发现无人机在悬停时高度缓慢下降，首先应检查加速度计Z轴零偏是否过大，以及旋转矩阵是否准确。