MATLAB - 小车倒立摆的非线性模型预测控制（NMPC）

系列文章目录

前言

一、摆锤/小车组件

二、系统方程

三、控制目标

四、控制结构

五、创建非线性 MPC 控制器

六、指定非线性设备模型

七、定义成本和约束

八、验证非线性 MPC 控制器

九、状态估计

十、MATLAB 中的闭环仿真

十一、使用 MATLAB 中的 FORCESPRO 求解器进行闭环仿真

十二、Simulink 中的闭环仿真

十三、使用 Simulink 中的 FORCESPRO 求解器进行闭环仿真

十四、结论

前言

本示例使用非线性模型预测控制器对象和块实现对小车上倒立摆的摆动和平衡控制。

本示例需要 Optimization Toolbox™ 软件为非线性 MPC 提供默认的非线性编程求解器，以计算每个控制间隔的最优控制动作。

一、摆锤/小车组件

本例中的被控对象是摆锤/小车组件，其中 z 是小车位置，θ 是摆锤角度。该系统的可控输入变量是作用在小车上的可变力 F。力的范围在 -100 和 100 之间。脉冲干扰 dF 也会推动摆。

二、系统方程

系统方程如下，其中 l 是到摆锤质心的距离，m 和 M 分别是摆锤和小车的质量，Kd 是粘性摩擦阻尼项。

$\frac{\partial^2}{\partial t^2}z(t)=\frac{-lm\sin(\theta(t))\left(\frac{\partial}{\partial t}\theta(t)\right)^2-\mathrm{Kd}\frac{\partial}{\partial t}z(t)+F+gm\cos(\theta(t))\sin(\theta(t))}{-m\cos(\theta(t))^2+M+m}$

$g\sin(\theta(t))=\frac{-lm\cos(\theta(t))^{2}+Ml+lm}{M+m}\frac{\partial^{2}}{\partial t^{2}}\theta(t)+\frac{lm\cos(\theta(t))\sin(\theta(t))\left(\frac{\partial}{\partial t}\theta(t)\right)^{2}+\mathrm{Kd}\cos(\theta(t))\frac{\partial}{\partial t}z(t)-F\cos(\theta(t))}{M+m}$

通过第二项中的长度 l 和最后一项中的 theta 余弦，并在左侧分离出 theta 的二次导数，就可以从第二个方程中推导出表示角加速度与角速度和线速度函数关系的公式。有关如何推导方程的详细说明，请参阅推导运动方程和仿真车极系统（符号数学工具箱）。

系统方程编码在文件 pendulumCT0.m 中，其中不包括脉冲力 dF（设为零），因为它被视为控制器未知的干扰。显示文件。

type("pendulumCT0.m")

function dxdt = pendulumCT0(x, u)
%% Continuous-time nonlinear dynamic model of a pendulum on a cart
%
% 4 states (x): 
%   cart position (z)
%   cart velocity (z_dot): when positive, cart moves to right
%   angle (theta): when 0, pendulum is at upright position
%   angular velocity (theta_dot): anti-clockwise positive
% 
% 1 inputs: (u)
%   force (F): when positive, force pushes cart to right 
%
% Copyright 2018 The MathWorks, Inc.%#codegen%% parameters
M = 1;      % cart mass
m = 1;      % pendulum mass
g = 9.81;   % gravity of earth
l = 0.5;    % pendulum length
Kd = 10;    % cart damping%% Obtain x, u and y% x (state variables)
z_dot = x(2);
theta = x(3);
theta_dot = x(4);% u (input variable)
F = u;%% Compute dxdt
dxdt = [z_dot;...(   F - Kd*z_dot ...- m*l*theta_dot^2*sin(theta) ...+ m*g*sin(theta)*cos(theta)  ...)/(M + m*sin(theta)^2);...theta_dot;...(  g*sin(theta) + ...(F - Kd*z_dot - m*l*theta_dot^2*sin(theta))*cos(theta)/(M + m) ...)/(l - m*l*cos(theta)^2/(M + m));];

三、控制目标

假设摆锤/小车组件的初始条件如下。

小车在 z = 0 处静止。
摆锤处于向下的平衡位置，θ = -pi。

控制目标为

上摆控制：最初将摆锤向上摆动至 z = 0 和 theta = 0 的倒平衡位置。
小车位置参考跟踪：通过设定点的阶跃变化将小车移动到新位置，保持摆锤倒置。
摆锤平衡：当对倒立摆施加幅度为 2 的脉冲干扰时，保持摆的平衡，并使小车回到初始位置。

向下的平衡位置是稳定的，而倒置的平衡位置是不稳定的，这使得摆锤上升控制对单一线性控制器来说更具挑战性，而非线性 MPC 则可以轻松应对。

四、控制结构

