四旋翼无人机动力学建模与控制算法实践

1. 四旋翼无人机动力学建模基础

四旋翼无人机作为典型的欠驱动系统，其动力学建模是控制算法设计的基础。刚体动力学模型主要包含位置动力学和姿态动力学两部分，需要分别在惯性坐标系（地球坐标系）和机体坐标系下建立。

1.1 坐标系定义与转换

在无人机建模中，我们主要使用两种坐标系：

1. 惯性坐标系（E-frame）：固定于地面的右手坐标系，通常X轴指向正北，Y轴指向正东，Z轴垂直地面向下
2. 机体坐标系（B-frame）：固定在无人机质心的右手坐标系，X轴指向机头方向，Y轴指向右侧，Z轴垂直向下

坐标系转换通过旋转矩阵实现，采用Z-Y-X欧拉角（偏航ψ、俯仰θ、横滚φ）表示姿态：

code复制R = Rz(ψ) * Ry(θ) * Rx(φ) = 
[cosψcosθ  cosψsinθsinφ-sinψcosφ  cosψsinθcosφ+sinψsinφ;
 sinψcosθ  sinψsinθsinφ+cosψcosφ  sinψsinθcosφ-cosψsinφ;
 -sinθ     cosθsinφ               cosθcosφ              ]

注意：欧拉角存在万向节死锁问题，当俯仰角θ=±90°时会出现奇点。实际工程中可通过四元数或限制俯仰角范围避免。

1.2 动力学方程推导

四旋翼的刚体动力学模型基于牛顿-欧拉方程建立：

位置动力学：

code复制m * d²x/dt² = [0; 0; mg] - R * [0; 0; T]

其中T为四个旋翼产生的总升力，m为无人机质量，g为重力加速度。

姿态动力学：

code复制I * dω/dt + ω × (I * ω) = τ

其中I为惯性矩阵，ω为角速度，τ为控制力矩。

将上述方程展开得到完整的六自由度模型：

code复制ẍ = (cosφsinθcosψ + sinφsinψ) * T/m
ÿ = (cosφsinθsinψ - sinφcosψ) * T/m
z̈ = g - (cosφcosθ) * T/m
φ̈ = θ̇ψ̇(Iyy-Izz)/Ixx + τφ/Ixx
θ̈ = φ̇ψ̇(Izz-Ixx)/Iyy + τθ/Iyy
ψ̈ = φ̇θ̇(Ixx-Iyy)/Izz +