如果對Carsim的基礎(chǔ)使用還不了解，可以參考：【Carsim Simulink自動駕駛仿真】基于MPC的速度控制
如果對MPC算法原理不清楚，可以參考：如何理解MPC模型預(yù)測控制理論
項(xiàng)目介紹：
教程為北理工的無人駕駛車輛模型預(yù)測控制第2版。所用的仿真軟件為Carsim2020.0和MatlabR2021a。使用MPC控制思想對車輛進(jìn)行軌跡跟蹤控制，并給出仿真結(jié)果。

整整弄了兩天，踩了無數(shù)的坑，所以篇幅比較大，如果還有什么其他問題，歡迎一起討論。

從網(wǎng)上下載下來的代碼運(yùn)行不了，所以本文的代碼是經(jīng)過修改和調(diào)試的。

基于MPC的軌跡跟蹤控制

• 效果圖
• • 直線跟蹤
  • • 3m/s
    • 5m/s
    • 10m/s
  • 圓跟蹤
  • • 3m/s
    • 5m/s
    • 10m/s
• MATLAB框架搭建
• 代碼的數(shù)學(xué)遺漏問題與修改
• • 運(yùn)動學(xué)模型少L
  • 改寫之后的目標(biāo)函數(shù)線性項(xiàng)少系數(shù)
• 代碼中的其他問題及討論
• • 輸入維度不匹配的情況
  • 索引超出數(shù)組范圍
  • • 1.變量的范圍
    • 2.換一個(gè)求解器
    • 3.初始值全部設(shè)為0
    • 4.對于kesi(4)和kesi(5)的賦值 （這個(gè)是我的問題）
    • • 一些理解
• 參數(shù)的選取
• 速度問題
• 附錄：全代碼(修改過后的)

效果圖

因?yàn)閰?shù)經(jīng)過了調(diào)整，所以效果上和書上不一樣。

直線跟蹤

圖像為：路徑，速度，誤差和前輪偏角

前輪偏角的局部放大圖為：

3m/s

5m/s

10m/s

速度的具體圖像為：

圓跟蹤

圖像為：路徑，速度和前輪偏角(誤差不知道該在Carsim中怎么畫，知道的大佬請私信，嗚嗚嗚)：

軌跡放大之后：

前輪偏角為：

前輪偏角放大之后：

3m/s

速度為：

5m/s

速度為：

10m/s

速度為：

MATLAB框架搭建

書中給出的MATLAB框架如下：

因?yàn)閷τ谶@種方法輸出需要再另外處理。所以我這里直接將速度乘3.6，角度也在程序中直接處理了。對應(yīng)的框架圖為：

代碼的數(shù)學(xué)遺漏問題與修改

書中的代碼和下載下來的代碼不一樣，所以還是檢查一下這兩個(gè)地方，

運(yùn)動學(xué)模型少L

在書中，推導(dǎo)運(yùn)動學(xué)模型的時(shí)候，最后一個(gè)數(shù)字分母是有一個(gè)l的：

但是在代碼中，這個(gè)地方少了，需要加上。

改寫之后的目標(biāo)函數(shù)線性項(xiàng)少系數(shù)

這里的2在書中沒有少，但是下載下來的代碼中沒有這個(gè)系數(shù)。如果沒有的可以加一下。

代碼中的其他問題及討論

輸入維度不匹配的情況

看一下S-Function的輸入是3個(gè)還是5個(gè)，輸出是2個(gè)還是直接給出5個(gè)，對應(yīng)的代碼中也應(yīng)該和其一樣。具體在S-Function中是初始化的第一個(gè)函數(shù)中的：

sizes.NumOutputs =5;%[speed,steering]sizes.NumInputs =3;

索引超出數(shù)組范圍

這個(gè)是我的主要問題，之所以報(bào)錯(cuò)，是因?yàn)榍蠼馄髑蠼獠怀鰜韮?yōu)化問題，方法的話我找了一些，然后自己也試了一些：

1.變量的范圍

這里有兩種情況：

• 第一種情況是檢查變量范圍delta_umin=[-0.05;-0.0082;];如果當(dāng)中的-0.05是0.05，將其加一個(gè)負(fù)號。
• 第二種情況其實(shí)擴(kuò)大變量的范圍可以求解，但是這里不建議，如果你在擴(kuò)大范圍之后可以求解，那么建議還是將其變回原來的數(shù)，更改其他值。

2.換一個(gè)求解器

使用interior-point-convex，而不是active-set

%options = optimset('Algorithm','active-set'); options = optimset('Algorithm','interior-point-convex');

3.初始值全部設(shè)為0

檢查初始化函數(shù)mdlInitializeSizes中的x0和U是否都為零。注意，這里的U表示的就是控制量與參考控制的差值。

