pure_pursuit_sim.m

clc;
clear;
close all;

% 参数定义
dt = 0.1; % 循环的时间，每次加0.1
Ld = 1.9; % 车辆的前后轮轴距
k = 0.7;  % 纯追踪公式里的参数K
L = 4;    % 目标点的预瞄距离L
T = 0.0;  % 起始时间0

% 目标轨迹  就是那个跟踪的轨迹，用公式表达出来
a = 10;
x1 = a+linspace（-aa）;               % 产生0~20 均布的线性矢量
y1 = 3./（1.0+exp（-0.5*（x1-a）））+1.5;  % 
plot（x1y1‘.‘）;

% 初始化车辆状态
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;

while（T<=4）  % 这个循环是用于设置整个程序跑的时间的，用来跟踪目标路径
    
    T=T+dt;
    % 更新目标点（预瞄距离）
    i=100;
    % D=norm（[x-x1（1i） y-y1（1i）]）;
    while（1）  % 这个循环是用来更新目标点的，不断计算当前点和目标路径间的距离，知道为L时为目标点
        D=norm（[x-x1（1i） y-y1（1i）]）;  % 计算两个点之间的直线距离函数（内部的）
        i=i-1;
        if（i<1）
            i=1;
            break;
        end

        if（D<=L）
            idx=i;
            break;
        end
    end

    % 纯追踪模型
    alpha = atan2（y1（1i） - y x1（1i） - x） - yaw;  % atan2函数计算点（xy）与x轴正向夹角即atan2（yx）.
    Lf= k * v;
    delt