手把手教用matlab做無人駕駛（十一）-- stanley控制算法

stanley算法是斯坦福大學開發的無人車，通過這樣設計橫向控制器，獲得了2005年度DARPA Grand Challenge的第一名，這個stanley算法相比較前一篇博客介紹的pure pursuit算法，優點就是既考慮了車身偏航角，又考慮了車與跟蹤路徑的橫向誤差距離，這個設計想法我覺得很棒。這篇文章可以在這個地址免費下載到：

http://xueshu.baidu.com/usercenter/paper/show?paperid=0e69039a5c58c8336ee5a08dfd0a6538&site=xueshu_se

對於已經工作的人而言，下載論文可能有時候可能有一點麻煩，這裏順便給出英文論文免費下載地址：http://tool.yovisun.com/scihub

 

                       

這裏我就不講解原理的證明了，可以自己去看文章。直接給出他這裏控制輸入的設計：

 

                                

                                          

現在，我們來看matlab/simulink的實現(代碼是在matlab官方老師代碼中改的，我相信看我博客的同學大多都是關注算法的，所以大家只要關注我寫的stanley control即可，不要糾結太多，如果想用瞭解更多並且有時間的也可以自己慢慢探索 ，同時也歡迎一起探討)：

注意：這裏stanley control模塊我用了stanley文章裏的圖片。

現在來看看代碼的實現：

% Editor:Robert
% Date:2019.6.11

function steercmd = fcn(RefPos,direction,CurPos,curvelocity)
Ksoft = 1;
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;

%rearPoseToFrontPose Transform pose from rear wheel to front wheel
tHat = [cos(RefPos(3)), sin(RefPos(3))];
CurPos(:, 1) = CurPos(:, 1) + wheelbase * cos(CurPos(3));
CurPos(:, 2) = CurPos(:, 2) + wheelbase * sin(CurPos(3));

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

% Tracking error vector
posError = -(d(1)*tHat(2) - d(2)*tHat(1));

delta = -(angError + atan(gain * posError/(Ksoft+curvelocity)));

delta=delta*180/pi;

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

steercmd = delta;
 

上面就是這個算法的整個代碼的實現，代碼中有部分註釋，這裏也講一下：

1.首先，要角度要轉換爲弧度。

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

2.下面代碼就是讓0度對應0度，360度對應360

%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;

3.其實最應該注意的是要把後輪座標系轉化到前輪座標中

%rearPoseToFrontPose Transform pose from rear wheel to front wheel
tHat = [cos(RefPos(3)), sin(RefPos(3))];
CurPos(:, 1) = CurPos(:, 1) + wheelbase * cos(CurPos(3));
CurPos(:, 2) = CurPos(:, 2) + wheelbase * sin(CurPos(3));

4.仿真結果如下：

代碼下載地址:https://download.csdn.net/download/caokaifa/11236591

注意：這個程序下載打開直接運行matlab/simulink程序：LateralControlStanleyKinematic.slx就好出現上面的結果。