在无人机自主飞行系统中，精确的路径跟踪能力是核心挑战之一。采用状态空间建模结合模型预测控制（MPC）方法，能够有效应对四旋翼飞行器复杂的非线性动力学特性。本文聚焦于非线性模型预测控制（NMPC）在该场景下的应用，通过构建完整的动态模型、设计约束优化控制器，并借助MATLAB进行仿真验证。

四旋翼系统动力学建模

四旋翼的运动行为由其运动学与动力学方程共同描述。其中，运动学关系定义了位置与姿态随时间的变化规律，而动力学则揭示了推力与力矩如何影响飞行器的加速度和角速度。以牛顿-欧拉方法为基础，可建立包含六个自由度的状态空间表达式。

在仿真环境中，首先设定基本物理参数：

% 四旋翼系统参数
mass = 0.5;           % kg
gravity = 9.81;       % m/s²
inertia_xx = 0.005;
inertia_yy = 0.005;
inertia_zz = 0.008;

多变量状态空间模型构建

非线性状态方程

完整反映飞行器动态特性的非线性模型以状态向量形式表示为：

$$ \dot{\mathbf{x}} = \mathbf{f}(\mathbf{x}, \mathbf{u}) $$

其中状态量 $\mathbf{x}$ 包含三维位置、三轴姿态角、线速度及角速度，输入 $\mathbf{u}$ 对应四个电机的总推力与力矩分配。

以下为MATLAB函数实现的非线性动力学模块：

function dx = quadrotor_dynamics(~, x, u)
    % 状态: [x; y; z; roll; pitch; yaw; vx; vy; vz; p; q; r]
    % 输入: [thrust1; thrust2; thrust3; thrust4]
    
    m = 0.5; g = 9.81;
    Ixx = 0.005; Iyy = 0.005; Izz = 0.008;
    
    phi = x(4); theta = x(5); psi = x(6);
    vx = x(7); vy = x(8); vz = x(9);
    p = x(10); q = x(11); r = x(12);
    
    % 推力合成
    F_total = u(1) + u(2) + u(3) + u(4);
    
    dx = zeros(12, 1);
    dx(1) = vx;
    dx(2) = vy;
    dx(3) = vz;
    dx(4) = p + sin(phi)*tan(theta)*q + cos(phi)*tan(theta)*r;
    dx(5) = cos(phi)*q - sin(phi)*r;
    dx(6) = (sin(phi)/cos(theta))*q + (cos(phi)/cos(theta))*r;
    dx(7) = (F_total * (cos(phi)*sin(psi) + sin(phi)*sin(theta)*cos(psi))) / m;
    dx(8) = (F_total * (cos(phi)*cos(psi) - sin(phi)*sin(theta)*sin(psi))) / m;
    dx(9) = (F_total * cos(phi)*cos(theta)) / m - g;
    dx(10) = (Iyy - Izz) * q * r / Ixx;
    dx(11) = (Izz - Ixx) * p * r / Iyy;
    dx(12) = (Ixx - Iyy) * p * q / Izz;
end

局部线性化模型

为了简化控制设计，可在平衡点附近对非线性系统进行泰勒展开，获得近似线性模型：

$$ \dot{\mathbf{x}} = \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u} $$

利用MATLAB的 linmod 或 linearize 工具可自动完成此过程，得到适用于线性MPC设计的系统矩阵。

基于约束的模型预测控制设计

线性MPC控制器实现

针对线性模型，使用MATLAB MPC Toolbox快速搭建控制器框架：

% 构造线性系统模型
sys_lin = ss(A, B, C, D);

% 创建MPC对象
mpc_ctrl = mpc(sys_lin);

% 设置控制参数
mpc_ctrl.PredictionHorizon = 10;
mpc_ctrl.ControlHorizon = 5;

% 添加硬件约束
mpc_ctrl.MV.Max = 10;   % 最大控制输出
mpc_ctrl.MV.Min = 0;
mpc_ctrl.OV.Max = 1;    % 输出上限
mpc_ctrl.OV.Min = -1;

非线性MPC优化求解

对于更真实的情况，直接基于非线性模型进行优化更具优势。使用 fmincon 求解器配合目标函数定义如下：

function J = tracking_cost(u_seq, x_current, ref_traj)
    % u_seq: 预测控制序列
    % x_current: 当前状态
    % ref_traj: 参考轨迹
    
    x_next = ode45(@(t,x) quadrotor_dynamics(t,x,u_seq), [0 0.01], x_current);
    error = x_next(end, 1:3) - ref_traj(1:3);
    J = sum(error.^2) + 0.1*sum(u_seq.^2); % 加入惩罚项
end

% 优化求解
opt = optimoptions('fmincon', 'Display', 'off');
[best_u, ~] = fmincon(@(u) tracking_cost(u, x_curr, ref), ...
                      u_init, [], [], [], [], lb, ub, [], opt);

仿真实现与结果分析

将控制器嵌入主循环，逐步执行预测-控制-积分流程：

% 初始化
x0 = zeros(12, 1);
time_span = 0:0.01:10;
states = zeros(length(time_span), 12);
controls = zeros(length(time_span), 4);

for i = 1:length(time_span)-1
    % 调用NMPC获取最优控制
    u_opt = solve_nmpc(states(i,:), ref_trajectory(i,:));
    
    controls(i,:) = u_opt';
    
    % 积分下一时刻状态
    [~, x_next] = ode45(@(t,x) quadrotor_dynamics(t,x,u_opt), ...
                        time_span(i:i+1), states(i,:)');
    states(i+1,:) = x_next(end,:);
end

% 绘图显示轨迹跟踪效果
figure;
subplot(3,1,1);
plot(time_span, states(:,1), 'b', time_span, ref_trajectory(:,1), 'r--');
xlabel('Time (s)'); ylabel('X Position (m)');
legend('Actual', 'Reference');
% Y/Z方向同理绘制

路径跟踪的本质

无论采用线性或非线性策略，路径跟踪的核心目标始终是寻找一组最优控制指令，使系统实际轨迹与期望轨迹之间的偏差最小化。非线性MPC通过在每个采样周期内求解一个带约束的非线性优化问题，能够更准确地处理强耦合、时变非线性系统，从而提升跟踪精度与鲁棒性。

通过上述从建模到控制器设计再到仿真实验的完整流程，展示了基于状态空间模型的非线性预测控制在四旋翼路径跟踪中的强大潜力，为复杂飞行任务提供了高精度、高稳定性的解决方案。