柚子快報(bào)激活碼778899分享：控制理論 自動(dòng)駕駛縱向控制算法

http://yzkb.51969.com/

? 本文來源——b站忠厚老實(shí)的老王，鏈接：忠厚老實(shí)的老王投稿視頻-忠厚老實(shí)的老王視頻分享-嗶哩嗶哩視頻 (bilibili.com)，侵刪。

功率和轉(zhuǎn)速之間的關(guān)系就是：功率P等于轉(zhuǎn)矩M乘以轉(zhuǎn)速ω。并不是油門越大加速度就越大。

發(fā)動(dòng)機(jī)和電機(jī)的轉(zhuǎn)速扭矩圖對(duì)比：

?電機(jī)的效率一般可以高達(dá)百分之九十，在高速區(qū)一般是遵循P=Fv的關(guān)系。

無人駕駛車輛一般在電車上面部署，這里只需要弄清楚電機(jī)的轉(zhuǎn)速扭矩曲線即可，如下：

如上，電機(jī)在到達(dá)最大功率前，是以最大扭矩運(yùn)行的，等轉(zhuǎn)速到達(dá)最大功率之后，就以恒功率運(yùn)行。按照這個(gè)特性可以造一段電機(jī)的轉(zhuǎn)矩曲線（假設(shè)油門開度和功率是線性關(guān)系），如下：

?上面的黑線對(duì)應(yīng)的油門踩到底時(shí)對(duì)應(yīng)轉(zhuǎn)速——扭矩曲線，這里假設(shè)油門開度和功率是線性關(guān)系，也就是當(dāng)油門是0.2開度時(shí)，對(duì)應(yīng)的功率是380*0.2=76kw。

在2016.1版的carsim中沒有電車車型，可以手動(dòng)配置上面的電機(jī)參數(shù)，模擬出來一個(gè)電車，具體操作如下：

模型的輸入是發(fā)動(dòng)機(jī)的扭矩：

輸出是汽車的縱向速度和加速度以及曲軸的轉(zhuǎn)速：

變速器的設(shè)置如下：

設(shè)置如上，將傳動(dòng)比都設(shè)置成2，相當(dāng)于沒有變速器，將每個(gè)檔位的轉(zhuǎn)動(dòng)慣量都設(shè)置成0.04，將每個(gè)檔位的機(jī)械效率都設(shè)置成0.92。

道路設(shè)置如下：

剎車/油門標(biāo)定表的制作（重點(diǎn)）

原理：給定一個(gè)恒定的油門，車跑起來會(huì)后，會(huì)輸出一個(gè)速度和加速度。標(biāo)定表就是找到油門和v、a之間的關(guān)系。

基于而言：

如上，thr是油門，就是踩不同的油門，就得到不同的速度和加速曲線，例如上面當(dāng)油門開度是0.3時(shí)，速度和加速度曲線就可以畫出來，速度和加速度可以合并到一個(gè)圖上。一般來說速度和加速度曲線的一般趨勢(shì)都是上圖的趨勢(shì)，也就是速度越高，加速度越差，因?yàn)镻=Mω，速度很大時(shí)，力矩就會(huì)變小。

?傳統(tǒng)做實(shí)驗(yàn)的方法有幾個(gè)缺點(diǎn)：①試驗(yàn)路面必須平直并且足夠長(zhǎng)；②方向盤不能動(dòng)，但是在實(shí)際試驗(yàn)過程中方向盤不可能完全不動(dòng)；③無法解決車輛在行駛過程中質(zhì)量變換的問題。乘用車還不是很明顯，但在裝載貨物的大貨車上面空載和滿載時(shí)的標(biāo)定表不適配，需要在不用載荷時(shí)標(biāo)定，比較繁瑣?，F(xiàn)在比較新興的標(biāo)定方法就是使用深度學(xué)習(xí)模型把v、a和質(zhì)量作為特征進(jìn)行訓(xùn)練深度學(xué)習(xí)在線進(jìn)行更新，可以適應(yīng)不同質(zhì)量。下面使用傳統(tǒng)的方法來制作標(biāo)定表：

在carsim中設(shè)置輸入\輸出進(jìn)行制作標(biāo)定表：

由于要標(biāo)定油門和剎車，所以輸入是油門和剎車的制動(dòng)壓力。

simulink模型如下：

?其中，thr是一個(gè)constant模塊，將其使用thr參數(shù)，vx和ax是兩個(gè)to workspace模塊，可以將跑出來的結(jié)果導(dǎo)入的工作區(qū)中，這里首先標(biāo)定油門，所以將剎車設(shè)置成0。

.m文件：

