clc; %这个版本是正常的
clear;
%%
%参数设定 最大速度 最大加速度 行走距离 插补周期
vmax = 500 ;
amax = 50 ;
s = 20000 ;
Ts = 0.04;
%% 判断是否有匀速段
S1 = 0.5*vmax^2/amax ;
S2 = s -2*S1;
if S2<=0
lab = 2 ; %切换不同的方程
else
lab = 1;
end
figure ;
fig1 = subplot(4,1,1);
ylabel('position');
hold on;
grid on;
fig2 = subplot(4,1,2);
ylabel('vel');
hold on;
grid on;
fig3 = subplot(4,1,3);
ylabel('acc');
xlabel('time');
hold on;
grid on;
fig4 = subplot(4,1,4);
ylabel('n');
xlabel('time');
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);
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 ;
plot(fig1,t,s1,'- .');
plot(fig2,t,ds1,'- .');
plot(fig3,t,dds1,'*');
hold on;
grid on;
end
if (t1<t && t<=t1+t2)
s2 = b0+b1*t;
ds2 = b1;
dds2 = 0;
plot(fig1,t,s2,'- .');
plot(fig2,t,ds2,'- .');
plot(fig3,t,dds2,'*');
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 ;
plot(fig1,t,s3,'- .');
plot(fig2,t,ds3,'- .');
plot(fig3,t,dds3,'*');
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 ;
plot(fig1,t,s1,'- .');
plot(fig2,t,ds1,'- .');
plot(fig3,t,dds1,'*');
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;
plot(fig1,t,s3,'- .');
plot(fig2,t,ds3,'- .');
plot(fig3,t,dds3,'*');
hold on;
grid on;
end
end
end
梯形加减速规划的matlab仿真
最新推荐文章于 2024-07-06 18:35:01 发布