本人也是剛開始探索，大家一起討論一起進步！
項目介紹：
教程為北理工的無人駕駛車輛模型預測控制第2版，代碼為開源代碼。所用的仿真軟件為Carsim2020.0和MatlabR2021a。使用MPC控制思想對車輛進行速度控制，并給出仿真結果。
效果如下：

基于MPC的速度控制

• Carsim的基本配置
• • 車型的選擇
  • 工況設置
• Carsim與Matlab的接口設置
• Matlab 框架搭建
• MPC速度控制理論
• • 問題描述
  • 速度跟蹤MPC建模
  • MPC求解轉化為二次型規劃求解(把模型約束放入目標函數)
  • • 使用模型，對未來的狀態和控制進行預測，并給出解析結果
    • 將預測的表達式帶入目標函數，對其進行改寫
    • 統一控制變量
    • 寫成二次型問題
• 后處理
• 附錄：Matlab全代碼

Carsim的基本配置

Carsim一共可以分為三大部分：

• 前處理：涉及對車型的選擇和對工況的設置
• 計算：可以使用Carism直接計算，也可以外接其他計算軟件，比如Matlab。
• 后處理：將運行的結果表現成動畫和圖像。
  具體分布如下：
  

車型的選擇

車型選擇E-Class SUV，最好開一個新的Dataset。

工況設置

新開一個Dataset。因為我們需要在MATLAB中控制汽車，不需要模型自身施加驅動力和制動力所以驅動控制和制動控制設置為無。擋位選擇自動升檔。本項目無橫向控制，所以轉向盤選擇”Driver path follower“控制。道路設置長一點，仿真設置120s，具體參數設置如下：

Carsim與Matlab的接口設置

在設置完前處理之后，準備使用MATLAB中的Simulink對其進行控制和計算。在計算那一欄的Model中選擇Model:Simulink。之后建立一個新的Dataset。具體設置如下：

這里的輸入設置為IMP_THROTTLE_ENGINE（油門輸入）、IMP_PCON_BK制動主缸壓力[MPa]。

輸出設置為車速和加速度：

設置好之后點擊：Send to Simulink

Matlab 框架搭建

在使用Carsim發送過去數據之后，點擊MATLAB中的Simulink，此時的Library Browser中應該會出現Carsim S-Function：

將Carsim S-Functioin拖入Simulink模塊，同時在User-Defined Functions中找到S-Function并拖入Simulink模塊。構建如下圖所示的框架：

這里稍微解釋一下這個框架：
我們希望使用MPC對汽車的速度進行控制。對于MPC，我們的輸入是自己設定的期望速度，汽車當前的速度和加速度，這些量暫且統一稱為決策變量，根據這些變量，MPC計算之后會給出相應的控制，讓汽車去調整到我們期望的速度，這里的控制是油門和制動主缸壓力，這樣就構成了一個反饋。因為在使用MPC的時候，我們還希望看一些其他量的變化，所以在MPC模塊的輸出不僅僅是兩個控制量，還會有其他的量，使用Terminator模塊將不需要輸入進Carsim模塊終止掉。使用To Workspace模塊將MPC所有輸出模塊導入到MATLAB里面，方便畫圖。

如果不需要看其他參數，可以直接構建如下框架：

MPC速度控制理論

問題描述

汽車的速度控制可以參考下圖(來自北理工的書)：其可以分為上位控制器和下位控制器。

在機器人控制中可以分為底層控制-感知-決策。感知方面其實我們默認是做好的，而且本例也不涉及。這個上位控制可以理解為決策，根據車輛的當前速度加速度狀態和期望的速度，判斷該如何進行加速度的選擇。下位控制器可以理解為底層控制，當車輛做出決策給出應當進行的控制的時候，需要準確的進行這個控制實現。

速度跟蹤MPC建模

