%兩輪差分驅動機器人軌跡跟蹤adapt

clear?all;clc;
c1=3.6;c2=2.1;c3=-2.5;?????????????????????????????????
dt=0.2;d=0.05;???????????????????????????????????%時間間隔
xc=[2?4?10?13?15?18?23?20?6?14?18?16];
yc=[8?15?21?26?28?27?21?12?14?16?22?21];????%被跟蹤軌跡（路徑），以離散點表示
wr（1）=0;vr（1）=1.4;??????????????????????????%被跟蹤軌跡初始角速度、初始線速度
a=length（xc）;???????????????????????????????%離散點個數
xs（1）=12;ys（1）=1;ths（1）=pi/14;ws（1）=1;vs（1）=1.2;????%實際軌跡的初始點信息，即出發點位置、角度、角速度、線速度
k=1;
%將被跟蹤軌跡分段作為跟蹤直線處理，相鄰離散點之間為一段新的被跟蹤軌跡
for?i=1:a-1???
????dy=yc（i+1）-yc（i）;
????dx=xc（i+1）-xc（i）;
????n（i）=sqrt（dy^2+dx^2）/dt;????????????%每個分段再分為若干時間段
????thr（1）=atan（dy/dx）;th（1）=ths（k）;????%每個分段軌跡的初始角度，實際軌跡的初始角度
????if?dx<0
????????thr（1）=pi+atan（dy/dx）;th（1）=pi+ths（k）;
????end
????x（1）=xs（k）;y（1）=ys（k）;w（1）=ws（k）;?v（1）=vs（k）;???%每個分段實際軌跡的初始點信息，即出發點位置、角速度、線速度
????xr（1）=xc（i）;yr（1）=yc（i）;wr=0;vr=1;??????????????%每個分段被跟蹤軌跡的初始點信息，即出發點位置、角速度、線速度???
????for?j=1:n（i）-1????
????????%運動學模型
????????thr（j）=thr（1）;
????????xr（j+1）=xr（j）+dt*vr*cos（thr（j））;????????????
????????yr（j+1）=yr（j）+dt*vr*sin（thr（j））;
????????%運動學模型
????????th（j+1）=th（j）+dt*w（j）;
????????x（j+1）=x（j）+dt*v（j）*cos（th（j））;?????????
????????y（j+1）=y（j）+dt*v（j）*sin（th（j））;?
????????%每個分段的跟蹤誤差
????????e=[?cos（th（j））*（xr（j）-x（j））+sin（th（j））*（yr（j）-y（j））;
????????????-sin（th（j））*（xr（j）-x（j））+cos（th