這里主要要將to workspace模塊中模塊屬性取消勾選單一仿真輸出，否則會(huì)報(bào)錯(cuò)。?取消方法：to workspace模塊中消除out.前綴的方法：simulink->建模->模型設(shè)置->數(shù)據(jù)導(dǎo)入/導(dǎo)出->單一仿真輸出取消勾選->點(diǎn)擊應(yīng)用確定

運(yùn)行后，vx和ax等參數(shù)就傳到了工作區(qū)：

?

其中table就是最終的標(biāo)定表：

?在2D-lookup table模塊中進(jìn)行如下設(shè)置：

加速度控制仿真框圖如下：

加速度示波器中速度穩(wěn)定在3一段時(shí)間后下降，這是因?yàn)殡姍C(jī)特性，速度有限，不能再保持較高車速的情況下還有較大的加速度。

?速度控制框圖，設(shè)置期望速度是10，如下（如果控制效果不是很好，就吧油門標(biāo)細(xì)一點(diǎn)）：

輸出的速度信號(hào)的示波器圖如下：

這里的加速度設(shè)置為期望的車速和實(shí)際車速的差值，將這個(gè)相減的結(jié)果輸入到加速度信號(hào)中。加速度不能直接使用輸出的加速度，因?yàn)榧铀俣群退俣纫ヅ洌@里規(guī)劃的速度是10，初速度是0，從0到10，剛開始必然要有一個(gè)很大的加速度，讓車加速，后期加速度逐漸減小，最好的結(jié)果就是速度到10時(shí)，加速度剛好是0。如果直接使用車的實(shí)際加速度，這個(gè)實(shí)際的加速度并不一定和車速相匹配。下面演示直接使用實(shí)際加速度信號(hào)和將加速度信號(hào)設(shè)置成時(shí):

可以看出當(dāng)速度和加速不匹配時(shí)，跟蹤速度的效果很差。相匹配的速度和加速度應(yīng)該是當(dāng)速度差距越大時(shí)，加速度越大，速度差距越小，加速度越小。直到速度到達(dá)期望速度時(shí)，加速度0。

剎車的標(biāo)定表和油門標(biāo)定表的制作方法相似：

標(biāo)定的simulink系統(tǒng)框圖：

?首先在simulink中將油門設(shè)置成0，在carsim中將車輛的初速度設(shè)置為180km/h。

剎車標(biāo)定代碼：

標(biāo)定表如下：

?在第161列第一個(gè)數(shù)據(jù)對(duì)應(yīng)的是加速度等于0，速度也等于0（坐標(biāo)是（0,0）處于停車狀態(tài)），但是此時(shí)擬合出來的制動(dòng)壓力很大為6點(diǎn)幾，這是因?yàn)閯x車的目的最后都是加速度為0，速度為0，所有曲線的焦點(diǎn)都是（0,0），這個(gè)坐標(biāo)具有奇異性，這里matlab取了其平均值，這里對(duì)trabebr這個(gè)表做出修改，將這個(gè)坐標(biāo)位置的剎車壓力設(shè)置成0.3。

以-3減加速度控制：

加速度的示波器：

以0為期望速度的剎車控制系統(tǒng)框圖：

?加速度控制信號(hào)：

縱向雙pid控制

首先將剎車和油門的標(biāo)定表合并，將兩者合并成一張標(biāo)定表的好處就是控制時(shí)制動(dòng)和油門會(huì)自動(dòng)的控制平滑，不太會(huì)出現(xiàn)油門踩過了切換到剎車再切換到油門這樣的不平順的情況。合并一共需要三個(gè)m文件，前兩個(gè)文件分別采集油門和剎車數(shù)據(jù)，油門和剎車的標(biāo)定過程不能同時(shí)標(biāo)定在一個(gè)m文件中，因?yàn)槌跛俣炔灰粯樱烷T的初速度是從0開始慢慢增大，剎車的初速度是從180或者144逐漸減小到0。第三個(gè)文件合并數(shù)據(jù)。

首先標(biāo)定的模型框圖：

油門標(biāo)定的代碼：

剎車標(biāo)定的代碼：

合并油門和剎車的代碼：

整體控制框圖：

? 以10為期望速度，用10和當(dāng)前速度相減可以得到加速度信號(hào)，這是一個(gè)簡(jiǎn)單的pid控制，也就是當(dāng)當(dāng)前速度距離期望速度很遠(yuǎn)時(shí)，有一個(gè)較大的加速度，距離很近時(shí)，加速度就比較小。這樣可以基本可以實(shí)現(xiàn)對(duì)期望速度的控制，但是有穩(wěn)態(tài)誤差：

