clc;
clear;
close?all;

%?參數(shù)定義
dt?=?0.1;?%?循環(huán)的時(shí)間，每次加0.1
Ld?=?1.9;?%?車輛的前后輪軸距
k?=?0.7;??%?純追蹤公式里的參數(shù)K
L?=?4;????%?目標(biāo)點(diǎn)的預(yù)瞄距離L
T?=?0.0;??%?起始時(shí)間0

%?目標(biāo)軌跡??就是那個(gè)跟蹤的軌跡，用公式表達(dá)出來
a?=?10;
x1?=?a+linspace（-aa）;???????????????%?產(chǎn)生0~20?均布的線性矢量
y1?=?3./（1.0+exp（-0.5*（x1-a）））+1.5;??%?
plot（x1y1‘.‘）;

%?初始化車輛狀態(tài)
idx?=?0;
x?=?0.0;????%?車輛起始位置坐標(biāo)
y?=?1.5;????%?車輛起始位置坐標(biāo)
yaw?=?0.0;??%?車輛航向角0度，即x軸正方向
v?=?20/3.6;?%?車輛的速度
X=x;
Y=y;
Yaw=yaw;
V=v;

while（T<=4）??%?這個(gè)循環(huán)是用于設(shè)置整個(gè)程序跑的時(shí)間的，用來跟蹤目標(biāo)路徑
????
????T=T+dt;
????%?更新目標(biāo)點(diǎn)（預(yù)瞄距離）
????i=100;
????%?D=norm（[x-x1（1i）?y-y1（1i）]）;
????while（1）??%?這個(gè)循環(huán)是用來更新目標(biāo)點(diǎn)的，不斷計(jì)算當(dāng)前點(diǎn)和目標(biāo)路徑間的距離，知道為L(zhǎng)時(shí)為目標(biāo)點(diǎn)
????????D=norm（[x-x1（1i）?y-y1（1i）]）;??%?計(jì)算兩個(gè)點(diǎn)之間的直線距離函數(shù)（內(nèi)部的）
????????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函數(shù)計(jì)算點(diǎn)（xy）與x軸正向夾角即atan2（yx）.
????Lf=?k?*?v;
????delt