梯形加减速规划仿真（给定速度与加速度以及位移）

最新推荐文章于 2022-07-07 22:50:13 发布

dahai_OF_foshan

最新推荐文章于 2022-07-07 22:50:13 发布

阅读量464

点赞数

分类专栏：机器人学学习与实践

本文链接：https://blog.csdn.net/dahai_OF_foshan/article/details/118420497

版权

MATLAB 路径规划圆心角弧长减速方式轨迹控制

关键词由CSDN通过智能技术生成

机器人学学习与实践专栏收录该内容

4 篇文章 1 订阅

订阅专栏

clc;   %这个版本是正常的
clear;
% L=n×π×r/180，L=α× r。其中n是圆心角度数（角度制），r是半径，L是圆心角弧长，α是圆心角度数（弧度制）

%% 构造二维平面与几何圆参数

%参数设定  最大速度  最大加速度  行走距离 插补周期
CenterPoint = [2000 2000];
R = 3000;
L = 2*3.14*R;
%%  
vmax = 500 ;
amax = 100 ;
s  = L ;
Ts = 0.4;

%%
S1 = 0.5*vmax^2/amax ;
S2 = s -2*S1;
if S2<=0
    lab = 2 ;  %切换不同的方程
else
    lab = 1;
end

        figure ;
        fig1 = subplot(2,2,1);
        ylabel('position');
        hold on;
        grid on;   
        fig2 = subplot(2,2,2);
        ylabel('vel');
        hold on;
        grid on;
        fig3 = subplot(2,2,3);
        ylabel('acc');
        xlabel('time');
        hold on;
        grid on;
        fig4 = subplot(2,2,4);
        ylabel('Y');
        xlabel('X');
        hold on;
        grid on;



%%  不同的减速方式
switch lab
    case 1
        S1 = 0.5*vmax^2/amax ;
        S2 = s -2*S1;
        S3 = S1 ;
        t1 = vmax/amax ;
        t2 = S2/vmax ;
        t3 = t1;
        a0 = 0;
        a1 = 0 ;
        a2 = amax/2 ;

        b0 = -0.5*vmax^2/amax ;                          
        b1 = vmax ;

        c2 = -a2 ;
        c1 = vmax+amax*(t1+t2) ;% 计算的时候按照总时间计算
        c0 =S1+S2-(c1*(t1+t2)+c2*(t1+t2)^2);
        
         plot(fig4,CenterPoint(1,1),CenterPoint(1,2),'*');
         hold on;
  for t = 0:Ts:(t1+t2+t3)      
        if (t<=t1) 
            s1 = a0 + a1*t+a2*t^2;
            ds1 = a1+2*a2*t;
            dds1 = 2*a2 ;
            n = s1/R;
            x = R*cos(n)+CenterPoint(1,1);
            y = R*sin(n)+CenterPoint(1,2);
            plot(fig1,t,s1,'- .');
            plot(fig2,t,ds1,'- .');
            plot(fig3,t,dds1,'*');
            plot(fig4,x,y,'- .');
            hold on;
            grid on;
        end
        if (t1<t && t<=t1+t2)
            s2 = b0+b1*t;
            ds2 = b1;
            dds2 = 0;
            n = s2/R;
            x = R*cos(n)+CenterPoint(1,1);
            y = R*sin(n)+CenterPoint(1,2);
            plot(fig1,t,s2,'- .');
            plot(fig2,t,ds2,'- .');
            plot(fig3,t,dds2,'*');
            plot(fig4,x,y,'- .');
            hold on;
            grid on; 
        end
        if((t1+t2<t && t<=t1+t2+t3))
            s3 = c0+c1*t+c2*t^2;
            ds3 = c1 + 2*c2*t ;
            dds3 = 2*c2 ;
            n = s3/R;
            x = R*cos(n)+CenterPoint(1,1);
            y = R*sin(n)+CenterPoint(1,2);
            plot(fig1,t,s3,'- .');
            plot(fig2,t,ds3,'- .');
            plot(fig3,t,dds3,'*');
            plot(fig4,x,y,'- .');
            hold on;
            grid on;  
        end
  end
    case 2
        S1 = s/2 ;
        S3 = s/2;
        t1  = sqrt(2*S1/amax);
        t3=t1;
        
        a0 = 0;
        a1 = 0 ;
        a2 = amax/2 ;

        c2 = -a2 ;
        c1 = a1+4*a2*t1;
        c0 = S1 -c1*t1-c2*t1^2;
        
    for t = 0:Ts:(t1+t3)
         if (t<=t1) 
            s1 = a0 + a1*t+a2*t^2;
            ds1 = a1+2*a2*t;
            dds1 = 2*a2 ;
            n = s1/R;
            x1 = R*cos(n)+CenterPoint(1,1);
            y1 = R*sin(n)+CenterPoint(1,2);
            plot(fig1,t,s1,'- .');
            plot(fig2,t,ds1,'- .');
            plot(fig3,t,dds1,'*');
            plot(fig4,x1,y1,'*');
            hold on;
            grid on;
         end 
        if (t1<t && t<(t1+t3))
            s3 = c0+c1*t+c2*t^2;
            ds3 = c1 + 2*c2*t ;
            dds3 = 2*c2;
            n = s3/R;
            x2 = R*cos(n)+CenterPoint(1,1);
            y2 = R*sin(n)+CenterPoint(1,2);
            plot(fig1,t,s3,'- .');
            plot(fig2,t,ds3,'- .');
            plot(fig3,t,dds3,'*');
            plot(fig4,x2,y2,'*');
            hold on;
            grid on;
        end
    end    
end