如果對Carsim的基礎使用還不了解，可以參考：【Carsim Simulink自動駕駛仿真】基于MPC的速度控制
如果對MPC算法原理不清楚，可以參考：如何理解MPC模型預測控制理論
項目介紹：
教程為北理工的無人駕駛車輛模型預測控制第2版。所用的仿真軟件為Carsim2020.0和MatlabR2021a。前幾章處理的場景都是低速的情況，所以運動學的精度就可以保證很好的控制，但是當處于高速的時候，由于種種原因，比如輪胎不能看成剛體，單純使用運動學不能很好的進行控制，就需要使用動力學模型，第五章使用考慮輪胎滑移的動力學模型對汽車進行一個轉彎控制。

第五章的代碼沒有多少問題，但是對代碼中的一些數學原理問題有需要討論的地方。

基于MPC的軌跡跟蹤控制

• 效果圖
• MATLAB框架搭建
• 輪胎滑移的動力學
• 在MPC中對動力學處理的討論
• • 對誤差中加入非線性動力學的討論
• 附錄：全代碼

效果圖

控制量是前輪轉角，目標是讓汽車跟蹤縱向函數和橫擺角函數：


這里畫了4個圖，分別是Y-X，橫擺角-X，前輪轉角-t,前輪胎側傾-t。

MATLAB框架搭建

MATLAB框架沿襲第四章的思路就可以：

輪胎滑移的動力學

這里我們可以不用關注動力學模型是如何推導的，主要看的是怎么用，首先看一下動力學${\dot{x}}_{dyn}=f_{dyn}({\dot{x}}_{dyn},u_{dyn})$的具體形式：
$$\begin{gathered} m{\dot{v}}_y=?m{v}_x\dot{\phi }+2[C_{\alpha f}({\delta }_f?\frac{v_y+l_f\dot{\phi }}{v_x})+C_{\alpha r}\frac{l_r\dot{\phi }?v_y}{v_x}] \\ m{\dot{v}}_x=m{v}_y\dot{\phi }+2[C_{lf}{s}_f+C_{\alpha f}({\delta }_f?\frac{v_y+l_f\dot{\phi }}{v_x}){\delta }_f+C_{lr}{s}_r] \\ {I}_z\ddot{\phi }=2[l_f{C}_{\alpha f}({\delta }_f?\frac{v_y+l_f\dot{\phi }}{v_x})?l_f{C}_{\alpha r}\frac{l_r\dot{\phi }?v_y}{v_x}] \\ \dot{y}=v_{x}sin\phi +v_{y}cos\phi \\ \dot{x}=v_{x}cos\phi ?v_{y}sin\phi \end{gathered}$$
這里其實只需要知道，狀態變量為：$\dot{y},\dot{x},\phi ,\dot{\phi },y,x$，控制變量為：${\delta }_f$就足夠了，至于其他的問題不是我們關注的問題。我們這里處理的是高速問題，所以對MPC的實時性有很高的要求。因為動力學方程是非線性的，非線性模型預測控制難以滿足，所以這里繼續采用線性時變模型預測控制，因此需要對非線性動力學模型進行線性化。即將${\dot{x}}_{dyn}=f_{dyn}({\dot{x}}_{dyn},u_{dyn})$變成${\dot{x}}_{dyn}=A_{dyn}(t)x_{dyn}(t)+B_{dyn}(t)u_{dyn}(t)$。之后將其離散化處理，得到離散化的表達式：
$$x_{dyn}(k+1)=A_{dyn}(k)x_{dyn}(k)+B_{dyn}(k)u_{dyn}(k)$$
這里，$A_{dyn}(k)=I+T{A}_{dyn}(t)，B_{dyn}(k)=T{B}_{dyn}(t)$。

在MPC中對動力學處理的討論

按照前幾章的思路，有了運動學方程或者動力學方程之后帶進去算就好，但是這里的代碼我發現不僅使用了線性的動力學模型，也是用了非線性模型。在代碼體現在(220-221行)：

上面是原來的代碼，下面是按照原來前幾章的思路的代碼，兩個誤差效果如下：

重點看一下曲線，紅色的是原代碼跑出的結果，對應第一行代碼中的誤差，藍色的是我認為按照前幾章的思路做出的結果，對應第二行，也就是沒備注的代碼：

