1. 四旋翼飞行器圆形轨迹跟踪控制概述
去年夏天调试无人机时，我发现要让四旋翼稳定跟踪圆形轨迹并非易事。传统PID控制在轨迹跟踪中常出现相位滞后和超调问题，特别是在转弯处尤为明显。经过多次尝试，最终采用模型预测控制(MPC)方案实现了0.1rad/s角速度下的5米高度圆形轨迹跟踪，水平位置误差控制在±0.15米以内。
这个方案的核心在于：通过建立状态空间模型，在每个控制周期求解有限时域内的最优控制问题。与PID的"事后补偿"不同，MPC具有"前瞻性"——它能基于当前状态预测未来多步的系统行为，并据此计算出最优控制序列。这就好比老司机开车时会预判前方路况提前调整方向，而不是等偏离车道后才急打方向盘。
2. 系统建模与控制器设计
2.1 状态空间模型构建
在水平面轨迹跟踪场景中，我简化了六自由度模型，重点关注x-y平面的位置控制。状态向量选取为[x, y, z, vx, vy, vz]，控制输入为三个轴向的加速度指令。对应的离散状态方程矩阵为：
matlab复制A = [1 0 0 dt 0 0; % x位置更新
0 1 0 0 dt 0; % y位置更新
0 0 1 0 0 dt; % z位置更新
0 0 0 1 0 0; % x速度保持
0 0 0 0 1 0; % y速度保持
0 0 0 0 0 1]; % z速度保持
B = [0.5*dt^2 0 0; % x加速度影响
0 0.5*dt^2 0; % y加速度影响
0 0 0.5*dt^2; % z加速度影响
dt 0 0; % x速度变化
0 dt 0; % y速度变化
0 0 dt]; % z速度变化
其中dt=0.1s为控制周期。这个模型将复杂的动力学简化为双积分器系统，实测表明对低速轨迹跟踪足够精确。需要注意的是，z轴单独控制高度，与水平面解耦处理。
2.2 参考轨迹生成
目标圆形轨迹参数化表示为：
matlab复制radius = 5; % 圆形轨迹半径(m)
omega = 0.1; % 角速度(rad/s)
z_ref = 5; % 恒定高度(m)
x_ref = radius * cos(omega * k*dt);
y_ref = radius * sin(omega * k*dt);
这里k为当前步数。为避免初始阶跃冲击，我采用了渐进式启动策略：前5秒内将参考半径从0线性增加到5米，使无人机平滑进入圆形轨迹。
3. MPC控制器实现细节
3.1 优化问题构建
在每个控制周期求解如下优化问题：
code复制min Σ(||x(k+i|k)-x_ref(k+i)||_Q + ||u(k+i)||_R)
s.t. x(k+i+1|k) = Ax(k+i|k) + Bu(k+i)
u_min ≤ u(k+i) ≤ u_max
使用MATLAB MPC工具箱配置参数：
matlab复制mpcObj = mpc(A, B, dt);
mpcObj.PredictionHorizon = 10; % 预测步长1秒
mpcObj.ControlHorizon = 3; % 控制步长0.3秒
mpcObj.Weights.OutputVariables = [10 10 5 1 1 1];
mpcObj.Weights.ManipulatedVariables = [0.1 0.1 0.1];
mpcObj.Weights.ManipulatedVariablesRate = [1 1 1];
权重设置经验：
- 位置误差权重 > 速度误差权重
- x/y权重相同保证各向同性
- 控制量权重过大会导致响应迟缓
3.2 实时控制流程
实际飞行中的控制闭环包含以下步骤：
- 通过IMU和视觉里程计获取当前状态x(k)
- 生成未来N步的参考轨迹x_ref(k+1)...x_ref(k+N)
- 调用mpcmove求解最优控制序列u(k)...u(k+M-1)
- 仅执行第一个控制量u(k)
- 等待下一个控制周期重复流程
关键技巧：在嵌入式平台实现时，可将QP求解替换为更高效的显式MPC或ADMM算法，将计算时间从50ms缩短到5ms以内。
4. 调试经验与性能优化
4.1 参数整定方法
通过"先比例后微分"的原则调整权重：
- 先增大Q对角元素使系统快速响应
- 出现振荡时适当增加R权重
- 最后调整预测时域平衡实时性与性能
实测发现当时域N=10、M=3时，在树莓派4B上单步求解耗时约35ms，满足实时性要求。
4.2 典型问题排查
问题1：转弯处轨迹外偏
- 原因：向心力不足
- 解决：临时增加Q矩阵中位置误差权重
问题2：高度方向振荡
- 原因：z轴响应过快
- 解决：降低z轴权重或增加低通滤波
问题3：计算延迟导致相位滞后
- 原因：求解耗时过长
- 解决：减少预测时域或改用显式MPC
5. 扩展应用与改进方向
当前方案可进一步扩展：
- 加入风扰观测器补偿外界干扰
- 采用自适应MPC在线调整模型参数
- 结合视觉信息实现动态避障
在最近一次户外测试中，该系统在3级风况下仍能保持轨迹跟踪误差在0.3米以内。一个实用的建议是：在地面站实时绘制预测轨迹与实际轨迹的对比图，这能直观显示MPC的前瞻控制效果。