526互联

自动驾驶-预瞄-Pure pursuit纯跟踪算法-MATLAB实现

发布时间 2023-04-05 15:33:01作者: VotaverUno

有空把引入、逻辑、原理介绍给写了，目前先给大家看看代码。

将来写大概会分成这么几块：

  • 汽车运动学自行车模型
  • 跟踪算法主流模型及特点
  • 纯跟踪算法原理推导
  • 代码介绍

代码原创，来之不易，请勿不注明转载。

喜欢点个赞吧！网上许多代码都跑不起来hh

clc;
clear;

% ------------form road------------
cx = 0:0.1:50;
cx = cx';
for i = 1:length(cx)
    cy(i) = sin(cx(i)/5)*cx(i)/2;
end

%--------form initial state---------
L = 2.9  ;% [m] wheel base of vehicle
i = 1;
target_speed = 2.5;
T = 80;
x = 0; y = -3; yaw = 0; v = 0;
time = 0;

%--------how to update--------------
lastIndex = length(cx)-1;
dt = 0.1  ;% [s]
Lfc = 1.0;  % look-ahead distance
k = 0.1;  % look forward gain
Kp = 1.0 ; % speed propotional gain
Lf = k * v + Lfc;
%--------show initial state---------

target_ind= calc_target_index(x,y,cx,cy,Lf)



%--------Update now keep on track-----
while T > time && target_ind < lastIndex-10
    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)
    subplot(1,2,1)
    plot(cx,cy,'b',x,y,'r-*')
    title(['time=' num2str(time), 'v=' num2str(v),'ind=' num2str(target_ind)])
    drawnow
    hold on
    box off
% should also show errors
end

%-----growing speed-----------------
function [a] = PIDcontrol(target_v, current_v, Kp)
    a = Kp * (target_v - current_v);
end
%-----pute pursuit-------------------
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;
%--------should also show lateral error---------
    latError =abs((ty-y)*cos(yaw) - (tx-x)*sin(yaw))     
    subplot(1,2,2)
    plot(x,latError,'b-*')
    title(['latError=' num2str(latError)])
    drawnow
    hold on
    box off
%------------------------------------------------
    Lf = k * v + Lfc;
    delta = atan(2*L * sin(alpha)/Lf)  ;
end
%-----update state-------------------
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
%-----find the nearest point---------
function [ind] = calc_target_index(x,y, cx,cy,Lf)
    N =  length(cx);
    %N =  length(cx)-11;
    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;
    ind = ind + 10;

end

结果图是实时的，包含一个路径跟踪展示和横向误差图，大概如下（图形并不精美，展示原理所用）：


凑字数凑字数使得可以投稿！不用理会
凑字数凑字数使得可以投稿！不用理会
凑字数凑字数使得可以投稿！不用理会