此時(shí)可以加一個(gè)比例增益也就是P控制，比例控制一般不能消除穩(wěn)態(tài)誤差，但是隨著比例增益k的增加，控制輸入量與誤差成正比，同時(shí)k越大，系統(tǒng)的響應(yīng)速度越快，穩(wěn)態(tài)誤差也會(huì)無限的接近于零，但在實(shí)際應(yīng)用中比例系數(shù)不能無限增大。下面是當(dāng)比例增益是10的輸出情況，可以看出系統(tǒng)響應(yīng)速度更快，也更接近于10，但是有明顯振蕩，這是因?yàn)楸壤鲆嬖黾恿?0倍，系統(tǒng)對(duì)誤差更加敏感，稍微的誤差就會(huì)引起響應(yīng)，所以比例增益不是越大越好。

在carsim中將車輛的初速度設(shè)置為180km/h，使其減速到10m/s時(shí)的情況：

下面直接使用simulink中自帶的pid模塊，將比例增益設(shè)置為2，積分增益設(shè)為0.1，微分增益設(shè)置為0，此時(shí)輸出會(huì)出現(xiàn)明顯的超調(diào)，這是因?yàn)橐敕e分后會(huì)使得原來的一階系統(tǒng)變成二階系統(tǒng)，積分控制是可以消除穩(wěn)態(tài)誤差，只要實(shí)際速度和期望速度的差值不是0，積分隨著時(shí)間的增長(zhǎng)越來越大，所以只要時(shí)足夠長(zhǎng)，它一定可以收斂到期望值，如圖：

微分控制中微分項(xiàng)與誤差成正比，所以可以預(yù)測(cè)誤差的變化趨勢(shì)并提前做出反應(yīng)，微分可以抑制超調(diào)，d控制也會(huì)增加系統(tǒng)的響應(yīng)速度。

對(duì)d進(jìn)行一下設(shè)置：

輸出如下：

這里只用了比例微分控制，這里的比例項(xiàng)沒有引起超調(diào)，所以對(duì)超調(diào)的控制效果不明顯，但在實(shí)際應(yīng)用中比例項(xiàng)是可以會(huì)引起超調(diào)，因?yàn)樵趯?shí)際應(yīng)用中信號(hào)是有延遲的，下面可以加延遲模塊模擬下實(shí)際應(yīng)用的情況，假設(shè)油門和剎車信號(hào)到執(zhí)行有100毫秒的延遲，速度從傳感器出來有10ms的延遲。一般微分對(duì)比例帶來的超調(diào)有較好的效果，但是微分對(duì)積分帶來的超調(diào)效果不好，微分增益一般不能設(shè)置過大，過大會(huì)引起高頻噪聲。

模擬實(shí)際中的路徑跟蹤，框圖如下：

自動(dòng)駕駛橫縱向控制總結(jié)：

?

軌跡規(guī)劃是包含時(shí)間信息的，給定起點(diǎn)和終點(diǎn)的信息可以做很多場(chǎng)景，例如下面：

下面在絕對(duì)坐標(biāo)系下規(guī)劃，可以將問題轉(zhuǎn)換成：

?y是汽車橫向方向，y的自變量是x，因?yàn)闄M向是由縱向引發(fā)的，但是只用y(x)來做軌跡規(guī)劃是不夠的，做軌跡規(guī)劃的必須都是與時(shí)間相關(guān)的，所以要對(duì)y'（y對(duì)x進(jìn)行求導(dǎo)）轉(zhuǎn)換到y(tǒng)·（y對(duì)時(shí)間進(jìn)行求導(dǎo)）進(jìn)行轉(zhuǎn)換。

現(xiàn)在就可以根據(jù)上面的邊界邊界條件以及時(shí)間T，規(guī)劃出一條軌跡，生成x(t)、y(x)，在通過上面的轉(zhuǎn)換關(guān)系算出y(t)。下面使用五次多項(xiàng)式，五次多項(xiàng)式對(duì)應(yīng)由六個(gè)系數(shù)，正好對(duì)應(yīng)著上面x的六個(gè)邊界條件。對(duì)于y也一樣。

路徑的匹配點(diǎn)與時(shí)間相關(guān)，每過一個(gè)時(shí)間，規(guī)劃器會(huì)發(fā)出一個(gè)規(guī)劃的時(shí)間信息xr，yr、θr、kr。其中xr與yr就是規(guī)劃器的xt與yt，與時(shí)間自然相關(guān)，θr、kr與時(shí)間產(chǎn)生關(guān)系如下：

?規(guī)劃使用大地坐標(biāo)系，自然坐標(biāo)系。

?小結(jié)：規(guī)劃到控制的邏輯

前4個(gè)公式是橫向控制需要的；后面是縱向控制需要的，其中es和s·也是有橫向中得來的，所以縱向控制的輸入是3個(gè)。

路徑規(guī)劃算法接口：

需要注意的是這里的路徑規(guī)劃和之前橫向控制的路徑規(guī)劃不一樣，必須要算出每個(gè)輪子精確的側(cè)偏剛度，這里知道精確的垂向力，然后根據(jù)不同的垂向力去查曲線再估算出側(cè)偏剛度。