function [sys,x0,str,ts] = mdlInitializeSizes %找到以下兩個(gè)值 x0 = [0;0;0]; U=[0;0];

4.對于kesi(4)和kesi(5)的賦值 （這個(gè)是我的問題）

這個(gè)我認(rèn)為爭議比較大，但是改了之后確實(shí)沒有報(bào)錯(cuò)了。
先說一下，按照理論：

kesi(4)和kesi(5)就是這里的控制變量$\tilde{u}$。而這個(gè)$\tilde{u}$根據(jù)定義是：

是當(dāng)前速度與參考速度的插值，所以這里我們應(yīng)該將Carsim中的5個(gè)變量全部輸入到S-Funciton中，然后進(jìn)行如下操作：

U(1) = u(4)/3.6 - vd1;steer_SW = u(5)*pi/180;steering_angle = steer_SW/18;U(2) = steering_angle - vd2;kesi(4)=U(1); % vel-vel_refkesi(5)=U(2); % steer_angle - steering_ref

之前我從來沒有懷疑過這里，直到我看到書中的代碼和網(wǎng)上下載的不一樣的時(shí)候，我就嘗試了一下書上的代碼，即將前四行全部注釋掉：

% U(1) = u(4)/3.6 - vd1; % steer_SW = u(5)*pi/180; % steering_angle = steer_SW/18; % U(2) = steering_angle - vd2;kesi(4)=U(1); % vel-vel_refkesi(5)=U(2); % steer_angle - steering_ref

這樣改之后我的代碼就不會出現(xiàn)問題了。

一些理解

首先我們要清楚，kesi(4,5)是控制減去參考控制，求解器解出來的X是前后兩次的控制差值。
我們先看能跑對的程序，大致過程可以簡化成設(shè)置值，求解，給出輸出三步：

U = [0,0] for 循環(huán)： #第一步：設(shè)置kesikesi(4)=U(1); % vel-vel_refkesi(5)=U(2); % steer_angle - steering_ref #第二步：求解[X,~,~]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub,[],options); #第三步：給出輸出u_dot(1)=X(1);u_dot(2)=X(2);U(1)=kesi(4)+u_dot(1);%用于存儲上一個(gè)時(shí)刻的控制量U(2)=kesi(5)+u_dot(2);u_real(1) = U(1) + vd1;u_real(2) = U(2) + vd2;

這里列一個(gè)表格：

時(shí)間求解之前的U，也就是上一步的Ukesiu_dot求解之后的U輸出

k-1	$u_{k?1}?u_{k?1}^{ref}$	$u_{k?1}?u_{k?1}^{ref}$	$u_k?u_k^{ref}?u_{k?1}+u_{k?1}^{ref}$	$u_{k?1}?u_{k?1}^{ref}+u_k?u_k^{ref}?u_{k?1}+u_{k?1}^{ref}=u_k?u_k^{ref}$	u_k
之后就可以征程循環(huán)。注意這里的初始值設(shè)置為[0,0]是合理的。

我出錯(cuò)的程序：

U = [0,0] for 循環(huán)： #第一步：設(shè)置kesiU(1) = u(4)/3.6 - vd1;steer_SW = u(5)*pi/180;steering_angle = steer_SW/18;U(2) = steering_angle - vd2;kesi(4)=U(1); % vel-vel_refkesi(5)=U(2); % steer_angle - steering_ref #第二步：求解[X,~,~]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub,[],options); #第三步：給出輸出u_dot(1)=X(1);u_dot(2)=X(2);U(1)=kesi(4)+u_dot(1);%用于存儲上一個(gè)時(shí)刻的控制量U(2)=kesi(5)+u_dot(2);u_real(1) = U(1) + vd1;u_real(2) = U(2) + vd2;

建立同樣的表格，這里唯一不一樣的是，之前的輸入其實(shí)是記憶到了程序里面，這里的輸入變成了從程序中獲取：

時(shí)間求解之前的U，也就是上一步的Ukesiu_dot求解之后的U輸出

k-1	$u_{k?1}^{input}?u_{k?1}^{ref}$	$u_{k?1}^{input}?u_{k?1}^{ref}$	$u_k?u_k^{ref}?u_{k?1}^{input}+u_{k?1}^{ref}$	$u_k?u_k^{ref}$	u_k
k	$u_k^{input}?u_k^{ref}$
但是按照道理來說，這個(gè)和上一個(gè)表格是一樣的，這里不一樣的是速度輸入之后需要進(jìn)行變換，輸出的時(shí)候也要進(jìn)行變換，這變換之后會存在的誤差比較大。還有一種可能，這里其實(shí)輸入進(jìn)來的是速度和方向盤轉(zhuǎn)角，這個(gè)方向盤轉(zhuǎn)角和輪胎轉(zhuǎn)角之間有一個(gè)系數(shù)，這個(gè)系數(shù)這里設(shè)置的18，但是具體設(shè)置多少，也沒有具體的深究。
總之我個(gè)人感覺可能是上一時(shí)刻的輸出$u_k$和這一時(shí)刻的輸入$u_k^{input}$可能不一樣。希望各位大佬也給出一些解釋。