車輛的縱向控制可以使用一階慣性系統來表示：
$$\dot{a}=\frac{K}{{\tau }_d}(a_{des}?a)$$
其中$K=1$為系統的增益；${\tau }_d$為時間常數。將上式和速度加速度關系寫成矩陣形式，變成狀態方程：
$$\dot{x}=Ax+Bu$$
展開表示為：
$$\left(\begin{array}{c}\dot{v}\\ \dot{a}\end{array}\right)=\left(\begin{array}{cc}0& 1\\ 0& \frac{?1}{{\tau }_d}\end{array}\right)\left(\begin{array}{c}v\\ a\end{array}\right)+\left(\begin{array}{c}0\\ \frac{K}{{\tau }_d}\end{array}\right)a_{des}$$
使用前向歐拉法，進行離散：
$$x(k+1)=A_{k}x(k)+B_{k}u(k)$$
展開表示為：
$$\left(\begin{array}{c}v(k+1)\\ a(k+1)\end{array}\right)=\left(\begin{array}{cc}1& T_s\\ 0& 1?\frac{T_s}{{\tau }_d}\end{array}\right)\left(\begin{array}{c}v(k)\\ a(k)\end{array}\right)+\left(\begin{array}{c}0\\ \frac{K{T}_s}{{\tau }_d}\end{array}\right)a_{des}(k)$$
速度$v$作為系統的輸出，輸出方程可以表示為：
$$y(k)=Cx(k),C=[10]$$
以上就是模型預測控制中的模型建立。

系統的控制目標定義為：速度跟蹤精度，同時還要避免過大的加速度和沖擊。
$$J=\sum _{i=1}^{N_p}(y_p(k+i|k)?y_{ref}(k+i|k){)}^2+\sum _{i=1}^{N_c}(\Delta u(k+i){)}^2$$
系統的約束為加速度及其變化率；
$$\begin{gathered} u_{min}\le u(k+i)\le u_{max},i=0,1,...,N_e?1 \\ \Delta u_{min}\le \Delta u(k+i)\le \Delta u_{max},i=0,1,...,N_c?1 \end{gathered}$$
這樣我們就把MPC的模型建立完畢了，基本框架為：
$$\begin{gathered} minJ \\ s.t.x(k+1)=A_{k}x(k)+B_{k}u(k) \\ {u}_{min}\le u(k+i)\le u_{max},i=0,1,...,N_e?1 \\ \Delta u_{min}\le \Delta u(k+i)\le \Delta u_{max},i=0,1,...,N_c?1 \end{gathered}$$

MPC求解轉化為二次型規劃求解(把模型約束放入目標函數)

這部分理論性比較強，MPC理論推導參考如何理解MPC模型預測控制理論，這里主要說一下思路和編程所用的公式。

使用模型，對未來的狀態和控制進行預測，并給出解析結果

首先為了推導方便，我們進行如下定義：
$$\xi (k|t)=[x(k),u(k?1){]}^T$$
這樣我們的離散模型可以寫成：
$$\begin{gathered} \xi (k+1)=\widetilde{A_k}\xi (k)+\widetilde{B_k}\Delta u(k) \\ \eta (k)=\widetilde{C_k}\xi (k) \end{gathered}$$
其中這里面的$\widetilde{A_k}=\left(\begin{array}{cc}{A}_k& B_k\\ 0_{m\times n}& I_m\end{array}\right),\widetilde{B_k}=\left(\begin{array}{c}{B}_k\\ I_m\end{array}\right),\widetilde{C_k}=(C_k\mathrm{０})$。這里$m=1,n=2$

證明的具體過程就不在這里贅述，主要思路是，我們希望通過當前的狀態來給出控制，那么觀察離散模型，可以發現，通過當前的狀態，根據離散模型，其實是能給出下一時刻的狀態的，甚至是輸入，而當得到下一時刻狀態的時候又可以根據離散的方程進行預測，得到下下時刻的狀態，如此迭代，可以得到預測時域內所有時刻的狀態和輸入，換句話說這些都可以用初始狀態和輸入來表示。我們將這些統一寫成矩陣形式：
$$Y=\Phi \xi (k)+\Theta \Delta U$$
具體形式為：

將預測的表達式帶入目標函數，對其進行改寫

我們目前拿到了預測的速度和輸入，將其帶入到目標函數中，經過改寫可以變成：