function [vp,ap,xr,yr,thetar,kr] = fcn(t)

dx=100;

dy=100;%dy為100，時(shí)間是50，為低速大轉(zhuǎn)向工況

T=50;

% 初始和最終條件

xstart=[0,0,0];

xend=[dx,0,0];

ystart=[0,0,0];

yend=[dy,0,0];

% 計(jì)算 x(t)和y(t) 軌跡的系數(shù),使用五次多項(xiàng)式

a=zeros(1,6);

b=zeros(1,6);

a(1)=xstart(1);

a(2)=xstart(2);

a(3)=xstart(3)/2;

A1=[T^3,T^4,T^5;

3*T^2,4*T^3,5*T^4;

6*T,12*T^2,20*T^3];

B1=[xend(1)-a(1)-a(2)*T-a(3)*T^2;

xend(2)-a(2)-2*a(3)*T;

xend(3)-2*a(3)];

xs=inv(A1)*B1;

a(4)=xs(1);

a(5)=xs(2);

a(6)=xs(3);

b(1)=ystart(1);

b(2)=ystart(2);

b(3)=ystart(3)/2;

A2=[dx^3,dx^4,dx^5;

3*dx^2,4*dx^3,5*dx^4;

6*dx,12*dx^2,20*dx^3];

B2=[yend(1)-b(1)-b(2)*dx-b(3)*dx^2;

yend(2)-b(2)-2*b(3)*dx;

yend(3)-2*b(3)];

ys=inv(A2)*B2;

b(4)=ys(1);

b(5)=ys(2);

b(6)=ys(3);

% 在時(shí)間 t 處計(jì)算位置 (xr, yr)

xr=a(1)+a(2)*t+a(3)*t^2+a(4)*t^3+a(5)*t^4+a(6)*t^5;

yr=b(1)+b(2)*xr+b(3)*xr^2+b(4)*xr^3+b(5)*xr^4+b(6)*xr^5;

% 一階導(dǎo)數(shù) (速度)

xr_dot=a(2)+2*a(3)*t+3*a(4)*t^2+4*a(5)*t^3+5*a(6)*t^4;

yr_dx=b(2)+2*b(3)*xr+3*b(4)*xr^2+4*b(5)*xr^3+5*b(6)*xr^4;

yr_dot=yr_dx*xr_dot;

% 方向角度 (thetar)

thetar=atan(yr_dx);

xr_dot2=2*a(3)+6*a(4)*t+12*a(5)*t^2+20*a(6)*t^3;

yr_dx2=2*b(3)+6*b(4)*xr+12*b(5)*xr^2+20*b(6)*xr^3;

yr_dot2=yr_dx2*xr_dot^2+yr_dx*xr_dot2;

kr=yr_dx2/((1+yr_dx^2)^1.5);

% 計(jì)算速度 (vp) 和加速度 (ap)

vp=sqrt(xr_dot^2+yr_dot^2);

if xr_dot2>=0 %ap可正可負(fù)，在加速時(shí)是正的，減速時(shí)是負(fù)的。

ap=sqrt(xr_dot2^2+yr_dot2^2);

else

ap=-sqrt(xr_dot2^2+yr_dot2^2);

end

仿真框圖：

在低速打轉(zhuǎn)向時(shí)的跟蹤效果：

?在低速大轉(zhuǎn)向時(shí)跟蹤效果良好。

在平時(shí)搭建模型跑出得結(jié)果中橫向誤差很大時(shí)，大概率原因如下：（1）不足轉(zhuǎn)向（2）調(diào)解LQR，或者調(diào)節(jié)權(quán)重矩陣Q調(diào)大，給ed一個(gè)較大的懲罰值，但是也不能太大，太大會(huì)引起超調(diào)。（3）將輪胎的側(cè)偏剛度估計(jì)地準(zhǔn)一些。

下面演示如果調(diào)節(jié)因?yàn)檗D(zhuǎn)向不足引起得橫向誤差過大。

汽車有不足轉(zhuǎn)向的原因：

?處理方法：對(duì)橫向誤差使用pid的積分模塊，對(duì)橫向誤差進(jìn)行積分，將誤差積分補(bǔ)償?shù)接?jì)算出的角度（計(jì)算出來的轉(zhuǎn)角減去誤差積分）。理解為，angle是向左為正，ed也是向左為正，當(dāng)橫向誤差ed是正值時(shí)，相當(dāng)于向左打多了，此時(shí)減去它相當(dāng)于往右回正了一點(diǎn)。

修改后的模型框圖：

柚子快報(bào)激活碼778899分享：控制理論 自動(dòng)駕駛縱向控制算法

http://yzkb.51969.com/

相關(guān)鏈接