四旋翼无人机simulink轨迹跟踪 mpc

四旋翼无人机的轨迹跟踪总让人头秃——既要考虑空气动力学又要处理姿态耦合，传统PID刚调完俯仰角，偏航角又飘了。这时候Model Predictive Control（MPC）就像个自带预判的超管，提前算好未来几步的控制量，直接让无人机贴着参考轨迹飞。

先给飞机建个模。在Simulink里拖出六个微分方程模块，分别对应位置(x,y,z)和欧拉角(φ,θ,ψ)的动态。注意这里用四元数转换会更好，不过为了实时性还是选择了欧拉角简化：

% 四旋翼动力学核心方程片段
function dx = quad_dynamics(t, x, u)
    Ix = 0.03;  % 滚转惯量
    g = 9.81;
    dx = zeros(12,1);
    % 位置导数
    dx(1:3) = x(4:6); 
    % 牛顿部分
    dx(4) = (sin(x(11))*sin(x(9)) + cos(x(11))*sin(x(10))*cos(x(9)))*u(1);
    dx(5) = (-sin(x(9))*cos(x(11)) + sin(x(11))*sin(x(10))*cos(x(9)))*u(1);
    dx(6) = u(1)*cos(x(10))*cos(x(9)) - g;
    % 角速度微分
    dx(7:9) = [ (u(2) - x(8)*x(9)*(Iy-Iz))/Ix ; 
               %...省略其他轴的惯量计算
              ];
end

建模时最容易踩的坑是坐标系转换。有次仿真时无人机突然倒着飞，查了半天发现是机体坐标系和地面系搞反了。建议把旋转矩阵单独封装成函数反复验证。

MPC控制器配置才是重头戏。用MPC Designer生成代码后别急着跑，先改这几个参数：

mpcobj.PredictionHorizon = 15;  % 预测步长
mpcobj.ControlHorizon = 3;     % 控制步长
mpcobj.Weights.OutputVariables = [10 10 1 0.5];  % 位置权重>姿态
mpcobj.Weights.ManipulatedVariablesRate = [0.1 0.1 0.1 0.1]; % 抑制电机突变

实测发现输出权重里高度方向要给高点，不然遇到风扰容易掉高。控制量变化率权重能防止电机指令跳变，避免烧电调。

四旋翼无人机simulink轨迹跟踪 mpc

调试时开着Simulink的Scope看各状态量变化，突然发现滚转角跟踪滞后。这时候别急着调参，先检查预测模型是否准确——把MPC的Plant模型换成实际动力学模型再跑一遍，如果误差消失说明需要重新辨识模型参数。

最终效果要看跟踪误差曲线和电机转速曲线。好的MPC控制应该像这样：

• X/Y方向误差不超过5cm（在2m/s速度下）
• 四个电机转速曲线平滑，没有锯齿状波动
• 遭遇阵风扰动后能在0.8秒内恢复跟踪

有个骚操作是在目标轨迹生成器里加入障碍物规避算法，让MPC的参考轨迹动态变化。这时候需要把预测时域拉长到25步以上，不过得换用QP求解器的快速模式，否则实时性撑不住。

最后提醒：别在树莓派上跑这个仿真！之前用Jetson Nano实时测试时，发现Simulink生成代码的效率比手写C++低30%，后来改用S-function封装C++求解器才搞定。