其中：

其實相當于把模型約束放進目標函數中！！！

統一控制變量

現在還剩的約束時控制量和控制增量的約束。而控制量和控制增量存在如下關系：
$$u(k+i)=u(k+i?1)+\Delta u(k)$$
這樣可以將最后兩個約束寫成：

這里：

$A_k$為：

寫成二次型問題

通過上述三個步驟，就可以把MPC問題變成一個二次型求解問題：

總結一下具體思路：一句話就是把模型放進目標函數中。分步驟說就是首先用建立的模型，當前狀態寫出預測內的狀態和控制。之后將其帶入目標函數，并進行整理。最后將剩下的兩個約束寫成一個統一的形式。這樣構建出來的問題使用MATLAB自帶的二次規劃求解器就可以求解。

注：上述公式也是代碼里面的核心公式，如果代碼看不懂，可以從這里面尋找答案。

后處理

我們希望跟蹤的速度為：

后處理有兩種方式：
一種是使用MATLAB：
在命令行窗口輸入plot((1:2400),out.y.signals.values(:,4))或者plot(out.y.time,out.y.signals.values(:,4))(因為采樣頻率是0.05秒一次，一共運行120s)。

做出圖像為：

還可以使用Carsim的后處理模塊，這里主要是速度跟蹤，所以需要Longitudinal speed和time的關系圖像。

同時Carsim中還提供了仿真的汽車跟蹤動畫。（如本文開頭展示的動畫）

附錄：Matlab全代碼

