四旋翼无人机双闭环PID控制与动力学建模详解

1. 四旋翼无人机控制系统的核心挑战

四旋翼无人机（UAV）的飞行控制本质上是一个六自由度（6-DoF）的强耦合非线性系统控制问题。我在2018年参与农业植保无人机项目时，曾亲眼目睹过由于姿态控制精度不足导致的药液喷洒不均匀问题——当时飞行器在转弯时出现了明显的横滚振荡，导致农田边缘区域出现重复喷洒。这个案例让我深刻认识到：要实现精准的飞行控制，必须同时解决位置跟踪和姿态稳定两个层面的控制问题。

传统单级PID控制在处理四旋翼控制时存在明显局限。当无人机在3D空间中进行轨迹跟踪时，位置控制环的输出（期望姿态）与姿态控制环的实际响应之间会产生动态耦合。举个具体例子：假设无人机需要从点A直线飞往点B，此时位置环计算出需要10度俯仰角才能产生足够的向前推力。但如果姿态环响应滞后或超调，实际可能产生15度俯仰，这会导致无人机加速过快，进而触发位置环的逆向调节，最终形成振荡。

2. 系统动力学建模的关键细节

2.1 机体坐标系与欧拉角参数化

在North-East-Down（NED）地面坐标系下，我们定义机体坐标系的x轴指向无人机前方（通常以电机1和电机3的连线方向为前方），z轴垂直向下。欧拉角变换采用Z-Y-X顺序，即先偏航（ψ）、再俯仰（θ）、最后横滚（φ）。这种选择在实践中被广泛采用，因为：

• 避免了万向节锁死问题（当俯仰角为±90°时）
• 与大多数飞控传感器的输出约定一致
• 便于分离偏航控制与其他旋转自由度

旋转矩阵R的具体形式为：

matlab复制R = [cosθ*cosψ, sinφ*sinθ*cosψ-cosφ*sinψ, cosφ*sinθ*cosψ+sinφ*sinψ;
     cosθ*sinψ, sinφ*sinθ*sinψ+cosφ*cosψ, cosφ*sinθ*sinψ-sinφ*cosψ;
     -sinθ,     sinφ*cosθ,                  cosφ*cosθ];

2.2 刚体动力学方程推导

考虑质量m和惯性矩阵I=diag([Ixx,Iyy,Izz])的刚体模型，牛顿-欧拉方程可表示为：

平移动力学：

math复制m\ddot{\xi} = R F^b + F_{ext}

其中ξ=[x,y,z]^T是位置向量，F^b=[0,0,T