• 大小: 3KB
    文件类型: .m
    金币: 1
    下载: 0 次
    发布日期: 2021-06-16
  • 语言: Matlab
  • 标签: backstepping  

资源简介

基于两轮驱动的机器人轨迹跟踪算法——backstepping

资源截图

代码片段和文件信息

%两轮差分驱动机器人轨迹跟踪:back-stepping
clear all;clc;
ka=0.6;kb=0.5;kc=1.1;                    %控制律参数
dt=0.2;                                    %时间间隔
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(j))*(yr(j)-y(j)

评论

共有 条评论