4. 坐标系与运动建模

无人飞行器自主起降控制的第一步，不是调参数，而是把“飞机在哪里、朝哪、怎么动”用一套无歧义的数学语言描述清楚。起降阶段尤其敏感：离地几厘米时，地面效应、支腿接触、风切变都会让姿态和高度快速变化，如果坐标系定义混乱或旋转顺序写错，控制器会把“抬头”当成“低头”，把“侧滑”当成“偏航”，后果直接是翻机或硬着陆。本节从工程实现角度，把坐标系、姿态表示和起降运动方程串成一条可落地的建模链。

一、三个坐标系：各司其职，不可混用

工程上通常同时使用三个右手坐标系，它们的原点和轴向定义如下：

• 惯性系（I系，ECI）：原点在地心，Z轴指向北极，X轴指向春分点。它近似惯性空间，用于牛顿第二定律成立的高精度积分，但数值大、不直观，一般不直接用于起降控制。
• 地面系（E系，NED）：原点取在起飞点或着陆点，X轴指北、Y轴指东、Z轴指地。这是起降控制最常用的参考系，高度用 -Z 表示（向上为正时取 Z 向上，本文统一采用 NED，即 Z 向下为正，高度 h = -z）。
• 机体系（B系）：原点在机体中心（或重心），X轴指机头、Y轴指右翼、Z轴指机腹。所有机载传感器（IMU、气压计、GPS 天线相位中心）和力/力矩都定义在机体系。

三者关系可以概括为：地面系是“地图”，机体系是“身体”，惯性系是“物理定律的舞台”。起降控制中，位置、速度、高度在地面系描述；角速度、加速度、气动力在机体系测量；两者通过姿态旋转矩阵连接。

二、姿态角与旋转矩阵：从欧拉到矩阵

姿态角（欧拉角）采用 Z-Y-X 顺序：先绕 Z 轴偏航 ψ，再绕 Y 轴俯仰 θ，最后绕 X 轴滚转 φ。对应的旋转矩阵 R_BE（机体系到地面系）为：

R_BE = Rz(ψ) * Ry(θ) * Rx(φ)

其中：
Rx(φ) = [1, 0, 0; 0, cosφ, -sinφ; 0, sinφ, cosφ]
Ry(θ) = [cosθ, 0, sinθ; 0, 1, 0; -sinθ, 0, cosθ]
Rz(ψ) = [cosψ, -sinψ, 0; sinψ, cosψ, 0; 0, 0, 1]

注意方向约定：R_BE 把机体系向量转到地面系，即 v_E = R_BE * v_B。若要把地面系向量转到机体系，用转置 R_EB = R_BE^T。起降阶段俯仰角 θ 接近 ±90° 时，欧拉角会出现万向节死锁，此时必须切换到四元数或旋转矩阵，不能继续用欧拉角做姿态解算。

三、四元数：避免奇异的姿态表示

四元数 q = [q0, q1, q2, q3]^T，其中 q0 为标量部分，q1~q3 为矢量部分，满足 q0²+q1²+q2²+q3²=1。从欧拉角构造四元数（Z-Y-X 顺序）：

q0 = cos(φ/2)cos(θ/2)cos(ψ/2) + sin(φ/2)sin(θ/2)sin(ψ/2)
q1 = sin(φ/2)cos(θ/2)cos(ψ/2) - cos(φ/2)sin(θ/2)sin(ψ/2)
q2 = cos(φ/2)sin(θ/2)cos(ψ/2) + sin(φ/2)cos(θ/2)sin(ψ/2)
q3 = cos(φ/2)cos(θ/2)sin(ψ/2) - sin(φ/2)sin(θ/2)cos(ψ/2)

四元数更新用一阶龙格-库塔或毕卡法：q_dot = 0.5 * q ⊗ ω_B，其中 ω_B 是机体系角速度。起降阶段采样周期通常取 1~5 ms，四元数归一化每步执行一次，防止数值漂移。姿态解算后，再反解欧拉角用于显示和低层控制：φ = atan2(2(q0q1+q2q3), 1-2(q1²+q2²))，θ = asin(2(q0q2-q3q1))，ψ = atan2(2(q0q3+q1q2), 1-2(q2²+q3²))。

四、起降阶段运动方程：从牛顿到可执行模型

起降阶段运动方程分平动和转动两组。平动在地面系写：m·v_dot_E = R_BE·F_B + m·g_E，其中 g_E = [0, 0, g]^T（NED 下重力沿 Z 正向），F_B 是机体系合外力（旋翼拉力、气动力、地面支反力）。转动在机体系写：J·ω_dot_B + ω_B×(J·ω_B) = M_B，J 为转动惯量矩阵，M_B 为合外力矩。

起降阶段与巡航最大的区别在于地面接触约束。着陆时，支腿与地面接触会产生法向力 N 和摩擦力 f，此时运动方程需增加接触项：m·v_dot_E = R_BE·F_B + m·g_E + F_contact_E。接触力在机体系下可分解为 N_B 和 f_B，再经 R_BE 投影到地面系。判断是否离地，常用条件：N ≤ 0 且高度 h > 0.05 m，或支腿行程传感器读数归零。

一个可执行的起降建模流程如下：

1. 初始化：q = [1,0,0,0]，p_E = [0,0,0]，v_E = [0,0,0]，ω_B = [0,0,0]。
2. 每步读取 IMU 角速度 ω_B 和加速度 a_B，气压计高度 h，GPS 位置 p_E。
3. 四元数更新：q ← q + 0.5·q⊗ω_B·dt，归一化。
4. 由 q 计算 R_BE，将 a_B 投影到地面系：a_E = R_BE·a_B + g_E。
5. 积分得 v_E 和 p_E，高度 h = -p_E(3)。
6. 接触判断：若 h ≤ 0.02 m 且 v_E(3) ≥ 0，置接触标志，加入接触力项。
7. 输出姿态角 φ, θ, ψ 供显示和低层控制。

把坐标系、姿态表示和运动方程统一到同一套约定后，后续的起降控制器设计、状态估计和故障判断才有共同基准。实际工程中，建议在代码里显式命名 R_BE、R_EB、q_BE，并在注释中写明旋转顺序和正方向，避免团队协作时出现“同名不同义”的隐患。