matlab練習程序（PID路徑跟蹤）

首先要確定反饋量和控制量，這里反饋量用的是車和最近路徑點的橫向誤差，控制量用的是前輪轉角。

算法如下：

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

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

3. 以該誤差作為反饋測量值通過pid生成控制量即前輪轉角。

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

我這里用一個正弦函數作為已知路徑，PID參數是試出來的，如果換個差距比較大的參數應該就不行了。

matlab代碼如下：

clear all;close all;clc;

kp = 10;
ki = 0.01;
kd = 30;

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

x = 0:0.1:50;
y = sin(x/5);
path = [x' y'];
plot(path(:,1),path(:,2),'r.');
hold on;

intergral = 0;
pre_err =0;
for i=1:length(path)
    
    d = path(:,1:2) - curp(1:2);
    dis = d(:,1).^2 + d(:,2).^2;
    [~,ind] = min(dis);                                     %找路徑最近點索引
    
    dx = curp(1) - path(ind,1);
    dy = curp(2) - path(ind,2);
    
    e = (sin(curp(3) - atan2(dy,dx)))*sqrt(dx*dx+dy*dy);    %橫向偏差作為測量
     
    intergral = intergral + e;
    u = kp*e + ki*intergral + kd*(e - pre_err);             %pid生成控制量，前輪轉角
    pre_err = e;

    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

%axis equal;

結果如下：

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