手把手教用matlab做無人駕駛（十三）-- 後輪反饋控制算法（Rear wheel feedback）

後輪反饋控制算法：Rear wheel feedback 文章題目：A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles

文章下載地址：http://xueshu.baidu.com/usercenter/paper/show?paperid=114c0fdaa1d0d5ed6f114fa66ffe4643&site=xueshu_se

 

 

後輪反饋式是利用後輪中心的跟蹤偏差來進行轉向控制量計算的方法，根據運動學方程(2-1)及車輛後輪與參考路徑的幾何關係，可推導出參考路徑座標系(𝑠, 𝑙)，Ferent 座標系，下  的變化率爲:

̇其中，𝑒表示參考路徑上離本車最近點和本車的橫向位置誤差，  爲參考路徑上離本車最近點的切線角和本車的橫擺角的誤差值，𝑘(𝑠)爲參考點𝑠處的曲率，  爲車輛後輪線速度，𝜔爲車身橫擺角速度

對於二次連續可導的參考線，需要設計車身橫擺角速度𝜔保證在李亞普洛夫方程  下局部漸進收斂。李亞普洛夫穩定用數學的語言描述爲:可控可微分的狀態方程  ，在給定的參考軌跡  下，滿足下列條件： t;lt;𝑡_2lt;𝑡_2𝑡_2，存在𝛽>0:

對於李亞普洛夫穩定也分爲漸進穩定和指數穩定。漸進穩定指對於時變系統，𝛽在條件(1)下獨立於時間  ;指數穩定指收斂率是以指數下降。
對於後輪反饋式算法，爲了保證車輛的李亞普洛夫穩定性，𝜔可表示爲式(2-10)

其中，  爲橫擺角偏差反饋控制增益，  爲橫向位置偏差反饋控制增益。因此，根據運動學方程(2-1)，可得到前輪轉角𝛿爲:

這裏大概講解了一下原理，具體大家可以下載論文看看。

現在給出matlab代碼，這裏的代碼是在我前面博客中stanley算法代碼中改過來的：

% Editor:Robert
% Date:2019.6.11

function steercmd = fcn(RefPos,direction,CurPos,curvelocity,curvature)
KTH = 60;
KE=0.5;
gain=20;
wheelbase=0.15;
% Clip heading angle to be within [0, 360) and convert to radian

RefPos(3) =RefPos(3)*pi/180 ;
CurPos(3)= CurPos(3)*pi/180;

%to the interval [0 2*pi] such that zero maps to
%zero and 2*pi maps to 2*pi
twoPiRefPos = cast(2*pi, 'like', RefPos(3));
twoPiCurPos = cast(2*pi, 'like', CurPos(3));

 positiveInputRefPos = RefPos(3) > 0;
 positiveInputCurPos = CurPos(3) > 0;

 thetaRef = mod(RefPos(3), twoPiRefPos);
 thetaCur = mod(CurPos(3), twoPiRefPos);
 
 positiveInputRefPos = (thetaRef == 0) & positiveInputRefPos;
 thetaRef = thetaRef + twoPiRefPos*positiveInputRefPos; 
 
positiveInputCurPos = (thetaCur == 0) & positiveInputCurPos;
thetaCur = thetaCur + twoPiCurPos*positiveInputCurPos;

RefPos(3)=thetaRef ;
CurPos(3)=thetaCur;

%This function ensures that angError is in the range [-pi,pi).

angError =CurPos(3)-RefPos(3);

piVal = cast(pi, 'like', angError);

twoPi = cast(2*pi, 'like', angError);

positiveInput = (angError+piVal)> 0;

theta = mod((angError+piVal), twoPi); 

positiveInput = (theta == 0) & positiveInput;
theta = theta + twoPi*positiveInput; 

theta=theta-piVal;

angError=theta;

d = norm(CurPos(1:2) - RefPos(1:2));

if  angError==0
    omega=0;
else
    omega = curvelocity*curvature*cos(angError)/(1-curvature*d)-KTH*abs(curvelocity)*angError-KE*curvelocity*sin(angError)*d/angError
end

delta=atan2(wheelbase*omega/curvelocity,1);

delta=delta*180/pi;

delta = sign(delta) * min(abs(delta), 35);

steercmd = delta;
這裏的代碼跟前面的基本一樣，不同的就是omega = curvelocity*curvature*cos(angError)/(1-curvature*d)-KTH*abs(curvelocity)*angError-KE*curvelocity*sin(angError)*d/angError，這個對於上面公式(2-10)和delta=atan2(wheelbase*omega/curvelocity,1)對應公式（2-11）。

仿真框架圖：

 

仿真結果：