自動駕駛-預瞄-Pure pursuit純跟蹤演算法-MATLAB實現

有空把引入、邏輯、原理介紹給寫了，目前先給大家看看代碼。

將來寫大概會分成這麼幾塊：

• 汽車運動學自行車模型
• 跟蹤演算法主流模型及特點
• 純跟蹤演算法原理推導
• 代碼介紹

代碼原創，來之不易，請勿不註明轉載。

喜歡點個贊吧！網上許多代碼都跑不起來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

結果圖是實時的，包含一個路徑跟蹤展示和橫向誤差圖，大概如下（圖形並不精美，展示原理所用）：

湊字數湊字數使得可以投稿！不用理會
湊字數湊字數使得可以投稿！不用理會
湊字數湊字數使得可以投稿！不用理會