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 中生成代碼。