%兩輪差分驅(qū)動(dòng)機(jī)器人軌跡跟蹤：back-stepping
clear?all;clc;
ka=0.6;kb=0.5;kc=1.1;????????????????????%控制律參數(shù)
dt=0.2;????????????????????????????????????%時(shí)間間隔
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];????%被跟蹤軌跡（路徑），以離散點(diǎn)表示
wr（1）=0;vr（1）=1.4;??????????????????????????%被跟蹤軌跡初始角速度、初始線速度
a=length（xc）;???????????????????????????????%離散點(diǎn)個(gè)數(shù)
xs（1）=12;ys（1）=1;ths（1）=pi/14;ws（1）=1;vs（1）=1.2;????%實(shí)際軌跡的初始點(diǎn)信息，即出發(fā)點(diǎn)位置、角度、角速度、線速度
k=1;
%將被跟蹤軌跡分段作為跟蹤直線處理，相鄰離散點(diǎn)之間為一段新的被跟蹤軌跡
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;????????????%每個(gè)分段再分為若干時(shí)間段
????thr（1）=atan（dy/dx）;th（1）=ths（k）;????%每個(gè)分段軌跡的初始角度，實(shí)際軌跡的初始角度
????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）;???%每個(gè)分段實(shí)際軌跡的初始點(diǎn)信息，即出發(fā)點(diǎn)位置、角速度、線速度
????xr（1）=xc（i）;yr（1）=yc（i）;wr=0;vr=1;??????????????%每個(gè)分段被跟蹤軌跡的初始點(diǎn)信息，即出發(fā)點(diǎn)位置、角速度、線速度???
????for?j=1:n（i）-1????
????????%被跟蹤軌跡運(yùn)動(dòng)學(xué)模型
????????thr（j）=thr（1）;
????????xr（j+1）=xr（j）+dt*vr*cos（thr（j））;????????????
????????yr（j+1）=yr（j）+dt*vr*sin（thr（j））;
????????%實(shí)際軌跡運(yùn)動(dòng)學(xué)模型
????????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））;?
????????%每個(gè)分段的跟蹤誤差
????????e=[?cos（th（j））*（xr（j）-x（j））+sin（th（j））*（yr（j）-y（j））;
????????????-sin（th（j））*（xr（j）-x（j））+cos（th（j））*（yr（j）-y（j）