基于状态空间模型预测控制的四旋翼路径跟踪：非线性模型预测控制（NMPC）探秘

jFWTpMJfJjq

217人浏览 · 2026-03-24 09:00:00

jFWTpMJfJjq · 2026-03-24 09:00:00 发布

非线性模型预测控制 nmpc 基于状态空间模型预测控制的四旋翼路径跟踪实现 1. 利用已有的四旋翼运动学与动力学模型 2. 建立MIMO状态空间模型，包括非线性模型与简化后的线性模型 3. 引入约束MPC控制，分别设计线性MPC控制器与非线性MPC控制器。 4. 基于matlab的仿真实验，运行获得轨迹跟踪的图片和数据 5. 跟踪问题就是找到一个合适的控制输入，使得跟踪误差最小

在无人机控制领域，路径跟踪是一个关键问题，而基于状态空间模型预测控制为四旋翼实现精准路径跟踪提供了强大的方法，其中非线性模型预测控制（NMPC）更是有着独特的魅力。

利用四旋翼运动学与动力学模型

四旋翼飞行器的运动学和动力学模型是其控制的基础。简单来说，运动学描述了四旋翼位置和姿态随时间的变化关系，而动力学则揭示了力和力矩如何影响其运动。以常见的牛顿 - 欧拉方程为基础来建立模型，例如描述四旋翼在空间中的位置$[x, y, z]$和姿态$[\phi, \theta, \psi]$（分别为滚转、俯仰和偏航角）与施加的力和力矩之间的关系。

在Matlab中，我们可以定义相关参数，比如：

% 定义四旋翼的物理参数
m = 0.5; % 质量
g = 9.81; % 重力加速度
Ixx = 0.005; Iyy = 0.005; Izz = 0.008; % 转动惯量

这里定义了四旋翼的质量、重力加速度以及转动惯量，这些参数在后续的模型建立和控制算法设计中都至关重要。

建立MIMO状态空间模型

非线性模型

多输入多输出（MIMO）状态空间模型能更好地描述四旋翼这种复杂系统。非线性模型考虑了系统的完整动力学特性，包含了各种非线性因素，如空气动力学效应等。状态方程可以写为：

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

其中，$\mathbf{x}$是状态向量，包含位置、速度、姿态和角速度等信息，$\mathbf{u}$是控制输入向量，通常是四个旋翼产生的推力。

在Matlab中，我们可以用函数来实现这个非线性模型：

function dxdt = nonlinear_model(t, x, u)
    % x: [x; y; z; phi; theta; psi; vx; vy; vz; p; q; r]
    % u: [u1; u2; u3; u4]
    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);
    u1 = u(1); u2 = u(2); u3 = u(3); u4 = u(4);
    
    dxdt = zeros(12, 1);
    dxdt(1) = vx;
    dxdt(2) = vy;
    dxdt(3) = vz;
    dxdt(4) = p + sin(phi)*tan(theta)*q + cos(phi)*tan(theta)*r;
    dxdt(5) = cos(phi)*q - sin(phi)*r;
    dxdt(6) = (sin(phi)/cos(theta))*q + (cos(phi)/cos(theta))*r;
    dxdt(7) = (u1*(cos(phi)*sin(psi)+sin(phi)*sin(theta)*cos(psi)))/m;
    dxdt(8) = (u1*(cos(phi)*cos(psi)-sin(phi)*sin(theta)*sin(psi)))/m;
    dxdt(9) = (u1*cos(phi)*cos(theta))/m - g;
    dxdt(10) = (Iyy - Izz)/Ixx*q*r;
    dxdt(11) = (Izz - Ixx)/Iyy*p*r;
    dxdt(12) = (Ixx - Iyy)/Izz*p*q;
end

这个函数nonlinear_model根据给定的状态x和控制输入u计算状态的导数dxdt，完整地体现了四旋翼的非线性动力学特性。

简化后的线性模型

虽然非线性模型精确，但计算复杂。在一些情况下，我们可以对其进行线性化处理，得到简化的线性模型。通常在平衡点附近进行线性化，线性化后的状态空间模型可以表示为：

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