在本示例中，非线性 MPC 控制器的输入/输出配置如下。

一个可控输入变量：变量力 (F)
两个可观测输出：小车位置 (z) 和摆锤角度 (Theta)

另外两个状态：小车速度 (zdot) 和摆锤角速度 (thetadot)不可观测。

小车位置（z）的设定点可以变化，而摆锤角度（θ）的设定点始终为 0（倒置平衡位置）。

五、创建非线性 MPC 控制器

使用 nlmpc 对象创建一个具有适当尺寸的非线性 MPC 控制器。在本示例中，预测模型有 4 个状态、2 个输出和 1 个输入（可控变量或 MV）。

nx = 4;
ny = 2;
nu = 1;
nlobj = nlmpc(nx, ny, nu);

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

预测模型的采样时间为 0.1 秒，与控制器的采样时间相同。

Ts = 0.1;
nlobj.Ts = Ts;

将预测范围设为 10，这个时间长度足以捕捉被控对象的主要动态，但又不会太长，以免影响计算效率。

nlobj.PredictionHorizon = 10;

将控制范围设为 5，这个时间长度足以让控制器有足够的自由度来处理不稳定模式，同时又不会引入过多的决策变量。

nlobj.ControlHorizon = 5;

六、指定非线性设备模型

非线性模型预测控制的主要优势在于，它使用非线性动态模型来预测被控对象未来在各种运行条件下的行为。

这种非线性模型通常是由一组微分和代数方程 (DAE) 组成的第一原理模型。在本示例中，离散时间小车和摆锤系统定义在 pendulumDT0 函数中。该函数使用多步正向欧拉法在控制间隔之间对连续时间模型 pendulumCT0 进行积分。非线性状态估计器也使用同一函数。显示离散模型

type("pendulumDT0.m")

function xk1 = pendulumDT0(xk, uk, Ts)
%% Discrete-time nonlinear dynamic model of a pendulum on a cart at time k
%
% 4 states (xk): 
%   cart position (z)
%   cart velocity (z_dot): when positive, cart moves to right
%   angle (theta): when 0, pendulum is at upright position
%   angular velocity (theta_dot): anticlockwise positive
% 
% 1 inputs: (uk)
%   force (F): when positive, force pushes cart to right 
%
% xk1 is the states at time k+1.
%
% Copyright 2018 The MathWorks, Inc.%#codegen% Repeat application of Euler method sampled at Ts/Nd.
Nd = 10;
delta = Ts/Nd;
xk1 = xk;
for ct=1:Ndxk1 = xk1 + delta*pendulumCT0(xk1,uk);
end
% Note that we choose the Euler method (first order Runge-Kutta method)
% because it is more efficient for plant with non-stiff ODEs.  You can
% choose other ODE solvers such as ode23, ode45 for better accuracy or
% ode15s and ode23s for stiff ODEs.  Those solvers are available from
% MATLAB.

将离散化模型设置为非线性 MPC 控制器使用的预测模型。

nlobj.Model.StateFcn = "pendulumDT0";

要使用离散时间模型，请将控制器的 Model.IsContinuousTime 属性设置为 false。

nlobj.Model.IsContinuousTime = false;

预测模型使用可选参数 Ts 表示采样时间。使用该参数意味着，如果在设计过程中更改了预测采样时间，则无需修改摆锤 DT0 文件。

nlobj.Model.NumberOfParameters = 1;

两个被控对象的输出分别是模型中的第一和第三状态，即小车位置和摆锤角度。相应的输出函数定义在 pendulumOutputFcn 函数中。

nlobj.Model.OutputFcn = 'pendulumOutputFcn';

最佳做法是尽可能提供解析雅各布函数，因为它们能显著提高仿真速度。在本例中，使用匿名函数为输出函数提供雅各布函数。

nlobj.Jacobian.OutputFcn = @(x,u,Ts) [1 0 0 0; 0 0 1 0];

由于您没有提供状态函数的雅各布，非线性 MPC 控制器会在优化过程中使用数值扰动来估计状态函数的雅各布。这样做会在一定程度上降低仿真速度。

七、定义成本和约束

与线性 MPC 相似，非线性 MPC 在每个控制区间都要解决一个受约束的优化问题。不过，由于被控对象模型是非线性的，因此非线性 MPC 将最优控制问题转换为一个具有非线性成本函数和非线性约束的非线性优化问题。

本例中使用的成本函数与线性 MPC 使用的标准成本函数相同，其中强制执行了输出参考跟踪和可控输入变量移动抑制。因此，请指定标准的 MPC 调整权重。

nlobj.Weights.OutputVariables = [3 3];
nlobj.Weights.ManipulatedVariablesRate = 0.1;

小车位置限制在 -10 至 10 的范围内。

