1. 四旋翼MPC控制概述
四旋翼无人机的轨迹跟踪控制一直是自动控制领域的经典问题。相比传统的PID控制,模型预测控制(MPC)因其能够显式处理约束和优化未来行为的特性,在无人机控制中展现出独特优势。不过在实际实现时,MPC的计算复杂度常常让人望而却步——特别是当我们需要在嵌入式设备上实时运行时。
我在过去三年里为不同型号的四旋翼实现过七种MPC变体,发现Simulink平台特别适合这类算法的快速原型开发。它提供的MATLAB Function块和S-function机制,既能保持建模灵活性,又能生成高效的C代码。下面我就以最常用的线性MPC为例,分享一套经过实战检验的实现方案。
2. 动力学建模关键点
2.1 欧拉角模型的取舍
初学者最常犯的错误就是直接套用教科书上的六自由度刚体模型。实际上,四旋翼的动力学特性允许我们做一些合理简化:
matlab复制function [dx] = quadcopterModel(x, u)
% 状态量: [x y z phi theta psi vx vy vz p q r]
g = 9.81; m = 1.2; Ix = 0.034; Iy = 0.045; Iz = 0.097;
% 简化的旋转矩阵(ZYX顺序)
R = [cos(x(5))*cos(x(6)) ...
sin(x(4))*sin(x(5))*cos(x(6)) - cos(x(4))*sin(x(6)) ...
cos(x(4))*sin(x(5))*cos(x(6)) + sin(x(4))*sin(x(6));
cos(x(5))*sin(x(6)) ...
sin(x(4))*sin(x(5))*sin(x(6)) + cos(x(4))*cos(x(6)) ...
cos(x(4))*sin(x(5))*sin(x(6)) - sin(x(4))*cos(x(6));
-sin(x(5)) ...
sin(x(4))*cos(x(5)) ...
cos(x(4))*cos(x(5))];
% 动力学方程
dx(1:3) = x(7:9);
dx(4:6) = [1 sin(x(4))*tan(x(5)) cos(x(4))*tan(x(5));
0 cos(x(4)) -sin(x(4));
0 sin(x(4))/cos(x(5)) cos(x(4))/cos(x(5))] * x(10:12);
dx(7:9) = (R*[0;0;u(1)])/m - [0;0;g];
dx(10:12) = inv([Ix 0 0; 0 Iy 0; 0 0 Iz]) * ...
([u(2);u(3);u(4)] - cross(x(10:12),[Ix;Iy;Iz].*x(10:12)));
end
这个模型有几点需要注意:
- 欧拉角在θ=±90°时会出现万向节锁死,这在特技飞行时会出问题
- 忽略了空气阻力项,在高速运动时会有明显误差
- 假设电机响应是理想的,没有考虑延迟和饱和
实际项目中如果飞行包线较大,建议改用四元数表示姿态。不过对于常规的轨迹跟踪任务,这个简化模型已经足够。
2.2 模型线性化技巧
MPC需要线性模型来进行预测,我通常在悬停点附近进行雅可比矩阵线性化:
matlab复制% 在悬停状态(x0,u0)处线性化
x0 = zeros(12,1);
u0 = [m*g; 0; 0; 0];
[A,B] = jacobianest(@(x,u)quadcopterModel(x,u),x0,u0);
C = eye(12);
D = zeros(12,4);
sys_linear = ss(A,B,C,D);
线性化时常见的坑:
- 雅可比矩阵计算不准确会导致预测失准
- 没有检查可控性矩阵的秩
- 忽略了采样时间对离散化模型的影响
我习惯用MATLAB的jacobianest函数而非手动求导,它能自动计算数值雅可比,避免符号运算的麻烦。
3. MPC控制器实现
3.1 二次规划问题构建
MPC的核心是每个控制周期求解如下优化问题:
matlab复制% 预测时域N=10
Q = diag([10 10 10 5 5 1 1 1 1 1 1 1]); % 状态权重
R = 0.1*eye(4); % 控制量权重
% 构造预测矩阵
[Phi, Gamma] = predict_mats(A,B,N);
Q_bar = kron(eye(N),Q);
R_bar = kron(eye(N),R);
% 目标函数矩阵
H = Gamma'*Q_bar*Gamma + R_bar;
f = Gamma'*Q_bar*Phi*x_current;
这里有个关键技巧:使用预测矩阵提前计算好Φ和Γ矩阵,避免在线重复计算:
matlab复制function [Phi, Gamma] = predict_mats(A,B,N)
Phi = [];
Gamma = zeros(size(A,1)*N, size(B,2)*N);
for i = 1:N
Phi = [Phi; A^i];
for j = 1:i
Gamma((i-1)*size(A,1)+1:i*size(A,1), (j-1)*size(B,2)+1:j*size(B,2)) = A^(i-j)*B;
end
end
end
3.2 约束处理实战经验
约束设置不当是导致MPC失败的主要原因之一。我的标准约束配置如下:
matlab复制% 输入约束
umin = [5; -0.5; -0.5; -0.2];
umax = [20; 0.5; 0.5; 0.2];
% 状态约束(防止过度倾斜)
phi_max = deg2rad(30);
theta_max = deg2rad(25);
% 构造约束矩阵
A_con = [kron(eye(N), [eye(4); -eye(4)]); % 输入约束
kron(eye(N-1), [zeros(2,4), [1 0;0 1], zeros(2,6)])]; % 姿态约束
b_con = [repmat([umax; -umin], N, 1);
repmat([phi_max; theta_max], N-1, 1)];
特别注意:
- 姿态约束只需加到前N-1步,因为最后一步不影响当前控制
- 约束矩阵的稀疏性可以大幅提升求解效率
- 实际项目中还要考虑控制增量约束
4. 仿真与调试技巧
4.1 典型问题诊断
当出现"点头"现象时,通常是因为:
- 状态权重矩阵Q中位置误差权重过大
- 预测时域N设置过短
- 缺少速度或角速度的微分约束
我的调参经验是:
- 先调姿态环(Q中角度相关项)
- 再调位置环(Q中位置相关项)
- 最后调整控制量权重R
一个好的权重比例是:位置误差:角度误差:角速度 ≈ 10:5:1
4.2 结果分析方法
完整的仿真分析应该包括:
matlab复制% 轨迹对比
figure('Position',[100 100 800 600])
subplot(2,2,[1 3])
plot3(ref(:,1),ref(:,2),ref(:,3),'r--','LineWidth',2)
hold on
plot3(actual(:,1),actual(:,2),actual(:,3),'b-')
legend('参考','实际')
title('三维轨迹')
% 误差分析
subplot(2,2,2)
plot(time, vecnorm(ref(:,1:3)-actual(:,1:3),2,2))
title('位置误差范数')
% 控制量检查
subplot(2,2,4)
stairs(time, u_hist(:,1))
hold on
plot([time(1) time(end)], [umax(1) umax(1)], 'r--')
plot([time(1) time(end)], [umin(1) umin(1)], 'r--')
title('推力指令')
特别注意控制量的变化率,我常用这个指标检查是否需添加速率约束:
matlab复制du = diff(u_hist);
fprintf('最大推力变化率: %.2f N/s\n', max(abs(du(:,1)/dt)));
5. 工程实现建议
5.1 代码生成优化
在Simulink中准备代码生成时:
- 将QP求解器封装成Level-2 S-function
- 启用仿真加速模式(Accelerator)
- 设置合理的堆栈大小
我的S-function模板包含以下关键部分:
matlab复制function mpc_sfun(block)
setup(block);
function setup(block)
block.NumInputPorts = 2; % 状态和参考
block.NumOutputPorts = 1; % 控制量
block.SetPreCompInpPortInfoToDynamic;
block.SetPreCompOutPortInfoToDynamic;
block.SampleTimes = [Ts 0]; % 离散采样
block.RegBlockMethod('Outputs', @Outputs);
end
function Outputs(block)
x = block.InputPort(1).Data;
ref = block.InputPort(2).Data;
% 构造QP并求解
[u_opt, status] = solve_mpc(x, ref);
if status > 0
block.OutputPort(1).Data = u_opt;
else
% 异常处理
end
end
end
5.2 实时性保障
在Pixhawk等飞控上实时运行MPC的要点:
- 使用qpOASES等嵌入式QP求解器
- 限制最大迭代次数
- 采用热启动技巧
实测在STM32H7上,一个20变量的QP问题能在5ms内求解完毕,满足100Hz的控制频率要求。关键是要预计算所有不依赖状态的矩阵,在线只计算f向量。
最后提醒:仿真通过不等于实机能飞!务必在安全环境下进行系留测试,逐步放开约束条件。我在第一次实飞测试时,因为没考虑电机延迟,差点酿成事故——MPC给出的控制指令太激进,导致电机响应跟不上。后来增加了低通滤波和速率限制才解决问题。