四旋翼无人机MPC轨迹跟踪的Simulink实现与优化

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

四旋翼无人机作为典型的欠驱动系统，其姿态与位置控制存在强耦合特性。在实际飞行中，我们常常需要无人机能够精准跟踪预设轨迹——无论是进行航拍测绘、物资投送还是编队飞行，轨迹跟踪性能都是衡量飞控系统优劣的关键指标。

传统PID控制虽然结构简单，但在处理非线性、强耦合系统时往往力不从心。特别是在遇到风扰、负载变化等不确定因素时，固定参数的PID控制器难以保证稳定的跟踪精度。而模型预测控制（MPC）通过在线滚动优化，能够更好地处理系统约束和时变特性，这使其成为无人机先进控制的理想选择。

Simulink作为多域仿真平台，为MPC算法的开发验证提供了完整工具链。从被控对象建模、控制器设计到硬件在环测试，工程师可以在统一环境中完成全流程开发。这种"模型即代码"（Model-Based Design）的方式大幅降低了算法实现门槛。

2. MPC算法框架解析

2.1 预测模型构建

四旋翼的运动学模型通常分解为位置环和姿态环两个层级。在Simulink中，我们采用以下状态空间表示：

code复制ẋ = vx
ẏ = vy
ż = vz
v̇x = (cosφsinθcosψ + sinφsinψ)U₁/m
v̇y = (cosφsinθsinψ - sinφcosψ)U₁/m
v̇z = (cosφcosθ)U₁/m - g

其中[φ,θ,ψ]分别为滚转、俯仰和偏航角，U₁为总升力。这个强非线性模型需要经过合理的线性化处理才能用于MPC。常见的做法是在每个采样周期进行泰勒展开，得到局部线性模型。

提示：实际建模时建议使用Simulink的Model Linearizer工具，它可以自动在工作点附近生成线性化模型，避免手动推导错误。

2.2 优化问题构建

MPC的核心是每个控制周期求解如下优化问题：

code复制min J = Σ( x(k+i|k)-x_ref )ᵀQ( x(k+i|k)-x_ref ) 
       + Σ u(k+i|k)ᵀRu(k+i|k)
s.t.  x(k+i+1|k) = Ax(k+i|k) + Bu(k+i|k)
      u_min ≤ u(k+i|k) ≤ u_max
      Δu_min ≤ Δu(k+i|k) ≤ Δu_max

在Simulink中，我们可以通过MPC Controller模块直接配置这些参数。其中需要特别关注：

1. 预测时域(Prediction Horizon)：通常选择15-20步，对应1.5-2秒的预测范围
2. 控制时域(Control Horizon)：建议设为预测时域的1/3到1/2
3. 权重矩阵Q/R：需要平衡状态跟踪与控制量消耗，一般先令Q=diag([10,10,10,1,1,1])

2.3 约束处理技巧

四旋翼的物理约束主要包括：

• 电机推力上下限（对应U₁的范围）
• 姿态角安全范围（如|φ|,|θ|≤30°）
• 控制量变化率限制（防止电机响应过快）

在Simulink中配置约束时，建议采用"软约束"方式处理状态约束，避免优化问题不可行。例如对位置误差设置松弛变量：

code复制|x - x_ref| ≤ x_max + ε
ε ≥ 0

3. Simulink实现详解

3.1 建模规范建议

1. 模块化设计：将无人机模型、MPC控制器、轨迹生成器分别封装为子系统
2. 信号命名：对所有输入输出信号使用有意义的命名（如"phi_cmd"而非"In1"）
3. 采样时间统一：确保所有离散模块使用相同采样时间（建议50-100Hz）
4. 总线信号：使用Bus Creator组织相关信号（如将所有状态变量打包）

3.2 MPC模块配置步骤

1. 从Model Predictive Control Toolbox拖入MPC Controller模块
2. 双击模块启动配置向导：
   • 指定线性化模型（可从工作区导入或直接线性化Simulink模型）
   • 设置预测时域和控制时域
   • 配置权重矩阵和约束条件
3. 生成控制器并自动创建Simulink封装模块

3.3 典型参数调试流程

1. 先调位置环：固定Q中的角度权重为0，只调整位置相关权重
2. 再调姿态环：逐步增加角度权重，观察响应曲线
3. 最后调输入权重R：在保证性能的前提下尽量增大R以减少控制消耗

实测技巧：使用Simulink的Optimization Tool可以自动调节权重参数。先勾选"Enable automated tuning"，然后指定期望的上升时间、超调量等指标，工具会自动寻找最优参数组合。

4. 轨迹跟踪性能优化

4.1 参考轨迹处理

对于高速运动的无人机，直接跟踪离散轨迹点可能导致控制量剧烈波动。推荐的处理方法：

1. 轨迹平滑：使用三次样条插值生成连续参考轨迹
2. 轨迹预估：在MPC中引入前馈项，提前补偿预期加速度
3. 重规划机制：当跟踪误差超过阈值时触发局部轨迹重新规划

在Simulink中，可以使用Signal Processing Toolbox的spline函数实现实时插值：

matlab复制function [pos_ref, vel_ref] = trajectory_interp(t, waypoints)
    pp = spline(waypoints.time, waypoints.pos);
    pos_ref = ppval(pp, t);
    vel_ref = ppval(fnder(pp), t); 
end

4.2 抗扰增强策略

为提高抗风扰能力，可以在基础MPC框架上增加：

1. 扰动观测器：设计Luenberger观测器估计持续风扰
2. 积分环节：在代价函数中增加误差积分项
3. 自适应权重：根据误差大小动态调整Q矩阵

一个有效的实现方案是在MPC外层包裹抗扰补偿器：

code复制u_mpc = MPC_Controller(x_ref, x);
u_total = u_mpc + K*d_hat;  // d_hat为估计扰动

5. 硬件部署注意事项

5.1 代码生成优化

使用Simulink Coder生成嵌入式代码时需注意：

1. 选择适当的求解器：固定步长离散求解器（如ode1）
2. 内存分配：启用动态内存分配以节省RAM
3. 数据类型：统一使用single精度减少计算负担
4. 函数封装：将MPC求解器生成为可重入函数

5.2 实时性保障

MPC的在线优化计算量较大，在资源受限的飞控板上需要特别优化：

1. 简化模型：在保持主要动态特性的前提下降低状态维度
2. 热启动：使用上一周期的解作为当前优化的初始猜测
3. 提前终止：设置最大迭代次数（如20次）保证实时性

实测数据显示，在STM32H7平台上，经过优化的MPC算法可以在5ms内完成10状态、2输入系统的20步预测计算。

6. 典型问题排查指南

我在实际项目中遇到的一个典型问题：无人机在高速转弯时出现高度跌落。最终发现是俯仰角约束设置过小（仅25°），导致控制器无法生成足够的向心力。将约束放宽到35°后问题解决，但同时需要加强结构强度以应对更大的气动载荷。