MATLAB - 利用非线性模型预测控制（Nonlinear MPC）来控制四旋翼飞行器

系列文章目录

---

前言

本示例展示了如何利用非线性模型预测控制（MPC）为四旋翼飞行器设计一个跟踪轨迹的控制器。

---

一、四旋翼模型

四旋翼飞行器有四个向上的旋翼。从四旋翼飞行器的质量中心出发，旋翼呈等距离的正方形排列。四旋翼飞行器动力学数学模型采用欧拉-拉格朗日方程 [1]。

四旋翼飞行器的十二种状态为

其中

• [x,y,z]表示惯性参考系中的位置。
• 角度位置[j,θ,ψ]分别表示滚动、俯仰和偏航。
• 其余状态为位置和角度的速度。

四旋翼飞行器的控制输入（也称为操纵变量，用 MV 表示）是四个旋翼的角速度平方：

这些控制输入可在机身 Z 轴方向产生力、扭矩和推力。在此示例中，每个状态都是可测量的，控制输入范围限制为：

?

非线性模型预测控制器使用的预测模型包括一个状态函数（表示作为当前状态和输入函数的状态导数）和一个状态雅各布函数（分别表示状态函数相对于状态和输入的导数）。这两个函数都是通过 Symbolic Math Toolbox? 软件建立和导出的。更多详情，请参阅为非线性模型预测控制推导四旋翼飞行器动力学（符号数学工具箱）。

调用脚本 getQuadrotorDynamicsAndJacobian 生成状态及其雅各布函数并写入文件。

getQuadrotorDynamicsAndJacobian;

getQuadrotorDynamicsAndJacobian 脚本生成以下文件：

• QuadrotorStateFcn.m - 状态函数
• QuadrotorStateJacobianFcn.m - 状态雅各布函数

有关任一函数的详细信息，请打开相应文件。

二、设计非线性模型预测控制器

创建一个具有 12 个状态、12 个输出和 4 个输入的非线性 MPC 对象。默认情况下，所有输入均为操纵变量 (MV)。

nx = 12;
ny = 12;
nu = 4;
nlmpcobj = nlmpc(nx, ny, nu);

Zero weights are applied to one or more OVs because there are fewer MVs than OVs.

?使用函数名称指定预测模型状态函数。也可以使用函数句柄指定函数。

nlmpcobj.Model.StateFcn = "QuadrotorStateFcn";

最佳做法是为预测模型提供分析雅各布。这样做可以大大提高仿真效率。使用函数句柄指定返回状态函数雅各布的函数。

nlmpcobj.Jacobian.StateFcn = @QuadrotorStateJacobianFcn;

?修复随机发生器种子，实现可重复性。

rng(0)

要检查 nlobj 的预测模型函数是否有效，请使用 validateFcns 对状态输入空间中的一个随机点进行检查。

validateFcns(nlmpcobj,rand(nx,1),rand(nu,1));

Model.StateFcn is OK.
Jacobian.StateFcn is OK.
No output function specified. Assuming "y = x" in the prediction model.
Analysis of user-provided model, cost, and constraint functions complete.

指定采样时间为 0.1 秒，预测范围为 18 步，控制范围为 2 步。

Ts = 0.1;
p = 18;
m = 2;
nlmpcobj.Ts = Ts;
nlmpcobj.PredictionHorizon = p;
nlmpcobj.ControlHorizon = m;

?将所有四个控制输入限制在 [0,10] 范围内。同时将控制输入变化率限制在 [-2,2] 的范围内，以防止突然和粗暴的移动。

nlmpcobj.MV = struct( ...
    Min={0;0;0;0}, ...
    Max={10;10;10;10}, ...
    RateMin={-2;-2;-2;-2}, ...
    RateMax={2;2;2;2} ...
    );

?非线性 MPC 的默认代价函数是标准二次代价函数，适用于参考跟踪和干扰抑制。在本例中，要求前 6 个状态 [x,y,z,?,θ,ψ] 遵循给定的参考轨迹。由于 MV 的数量（4 个）少于参考输出轨迹的数量（6 个），因此没有足够的自由度来独立跟踪所有输出的轨迹。

nlmpcobj.Weights.OutputVariables = [1 1 1 1 1 1 0 0 0 0 0 0];

