四旋翼无人机MPC控制：从建模到轨迹跟踪实战

1. 四旋翼控制：从数学建模到轨迹跟踪实战

四旋翼无人机的控制问题就像教一个刚学会走路的孩子跳芭蕾——既要保持平衡，又要精准完成复杂动作。作为一名控制工程师，我花了三年时间研究各种控制算法在四旋翼上的应用，今天要分享的是最实用的模型预测控制(MPC)实现方案。这个方案已经在我们实验室的多个无人机项目中得到验证，跟踪精度能达到厘米级。

2. 四旋翼建模：运动学与动力学基础

2.1 坐标系定义与运动学方程

四旋翼建模首先要明确两个关键坐标系：

• 地面坐标系（惯性系）：固定在地面，Z轴向上
• 机体坐标系：固定在无人机上，X轴指向机头方向

它们之间的转换通过旋转矩阵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θ];

这个看似复杂的矩阵其实很好理解：它由三个基本旋转矩阵相乘得到，分别对应横滚(φ)、俯仰(θ)和偏航(ψ)角。实际编程时我习惯用符号运算验证矩阵的正确性：

matlab复制syms phi theta psi real
Rx = [1 0 0; 0 cos(phi) -sin(phi); 0 sin(phi) cos(phi)];
Ry = [cos(theta) 0 sin(theta); 0 1 0; -sin(theta) 0 cos(theta)]; 
Rz = [cos(psi) -sin(psi) 0; sin(psi) cos(psi) 0; 0 0 1];
R = Rz*Ry*Rx;  % 验证旋转矩阵

注意：欧拉角微分方程存在万向节锁问题，当θ=±90°时会出现奇点。但在四旋翼正常飞行中俯仰角很少超过60°，所以这个模型仍然适用。

2.2 动力学建模与耦合效应

动力学模型需要考虑力和力矩的作用。采用牛顿-欧拉方程建模时，关键参数包括：

• 质量m=1.5kg（典型的小型四旋翼）
• 转动惯量J=diag([0.03,