nlobj.OV(1).Min = -10;
nlobj.OV(1).Max = 10;

力的范围在 -100 和 100 之间。

nlobj.MV.Min = -100;
nlobj.MV.Max = 100;

八、验证非线性 MPC 控制器

设计完非线性 MPC 控制器对象后，最好检查一下为预测模型、状态函数、输出函数、自定义成本和自定义约束定义的函数及其雅各布系数。为此，请使用 validateFcns 命令。该功能可检测这些函数中的任何尺寸和数值不一致之处。

x0 = [0.1;0.2;-pi/2;0.3];
u0 = 0.4;
validateFcns(nlobj,x0,u0,[],{Ts});

Model.StateFcn is OK.
Model.OutputFcn is OK.
Jacobian.OutputFcn is OK.
Analysis of user-provided model, cost, and constraint functions complete.

九、状态估计

在本例中，只有两个设备状态（小车位置和摆锤角度）是可观测的。因此，您需要使用扩展卡尔曼滤波器来估计四个被控对象的状态。其状态转换函数在 pendulumStateFcn.m 中定义，测量函数在 pendulumMeasurementFcn.m 中定义。

EKF = extendedKalmanFilter(@pendulumStateFcn, @pendulumMeasurementFcn);

十、MATLAB 中的闭环仿真

通过设置初始被控对象状态和输出值来指定仿真的初始条件。此外，还要指定扩展卡尔曼滤波器的初始状态。

仿真区域的初始条件如下。

小车静止在 z = 0 处。
摆锤处于向下的平衡位置，θ = -pi。

x = [0;0;-pi;0];
y = [x(1);x(3)];
EKF.State = x;

mv 是在任何控制间隔内计算出的最佳控制移动量。初始化 mv 为零，因为一开始施加在小车上的力为零。

mv = 0;

在仿真的第一阶段，摆锤从向下的平衡位置向上摆动到倒平衡位置。该阶段的状态参考值均为零。

yref1 = [0 0];

10 秒后，小车从位置 0 移动到位置 5。设置该位置的状态参考点。

yref2 = [5 0];

使用 nlmpcmove 命令计算每个控制间隔的最佳控制移动。该函数构建了一个非线性编程问题，并使用优化工具箱中的 fmincon 函数进行求解。

使用 nlmpcmoveopt 对象指定预测模型参数，并将该对象传递给 nlmpcmove。

nloptions = nlmpcmoveopt;
nloptions.Parameters = {Ts};

运行仿真 20 秒。

Duration = 20;
hbar = waitbar(0,'Simulation Progress');
xHistory = x;
for ct = 1:(20/Ts)% Set referencesif ct*Ts<10yref = yref1;elseyref = yref2;end% Correct previous prediction using current measurement.xk = correct(EKF, y);% Compute optimal control moves.[mv,nloptions,info] = nlmpcmove(nlobj,xk,mv,yref,[],nloptions);% Predict prediction model states for the next iteration.predict(EKF, [mv; Ts]);% Implement first optimal control move and update plant states.x = pendulumDT0(x,mv,Ts);% Generate sensor data with some white noise.y = x([1 3]) + randn(2,1)*0.01;% Save plant states for display.xHistory = [xHistory x]; %#ok<*AGROW>waitbar(ct*Ts/20,hbar);
end
close(hbar)

绘制闭环响应图。

figuresubplot(2,2,1)
plot(0:Ts:Duration,xHistory(1,:))
xlabel('time')
ylabel('z')
title('cart position')subplot(2,2,2)
plot(0:Ts:Duration,xHistory(2,:))
xlabel('time')
ylabel('zdot')
title('cart velocity')subplot(2,2,3)
plot(0:Ts:Duration,xHistory(3,:))
xlabel('time')
ylabel('theta')
title('pendulum angle')subplot(2,2,4)
plot(0:Ts:Duration,xHistory(4,:))
xlabel('time')
ylabel('thetadot')
title('pendulum velocity')

摆角图显示，摆锤在两秒内成功摆起。在上摆过程中，小车发生位移，峰值偏差为-1，并在 2 秒钟左右回到原位。

小车位置图显示，小车在两秒内成功移动到 z = 5 处。在小车移动的同时，摆锤以 1 弧度（57 度）的峰值偏差发生位移，并在 12 秒左右恢复到倒平衡位置。

十一、使用 MATLAB 中的 FORCESPRO 求解器进行闭环仿真

您可以轻松地将第三方非线性编程求解器与使用模型预测控制工具箱软件设计的非线性 MPC 对象结合使用。例如，如果您安装了 Embotech 的 FORCESPRO 软件，就可以使用其 MPC 工具箱插件从 nlmpc 对象生成高效的自定义 NLP 求解器，并使用该求解器进行仿真和代码生成。