參數(shù)的選取

參數(shù)這里有4個(gè)，預(yù)測步長，控制步長，Q和R。這里Q和R都是對角陣，表格中的數(shù)值是對角陣中的數(shù)，而不是Q和R的實(shí)際值。
這里我的參數(shù)為：

預(yù)測步長($N_p$)控制步長(N_c)QR

100	30	1	100

我先調(diào)整的Q和R，當(dāng)軌跡不貼合的時(shí)候，調(diào)大Q,當(dāng)速度收斂太慢，震蕩太久的時(shí)候調(diào)整R。根據(jù)我的經(jīng)驗(yàn)Q取1就好，之后就根據(jù)速度圖像的震蕩和你的評判標(biāo)準(zhǔn)進(jìn)行選擇就好。
對于預(yù)測步長和控制步長，預(yù)測的越久，說明這個(gè)智能體越有遠(yuǎn)見，超調(diào)就會相對小一點(diǎn)，個(gè)人感覺控制步長太多不是什么好的事情，因?yàn)楸旧硐Ｍ囘M(jìn)行相對較少的控制。

速度問題

這里有一個(gè)小問題，書中的速度在初始時(shí)刻是沒有減速的：

而在我做的過程中，速度在一開始都有減少：

這里我找到了問題，是兩個(gè)前輪的速度在一開始不一樣，但是按照道理，輸入都是一樣的，出現(xiàn)這樣的差異不知道怎么解決。

附錄：全代碼(修改過后的)

