四旋翼无人机MPC控制仿真实践与调参指南

1. 四旋翼MPC控制仿真项目概述

去年实验室接手了一个农业植保无人机项目，客户要求飞行轨迹误差不超过20厘米。传统PID控制死活调不出理想效果，这才让我下定决心研究模型预测控制（MPC）。经过三个月的折腾，终于用MATLAB+CasADi搭出了一套可用的四旋翼MPC仿真系统。

这套仿真包含完整的动力学建模、MPC控制器设计和轨迹跟踪实现。最让我自豪的是解决了两个痛点：一是采用免安装的CasADi绿色版，避免了实验室电脑权限问题；二是用纯M代码实现了从建模到求解的完整流程，代码量控制在200行以内。

2. 核心原理与模型构建

2.1 四旋翼动力学建模关键

四旋翼的动力学模型本质上是个欠驱动系统——我们只有四个电机转速作为输入，却要控制六个自由度的运动。这个特性决定了模型必须精确描述力和力矩的传递关系。

核心状态方程包含12个状态量：

matlab复制% 状态量定义：
% [x y z]       - 位置坐标（米）
% [vx vy vz]    - 线速度（米/秒）
% [phi theta psi] - 横滚/俯仰/偏航角（弧度）
% [p q r]       - 角速度（弧度/秒）

旋转矩阵的构建是模型中最精妙的部分：

matlab复制R = [cos(theta)*cos(psi) sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi) cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi);
     cos(theta)*sin(psi) sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi) cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi);
     -sin(theta)         sin(phi)*cos(theta)                              cos(phi)*cos(theta)];

这个3×3矩阵完成了机体坐标系到世界坐标系的转换，相当于无人机的"空间定位系统"。

注意：欧拉角存在万向节死锁问题，当俯仰角θ接近±90°时会出现奇点。实际飞行中应避免大角度机动。

2.2 M