四旋翼无人机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度时会崩，实际项目建议用四元数。不过仿真嘛，咱们先凑合用这个简化版。

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))

跑完仿真别急着收工，记得检查控制量的变化率。有次忘了给控制增量加约束，结果电机指令疯狂跳变，仿真出来的控制曲线跟心电图似的——这要搁真机早炸了。