四旋翼无人机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]; % 最大推力/力矩 % 生成约束矩阵(预测时域N=10) 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))跑完仿真别急着收工，记得检查控制量的变化率。有次忘了给控制增量加约束，结果电机指令疯狂跳变，仿真出来的控制曲线跟心电图似的——这要搁真机早炸了。
