📚 课程目录第4章姿态动力学与运动学模型

4、姿态动力学与运动学模型：刚体转动方程、角速度与姿态变化率的关系、简化模型假设

无人机姿态控制的本质，是对机体角运动状态进行精确的估计与操控。要理解控制律设计，首先必须建立描述机体旋转运动的数学模型。本章从刚体转动方程出发，推导角速度与姿态变化率之间的运动学关系，并讨论工程中常用的简化假设。这些内容构成了飞控算法中姿态解算与控制律设计的物理基础。

上图展示了姿态控制模型的两大支柱：左侧为描述力矩与角速度关系的动力学方程，右侧为描述角速度与姿态角变化率的运动学方程。两者通过角速度向量 ω 相互耦合，共同构成飞控内环（角速度环）和外环（姿态角环）的数学模型。下面分别展开推导与工程应用。

核心原理：无人机在飞行中可视为刚体，其旋转运动遵循欧拉刚体转动方程。在机体坐标系（Body Frame）下，角动量定理可写为：

M = dH/dt = J·ω̇ + ω × (J·ω)

其中，M 为作用于机体的合力矩（N·m），J 为 3×3 惯性张量，ω = [p, q, r]ᵀ 为机体角速度（rad/s），ω̇ 为角加速度。展开后得到三个轴的标量方程：

Mx = Ixx·ṗ + (Izz - Iyy)·q·r
My = Iyy·q̇ + (Ixx - Izz)·p·r
Mz = Izz·ṙ + (Iyy - Ixx)·p·q

对于四旋翼等对称布局，通常假设惯性张量为对角阵，即 J = diag(Ixx, Iyy, Izz)，且 Ixx ≈ Iyy（对称布局）。此时方程简化为上述形式。力矩 M 来源于螺旋桨的拉力差和反扭矩差，是控制输入的直接体现。

符号	物理含义	单位	典型值（小型四旋翼）
Ixx, Iyy	横滚/俯仰转动惯量	kg·m²	0.005 - 0.02
Izz	偏航转动惯量	kg·m²	0.008 - 0.03
p, q, r	机体轴角速度	rad/s	±2 - ±5（机动时）
Mx, My, Mz	三轴力矩	N·m	0.1 - 1.0

判断方法：在飞控代码中，角速度环控制器输出的就是期望力矩 M，然后通过混控矩阵映射到四个电机的拉力指令。因此，转动方程是混控器设计的理论依据。若忽略陀螺耦合项（ω × Jω），则三轴完全解耦，可独立设计 PID 控制器。

核心原理：姿态运动学描述的是机体角速度 ω 与欧拉角变化率 Θ̇ = [φ̇, θ̇, ψ̇]ᵀ 之间的映射关系。由于欧拉角定义在惯性系（或地面系），而角速度定义在机体系，两者通过旋转矩阵关联：

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

该矩阵记为 W(Θ)，其逆矩阵用于从欧拉角变化率反算角速度。注意：当 θ = ±90° 时，tanθ 和 1/cosθ 趋向无穷，产生万向节锁（Gimbal Lock）问题，因此该表示法不适合全姿态飞行。

操作流程：在姿态解算中，通常先用陀螺仪积分得到角速度，再通过运动学方程更新欧拉角。但实际飞控多采用四元数避免奇异性，欧拉角仅用于显示和控制外环。若使用欧拉角直接积分，需注意以下步骤：

风险提示：当俯仰角接近 ±90° 时，运动学方程数值不稳定，导致姿态发散。工程上应限制欧拉角范围（如 ±45°），或改用四元数表示。

核心原理：在悬停或小角度机动条件下，可对运动学和动力学进行线性化。假设 φ、θ 较小（通常 < 15°），则 sinφ≈φ，cosφ≈1，tanφ≈φ，运动学矩阵简化为单位阵：

[φ̇]   [1  0  0] [p]       φ̇ ≈ p
[θ̇] ≈ [0  1  0] [q]   →   θ̇ ≈ q
[ψ̇]   [0  0  1] [r]       ψ̇ ≈ r

动力学方程中，陀螺耦合项 q·r、p·r、p·q 为二阶小量，可忽略。于是三轴完全解耦，每个轴可独立建模为：

Ixx·φ̈ = Mx   （横滚通道）
Iyy·θ̈ = My   （俯仰通道）
Izz·ψ̈ = Mz   （偏航通道）

这就是经典的二阶积分模型，是 PID 控制器设计的基础。实际飞控中，角速度环采用 PI 控制器，姿态环采用 P 控制器，正是基于该简化模型。

判断方法：在调参时，若发现横滚和俯仰通道相互干扰（如打俯仰舵时横滚漂移），说明耦合项不可忽略。此时应检查飞行器是否处于大角度或高速旋转状态，并考虑增加解耦补偿项。

场景：某四旋翼无人机在悬停测试中出现低频振荡（约 2 Hz），飞手反馈“机身前后点头，伴随轻微左右摇晃”。通过日志分析，发现俯仰角 θ 和横滚角 φ 均出现 ±5° 的等幅振荡，且相位差约 90°。

分析过程：

读取飞行日志：角速度 p、q 振荡频率一致，幅值约 ±20°/s。
检查模型：悬停时 θ≈0，满足小角度假设，但振荡幅值 5° 已接近线性化边界。
计算耦合项：q·r 和 p·r 的乘积项幅值约为 20×20×(π/180)² ≈ 0.12 rad²/s²，与 Ixx·ṗ 的典型值（0.01×20×(π/180) ≈ 0.0035）相比不可忽略。
结论：陀螺耦合项激发了横滚-俯仰交叉耦合，导致振荡。

操作步骤：

结果对比：修改后振荡幅值降至 ±1.5°，收敛时间从 3s 缩短至 1.2s。若直接忽略耦合项，仅调低 P 增益，振荡虽减弱但响应变慢，且在大机动时仍会复发。

复盘经验：简化模型适用于控制器设计，但调试时必须验证假设边界。当振荡频率接近陀螺耦合频率（约 1-3 Hz）时，应优先检查耦合补偿，而非盲目调参。

误区一：将运动学矩阵 W(Θ) 视为常数。实际上 W 随姿态变化，在大角度时误差显著。

误区二：忽略惯量积。对于非对称布局（如 X 型四旋翼），Ixy 不为零，会导致横滚-偏航耦合。

误区三：将简化模型用于全飞行包线。在特技飞行或强风扰动下，小角度假设失效，必须使用完整非线性模型。

总结：本章建立的刚体转动方程和运动学方程是飞控算法的基石。工程实践中，应遵循“先简化设计，后逐步补偿”的原则：初始使用解耦线性模型完成基本控制，再根据飞行数据逐步加入耦合补偿和非线性修正。理解模型的适用边界，比记住公式本身更为重要。