四旋翼无人机LPV-MPC控制与8字形轨迹跟踪实践

1. 项目背景与核心挑战

四旋翼无人机作为典型的欠驱动系统，其位置和姿态控制一直是非线性控制领域的热点问题。传统PID控制虽然简单易实现，但在应对复杂轨迹跟踪时往往显得力不从心。而模型预测控制（MPC）凭借其滚动优化和反馈校正的特性，特别适合处理这类多变量耦合系统的控制问题。

这个项目最吸引我的地方在于采用了线性参数变化（LPV）方法处理非线性系统。不同于简单的线性化，LPV-MPC通过将非线性系统表示为参数依赖的线性系统集合，既保留了MPC的计算效率，又兼顾了非线性系统的动态特性。我在去年调试一台工业机械臂时曾尝试过类似方法，当时为了实时更新系统矩阵绞尽脑汁。

2. 系统建模关键点解析

2.1 四旋翼动力学方程处理

建立准确的动力学模型是控制的基础。四旋翼的六自由度模型可以表示为：

matlab复制% 位置动力学
dx/dt = v;
m*dv/dt = [0;0;-mg] + R*[0;0;T];

% 姿态动力学
dΘ/dt = W*ω;
J*dω/dt = τ - ω×Jω;

其中R是旋转矩阵，W是角速度转换矩阵。这里有个易错点：很多初学者会忽略ω×Jω这一项，实际上在高速旋转时这项影响显著。我在第一次仿真时就因为漏掉这项导致姿态发散。

2.2 LPV模型转换技巧

将非线性模型转换为LPV形式的核心是选择合理的调度变量。对于四旋翼系统，我推荐采用以下参数化方式：

code复制A(ρ) = A0 + ρ1*A1 + ρ2*A2
B(ρ) = B0 + ρ1*B1

其中ρ可以是俯仰角、滚转角等关键状态变量。在Matlab中实现时，建议预先计算好各系数矩阵，避免在线计算消耗过多资源。这里分享一个调试技巧：先用固定参数验证控制器基本功能，再逐步引入参数变化。

3. 双闭环MPC设计详解

3.1 位置环控制器设计

位置环MPC的代价函数通常设计为：

matlab复制J = Σ( (x-x_ref)'Q(x-x_ref) + u'Ru ) 

Q矩阵的选取直接影响跟踪性能。我的经验是：对于8字形轨迹这种连续变化路径，位置误差权重应该比速度误差权重大3-5倍。在Matlab中可以通过mpcmove函数实现优化求解，但需要注意：

重要提示：预测时域不宜过长，一般选择20-30个采样步长。我测试发现超过50步会导致计算延迟，反而降低控制性能。

3.2 姿态环的特殊处理

姿态环需要特别注意欧拉角奇异性问题。我的解决方案是：

1. 在±80°时切换到四元数表示
2. 在MPC代价函数中加入姿态误差的余弦项：

matlab复制J_att = 1 - cos(θ-θ_ref)

这种处理方式在去年参加无人机竞赛时帮我们避免了多次翻转导致的失控。实际调试时，建议先用阶跃信号验证姿态环的响应速度，确保能跟上位置环的需求。

4. 8字形轨迹生成与跟踪

4.1 轨迹参数化方法

理想的8字形轨迹可以用Lissajous曲线表示：

matlab复制x_ref = A_x*sin(ωt);
y_ref = A_y*sin(2ωt+φ); 
z_ref = h_const;

但直接跟踪这种解析式轨迹在实际中会遇到两个问题：

1. 加速度不连续导致控制量突变
2. 拐点处容易超调

我的改进方案是：

1. 对参考轨迹进行一阶滤波
2. 在拐点前0.5s提前减速
3. 加入速度前馈补偿

4.2 跟踪性能优化技巧

通过多次实验，我总结了几个提升跟踪精度的关键点：

1. 采样时间选择：位置环20ms，姿态环5ms最佳
2. 权重调整策略：在轨迹曲率大的区段自动增大Q矩阵值
3. 抗风扰处理：加入扰动观测器，通过mpcstate对象实时更新状态

下表是我调试过程中总结的参数配置经验：

5. Matlab实现关键代码解析

5.1 LPV-MPC核心代码结构

matlab复制% 初始化MPC控制器
mpcobj = mpc(lti_model, Ts);
mpcobj.PredictionHorizon = 25;
mpcobj.ControlHorizon = 5;

% 在线更新LPV模型
function updateModel(rho)
    A = A0 + rho(1)*A1 + rho(2)*A2;
    B = B0 + rho(1)*B1;
    setmpcdata(mpcobj,'model',ss(A,B,C,D));
end

这段代码有个隐藏坑点：直接更新SS模型会导致控制器重置。正确做法是先用mpcdata获取当前状态，更新后再用setmpcdata恢复状态。

5.2 实时仿真循环框架

matlab复制for k = 1:Nsteps
    % 获取当前状态
    x = plant.State;
    
    % 计算调度变量
    rho = [sin(x(4)), cos(x(5))]; 
    
    % 更新LPV模型
    updateModel(rho);
    
    % MPC求解
    u = mpcmove(mpcobj, mpcstate, x, x_ref);
    
    % 更新系统状态
    plant.State = plant.A*x + plant.B*u;
end

在实现时强烈建议加入求解时间监控，我用tic/toc发现当求解时间超过采样周期50%时就需要优化QP参数了。

6. 典型问题排查指南

6.1 姿态环发散问题

现象：滚转角或俯仰角持续增大直至失控
排查步骤：

1. 检查ω×Jω项是否建模正确
2. 验证陀螺仪数据极性
3. 减小姿态环MPC的预测时域

6.2 轨迹跟踪滞后

解决方案：

1. 在MPC代价函数中加入速度误差项
2. 引入参考加速度前馈
3. 检查电机响应延迟，必要时增加PWM频率

6.3 实时性不足

优化方案：

1. 使用显式MPC替代在线优化
2. 采用C代码生成（需要Matlab Coder）
3. 减少优化变量维度

最后分享一个实用技巧：在调试初期，可以先用mpcDesigner可视化工具快速验证控制器结构，再逐步移植到脚本中实现自动化。这比直接写代码调试效率高得多。