沒有使用非線性動力學模型軌跡跟蹤會有一個提前量，跟蹤的沒有那么精確，但是可以從動畫效果來看誤差其實不是很大。而輪胎側偏比非線性小很多。這里還進行了步長的調整，會發現如果單純看曲線軌跡的話，單純的線性動力學參數可調整的范圍比較大，但是當控制步長調整太大的時候，會出現在跟蹤直線的情況下出現速度，前輪轉角小幅震蕩的情況，但是當使用這個參數對添加非線性的進行跟蹤的時候，連軌跡都是震蕩的，甚至會有靈車漂移的現象：

這里為什么在求誤差的時候要加上非線性動力學這一項，作者在代碼中有一個備注是參考falcone(B,4c)，但是我并沒有找到。

對誤差中加入非線性動力學的討論

先明確說一點，我其實沒有想明白，按照我之后的推導和理解，認為這里面有一項不是很好理解。這里可能會說的不是很明白，如果有大佬知道或者想討論，可以評論留言或者私信。
我們主要看一下PSI*kesi+GAMMA*PHI算出來的是什么，我們以$k=2$作為討論，不針對本例問題，單純當作一個數學推導，對于第一項PSI*kesi：
$$\begin{gathered} PSI=\left(\begin{array}{c}CA\\ C{A}^2\end{array}\right) \\ kesi=stat{e}_{linear}(k) \end{gathered}$$
這個結果也是前幾章直接減去的東西，加的這一項在后面。對于第二項GAMMA*PHI：
$$\begin{gathered} GAMMA=\left(\begin{array}{cc}C& 0\\ CA& C\end{array}\right) \\ PHI=\left(\begin{array}{c}stat{e}_{nolinear}(k+1)?stat{e}_{linear}(k+1)\\ stat{e}_{nolinear}(k+2)?stat{e}_{linear}(k+2)\end{array}\right) \end{gathered}$$
這里代碼中的PHI進行了簡化，都是用了(k+1)的值。在這里我們假設$C$是一個單位陣，也就是都輸出，看一下多減去的東西是什么：
$$\begin{array}{rl}ans& =\left(\begin{array}{c}stat{e}_{nolinear}(k+1)?stat{e}_{linear}(k+1)\\ Astat{e}_{nolinear}(k+1)?Astat{e}_{linear}(k+1)+stat{e}_{nolinear}(k+2)?stat{e}_{linear}(k+2)\end{array}\right)\end{array}$$
我們這里將第$k$時刻的非線性和線性的模型之間的誤差記錄為：$e_{model}(k)$。那么多家的這一項就變成了：
$$ans=\left(\begin{array}{c}{e}_{model}(k+1)\\ A{e}_{model}(k+1)+e_{model}(k+2)\end{array}\right)$$
所以個人認為，本來非線性模型和真車就有一個動力學誤差，為了計算時效性，把非線性變成線性又產生了一個誤差，這樣會有兩個誤差，按照我們以前的思路，是直接使用線性的，但是這樣誤差比較大，所以他這里的代碼可能是希望將非線性和線性的誤差考慮進去，但是這樣推到下來，$A{e}_{model}(k+1)$不是很清楚里面的意義。因為$A$本身就是線性模型弄出來的東西，現在還要再乘一個非線性的結果，這個地方很怪。

附錄：全代碼

