一级倒立摆控制 - 非线性 MPC 控制及 MATLAB 实现-编程知识

系列文章目录

前言

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

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

一、摆锤/小车装配

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

二、控制目标

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

小车在 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);

一个或多个 OV 采用零权重，因为 MV 比 OV 少。
预测模型的采样时间为 0.1 秒，与控制器的采样时间相同。

Ts = 0.1;
nlobj.Ts = Ts;

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

nlobj.PredictionHorizon = 10;

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

nlobj.ControlHorizon = 5;

五、指定非线性被控对象模型

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

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

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 中强制执行输出参考跟踪和操纵变量移动抑制。因此，请指定标准的 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);
The nlmpcToForces function generates a custom MEX function nlmpcmove_MyIPSolver, which you can use to speed up closed-loop simulation.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 中生成代码。