matlab練習程序（Pure Pursuit路徑跟蹤）

當時寫stanley就實現了，貼上來記錄一下。

方法示意圖：

控制率公式：

其中L爲軸距，e爲橫向誤差，v爲車輛速度，lambda和c爲控制參數。

算法步驟如下：

1. 根據當前定位結果找到路徑最鄰近點。

2. 計算該點與定位結果橫向誤差e。

3. 根據控制率公式計算出前輪轉角。

4. 將前輪轉角轉化爲航向角，帶入運動模型計算出下一時刻的位姿。

matlab代碼如下：

clear all;close all;clc;

v = 1;
dt = 0.1;
L=2.5;
lambda = 1;
c = 1;
curp=[0 0 0];

x = 0:0.1:50;
y = sin(x/5);
path = [x' y'];
for i=2:length(path)
    dx = path(i,1)-path(i-1,1);
    dy = path(i,2)-path(i-1,2);
    path(i-1,3) = atan2(dy,dx);
end
path(length(path),3) = path(length(path)-1,3);
plot(path(:,1),path(:,2),'r.');
hold on;

for i=1:length(path)
    
    d = path(:,1:2) - curp(1:2);
    dis = d(:,1).^2 + d(:,2).^2;
    [~,ind] = min(dis);                                     %找路徑最近點索引
    
    ind = ind + 20;                                         %預瞄點
    if ind >=length(path)
       ind = length(path); 
    end
    
    R = [cos(curp(3)) -sin(curp(3)) curp(1);
         sin(curp(3)) cos(curp(3)) curp(2);
         0 0 1];
    e = inv(R)*[path(ind,1);path(ind,2);1.0];
    ey = e(2);                                              %橫向偏差
     
    u = atan2(2*L*ey,(lambda*v+c)^2);                                  %期望前輪轉角
    
    curp(1) = curp(1) + dt*v*cos(curp(3));
    curp(2) = curp(2) + dt*v*sin(curp(3));
    curp(3) = curp(3) + dt*v*tan(u)/L;
    
    plot(curp(1),curp(2),'g.');
end

結果如下：

綠色爲跟蹤路徑，紅色爲已知路徑。

參考：

https://windses.blog.csdn.net/article/details/103502743