matlab实现纯跟踪算法

对《Pure Pursuit纯跟踪算法Python/Matlab算法实现》该博客matlab算法提升：

该博客链接：https://blog.csdn.net/gophae/article/details/102761138

该博客算法跑的效果：

• 路径没有完整跟下来，可以看出尾部少一块
  

---

我在该博客提供算法的基础上做了改进：

下面展示一些 内联代码片。

// An highlighted block
k = 0.1;  % look forward gain
Lfc = 1;  % look-ahead distance
Kp = 1.0 ; % speed propotional gain
dt = 0.1  ;% [s]
L = 2.9  ;% [m] wheel base of vehicle
cx = 0:0.1:50;
cx = cx';
for i = 1:length(cx)
cy(i) = sin(cx(i)/5)*cx(i)/2;                                               %曲线函数
% cy(i)=(3.75/(3.^5))*(6*i.^5-15*3*i.^4+10*3^2*i.^3);
% cy(i) = i;
end

i = 1;
target_speed = 3;
T = 80;
lastIndex = length(cx);                                                     %lastIndex=501
x = 0; y = -3; yaw = 0; v = 2;                                               %初始状态
time = 0;
Lf = k * v + Lfc;                                                           %相当于预瞄距离
figure
 
while T > time   
    [target_ind,~]= calc_target_index(x,y,cx,cy,Lf)                         %找到预瞄点
    ai = PIDcontrol(target_speed, v,Kp);                                    %pid控速度
    [~,After_Lf]= calc_target_index(x,y,cx,cy,Lf)  
    di = pure_pursuit_control(x,y,yaw,v,cx,cy,target_ind,k,Lfc,L,After_Lf);
    
    [x,y,yaw,v] = update(x,y,yaw,v, ai, di,dt,L)
    time = time + dt;
    pause(0.05)
    plot(cx,cy,'b',x,y,'r-*')
    drawnow
    hold on
end


function [x, y, yaw, v] = update(x, y, yaw, v, a, delta,dt,L)                 %更新状态
    x = x + v * cos(yaw) * dt;
    y = y + v * sin(yaw) * dt;
    yaw = yaw + v / L * tan(delta) * dt;
   v = v + a * dt;
end

function [a] = PIDcontrol(target_v, current_v, Kp)
a = Kp * (target_v - current_v);
end

function [delta] = pure_pursuit_control(x,y,yaw,v,cx,cy,ind,k,Lfc,L,Lf)        %纯跟踪
    tx = cx(ind);
    ty = cy(ind);
    
    %alpha = atan((ty-y)/(tx-x))-yaw;                                           %实际点的夹角-航向角
    alpha=atan2((ty-y),(tx-x))-yaw;
                                                                               %预瞄距离
    delta = atan(2*L * sin(alpha)/Lf)  ;
end

function [ind,After_Lf] = calc_target_index(x,y, cx,cy,Lf)                     %找到最近的那个
N =  length(cx); %N=501
Distance = zeros(N,1);
for i = 1:N
Distance(i) =  sqrt((cx(i)-x)^2 + (cy(i)-y)^2);
end
[distance, location]= min(Distance);
ind = location;                                                                %第几个数
Relative_distance=distance;                                                    %实际的数
% LL = 0;
%     while Lf > LL && (ind + 1) < length(cx)
%         dx = cx(ind + 1 )- cx(ind);
%         dy = cx(ind + 1) - cx(ind);
%         LL = LL + sqrt(dx * 2 + dy * 2);
%         ind  = ind + 1;
%     end
%     
h=sqrt(Lf^2-Relative_distance^2);
sum=0;
z=1;
 while z<(length(cx)-ind-1)
  sum=sum+sqrt((cx(ind+z+1)-cx(ind+z))^2+(cy(ind+z+1)-cy(ind+z))^2);
      z=z+1;
   if (sum>=h)
      break;
   end
 end 
 After_Lf=sqrt(sum^2+Relative_distance^2);
%  if After_Lf>=3
%      After_Lf=3
%  else
%      After_Lf=After_Lf
%  end
 ind=z+ind;
end

实现效果：

---