Pure Pursuit纯跟踪算法的Matlab算法实现

本文的python源代码来自：
https://github.com/gameinskysky/PythonRobotics/blob/master/PathTracking/pure_pursuit/pure_pursuit.py

纯跟踪算法的原理，详见https://blog.csdn.net/gophae/article/details/100012763

原文链接：https://blog.csdn.net/gophae/article/details/102761138

matlab源码如下:

k = 0.1;  % look forward gain
Lfc = 1.0;  % 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;
end

i = 1;
target_speed = 10/3.6;
T = 80;
lastIndex = length(cx);
x = 0; y = -3; yaw = 0; v = 0;
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);
  di = pure_pursuit_control(x,y,yaw,v,cx,cy,target_ind,k,Lfc,L,Lf);
    
    [x,y,yaw,v] = update(x,y,yaw,v, ai, di,dt,L)
    time = time + dt;
%     pause(0.1)
    plot(cx,cy,'b',x,y,'r-*')
    drawnow
    hold on
end
% plot(cx,cy,x,y,'*')
% hold on


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;
    
 Lf = k * v + Lfc;
 delta = atan(2*L * sin(alpha)/Lf)  ;
end

function [ind] = calc_target_index(x,y, cx,cy,Lf)
N =  length(cx);
Distance = zeros(N,1);
for i = 1:N
Distance(i) =  sqrt((cx(i)-x)^2 + (cy(i)-y)^2);
end
[~, location]= min(Distance);
ind = location;
% 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
%     
 ind = ind + 10
end

工作区变量如下:

展示效果如图：

标签：target,yaw,cy,算法,cx,Matlab,Pure,ind,dt
来源： https://blog.csdn.net/dongbao520/article/details/115674882