1. 项目背景与核心价值
四旋翼无人机作为典型的欠驱动系统，其控制问题一直是自动控制领域的研究热点。传统的PID控制虽然简单易实现，但在处理复杂轨迹跟踪任务时往往显得力不从心。而模型预测控制（MPC）凭借其滚动优化和反馈校正的特性，特别适合处理这类具有多变量耦合和状态约束的控制问题。
这个项目实现了基于线性参数变化（LPV）模型的MPC控制器，对无人机进行位置和姿态的双闭环控制，最终完成8字形轨迹跟踪任务。相比固定参数的MPC，LPV-MPC能够更好地适应无人机在不同飞行状态下的动态特性变化，这也是本项目的技术亮点所在。
2. 系统建模与控制器设计
2.1 四旋翼动力学模型
四旋翼的动力学可以分为位置动力学和姿态动力学两个子系统。位置动力学描述无人机在惯性坐标系中的平移运动：
$$
\begin{cases}
\ddot{x} = (\cos\phi\sin\theta\cos\psi + \sin\phi\sin\psi)\frac{U_1}{m} \
\ddot{y} = (\cos\phi\sin\theta\sin\psi - \sin\phi\cos\psi)\frac{U_1}{m} \
\ddot{z} = \cos\phi\cos\theta\frac{U_1}{m} - g
\end{cases}
$$
姿态动力学描述无人机绕机体坐标系的旋转运动：
$$
\begin{cases}
\ddot{\phi} = \dot{\theta}\dot{\psi}(\frac{I_y-I_z}{I_x}) - \frac{J_r}{I_x}\dot{\theta}\Omega + \frac{U_2}{I_x} \
\ddot{\theta} = \dot{\phi}\dot{\psi}(\frac{I_z-I_x}{I_y}) + \frac{J_r}{I_y}\dot{\phi}\Omega + \frac{U_3}{I_y} \
\ddot{\psi} = \dot{\phi}\dot{\theta}(\frac{I_x-I_y}{I_z}) + \frac{U_4}{I_z}
\end{cases}
$$
2.2 LPV-MPC控制器设计
LPV-MPC的核心思想是将非线性系统表示为参数依赖的线性系统：
$$
x(k+1) = A(\rho(k))x(k) + B(\rho(k))u(k)
$$
其中$\rho(k)$是时变参数向量。对于四旋翼系统，我们可以选择欧拉角作为调度变量：
$$
\rho(k) = [\phi(k), \theta(k), \psi(k)]^T
$$
MPC优化问题的目标函数通常设计为：
$$
J = \sum_{i=0}^{N_p-1} |x(k+i|k)-x_{ref}(k+i)|Q^2 + \sum^{N_c-1} |\Delta u(k+i|k)|_R^2
$$
3. 双闭环控制架构
3.1 外环位置控制器
外环MPC控制器负责生成期望的姿态角和总推力：
- 将位置误差转换为期望加速度
- 通过逆动力学计算所需的姿态角$\phi_d, \theta_d$和推力$U_1$
- 偏航角$\psi_d$可直接由轨迹给定
3.2 内环姿态控制器
内环MPC控制器跟踪外环给出的姿态指令：
- 接收外环输出的$\phi_d, \theta_d, \psi_d$
- 计算姿态误差并生成控制力矩$U_2, U_3, U_4$
- 考虑执行器饱和约束
3.3 双环协同工作流程
- 外环MPC以100Hz频率运行，解算期望姿态
- 内环MPC以200Hz频率运行，实现精确姿态跟踪
- 两环之间通过共享内存交换数据
4. 8字形轨迹生成与跟踪
4.1 轨迹参数化
8字形轨迹可以用以下参数方程表示：
$$
\begin{cases}
x(t) = A_x\sin(\omega t) \
y(t) = A_y\sin(2\omega t) \
z(t) = z_0 \
\psi(t) = \psi_0
\end{cases}
$$
其中$A_x$和$A_y$决定8字的尺寸，$\omega$控制飞行速度。
4.2 轨迹微分处理
MPC需要各阶微分信号作为参考：
- 位置：$p_{ref}, v_{ref}, a_{ref}$
- 姿态：$\eta_{ref}, \dot{\eta}_{ref}$
可以通过巴特沃斯滤波器对理想轨迹进行平滑处理，避免高频抖动。
5. Matlab实现详解
5.1 仿真框架搭建
matlab复制% 主仿真循环
for k = 1:N_sim
% 获取当前状态
x = drone.getState();
% 外环MPC计算
[eta_ref, U1] = outer_mpc.solve(x, traj_ref);
% 内环MPC计算
U234 = inner_mpc.solve(x(7:12), eta_ref);
% 执行控制量
drone.applyControl([U1; U234]);
% 状态更新
drone.update(dt);
end
5.2 MPC控制器实现
matlab复制function [u, info] = solve_mpc(x0, ref, mpc)
% 构造优化问题
prob = struct;
prob.x0 = x0;
prob.ref = ref;
% 设置LPV参数
prob.rho = computeSchedulingParameters(x0);
% 调用QP求解器
result = quadprog(prob.H, prob.f, prob.A, prob.b, [], [], prob.lb, prob.ub);
% 提取控制量
u = result(1:mpc.nu);
info = struct('status', result.status);
end
5.3 可视化实现
matlab复制function plotResults(log)
figure('Position', [100,100,1200,800]);
% 3D轨迹
subplot(2,2,1);
plot3(log.ref(:,1), log.ref(:,2), log.ref(:,3), 'r--');
hold on;
plot3(log.actual(:,1), log.actual(:,2), log.actual(:,3), 'b-');
legend('参考轨迹', '实际轨迹');
% 误差分析
subplot(2,2,2);
error = vecnorm(log.ref(:,1:3) - log.actual(:,1:3), 2, 2);
plot(log.time, error);
title('位置跟踪误差');
% 控制量可视化
subplot(2,2,3);
plot(log.time, log.control);
legend('U1', 'U2', 'U3', 'U4');
% 姿态角跟踪
subplot(2,2,4);
plot(log.time, log.attitude);
end
6. 关键参数调优指南
6.1 MPC参数选择
| 参数 | 作用 | 推荐值 | 调整技巧 |
|---|---|---|---|
| Np | 预测时域 | 15-25 | 增大可提高稳定性但增加计算量 |
| Nc | 控制时域 | 5-10 | 通常取Np的1/3到1/2 |
| Q | 状态权重 | diag([10,10,10,1,1,1]) | 位置权重>速度权重 |
| R | 控制权重 | diag([0.1,1,1,1]) | 避免控制量剧烈变化 |
6.2 无人机参数设置
matlab复制drone.mass = 1.2; % 质量(kg)
drone.Ixx = 0.023; % 转动惯量
drone.Iyy = 0.023;
drone.Izz = 0.046;
drone.arm_length = 0.25; % 机臂长度(m)
7. 常见问题与解决方案
7.1 发散问题排查
-
现象：仿真过程中状态突然发散
- 检查1：MPC求解是否成功（info.status）
- 检查2：动力学模型参数是否正确
- 检查3：约束条件是否合理
-
现象：轨迹跟踪存在稳态误差
- 方案1：增加积分项
- 方案2：调整Q矩阵中位置误差权重
7.2 实时性问题优化
-
代码优化：
- 使用预先计算的雅可比矩阵
- 启用MATLAB的加速模式
-
算法优化：
- 减少预测时域Np
- 使用显式MPC
8. 进阶改进方向
-
抗风扰设计：
- 增加扰动观测器
- 在MPC中考虑风场模型
-
硬件在环测试：
- 使用PX4硬件在环仿真
- 考虑通信延迟补偿
-
非线性MPC扩展：
- 直接基于非线性模型设计
- 使用更高效的求解器如ACADO
实际调试中发现，当8字形轨迹的转弯半径较小时，固定参数的MPC容易出现超调，而LPV-MPC通过参数自适应能够显著改善这一现象。建议在高速轨迹跟踪场景中优先考虑LPV方法。