# 手把手教用matlab做无人驾驶（十一）-- stanley控制算法

stanley算法是斯坦福大学开发的无人车，通过这样设计横向控制器，获得了2005年度DARPA Grand Challenge的第一名，这个stanley算法相比较前一篇博客介绍的pure pursuit算法，优点就是既考虑了车身偏航角，又考虑了车与跟踪路径的横向误差距离，这个设计想法我觉得很棒。这篇文章可以在这个地址免费下载到：

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

% 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.仿真结果如下：

08-04 5415

10-27 1573

12-12 1582

12-24 1453

06-10 3418

06-16 4531

07-27 1546

06-12

10-26

10-13 4881

03-26 4万+

03-18

09-02 9499

12-19 7972

04-05 4万+

09-26

09-23 513

11-20 552

10-18 4154

08-21 1759

07-23 4万+

03-27 1万+

05-14 2万+

09-02 1万+

08-12 6133

09-03 7260

02-15 1万+

03-23 2万+

#### 曾经废寝忘食学到的技术，现在都没用了......

©️2019 CSDN 皮肤主题: 大白 设计师: CSDN官方博客