1. 四旋翼飞行器MPC控制概述
四旋翼飞行器作为一种典型的欠驱动系统，通过四个旋翼的转速调节实现六自由度运动控制。这种独特的结构使其在航拍、巡检、救援等领域展现出巨大应用价值，但同时也带来了显著的控制挑战。传统PID控制在处理这类强耦合非线性系统时，往往面临超调量大、响应滞后等问题。
模型预测控制（MPC）通过滚动优化和反馈校正机制，展现出处理多约束非线性系统的独特优势。其核心在于：
- 预测模型：基于系统动力学预测未来状态
- 在线优化：每个控制周期求解最优控制序列
- 反馈校正：仅执行首步控制并滚动推进
在Matlab环境下实现MPC控制，可以利用其强大的矩阵运算能力和优化工具箱，显著降低算法实现难度。特别是对于学术研究，Matlab提供了从建模到仿真的完整工具链。
2. 系统建模与坐标系定义
2.1 坐标系建立
四旋翼控制系统需要明确定义两个关键坐标系：
-
惯性坐标系（世界坐标系）$O_E-X_EY_EZ_E$：
- $X_E$轴指向地理北向
- $Y_E$轴指向地理东向
- $Z_E$轴垂直地面向下
-
机体坐标系$O_B-X_BY_BZ_B$：
- 原点$O_B$位于飞行器质心
- $X_B$轴沿机臂向前
- $Y_B$轴沿右侧机臂
- $Z_B$轴垂直机身向下
2.2 运动学模型
位置运动学描述惯性系下的位置变化：
$$
\begin{aligned}
\dot{p} &= v \
\dot{v} &= R(\phi,\theta,\psi)\cdot\begin{bmatrix}0\0\T\end{bmatrix}/m - \begin{bmatrix}0\0\g\end{bmatrix}
\end{aligned}
$$
其中$R$为旋转矩阵，由Z-Y-X欧拉角表示：
$$
R = \begin{bmatrix}
c_\psi c_\theta & c_\psi s_\theta s_\phi - s_\psi c_\phi & c_\ψ s_\theta c_\phi + s_\ψ s_\phi \
s_\ψ c_\theta & s_\ψ s_\theta s_\phi + c_\ψ c_\phi & s_\ψ s_\theta c_\phi - c_\ψ s_\phi \
-s_\theta & c_\theta s_\phi & c_\theta c_\phi
\end{bmatrix}
$$
2.3 动力学模型
姿态动力学描述角速度变化：
$$
\begin{aligned}
\dot{\omega} &= J^{-1}(\tau - \omega\times J\omega) \
\dot{\Theta} &= E(\phi,\theta)\omega
\end{aligned}
$$
其中$J$为转动惯量矩阵，$E$为欧拉角速率转换矩阵：
$$
E = \begin{bmatrix}
1 & s_\phi t_\theta & c_\phi t_\theta \
0 & c_\phi & -s_\phi \
0 & s_\phi/c_\theta & c_\phi/c_\theta
\end{bmatrix}
$$
3. MPC控制器设计
3.1 双环控制架构
采用位置-姿态双环控制结构：
-
外环位置控制器：
- 输入：期望位置$p_{des}$
- 输出：期望姿态$\phi_{des},\theta_{des}$和总推力$T$
-
内环姿态控制器：
- 输入：期望姿态
- 输出：电机转速指令
3.2 预测模型离散化
采用前向欧拉法对连续模型离散化：
$$
x_{k+1} = x_k + T_s f(x_k,u_k)
$$
其中$T_s$为采样时间。对于线性化模型：
$$
x_{k+1} = A_k x_k + B_k u_k
$$
3.3 目标函数设计
多目标优化函数：
$$
J = \sum_{i=1}^{N_p} ||x_{k+i}-x_{ref}||Q^2 + \sum^{N_c-1} ||u_{k+i}||R^2 + \sum^{N_c-1} ||\Delta u_{k+i}||_S^2
$$
权重矩阵选择原则：
- $Q$：位置误差权重 > 姿态误差权重
- $R$：抑制过大控制输入
- $S$：平滑控制量变化
3.4 约束处理
关键约束包括：
matlab复制% 电机转速约束
umin = [0;0;0;0];
umax = [1000;1000;1000;1000];
% 姿态角约束
phi_max = 30*pi/180;
theta_max = 30*pi/180;
采用软化约束技术处理可能冲突的约束条件：
$$
g(x,u) \leq \epsilon, \quad \epsilon \geq 0
$$
4. Matlab实现关键代码
4.1 MPC问题设置
matlab复制% 定义预测时域和控制时域
Np = 20; % 预测步长
Nc = 5; % 控制步长
% 创建MPC控制器对象
mpcobj = mpc(model,Ts,Np,Nc);
% 设置权重矩阵
mpcobj.Weights.OutputVariables = [1 1 1 0.1 0.1 0.1];
mpcobj.Weights.ManipulatedVariables = [0.1 0.1 0.1 0.1];
mpcobj.Weights.ManipulatedVariablesRate = [0.01 0.01 0.01 0.01];
4.2 在线优化求解
matlab复制function [u, info] = MPC_Solver(x0, xref, mpcobj)
% 设置初始状态
mpcobj.Model.Nominal.X = x0;
% 更新参考轨迹
mpcobj.Model.Plant = setmpcsignals(mpcobj.Model.Plant,...
'MV',[1 2 3 4],'MD',[],'UD',[],'MO',[1:6],'UO',[]);
% 求解优化问题
[u, info] = mpcmove(mpcobj, x0, xref);
% 处理求解失败情况
if info.ExitFlag < 0
warning('MPC求解失败，使用上一控制量');
u = info.U1;
end
end
4.3 仿真主循环
matlab复制% 初始化
X = zeros(12,Nsim);
U = zeros(4,Nsim);
X(:,1) = x0;
for k = 1:Nsim-1
% 获取当前参考轨迹
xref = getReferenceTraj(k);
% MPC求解
[u, info] = MPC_Solver(X(:,k), xref, mpcobj);
% 系统仿真
X(:,k+1) = quadcopter_model(X(:,k), u, Ts);
% 存储控制量
U(:,k) = u;
end
5. 仿真结果分析
5.1 直线轨迹跟踪
跟踪性能指标对比：
| 指标 | MPC | PID |
|---|---|---|
| 最大位置误差(m) | 0.12 | 0.35 |
| 稳态误差(m) | 0.05 | 0.15 |
| 调节时间(s) | 2.1 | 3.8 |
MPC表现出更快的收敛速度和更小的超调量，尤其在转折点处优势明显。
5.2 圆形轨迹跟踪
关键性能数据：
- 径向误差RMS：MPC 0.08m vs PID 0.25m
- 最大角速度：MPC 1.2rad/s vs PID 2.5rad/s
- 能量消耗：MPC比PID低约15%
5.3 抗干扰测试
加入风速3m/s的侧向风扰后：
- MPC能在1.5s内恢复稳定跟踪
- PID出现持续振荡，最大偏差达0.8m
- MPC的干扰抑制能力显著优于PID
6. 工程实现建议
-
模型简化技巧：
- 对小角度飞行可线性化模型
- 对慢时变参数采用准静态假设
- 使用降阶模型减少计算量
-
实时性优化：
matlab复制% 使用更高效的QP求解器
options = optimoptions('quadprog','Algorithm','active-set');
% 代码生成加速
cfg = coder.config('lib');
codegen MPC_Solver -config cfg -args {x0, xref, mpcobj}
- 参数整定步骤：
- 先调整位置环权重$Q_p$
- 再调整姿态环权重$Q_\Theta$
- 最后调节控制量权重$R$
- 通过频域分析验证稳定性裕度
实际飞行测试表明，MPC控制虽然计算量较大，但在现代飞控硬件（如PX4）上已可实现100Hz以上的运行频率，完全满足实时控制需求。