四旋翼无人机simulink轨迹跟踪 mpc 文档解释说明四旋翼的轨迹跟踪算是控制领域的老朋友了今天咱们来聊聊怎么用Simulink搭个带模型预测控制MPC的实时代码。先扔个重点——MPC的核心在于在线求解优化问题但无人机这玩意儿状态变量多得很直接硬算怕是要崩。先看状态方程部分咱们在Simulink里用MATLAB Function块实现最方便。下面这段代码处理无人机动力学模型注意这里用了欧拉角简化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))]; % 推力分配 F [0; 0; u(1)]; torque [u(2); u(3); u(4)]; % 动力学方程 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*F)/m - [0; 0; g]; dx(10:12) inv([Ix 0 0; 0 Iy 0; 0 0 Iz]) * (torque - cross(x(10:12), [Ix; Iy; Iz].*x(10:12))); end这里有几个坑要注意欧拉角存在奇点问题当俯仰角接近±90度时会崩实际项目建议用四元数。不过仿真嘛咱们先凑合用这个简化版。四旋翼无人机simulink轨迹跟踪 mpc 文档解释说明MPC控制器部分建议用S函数实现核心是构造二次规划问题。重点看看约束设置这段% 构建输入约束矩阵 umin [5; -0.5; -0.5; -0.2]; % 最小推力/力矩 umax [20; 0.5; 0.5; 0.2]; % 最大推力/力矩 % 生成约束矩阵(预测时域N10) A_con kron(eye(N), [eye(4); -eye(4)]); b_con repmat([umax; -umin], N, 1); % 加入状态约束防止翻跟头 phi_limit deg2rad(30); theta_limit deg2rad(25); A_state blkdiag(kron(eye(N-1), [0 0 0 1 0 0 0 0 0 0 0 0; 0 0 0 0 1 0 0 0 0 0 0 0])); b_state repmat([phi_limit; theta_limit], N-1, 1); % 合并约束 A_total [A_con; A_state]; b_total [b_con; b_state];这里玩了个小花招——把姿态角约束转换成线性不等式约束。注意预测时域每步的状态约束要对应到状态向量的正确位置搞错索引会出大事。仿真时发现个有趣现象当权重矩阵Q里位置误差的权重超过角速度权重20倍时无人机会出现点头现象。这其实是因为系统在快速修正位置时牺牲了姿态稳定性调参时得盯着状态变化曲线慢慢找平衡。最后给个仿真结果分析的代码片段% 绘制三维轨迹对比 figure(Color,white) plot3(ref(:,1), ref(:,2), ref(:,3), r--, LineWidth,2) hold on plot3(logsout{1}.Values.Data(:,1),... logsout{1}.Values.Data(:,2),... logsout{1}.Values.Data(:,3), b-) legend(期望轨迹,实际轨迹) xlabel(X(m)); ylabel(Y(m)); zlabel(Z(m)) view(45,30) grid on % 计算跟踪误差指标 pos_error vecnorm(ref(:,1:3) - logsout{1}.Values.Data(:,1:3), 2, 2); fprintf(最大位置误差: %.2f m\n平均误差: %.2f m\n, max(pos_error), mean(pos_error))跑完仿真别急着收工记得检查控制量的变化率。有次忘了给控制增量加约束结果电机指令疯狂跳变仿真出来的控制曲线跟心电图似的——这要搁真机早炸了。