纯追踪算法模型 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（-aa）;
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;
%&&x<=40&&y<=4.5
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
  
%Pure Pursuit纯追踪模型
alpha = atan2（y1（1i） - y x1（1i） - x） - yaw;  %atan2函数计算点（xy）与x轴正向夹角即atan2（yx）.
%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 +