在此示例中，MV 也有额定目标（稍后为模拟设置）。这些目标是在不需要跟踪时为保持四旋翼飞行器漂浮而设置的平均值，可能会导致 MV 和 OV 参考跟踪目标之间的冲突。要优先考虑 OV 目标，可将平均 MV 跟踪优先级设置为低于平均 OV 跟踪优先级。

nlmpcobj.Weights.ManipulatedVariables = [0.1 0.1 0.1 0.1];

此外，通过指定 MV 变化率的调整权重，惩罚过于激进的控制行动。

nlmpcobj.Weights.ManipulatedVariablesRate = [0.1 0.1 0.1 0.1];

三、闭环模拟

按照目标轨迹对系统进行 20 秒钟的模拟。

% Specify the initial conditions
x = [7;-10;0;0;0;0;0;0;0;0;0;0];

% Nominal control target (average to keep quadrotor floating)
nloptions = nlmpcmoveopt;
nloptions.MVTarget = [4.9 4.9 4.9 4.9]; 
mv = nloptions.MVTarget;

使用 nlmpcmove 函数模拟闭环系统，使用 nlmpcmove 对象指定模拟选项。

% Simulation duration in seconds
Duration = 20;

% Display waitbar to show simulation progress
hbar = waitbar(0,"Simulation Progress");

% MV last value is part of the controller state
lastMV = mv;

% Store states for plotting purposes
xHistory = x';
uHistory = lastMV;

% Simulation loop
for k = 1:(Duration/Ts)

    % Set references for previewing
    t = linspace(k*Ts, (k+p-1)*Ts,p);
    yref = QuadrotorReferenceTrajectory(t);

    % Compute control move with reference previewing
    xk = xHistory(k,:);
    [uk,nloptions,info] = nlmpcmove(nlmpcobj,xk,lastMV,yref',[],nloptions);

    % Store control move
    uHistory(k+1,:) = uk';
    lastMV = uk;

    % Simulate quadrotor for the next control interval (MVs = uk) 
    ODEFUN = @(t,xk) QuadrotorStateFcn(xk,uk);
    [TOUT,XOUT] = ode45(ODEFUN,[0 Ts], xHistory(k,:)');

    % Update quadrotor state
    xHistory(k+1,:) = XOUT(end,:);

    % Update waitbar
    waitbar(k*Ts/Duration,hbar);
end

% Close waitbar 
close(hbar)

四、可视化和结果

绘制结果图，并比较计划和实际的闭环轨迹。

plotQuadrotorTrajectory;

?

?

由于 MV 的数量少于参考输出轨迹的数量，因此没有足够的自由度来独立跟踪所有 OV 的理想轨迹。

如图所示，状态 [x,y,z,j,θ,ψ] 和控制输入、

• 状态[x,y,z]在 7 秒内与参考轨迹非常吻合。
• 状态 [j,θ,ψ]在 9 秒内被驱动到零点附近。
• 控制输入在 10 秒钟左右被驱动到目标值 4.9。

您可以将四旋翼飞行器的轨迹制成动画。在 7 秒内，四旋翼飞行器靠近沿参考轨迹飞行的 "目标 "四旋翼飞行器。之后，四旋翼飞行器将紧跟参考轨迹。动画在 20 秒时结束。

animateQuadrotorTrajectory;

?

五、结论

本例展示了如何设计用于四旋翼飞行器轨迹跟踪的非线性模型预测控制器。四旋翼飞行器的动力学和 Jacobian 是通过 Symbolic Math Toolbox 软件得出的。四旋翼飞行器能紧密跟踪参考轨迹。?

参考资料

[1] Raffo, Guilherme V., Manuel G. Ortega, and Francisco R. Rubio. "An integral predictive/nonlinear??∞?control structure for a quadrotor helicopter".?Automatica 46, no. 1 (January 2010): 29–39. https://doi.org/10.1016/j.automatica.2009.10.018.

[2] Tzorakoleftherakis, Emmanouil, and Todd D. Murphey. "Iterative sequential action control for stable, model-based control of nonlinear systems."?IEEE Transactions on Automatic Control?64, no. 8 (August 2019): 3170–83. https://doi.org/10.1109/TAC.2018.2885477.

[3] Luukkonen, Teppo. "Modelling and control of quadcopter". Independent research project in applied mathematics, Aalto University, 2011.