自适应轨迹跟踪算法

%两轮差分驱动机器人轨迹跟踪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