纯追踪算法模型 matlab

【核心代码】

%参数定义
dt=0.1; %循环的时间，每次加0.1
Ld=1.9; %车辆的前后轮轴距
k=0.7;  %纯追踪公式里的参数K
L=4;    %目标点的预瞄距离L
T=0.0;  %起始时间0
%目标轨迹  就是那个跟踪的轨迹，用公式表达出来
a=10;
x1=a linspace(-a,a);
y1=3./(1.0 exp(-0.5*(x1-a))) 1.5;
%plot(x1,y1,'.');

%初始化车辆状态
idx=0;
x = 0.0;    %车辆起始位置坐标
y = 1.5;    %车辆起始位置坐标
yaw = 0.0;  %车辆航向角0度，即x轴正方向
v = 20/3.6; %车辆的速度
X=x;
Y=y;
Yaw=yaw;
V=v;
%&&x<=40&&y<=4.5
while(T<=4)  %这个循环是用于设置整个程序跑的时间的，用来跟踪目标路径
    
 T=T dt;
%更新目标点（预瞄距离）
i=100;
%D=norm([x-x1(1,i) y-y1(1,i)]);
while(1)  %这个循环是用来更新目标点的，不断计算当前点和目标路径间的距离，知道为L时为目标点
    D=norm([x-x1(1,i) y-y1(1,i)]);  %计算两个点之间的直线距离函数（内部的）
    i=i-1;
    if(i<1)
        i=1;
        break;
    end
    
    if(D<=L)
        idx=i;
        break;
    end
end
  
%Pure Pursuit纯追踪模型
alpha = atan2(y1(1,i) - y, x1(1,i) - x) - yaw;  %atan2函数计算点（x,y）与x轴正向夹角,即atan2(y,x).
%Lf= k * v   L;
Lf= k * v;
delta = atan2(2.0 * Ld * sin(alpha) / Lf, 1.0);

%更新车辆状态
x = x   v * cos(yaw) * dt;
y = y   v * sin(yaw) * dt;
yaw = yaw   v / L * tan(delta) * dt;
v = v   0 * dt;

X=[X,x];
Y=[Y,y];
Yaw=[Yaw,yaw];
V=[V,v];
end

% figure(1);
% backColor = [0 0 0];
% set(gca, 'color', backColor);
colordef black;
axis([0,30,0,6]);
xlabel('横向距离（m）');
ylabel('纵向距离（m）');
hold on;
plot([0,30],[6,6],'w','LineWidth',10)
hold on;
plot([0,30],[3,3],'--w','LineWidth',4)
hold on;
plot([0,30],[0,0],'w','LineWidth',10)
hold on;
p1=plot(x1,y1,'g.','LineWidth',4);
hold on;
%set(gcf,'outerposition',get(0,'screensize'));% matlab窗口最大化
for n=1:length(X)
    p2=plot(X(1:n),Y(1:n),'r-','LineWidth',2);
    legend([p1,p2],'目标轨迹','实际轨迹');%只显示指定图例
%     pause(0.05);
    drawnow;
    frame=getframe(gcf);
    imind=frame2im(frame);
    [imind,cm] = rgb2ind(imind,256);
    if n==1
         imwrite(imind,cm,'v=10,L=2,K(0.09，1.5).gif','gif', 'Loopcount',inf,'DelayTime',1e-4);
    else
         imwrite(imind,cm,'v=10,L=2,K(0.09，1.5).gif','gif','WriteMode','append','DelayTime',1e-4);
    end
end

% set(gca,'YLim',[0 6]);%X轴的数据显示范围
% hold on;
%plot(X,Y,'r-');
%plot(X(1,150:end),Y(1,150:end),'r.');