1. 项目概述
四旋翼无人机的精确轨迹跟踪一直是控制领域的研究热点。在实际应用中，8字形轨迹因其复杂的几何特性，常被用作验证无人机控制算法的基准测试。传统PID控制在处理这种非线性、强耦合系统时往往表现不佳，而线性MPC又难以适应系统的时变特性。
我最近在Matlab中实现了一个基于LPV-MPC的双闭环控制系统，专门用于解决四旋翼无人机的8字形轨迹跟踪问题。这个方案的核心创新点在于：
- 采用位置外环和姿态内环的双层控制架构
- 外环使用状态反馈线性化实现解耦
- 内环采用LPV-MPC来处理非线性约束
- 通过实时参数适配来应对系统动态变化
2. 系统建模与问题描述
2.1 四旋翼动力学模型
四旋翼无人机是一个典型的欠驱动系统，具有6个自由度（位置x,y,z和姿态ϕ,θ,ψ）但只有4个控制输入（四个电机的转速）。其非线性动力学方程可以表示为：
位置动力学：
mẍ = (cosϕsinθcosψ + sinϕsinψ)u₁
mÿ = (cosϕsinθsinψ - sinϕcosψ)u₁
mz̈ = (cosϕcosθ)u₁ - mg
姿态动力学：
Iₓϕ̈ = θ̇ψ̇(I_y - I_z) + lu₂
I_yθ̈ = ϕ̇ψ̇(I_z - Iₓ) + lu₃
I_zψ̈ = ϕ̇θ̇(Iₓ - I_y) + u₄
其中u₁到u₄是虚拟控制输入，与电机转速的关系为：
u₁ = b(ω₁² + ω₂² + ω₃² + ω₄²)
u₂ = b(-ω₂² + ω₄²)
u₃ = b(ω₁² - ω₃²)
u₄ = d(-ω₁² + ω₂² - ω₃² + ω₄²)
2.2 8字形轨迹生成
8字形轨迹由两个半径相同的圆在水平面拼接而成，同时垂直方向有线性爬升。具体数学表达式为：
x_ref(t) = R·sin(ωt)
y_ref(t) = R·sin(ωt)cos(ωt)
z_ref(t) = z₀ + v_z·t
其中ω=2π/T，T为完成一个完整8字形轨迹所需的时间。
3. 控制架构设计
3.1 双闭环控制结构
整个控制系统采用分层设计：
位置外环（100Hz更新）：
- 计算位置误差
- 通过状态反馈线性化生成期望姿态
- 输出总升力和期望姿态角
姿态内环（200Hz更新）：
- 接收外环输出的姿态参考
- 基于LPV模型构建MPC问题
- 求解最优控制输入
- 输出电机转速指令
3.2 位置外环设计
位置控制采用状态反馈线性化方法。首先定义误差变量：
e_p = p_ref - p
e_v = v_ref - v
然后设计控制律：
u₁ = m(g + z̈_des)/cosϕcosθ
ϕ_des = arcsin[(mẍ_des·sinψ - mÿ_des·cosψ)/u₁]
θ_des = arcsin[(mẍ_des·cosψ + mÿ_des·sinψ)/(u₁cosϕ)]
其中期望加速度通过PD控制生成：
a_des = K_p·e_p + K_d·e_v + a_ref
3.3 姿态内环LPV-MPC设计
3.3.1 LPV模型构建
将姿态动力学在操作点附近线性化，得到参数依赖的状态空间模型：
x(k+1) = A(ρ(k))x(k) + B(ρ(k))u(k)
y(k) = Cx(k)
其中ρ(k)是时变参数向量，包含当前姿态角和角速率信息。
3.3.2 MPC问题构建
在每个控制周期求解以下优化问题：
min Σ(||y(k+i)-r(k+i)||_Q + ||Δu(k+i)||_R)
s.t.
x(k+i+1) = A(ρ(k))x(k+i) + B(ρ(k))u(k+i)
u_min ≤ u(k+i) ≤ u_max
Δu_min ≤ Δu(k+i) ≤ Δu_max
使用QP求解器在线求解，仅应用第一个控制量。
4. Matlab实现细节
4.1 仿真环境设置
matlab复制% 无人机参数
params.m = 1.2; % 质量(kg)
params.g = 9.81; % 重力加速度
params.l = 0.3; % 轴距(m)
params.I = diag([0.01, 0.01, 0.02]); % 转动惯量
% 轨迹参数
R = 2; % 8字形半径(m)
T = 8; % 周期(s)
v_z = 0.375; % 垂直速度(m/s)
% 控制器参数
ctrl.Ts_pos = 0.02; % 位置环采样时间
ctrl.Ts_att = 0.005; % 姿态环采样时间
ctrl.N = 10; % 预测步长
4.2 LPV-MPC实现
matlab复制function [u_opt, status] = solve_mpc(x0, ref, params, ctrl)
% 构建QP问题
H = blkdiag(kron(eye(ctrl.N),ctrl.Q), kron(eye(ctrl.N),ctrl.R));
f = [-ref'*kron(eye(ctrl.N),ctrl.Q), zeros(1,4*ctrl.N)];
% 构建约束
A_eq = build_equality_constraints(x0, params, ctrl);
A_ineq = build_inequality_constraints(params, ctrl);
% 求解QP
options = optimoptions('quadprog','Display','off');
[u_opt, ~, status] = quadprog(H, f, A_ineq, b_ineq, A_eq, beq,...
lb, ub, [], options);
end
4.3 仿真主循环
matlab复制for t = 0:dt:T_final
% 轨迹生成
[p_ref, v_ref, a_ref] = gen_8shape_traj(t, R, T, v_z);
% 位置控制
[phi_des, theta_des, u1] = position_controller(p, v, p_ref, v_ref, a_ref);
% 姿态控制
[omega, status] = attitude_controller(att, [phi_des; theta_des; 0], u1);
% 动力学更新
[p, v, att, omega] = quadcopter_dynamics(p, v, att, omega, dt, params);
end
5. 性能分析与优化
5.1 跟踪精度对比
我们比较了三种控制策略在8字形轨迹上的表现：
| 指标 | PID控制 | LMPC | LPV-MPC |
|---|---|---|---|
| 最大位置误差(m) | 0.32 | 0.18 | 0.045 |
| 均方根误差(m) | 0.25 | 0.12 | 0.028 |
| 能量消耗(J) | 85.6 | 78.3 | 72.1 |
5.2 参数敏感性分析
LPV-MPC的性能很大程度上取决于预测时域N的选择。通过实验我们发现：
- N=5：计算量小但跟踪性能差（RMSE=0.062m）
- N=10：计算量与性能的良好平衡（RMSE=0.028m）
- N=15：性能提升有限（RMSE=0.025m）但计算时间增加50%
5.3 实时性优化
为了提升实时性能，我们采用了以下优化措施：
- 热启动：使用上一周期的解作为初始猜测
- 稀疏矩阵：利用QP问题的稀疏结构加速求解
- 代码生成：将关键函数转换为C代码
优化前后对比：
- 单次求解时间：从12ms降至3.2ms
- 最大计算延迟：从15ms降至5ms
6. 实际应用中的挑战与解决方案
6.1 模型失配问题
在实际应用中，我们遇到了以下模型失配情况：
- 电池电量下降导致电机效率变化
- 风扰等外部干扰
- 负载变化引起的质量/惯量变化
解决方案：
- 在线参数估计：增加扩展卡尔曼滤波器实时估计关键参数
- 鲁棒MPC：在优化问题中加入不确定性描述
- 自适应权重：根据跟踪误差动态调整Q矩阵
6.2 执行器饱和处理
电机转速存在物理限制（通常0-1000rad/s）。我们采用以下策略：
- 硬约束：直接在QP问题中加入上下限
- 优先级分配：当接近饱和时，优先保证姿态稳定
- 参考调整：平滑调整位置参考以避免突变
6.3 计算资源管理
在嵌入式平台上实现时遇到的计算挑战：
- 内存限制：采用稀疏矩阵存储
- 计算能力：使用定点数运算
- 实时性：设置求解时间上限，超时使用备用控制律
7. 扩展应用与未来方向
7.1 多机协同控制
当前框架可以扩展到多无人机系统：
- 增加避碰约束
- 分布式MPC架构
- 通信延迟补偿
7.2 视觉辅助导航
结合视觉传感器提升定位精度：
- 视觉惯性里程计
- 基于特征的定位
- 语义环境理解
7.3 强化学习优化
使用强化学习来优化MPC参数：
- 自动调整权重矩阵
- 学习预测模型
- 适应不同飞行模式
我在实际实现中发现，LPV-MPC虽然计算复杂度较高，但其优越的控制性能使其非常适合高精度轨迹跟踪任务。特别是在8字形轨迹的交叉点处，传统方法往往会出现明显的跟踪偏差，而LPV-MPC能够很好地保持跟踪精度。