1. 四旋翼飞行器与MPC控制概述
四旋翼飞行器作为一种典型的欠驱动系统,其控制问题一直是无人机领域的研究热点。这类飞行器通过四个旋翼转速的协调变化实现姿态和位置控制,但仅有四个控制输入(四个电机的推力)却需要同时控制六个自由度(位置x,y,z和姿态角φ,θ,ψ),这种特性使得传统PID控制在复杂任务中往往表现不佳。
模型预测控制(MPC)因其能够显式处理多输入多输出系统的约束优化问题,在四旋翼控制中展现出独特优势。MPC的核心思想是:在每个控制周期,基于当前状态和系统模型,求解一个有限时域的最优控制问题,但只执行第一个控制量,到下一周期重新进行预测和优化。这种滚动优化的策略特别适合四旋翼这种需要实时响应环境变化的系统。
提示:MPC的三要素是预测模型、滚动优化和反馈校正。在四旋翼控制中,预测模型通常采用线性化后的动力学方程,而优化目标则包含轨迹跟踪误差、控制量变化率等多个指标。
2. 多目标航点导航的问题建模
2.1 四旋翼动力学模型简化
为平衡计算复杂度和模型精度,我们采用以下简化假设:
- 飞行器结构刚体且对称
- 重心与几何中心重合
- 忽略地面效应和空气阻力
基于牛顿-欧拉方程,建立机体坐标系下的动力学模型:
code复制ẍ = (cosφsinθcosψ + sinφsinψ)U₁/m
ÿ = (cosφsinθsinψ - sinφcosψ)U₁/m
z̈ = (cosφcosθ)U₁/m - g
φ̈ = θ̇ψ̇(Iy-Iz)/Ix + lU₂/Ix
θ̈ = φ̇ψ̇(Iz-Ix)/Iy + lU₃/Iy
ψ̈ = φ̇θ̇(Ix-Iy)/Iz + U₄/Iz
其中U₁~U₄为虚拟控制量,与实际电机推力关系为:
code复制U₁ = F₁ + F₂ + F₃ + F₄ (总升力)
U₂ = -F₂ + F₄ (滚转力矩)
U₃ = -F₁ + F₃ (俯仰力矩)
U₄ = τ₁ - τ₂ + τ₃ - τ₄ (偏航力矩)
2.2 航点约束与目标函数设计
多目标航点导航的核心是在有限时间内依次通过多个指定位置。我们需要在MPC框架中将这一要求转化为优化问题的约束条件和目标函数。
对于N个航点,定义通过性约束:
code复制∃t∈[t_k, t_k+H], s.t. ||p(t)-p_waypoint_i||² ≤ ε, i=1,...,N
其中H为预测时域,ε为允许的位置误差。
目标函数通常包含:
- 轨迹跟踪误差:Σ||p(t)-p_ref(t)||²
- 控制能量消耗:Σ||u(t)||²
- 控制量变化率:Σ||u(t)-u(t-1)||²
- 航点到达时间惩罚:Σ(t_arrive_i - t_desired_i)²
3. MPC控制器设计与实现
3.1 模型离散化与线性化
为适用于数字控制,需将连续模型离散化。采用零阶保持法,采样周期T_s=0.02s:
code复制x_{k+1} = A_d x_k + B_d u_k
y_k = C_d x_k
在平衡点附近线性化(通常悬停状态):
- 小角度假设:sin(·)≈·, cos(·)≈1
- 忽略高阶耦合项:φ̇θ̇≈0
得到线性时不变(LTI)模型,便于在线优化求解。
3.2 优化问题构建
每个控制周期求解如下QP问题:
code复制min_U J = Σ_{k=0}^{H-1} [x_k'Qx_k + u_k'Ru_k + Δu_k'SΔu_k] + x_H'Px_H
s.t. x_{k+1} = Ax_k + Bu_k
u_min ≤ u_k ≤ u_max
Δu_min ≤ Δu_k ≤ Δu_max
x_min ≤ x_k ≤ x_max
||p(t_k)-p_waypoint_i|| ≤ ε, for some k
其中:
- Q,R,S,P为权重矩阵
- H为预测时域(通常10-20步)
- 航点约束通过松弛变量处理避免不可行
3.3 代码实现关键模块
3.3.1 主控制循环结构
matlab复制function [u, x_pred] = mpc_controller(x_current, waypoints)
persistent optimizer;
% 初始化QP求解器
if isempty(optimizer)
optimizer = setup_optimizer();
end
% 更新当前状态和航点信息
optimizer.update('x0', x_current);
optimizer.set('waypoints', waypoints);
% 求解最优控制序列
[sol, flag] = optimizer.solve();
% 返回第一个控制量
u = sol.u(1);
x_pred = sol.x;
end
3.3.2 YALMIP优化器配置
matlab复制function optimizer = setup_optimizer()
% 定义优化变量
U = sdpvar(repmat(4,1,H), ones(1,H)); % 控制序列
X = sdpvar(repmat(12,1,H+1), ones(1,H+1)); % 状态序列
% 初始约束
Constraints = [X{1} == x0];
% 构建预测模型约束
for k = 1:H
Constraints = [Constraints, X{k+1} == A*X{k} + B*U{k}];
Constraints = [Constraints, umin <= U{k} <= umax];
end
% 航点约束(松弛变量法)
slack = sdpvar(length(waypoints),1);
for w = 1:length(waypoints)
Constraints = [Constraints, ...
norm(X{closest_idx(w)}(1:3) - waypoints(w,:)) <= epsilon + slack(w)];
end
% 目标函数
Objective = 0;
for k = 1:H
Objective = Objective + X{k}'*Q*X{k} + U{k}'*R*U{k};
if k > 1
Objective = Objective + (U{k}-U{k-1})'*S*(U{k}-U{k-1});
end
end
Objective = Objective + X{H+1}'*P*X{H+1} + slack'*W*slack;
% 创建优化器对象
ops = sdpsettings('solver','quadprog','verbose',0);
optimizer = optimizer(Constraints, Objective, ops, ...
{'x0','waypoints'}, {'u','x'});
end
4. 仿真实验与结果分析
4.1 实验设置
- 仿真环境:MATLAB 2021b + Simulink
- 四旋翼参数:
- 质量m=1.2kg
- 轴距l=0.25m
- 惯性矩Ix=Iy=0.03kg·m², Iz=0.04kg·m²
- MPC参数:
- 预测时域H=15(对应0.3s)
- 采样时间Ts=0.02s
- 权重矩阵:
- Q=diag([10,10,10, 1,1,1, 0.1,0.1,0.1, 0.01,0.01,0.01])
- R=0.1*eye(4)
- S=0.01*eye(4)
4.2 航点飞行测试
设计包含5个航点的三维路径:
- (0,0,1) - 起飞点
- (2,0,1.5)
- (2,2,2)
- (0,2,1.5)
- (0,0,1) - 降落点
性能指标:
- 航点到达误差:<0.1m
- 最大跟踪误差:<0.15m
- 平均计算时间:<5ms/步
4.3 结果对比分析
与串级PID控制器对比:
| 指标 | MPC控制器 | PID控制器 |
|---|---|---|
| 最大位置误差 | 0.12m | 0.35m |
| 能量消耗 | 82J | 105J |
| 航点到达精度 | ±0.08m | ±0.25m |
| 抗风扰能力 | 优 | 中 |
MPC在以下方面表现突出:
- 航点切换更平滑,无超调
- 对模型不确定性和外部扰动更具鲁棒性
- 能够显式处理控制量约束,避免饱和
5. 工程实现中的关键问题
5.1 实时性优化技巧
- 热启动:使用上一周期的解作为当前优化的初始猜测
matlab复制if ~isempty(prev_sol)
optimizer.set('initial_guess', prev_sol);
end
- 降维处理:利用结构对称性减少优化变量
- 代码生成:将MATLAB代码转为C++提高执行效率
- 时域管理:动态调整预测时域(飞行阶段用长时域,精确控制阶段用短时域)
5.2 模型失配补偿
实际系统与模型差异主要来自:
- 未建模的空气动力学效应
- 电机响应延迟
- 电池电压衰减
补偿方案:
- 在线参数估计:扩展状态向量包含不确定参数
- 误差积分项:在目标函数中加入累积误差惩罚
- 多模型MPC:针对不同飞行阶段切换模型
5.3 硬件部署注意事项
- 传感器同步:确保IMU与定位系统时间对齐
- 执行器饱和处理:
matlab复制% 在约束中添加电机动力学限制
Constraints = [Constraints, ...
0 <= U{1}(i) <= f_max, ... % 单电机最大推力
abs(U{1}(i)-U_prev(i)) <= delta_f_max]; % 最大推力变化率
- 计算资源监控:实现"看门狗"机制防止求解超时
6. 扩展方向与进阶建议
-
非线性MPC:直接使用非线性模型,避免线性化误差
- 采用ACADO或CasADi工具链
- 需更强的计算硬件支持
-
分布式MPC:将位置控制和姿态控制分层设计
- 上层位置控制器生成姿态指令
- 下层姿态控制器快速响应
-
学习增强MPC:
- 使用神经网络拟合模型误差
- 强化学习优化权重矩阵
-
多机协同:扩展为多四旋翼系统的分布式MPC
- 增加防碰撞约束
- 通信拓扑设计
实际部署中发现,电机混控策略对控制性能影响显著。推荐采用PWM频率≥400Hz,并做电机-螺旋桨动平衡校准,可降低高频振动对控制精度的不利影响。