首先，使用 nlmpcToForces 命令生成自定义求解器。您可以使用 nlmpcToForcesOptions 命令选择使用内部点 (IP) 求解器或顺序二次编程 (SQP) 求解器。

options = nlmpcToForcesOptions();
options.SolverName = 'MyIPSolver';
options.SolverType = 'InteriorPoint';
options.Parameter = Ts;
options.x0 = [0;0;-pi;0];
options.mv0 = 0;
[coredata, onlinedata] = nlmpcToForces(nlobj,options);

nlmpcToForces 函数会生成一个自定义 MEX 函数 nlmpcmove_MyIPSolver，您可以用它来加快闭环仿真。

x = [0;0;-pi;0];
mv = 0;
EKF.State = x;
y = [x(1);x(3)];
hbar = waitbar(0,'Simulation Progress');
xHistory = x;
for ct = 1:(20/Ts)% Set referencesif ct*Ts<10onlinedata.ref = repmat(yref1,10,1);elseonlinedata.ref = repmat(yref2,10,1);end% Correct previous prediction using current measurement.xk = correct(EKF, y);% Compute optimal control moves using FORCESPRO solver.[mv,onlinedata,info] = nlmpcmove_MyIPSolver(xk,mv,onlinedata);% Predict prediction model states for the next iteration.predict(EKF, [mv; Ts]);% Implement first optimal control move and update plant states.x = pendulumDT0(x,mv,Ts);% Generate sensor data with some white noise.y = x([1 3]) + randn(2,1)*0.01;% Save plant states for display.xHistory = [xHistory x]; %#ok<*AGROW>waitbar(ct*Ts/20,hbar);endclose(hbar)

不出所料，闭环响应与使用 fmincon 得到的响应相似。

十二、Simulink 中的闭环仿真

在 Simulink® 中进行闭环仿真，验证非线性 MPC 控制器。

打开 Simulink 模型。

mdl = 'mpc_pendcartNMPC';
open_system(mdl)

在该模型中，非线性 MPC 控制器模块被配置为使用先前设计的控制器 nlobj。

为了在预测模型中使用可选参数，模型中的 Simulink 总线块连接到非线性 MPC 控制器块的 params 输入端口。要配置该总线块以使用 Ts 参数，请在 MATLAB® 工作区中创建一个总线对象，并配置总线创建器块以使用该对象。为此，请使用 createParameterBus 函数。在本示例中，将总线对象命名为 “myBusObject”。

createParameterBus(nlobj,[mdl '/Nonlinear MPC Controller'],'myBusObject',{Ts});

Simulink Bus object "myBusObject" created in the MATLAB Workspace.
Bus Creator block "mpc_pendcartNMPC/Nonlinear MPC Controller" is configured to use it.

运行仿真 30 秒。

open_system([mdl '/Scope'])
sim(mdl)

与 MATLAB 仿真相比，Simulink 中的非线性仿真得出了完全相同的摆动和小车位置跟踪结果。此外，在 20 秒的时间内对倒立摆施加了一个推力（脉冲干扰 dF）。非线性 MPC 控制器成功地拒绝了干扰，并使小车返回到 z = 5，摆锤返回到倒置平衡位置。

十三、使用 Simulink 中的 FORCESPRO 求解器进行闭环仿真

您还可以使用 Embotech FORCESPRO 软件中的 FORCES 非线性 MPC 块，使用生成的自定义 NLP 求解器仿真非线性 MPC。

如果您已安装 FORCESPRO 软件，只需将上述模型中的非线性 MPC 块替换为库浏览器中 FORCESPRO MPC 块部分的 FORCESPRO 非线性 MPC 块。在程序块对话框中，将 coredata 指定为控制器数据结构，并启用模型参数输入。如下图所示重新连接其余信号。

闭环响应与使用 fmincon 时类似。

十四、结论

本例说明了在 MATLAB 和 Simulink 中分别使用 nlmpc 对象和非线性 MPC 控制器块设计和仿真非线性 MPC 的一般工作流程。根据具体的非线性被控对象特性和控制要求，实施细节会有很大不同。主要的设计挑战包括

选择适当的范围、界限和权重
设计非线性状态估计器
设计定制的非线性成本函数和约束函数
选择求解器选项或自定义 NLP 求解器

您可以将本例中的函数和 Simulink 模型作为模板，用于其他非线性 MPC 设计和仿真任务。

模型预测控制工具箱软件中的 nlmpcmoveCodeGeneration 命令和 FORCESPRO 中的 nlmpcmoveForces 命令都支持在 MATLAB 中生成代码。模型预测控制工具箱软件中的非线性 MPC 块和 FORCESPRO 中的 FORCES 非线性 MPC 块都支持在 Simulink 中生成代码。