$\mathbf{y} = \mathbf{C}\mathbf{x} + \mathbf{D}\mathbf{u}$

其中，$\mathbf{A}$、$\mathbf{B}$、$\mathbf{C}$、$\mathbf{D}$是系统矩阵。在Matlab中，可以使用linmod函数对上述非线性模型进行线性化。

引入约束MPC控制

线性MPC控制器设计

模型预测控制（MPC）的核心思想是在每个采样时刻预测系统未来的输出，并通过优化算法计算出最优的控制输入序列。对于线性MPC，我们基于线性化后的状态空间模型进行设计。在Matlab中，可以使用mpc工具箱来设计线性MPC控制器。

% 假设已经得到线性化模型 sys_lin
sys_lin = ss(A, B, C, D);
mpcobj = mpc(sys_lin);
% 设置预测时域和控制时域
mpcobj.Np = 10;
mpcobj.Nc = 5;
% 定义约束
mpcobj.InputConstraints.UB = [10; 10; 10; 10];
mpcobj.InputConstraints.LB = [0; 0; 0; 0];
mpcobj.OutputConstraints.UB = [1; 1; 1; 1; 1; 1];
mpcobj.OutputConstraints.LB = [-1; -1; -1; -1; -1; -1];

这里设置了预测时域Np为10，控制时域Nc为5，并定义了输入输出的约束。通过这些设置，线性MPC控制器能够在满足约束的情况下，优化控制输入以实现路径跟踪。

非线性MPC控制器设计

非线性MPC则直接基于非线性状态空间模型进行优化。相比线性MPC，它能更好地处理系统的非线性特性，但计算量更大。在Matlab中，可以使用fmincon等优化函数结合非线性模型来实现非线性MPC。

% 定义目标函数，例如最小化跟踪误差
function cost = objective(u, x, ref)
    % 假设ref是参考轨迹
    % 根据u和x计算下一时刻的状态x_next
    x_next = nonlinear_model(0, x, u);
    % 计算跟踪误差
    error = x_next(1:3) - ref(1:3); % 仅考虑位置误差
    cost = sum(error.^2);
end

% 优化求解
options = optimoptions('fmincon', 'Display', 'off');
[x_opt, cost_opt] = fmincon(@(u) objective(u, x_current, ref_current), u0, [], [], [], [], lb, ub, [], options);

这里定义了目标函数objective，通过最小化跟踪误差来寻找最优的控制输入u。fmincon函数在给定的约束条件下（lb和ub分别为控制输入的下限和上限）求解最优解。

基于Matlab的仿真实验

完成控制器设计后，我们在Matlab中进行仿真实验。通过设置参考轨迹，运行仿真，得到轨迹跟踪的图片和数据。

% 初始化状态和时间
x0 = zeros(12, 1);
tspan = 0:0.01:10;
% 存储数据
x_data = zeros(length(tspan), 12);
u_data = zeros(length(tspan), 4);
x_data(1, :) = x0';
for k = 1:length(tspan)-1
    % 这里根据选择的控制器（线性MPC或非线性MPC）计算控制输入u
    % 假设使用非线性MPC
    u_current = calculate_nonlinear_mpc(x_data(k, :)', ref(tspan(k), :));
    u_data(k, :) = u_current';
    % 积分得到下一时刻状态
    [~, x_next] = ode45(@(t, x) nonlinear_model(t, x, u_current), [tspan(k) tspan(k+1)], x_data(k, :)');
    x_data(k+1, :) = x_next';
end
% 绘制轨迹跟踪图片
figure;
subplot(3,1,1);
plot(tspan, x_data(:, 1), 'b', tspan, ref(:, 1), 'r--');
xlabel('Time (s)'); ylabel('X position (m)');
legend('Actual', 'Reference');
% 类似地绘制Y和Z位置

这段代码初始化了状态和时间，在每个时间步根据选择的控制器计算控制输入，通过积分更新状态，并最终绘制出轨迹跟踪的图片。