function [sys,x0,str,ts] =MPCSpeedTrackingControl(t,x,u,flag) %***************************************************************% % Input: % t是采樣時間, x是狀態變量, u是輸入(是做成simulink模塊的輸入,即CarSim的輸出), % flag是仿真過程中的狀態標志(以它來判斷當前是初始化還是運行等) % Output: % sys輸出根據flag的不同而不同(下面將結合flag來講sys的含義), % x0是狀態變量的初始值, % str是保留參數,設置為空 % ts是一個1×2的向量, ts(1)是采樣周期, ts(2)是偏移量 %---------------------------------------------------------------% %***************************************************************% switch flagcase 0 %初始化%[sys,x0,str,ts]= mdlInitializeSizes;case 2 %更新%sys = mdlUpdates(t,x,u);%更新離散變量case 3 %輸出%sys = mdlOutputs(t,x,u);%計算輸出case {1,4,9} %不用的flagsys = [];otherwise % 其他flag情況 %error(['unhandled flag = ',num2str(flag)]); % Error handlingend % End sfuntmpl%==================================================================== %初始化，flag=0 %==================================================================== function [sys,x0,str,ts]= mdlInitializeSizessizes = simsizes;%用于設置模塊參數的結構體用simsizes來生成sizes.NumContStates = 0; %模塊連續狀態變量的個數sizes.NumDiscStates = 2; %模塊離散狀態變量的個數,實際上沒有用到這個數值，只是用這個來表示離散模塊sizes.NumOutputs = 6; %輸出量的個數，包括控制量和其他檢測量sizes.NumInputs = 2; %輸入量的個數，對應Carsim輸出量sizes.DirFeedthrough= 1; %沒太懂，好像是時變就需要設為1sizes.NumSampleTimes= 1; %采樣時間的個數sys = simsizes(sizes);x0 = zeros(sizes.NumDiscStates,1); %初始化狀態變量str = [];ts = [0.05 0]; %采樣時間：[period,offset]%設置一些全局變量global InitialGapflag;InitialGapflag = 0; global MPCParameters; MPCParameters.Np =30; %預測時域MPCParameters.Nc =30; %控制時域MPCParameters.Nx =2; %狀態變量MPCParameters.Nu =1; %控制輸入MPCParameters.Ny =1; %輸出變量MPCParameters.Ts =0.05; %設置采樣時間MPCParameters.Q =100; %權重MPCParameters.R =1; %權重MPCParameters.S =1; %權重MPCParameters.qp_solver = 0; %0: default, quadprog; 1:qpOASES; 2:CVXGENMPCParameters.refspeedT = 0; %0: default, step speed profile; %1:sine-wave speed profileMPCParameters.umin = -5.0; % 最大減速MPCParameters.umax = 3.5; % 最大加速MPCParameters.dumin = -5.0; % minimum limits of jerkMPCParameters.dumax = 5.0; % maximum limits of jerkglobal WarmStart;WarmStart = zeros(MPCParameters.Np,1);%結束 %============================================================================= %更新離散狀態量子函數 function sys=mdlUpdates(t,x,u)sys = x; %結束 %============================================================================= %計算輸出子函數，是速度跟蹤控制的主體。 function sys=mdlOutputs(t,x,u)global InitialGapflag;global MPCParameters;global WarmStart;a_des = 0; %初始化最優加速度控制量a_x = 0; %初始化加速度狀態量Vx = 0; %初始化車速t_Start = tic; %開始計時if InitialGapflag < 2 %去掉前兩次輸入InitialGapflag = InitialGapflag + 1;elseInitialGapflag = InitialGapflag + 1;%------------------Step(1).更新車輛縱向狀態--------------------%Vx = u(1)/3.6; %單位換算km/h->m/sa_x = u(2)*9.8; %單位換算g's->m/s^2kesi = [Vx;a_x]; %更新車輛狀態向量 %-------------------Step(2).設定期望速度-----------------------%switch MPCParameters.refspeedT%------------------設定階躍式目標車速--------------------------%case 0SpeedProfile = func_ConstantSpeed(InitialGapflag,MPCParameters);%------------------設定sine式目標車速--------------------------%otherwise % Unexpected flags %error(['unexpected speed-profile:',num2str(MPCParameters.refspeedT)]); % Error handlingend % end of switch %-------------------Step(3).調用MPC優化求解函數得到最優控制量-----------------------%Ts = MPCParameters.Ts;StateSpaceModel.A = [1 Ts;0 1];StateSpaceModel.B = [0; 1];StateSpaceModel.C = [1,0];[PHI, THETA] = func_Update_PHI_THETA(StateSpaceModel, MPCParameters);[H, ~, g] = func_Update_H_f(kesi, SpeedProfile, PHI, THETA, MPCParameters);switch MPCParameters.qp_solvercase 0 %default qp-solver:quadprog[A,b,Aeq,beq,lb,ub] = func_Constraints_du_quadprog(MPCParameters,a_x);options = optimset('Display','off', ...'TolFun', 1e-8, ...'MaxIter', 2000, ...'Algorithm', 'active-set', ...'FinDiffType', 'forward', ...'RelLineSrchBnd', [], ...'RelLineSrchBndDuration', 1, ...'TolConSQP', 1e-8); warning off all % close the warnings during computationU0 = WarmStart;[U, ~, EXITFLAG] = quadprog(H, g, A, b, Aeq, beq, lb, ub, U0, options); %WarmStart = shiftHorizon(U);if (1 ~= EXITFLAG) %if optimization NOT succeeded.U(1) = 0.0;fprintf('MPC solver not converged!\n'); enda_des = U(1);otherwise % Unexpected flags %error(['unexpected qp-solver, Sol_method=',num2str(flag)]); % Error handlingend % end of switch end % end of if Initialflag < 1 % %****Step(6): 由期望的加速度生成Throttle和Brake;********************%[Throttle, Brake] = func_AccelerationTrackingController(a_des);t_Elapsed = toc( t_Start ); %computation time sys = [Throttle; Brake;t_Elapsed; Vx; a_x; a_des]; % end %End of mdlOutputs%============================================================== % sub functions %==============================================================function [Vref] = func_ConstantSpeed(InitialGapflag, MPCParameters)%Ts = MPCParameters.Ts; %采樣時間=0.05sNp = MPCParameters.Np; %預測時域：30Vref = cell(Np,1);% 自定義階梯的形式if InitialGapflag < 400Vset = 10;elseif InitialGapflag < 800Vset = 10;elseif InitialGapflag < 1500Vset = 20;elseVset = 5;endendendfor i = 1:1:NpVref{i,1} = Vset;endfunction [PHI, THETA] = func_Update_PHI_THETA(StateSpaceModel, MPCParameters) %***************************************************************% % 預測輸出表達式 Y(t)=PHI*kesi(t)+THETA*DU(t) % Y(t) = [Eta(t+1|t) Eta(t+2|t) Eta(t+3|t) ... Eta(t+Np|t)]' %***************************************************************%Np = MPCParameters.Np;Nc = MPCParameters.Nc;Nx = MPCParameters.Nx;Ny = MPCParameters.Ny;Nu = MPCParameters.Nu;A = StateSpaceModel.A;B = StateSpaceModel.B;C = StateSpaceModel.C;PHI_cell = cell(Np,1); %PHI=[CA CA^2 CA^3 ... CA^Np]TTHETA_cell = cell(Np,Nc); for j = 1:1:NpPHI_cell{j,1}=C*A^j;for k = 1:1:Ncif k<=jTHETA_cell{j,k}=C*A^(j-k)*B;elseTHETA_cell{j,k} = zeros(Ny,Nu);endendendPHI=cell2mat(PHI_cell); % size(PHI)=[(Ny*Np) * Nx]THETA=cell2mat(THETA_cell);% size(THETA)=[Ny*Np Nu*Nc] % end %EoFfunction[H, f, g] = func_Update_H_f(kesi, SpeedProfile, PHI, THETA, MPCParameters) %***************************************************************% % trajectory planning %***************************************************************%Np = MPCParameters.Np;Nc = MPCParameters.Nc; Q = MPCParameters.Q;R = MPCParameters.R;Qq = kron(eye(Np),Q);Rr = kron(eye(Nc),R);Vref = cell2mat(SpeedProfile);error1 = PHI*kesi;H = THETA'*Qq*THETA +Rr;f = (error1'-Vref')*Qq*THETA;g = f';% end %EoFfunction [A, b, Aeq, beq, lb, ub] = func_Constraints_du_quadprog(MPCParameters, um)%************************************************************************% % generate the constraints of the vehicle % %************************************************************************%Np = MPCParameters.Np;Nc = Np; dumax = MPCParameters.dumax;umin = MPCParameters.umin; umax = MPCParameters.umax; Umin = kron(ones(Nc,1),umin);Umax = kron(ones(Nc,1),umax);Ut = kron(ones(Nc,1),um);%----(1) A*x<=b----------%A_t=zeros(Nc,Nc);for p=1:1:Ncfor q=1:1:Ncif p >= q A_t(p,q)=1;else A_t(p,q)=0;endend end A_cell=cell(2,1);A_cell{1,1} = A_t; %A_cell{2,1} = -A_t;A=cell2mat(A_cell); %b_cell=cell(2, 1);b_cell{1,1} = Umax - Ut; %b_cell{2,1} = -Umin + Ut;b=cell2mat(b_cell); % %----(2) Aeq*x=beq----------%Aeq = [];beq = [];%----(3) lb=<x<=ub----------%lb=kron(ones(Nc,1),-dumax);ub=kron(ones(Nc,1),dumax); % end %EoFfunction u0 = shiftHorizon(u) %shift control horizonu0 = [u(:,2:size(u,2)), u(:,size(u,2))]; % size(u,2))function [Throttle, Brake] = func_AccelerationTrackingController(ahopt) % 車輛下位控制器將期望加速度轉化為油門控制量和制動主缸壓力控制量K_brake = 0.3; %0.3K_throttle = 1; %0.05;Brake_Sat = 15;Throttle_Sat = 1;if ahopt < 0 % Brake controlBrake = -K_brake * ahopt;if Brake > Brake_SatBrake = Brake_Sat;endThrottle = 0;else % throttle control Brake = 0;Throttle = K_throttle *ahopt;if Throttle > Throttle_SatThrottle = Throttle_Sat;endif Throttle < 0Throttle = 0;endend % end %EoF

總結

以上是生活随笔為你收集整理的【Carsim Simulink自动驾驶仿真】基于MPC的速度控制的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。