function [sys,x0,str,ts] = chap5DMC(t,x,u,flag) % 狀態量=[y_dot,x_dot,phi,phi_dot,Y,X]，控制量為前輪偏角delta_fswitch flagcase 0[sys,x0,str,ts] = mdlInitializeSizes; % Initializationcase 2sys = mdlUpdates(t,x,u); % Update discrete statescase 3sys = mdlOutputs(t,x,u); % Calculate outputscase {1,4,9} % Unused flagssys = [];otherwiseerror(['unhandled flag = ',num2str(flag)]); % Error handling end % End of dsfunc.%============================================================== % 初始化 %============================================================== function [sys,x0,str,ts] = mdlInitializeSizessizes = simsizes; sizes.NumContStates = 0; sizes.NumDiscStates = 6; sizes.NumOutputs = 1; sizes.NumInputs = 8; sizes.DirFeedthrough = 1; % Matrix D is non-empty. sizes.NumSampleTimes = 1; sys = simsizes(sizes); x0 =[0.001;0.0001;0.0001;0.00001;0.00001;0.00001]; global U; U=[0];%控制量初始化,這里面加了一個期望軌跡的輸出，如果去掉，U為一維的 % global x; % x = zeros(md.ne + md.pye + md.me + md.Hu*md.me,1); % Initialize the discrete states. str = []; % Set str to an empty matrix. ts = [0.02 0]; % sample time: [period, offset] %End of mdlInitializeSizes%============================================================== % Update the discrete states %============================================================== function sys = mdlUpdates(t,x,u)sys = x; %End of mdlUpdate.function sys = mdlOutputs(t,x,u)global a b;global U;ticNx=6;%狀態量的個數Nu=1;%控制量的個數Ny=2;%輸出量的個數Np=35;%預測步長Nc=2;%控制步長%輸入接口轉換,x_dot后面加一個非常小的數，是防止出現分母為零的情況% y_dot=u(1)/3.6-0.000000001*0.4786;%CarSim輸出的是km/h，轉換為m/sy_dot=u(1)/3.6;x_dot=u(2)/3.6+0.0001;phi=u(3)*3.141592654/180; %CarSim輸出的為角度，角度轉換為弧度phi_dot=u(4)*3.141592654/180;Y=u(5);%單位為mX=u(6);%單位為米S_L=u(7);S_R=u(8);%% 車輛參數輸入 %syms sf sr;%分別為前后車輪的滑移率,需要提供Sf=0.2; Sr=0.2; %syms lf lr;%前后車輪距離車輛質心的距離，車輛固有參數lf=1.232;lr=1.468; %syms C_cf C_cr C_lf C_lr;%分別為前后車輪的縱橫向側偏剛度，車輛固有參數Ccf=66900;Ccr=62700;Clf=66900;Clr=62700; %syms m g I;%m為車輛質量，g為重力加速度，I為車輛繞Z軸的轉動慣量，車輛固有參數m=1723;g=9.8;I=4175;%% 參考軌跡生成shape=2.4;%參數名稱，用于參考軌跡生成dx1=25;dx2=21.95;%沒有任何實際意義，只是參數名稱dy1=4.05;dy2=5.7;%沒有任何實際意義，只是參數名稱Xs1=27.19;Xs2=56.46;%參數名稱X_predict=zeros(Np,1);%用于保存預測時域內的縱向位置信息，這是計算期望軌跡的基礎phi_ref=zeros(Np,1);%用于保存預測時域內的期望軌跡Y_ref=zeros(Np,1);%用于保存預測時域內的期望軌跡%% 以下計算kesi,即狀態量與控制量合在一起kesi=zeros(Nx+Nu,1);kesi(1)=y_dot;%u(1)==X(1)kesi(2)=x_dot;%u(2)==X(2)kesi(3)=phi; %u(3)==X(3)kesi(4)=phi_dot;kesi(5)=Y;kesi(6)=X;kesi(7)=U(1);delta_f=U(1);T=0.02;%仿真步長T_all=20;%總的仿真時間，主要功能是防止計算期望軌跡越界%% 關鍵參數設置Row=1000;%松弛因子權重Q_cell=cell(Np,Np);for i=1:1:Npfor j=1:1:Npif i==jQ_cell{i,j}=[200 0;0 100;];%Q_cell{i,j}=[2000 0;0 10000;];else Q_cell{i,j}=zeros(Ny,Ny); endend end R=5*10^4*eye(Nu*Nc);%R=5*10^5*eye(Nu*Nc);%% 動力學方程設置a=[ 1 - (259200*T)/(1723*x_dot), -T*(phi_dot + (2*((460218*phi_dot)/5 - 62700*y_dot))/(1723*x_dot^2) - (133800*((154*phi_dot)/125 + y_dot))/(1723*x_dot^2)), 0, -T*(x_dot - 96228/(8615*x_dot)), 0, 0T*(phi_dot - (133800*delta_f)/(1723*x_dot)), (133800*T*delta_f*((154*phi_dot)/125 + y_dot))/(1723*x_dot^2) + 1, 0, T*(y_dot - (824208*delta_f)/(8615*x_dot)), 0, 00, 0, 1, T, 0, 0(33063689036759*T)/(7172595384320*x_dot), T*(((2321344006605451863*phi_dot)/8589934592000 - (6325188028897689*y_dot)/34359738368)/(4175*x_dot^2) + (5663914248162509*((154*phi_dot)/125 + y_dot))/(143451907686400*x_dot^2)), 0, 1 - (813165919007900927*T)/(7172595384320000*x_dot), 0, 0T*cos(phi), T*sin(phi), T*(x_dot*cos(phi) - y_dot*sin(phi)), 0, 1, 0-T*sin(phi), T*cos(phi), -T*(y_dot*cos(phi) + x_dot*sin(phi)), 0, 0, 1];b=[ 133800*T/1723T*((267600*delta_f)/1723 - (133800*((154*phi_dot)/125 + y_dot))/(1723*x_dot))05663914248162509*T/14345190768640000]; d_k=zeros(Nx,1);%計算偏差state_k1=zeros(Nx,1);%預測的下一時刻狀態量，用于計算偏差%以下即為根據離散非線性模型預測下一時刻狀態量state_k1(1,1)=y_dot+T*(-x_dot*phi_dot+2*(Ccf*(delta_f-(y_dot+lf*phi_dot)/x_dot)+Ccr*(lr*phi_dot-y_dot)/x_dot)/m);state_k1(2,1)=x_dot+T*(y_dot*phi_dot+2*(Clf*Sf+Clr*Sr+Ccf*delta_f*(delta_f-(y_dot+phi_dot*lf)/x_dot))/m);state_k1(3,1)=phi+T*phi_dot;state_k1(4,1)=phi_dot+T*((2*lf*Ccf*(delta_f-(y_dot+lf*phi_dot)/x_dot)-2*lr*Ccr*(lr*phi_dot-y_dot)/x_dot)/I);state_k1(5,1)=Y+T*(x_dot*sin(phi)+y_dot*cos(phi));state_k1(6,1)=X+T*(x_dot*cos(phi)-y_dot*sin(phi));d_k=state_k1-a*kesi(1:6,1)-b*kesi(7,1);%根據falcone公式（2.11b）求得d(k,t)d_piao_k=zeros(Nx+Nu,1);%d_k的增廣形式，參考falcone(B,4c)d_piao_k(1:6,1)=d_k;d_piao_k(7,1)=0;A_cell=cell(2,2);B_cell=cell(2,1);A_cell{1,1}=a;A_cell{1,2}=b;A_cell{2,1}=zeros(Nu,Nx);A_cell{2,2}=eye(Nu);B_cell{1,1}=b;B_cell{2,1}=eye(Nu);%A=zeros(Nu+Nx,Nu+Nx);A=cell2mat(A_cell);B=cell2mat(B_cell);% C=[0 0 1 0 0 0 0;0 0 0 1 0 0 0;0 0 0 0 1 0 0;];%這是和輸出量緊密關聯的C=[0 0 1 0 0 0 0;0 0 0 0 1 0 0;];PSI_cell=cell(Np,1);THETA_cell=cell(Np,Nc);GAMMA_cell=cell(Np,Np);PHI_cell=cell(Np,1);for p=1:1:NpPHI_cell{p,1}=d_piao_k;%理論上來說，這個是要實時更新的，但是為了簡便，這里又一次近似for q=1:1:Npif q<=pGAMMA_cell{p,q}=C*A^(p-q);else GAMMA_cell{p,q}=zeros(Ny,Nx+Nu);end endendfor j=1:1:NpPSI_cell{j,1}=C*A^j;for k=1:1:Ncif k<=jTHETA_cell{j,k}=C*A^(j-k)*B;else THETA_cell{j,k}=zeros(Ny,Nu);endendendPSI=cell2mat(PSI_cell);%size(PSI)=[Ny*Np Nx+Nu]THETA=cell2mat(THETA_cell);%size(THETA)=[Ny*Np Nu*Nc]GAMMA=cell2mat(GAMMA_cell);%大寫的GAMMAPHI=cell2mat(PHI_cell);Q=cell2mat(Q_cell);H_cell=cell(2,2);H_cell{1,1}=THETA'*Q*THETA+R;H_cell{1,2}=zeros(Nu*Nc,1);H_cell{2,1}=zeros(1,Nu*Nc);H_cell{2,2}=Row;H=cell2mat(H_cell);error_1=zeros(Ny*Np,1);Yita_ref_cell=cell(Np,1);for p=1:1:Npif t+p*T>T_allX_DOT=x_dot*cos(phi)-y_dot*sin(phi);%慣性坐標系下縱向速度X_predict(Np,1)=X+X_DOT*Np*T;%X_predict(Np,1)=X+X_dot*Np*t;z1=shape/dx1*(X_predict(Np,1)-Xs1)-shape/2;z2=shape/dx2*(X_predict(Np,1)-Xs2)-shape/2;Y_ref(p,1)=dy1/2*(1+tanh(z1))-dy2/2*(1+tanh(z2));phi_ref(p,1)=atan(dy1*(1/cosh(z1))^2*(1.2/dx1)-dy2*(1/cosh(z2))^2*(1.2/dx2));Yita_ref_cell{p,1}=[phi_ref(p,1);Y_ref(p,1)];elseX_DOT=x_dot*cos(phi)-y_dot*sin(phi);%慣性坐標系下縱向速度X_predict(p,1)=X+X_DOT*p*T;%首先計算出未來X的位置，X(t)=X+X_dot*tz1=shape/dx1*(X_predict(p,1)-Xs1)-shape/2;z2=shape/dx2*(X_predict(p,1)-Xs2)-shape/2;Y_ref(p,1)=dy1/2*(1+tanh(z1))-dy2/2*(1+tanh(z2));phi_ref(p,1)=atan(dy1*(1/cosh(z1))^2*(1.2/dx1)-dy2*(1/cosh(z2))^2*(1.2/dx2));Yita_ref_cell{p,1}=[phi_ref(p,1);Y_ref(p,1)];endendYita_ref=cell2mat(Yita_ref_cell);error_1=Yita_ref-PSI*kesi-GAMMA*PHI; %求偏差%error_1 = Yita_ref-PSI*kesi;f_cell=cell(1,2);f_cell{1,1}=2*error_1'*Q*THETA;f_cell{1,2}=0; % f=(cell2mat(f_cell))';f=-cell2mat(f_cell);%% 以下為約束生成區域%控制量約束A_t=zeros(Nc,Nc);%見falcone論文 P181for p=1:1:Ncfor q=1:1:Ncif q<=p A_t(p,q)=1;else A_t(p,q)=0;endend end A_I=kron(A_t,eye(Nu));%求克羅內克積Ut=kron(ones(Nc,1),U(1));umin=-0.1744;%維數與控制變量的個數相同umax=0.1744;delta_umin=-0.0148*0.4;delta_umax=0.0148*0.4;Umin=kron(ones(Nc,1),umin);Umax=kron(ones(Nc,1),umax);%輸出量約束ycmax=[0.21;5];ycmin=[-0.3;-3];Ycmax=kron(ones(Np,1),ycmax);Ycmin=kron(ones(Np,1),ycmin);%結合控制量約束和輸出量約束A_cons_cell={A_I zeros(Nu*Nc,1);-A_I zeros(Nu*Nc,1);THETA zeros(Ny*Np,1);-THETA zeros(Ny*Np,1)};b_cons_cell={Umax-Ut;-Umin+Ut;Ycmax-PSI*kesi-GAMMA*PHI;-Ycmin+PSI*kesi+GAMMA*PHI};A_cons=cell2mat(A_cons_cell);%（求解方程）狀態量不等式約束增益矩陣，轉換為絕對值的取值范圍b_cons=cell2mat(b_cons_cell);%（求解方程）狀態量不等式約束的取值%狀態量約束M=10; delta_Umin=kron(ones(Nc,1),delta_umin);delta_Umax=kron(ones(Nc,1),delta_umax);lb=[delta_Umin;0];%（求解方程）狀態量下界，包含控制時域內控制增量和松弛因子ub=[delta_Umax;M];%（求解方程）狀態量上界，包含控制時域內控制增量和松弛因子%% 開始求解過程options = optimset('Algorithm','active-set');x_start=zeros(Nc+1,1);%加入一個起始點[X,fval,exitflag]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub,x_start,options);%% 計算輸出u_piao=X(1);%得到控制增量U(1)=kesi(7,1)+u_piao;%當前時刻的控制量為上一刻時刻控制+控制增量%U(2)=Yita_ref(2);%輸出dphi_refsys= U;toc % End of mdlOutputs.

總結

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

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