1. 无人机轨迹跟踪控制的技术挑战
四旋翼无人机的3D轨迹跟踪控制本质上是一个多变量、强耦合的非线性控制问题。在实际飞行中，我们需要同时控制四个电机的转速来实现六个自由度的运动（X/Y/Z轴平移和滚转/俯仰/偏航旋转），这种欠驱动特性使得控制算法设计尤为复杂。通过Matlab仿真验证，我们发现传统PID控制在简单直线轨迹跟踪时表现尚可（位置误差约±0.3m），但在处理8字形或螺旋上升等复杂轨迹时，误差会急剧增大到±1.5m以上。
关键难点：电机推力与机体姿态的非线性耦合会导致"控制冲突"现象——当尝试修正一个方向的误差时，可能在其他方向引入新的干扰。这种现象在快速机动时尤为明显。
2. LPV-MPC控制框架设计
2.1 动力学模型的LPV转化
四旋翼的非线性动力学方程通常表示为：
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
通过选择调度变量ρ=[V,h]（飞行速度V和高度h），我们可以将上述非线性模型转化为LPV形式：
code复制ẋ = A(ρ)x + B(ρ)u
y = Cx
其中状态量x=[位置；姿态；速度；角速度]，控制量u=[U₁,U₂,U₃,U₄]对应四个电机的合成控制量。
2.2 预测模型构建
在MPC框架中，我们采用离散化后的LPV模型作为预测模型：
code复制x(k+1) = A_d(ρ)x(k) + B_d(ρ)u(k)
y(k) = C_dx(k)
预测时域Np=20，控制时域Nc=5（采样周期0.05s），这意味着每次优化要计算未来1秒内的状态预测和0.25秒的控制序列。
3. 实时优化问题求解
3.1 目标函数设计
代价函数采用二次型形式：
code复制J = Σ( y(k)-r(k) )'Q( y(k)-r(k) ) + Σ Δu(k)'RΔu(k)
其中：
- Q=diag([10,10,10,1,1,1]) 位置误差权重＞姿态误差
- R=0.1*I₄ 控制增量权重
- 特别增加了对z轴误差的惩罚系数（Q₃₃=15），因为高度控制对安全至关重要
3.2 约束处理技巧
-
输入约束：
- 单个电机推力限制：0.5N ≤ f_i ≤ 5N
- 总推力限制：2N ≤ Σf_i ≤ 18N
-
状态约束：
- 滚转/俯仰角：|φ|,|θ| ≤ 30°
- 偏航角速率：|ψ̇| ≤ 1rad/s
实际编程中发现：直接将约束写成Aineq*u ≤ bineq形式会导致求解效率低下。改为使用MPC工具箱的mpcmove函数内置约束处理，计算速度提升3倍。
4. 仿真实现关键代码
4.1 LPV模型调度实现
matlab复制function [A,B] = lpv_model(rho)
V = rho(1); h = rho(2);
% 气动参数随高度变化
k_drag = 0.1*exp(-h/1000);
% 系统矩阵元素赋值
A21 = -k_drag*V;
B11 = cos(phi)*cos(theta)/m;
% ...其他矩阵元素计算
A = [zeros(3) eye(3);
diag([A21 A21 A21]) zeros(3)];
B = [zeros(3,4); B11*ones(1,4); ...];
end
4.2 实时优化主循环
matlab复制while t < t_end
% 获取当前状态和调度变量
x = get_state(uav);
rho = [norm(x(4:6)), x(3)];
% 更新预测模型
[A,B] = lpv_model(rho);
sys = ss(A,B,C,0);
mpcobj.Model.Plant = c2d(sys,Ts);
% 求解MPC
[u,info] = mpcmove(mpcobj,x,r);
% 施加扰动测试鲁棒性
if t > 2 && t < 2.5
u = u + 0.2*randn(4,1);
end
% 更新无人机状态
uav = update_dynamics(uav,u,Ts);
end
5. 性能优化与实测结果
5.1 计算效率提升
通过以下措施将单步计算时间从35ms降至8ms：
- 将QP求解器从默认的'interior-point'改为'active-set'
- 预计算所有可能的A(ρ),B(ρ)组合并建立查找表
- 使用Coder将关键函数生成Mex文件
5.2 典型轨迹跟踪结果
| 测试案例 | 最大位置误差 | 稳态误差 | 计算时间/步 |
|---|---|---|---|
| 直线轨迹 | 0.12m | 0.03m | 6ms |
| 圆形轨迹 | 0.25m | 0.08m | 7ms |
| 8字轨迹 | 0.38m | 0.15m | 9ms |
在加入20%随机推力扰动后，控制器仍能保持跟踪误差在安全范围内（＜0.5m），展现了良好的鲁棒性。
6. 工程实践中的经验总结
-
高度计噪声处理：
- 实测发现超声波高度计在1m以下会有约5%的波动
- 添加一阶低通滤波器：alpha=0.2，可有效平滑数据
-
电机响应滞后补偿：
- 大推力变化时电机有约50ms的响应延迟
- 在MPC模型中增加一阶滞后环节：τ=0.05s
-
突发风扰应对：
- 侧风＞3m/s时会导致明显的位置漂移
- 在目标函数中增加速度误差惩罚项可有效抑制振荡
这个方案在Matlab 2021b+Simulink环境下测试通过，完整工程文件包含：
- 主仿真脚本：drone_mpc_lpv_main.m
- LPV模型库：lpv_models.m
- 可视化工具：plot_trajectory_3d.m
- 测试场景：scenario_8shape.mat
实际部署时建议先用Coder生成C++代码，在PX4飞控上实测显示，在树莓派4B上单步计算耗时约12ms，满足实时性要求。