function [sys,x0,str,ts] = MPCtracking(t,x,u,flag)%控制量為速度和前輪偏角，模型是用運(yùn)動學(xué)模型switch flagcase 0[sys,x0,str,ts]=mdlInitializeSizes;case 2sys = mdlUpdates(t,x,u);case 3sys = mdlOutputs(t,x,u);case {1,4,9}sys = [];otherwiseerror(['unhandled flag =', num2str(flag)])end %End of MPCtracking%============================================================== % 初始化 %============================================================== function [sys,x0,str,ts] = mdlInitializeSizessizes = simsizes;sizes.NumContStates =0;sizes.NumDiscStates =3;sizes.NumOutputs =5;%[speed,steering]sizes.NumInputs =3;sizes.DirFeedthrough =1;sizes.NumSampleTimes =1;sys = simsizes(sizes);x0 = [0;0;0];global U;%存儲目前的控制變量[vel,delta]U=[0;0];str = [];ts = [0.05,0]; %采樣時(shí)間：[period,offset] %End of mdlInitializeSizes%============================================================== % 更新離散狀態(tài) %============================================================== function sys = mdlUpdates(t,x,u)sys = x; %End of mdlUpdate.%============================================================== % 計(jì)算輸出，核心輸出 %============================================================== function sys = mdlOutputs(t,x,u)global a b u_dot;global U;global kesi;ticNx = 3;%狀態(tài)量的個(gè)數(shù)Nu = 2;%控制量的個(gè)數(shù)Np = 100;%預(yù)測步長Nc = 30;%控制步長Row = 10; %松弛因子yaw_angle =u(3)*pi/180;%CarSim輸出的Yaw angle為角度，角度轉(zhuǎn)換為弧度%================================================== %最后調(diào)參 %==================================================u_bound1 = 0.2;u_bound2 = 0.436;delta1 = 0.05;delta2 = 0.0082;Q_a = 1;R_a = 100; %================================================== %構(gòu)建路徑 %================================================== % %直線路徑 % vd1 = 10;%ref_velocity % vd2 = 0;%ref_steering % r(1)=vd1*t; %ref_x % r(2)=5;%ref_y % r(3)=0;%ref_heading_angle% %半徑為35m的圓形軌跡, 圓心為(0, 35), 速度為3m/s % r(1)=25*sin(0.12*t); % r(2)=25+10-25*cos(0.12*t); % r(3)=0.12*t; % vd1=3; % vd2=0.104;%半徑為25m的圓形軌跡, 圓心為(0, 35), 速度為10m/sr(1)=25*sin(0.4*t);r(2)=25+10-25*cos(0.4*t);r(3)=0.4*t;vd1=10;vd2=0.104; % %半徑為25m的圓形軌跡, 圓心為(0, 35), 速度為5m/s % r(1)=25*sin(0.2*t); % r(2)=25+10-25*cos(0.2*t); % r(3)=0.2*t; % vd1=5; % vd2=0.104; %================================================== %構(gòu)建結(jié)束 %==================================================%添加轉(zhuǎn)化后的自變量kesikesi=zeros(Nx+Nu,1);kesi(1) = u(1)-r(1);%u(1)==X(1),x_offsetkesi(2) = u(2)-r(2);%u(2)==X(2),y_offsetheading_offset = yaw_angle - r(3); %u(3)==X(3),heading_angle_offsetif (heading_offset < -pi)heading_offset = heading_offset + 2*pi;endif (heading_offset > pi)heading_offset = heading_offset - 2*pi;endkesi(3)=heading_offset;% U(1) = u(4)/3.6 - vd1; % steer_SW = u(5)*pi/180; % steering_angle = steer_SW/18; % U(2) = steering_angle - vd2;kesi(4)=U(1); % vel-vel_refkesi(5)=U(2); % steer_angle - steering_refT = 0.05;L = 2.6;%輪間距%矩陣初始化t_d = r(3);%系數(shù)Q = Q_a*eye(Nx*Np,Nx*Np); % Q(Nx*Np,Nx*Np) = Q_a*0.01;R = R_a*eye(Nu*Nc);%運(yùn)動學(xué)方程u_dot = zeros(Nx,1);%%%%這里應(yīng)該沒有xa = [1 0 -vd1*sin(t_d)*T;0 1 vd1*cos(t_d)*T;0 0 1;];b = [cos(t_d)*T 0;sin(t_d)*T 0;tan(vd2)*T/L vd1*T/(L*(cos(vd2))^2);];%構(gòu)建新的狀態(tài)空間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=cell2mat(A_cell);B=cell2mat(B_cell);C=[ 1 0 0 0 0;0 1 0 0 0;0 0 1 0 0];PHI_cell=cell(Np,1);THETA_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;else THETA_cell{j,k}=zeros(Nx,Nu);endendendPHI=cell2mat(PHI_cell);%size(PHI)=[Nx*Np Nx+Nu]THETA=cell2mat(THETA_cell);%size(THETA)=[Nx*Np Nu*(Nc+1)]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); % H=(H+H')/2;error=PHI*kesi;f_cell=cell(1,2);f_cell{1,1} = 2*(error'*Q*THETA);f_cell{1,2} = 0;f=cell2mat(f_cell);%% 以下為約束生成區(qū)域%不等式約束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));%對應(yīng)于falcone論文約束處理的矩陣A,求克羅內(nèi)克積Ut=kron(ones(Nc,1), U);%umin=[-u_bound1; -u_bound2];%[min_vel, min_steer]維數(shù)與控制變量的個(gè)數(shù)相同umax=[u_bound1; u_bound2]; %[max_vel, max_steer],%0.436rad = 25degdelta_umin = [-delta1; -delta2]; % 0.0082rad = 0.47degdelta_umax = [delta1; delta2];Umin=kron(ones(Nc,1),umin);Umax=kron(ones(Nc,1),umax);A_cons_cell={A_I zeros(Nu*Nc, 1); -A_I zeros(Nu*Nc, 1)};b_cons_cell={Umax-Ut;-Umin+Ut};A_cons=cell2mat(A_cons_cell);%（求解方程）狀態(tài)量不等式約束增益矩陣，轉(zhuǎn)換為絕對值的取值范圍b_cons=cell2mat(b_cons_cell);%（求解方程）狀態(tài)量不等式約束的取值% 狀態(tài)量約束delta_Umin = kron(ones(Nc,1),delta_umin);delta_Umax = kron(ones(Nc,1),delta_umax);lb = [delta_Umin; 0];%（求解方程）狀態(tài)量下界ub = [delta_Umax; 10];%（求解方程）狀態(tài)量上界%嘗試初始值%x0 = zeros(Nc*Nu+1,1);%% 開始求解過程%options = optimset('Algorithm','active-set');options = optimset('Algorithm','interior-point-convex'); warning off all % close the warnings during computation [X,~,~]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub,[],options);%% 計(jì)算輸出 u_dot(1)=X(1);u_dot(2)=X(2);U(1)=kesi(4)+u_dot(1);%用于存儲上一個(gè)時(shí)刻的控制量U(2)=kesi(5)+u_dot(2);u_real(1) = U(1) + vd1;u_real(2) = U(2) + vd2;sys= [u_real(1)*3.6; u_real(2)*180/pi; u_real(2)*180/pi;0;0]; % vel, steering, x, ytoc% End of mdlOutputs.

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。