%% 梯形速度曲线
%% 假定初始速度和最终速度为零,给定最大的加速度、运行时间、开始角度 、结束角度
function trapezoid()
%给定初始条件
t0=0;
tf=10;
q0=0;
qf=20;
a_max=1;
if a_max>= 4*(qf-q0)/(tf-t0)^2
% 先判断能否求解到t_f,给定的最大加速度是否满足条件
% t_f 加速至匀速阶段的时间
t_f=tf/2-0.5*sqrt( ( tf^2*a_max-4*(qf-q0)) / (a_max));
i=1;
t=t0:0.1:tf;
n=size(t);
qt=zeros(n(2),1);
for t=t0:0.1:tf
if t<=t_f
qt(i)=q0+0.5*a_max*t*t;
elseif t_f<t && t<=tf-t_f
qt(i)=q0+0.5*a_max*t_f*t_f+a_max*t_f*(t-t_f);
else
qt(i)=qf-0.5*a_max*(t-tf)^2;
end
i=i+1;
end
figure(1)
plot(qt);
legend('位置曲线')
figure(2)
plot(diff(qt))
legend('速度曲线')
figure(3)
plot(diff(diff(qt)))
legend('加速度曲线')
else
disp('给定的加速度太小,不能求解出轨迹');
end
end
%% 梯形速度曲线
%% 给定开始角度 、结束角度、匀速阶段的速度、给定最大的加速度
function trapezoid_2()
%给定初始条件
t0=0;
q0=0;
qf=20;
v_max=3; %这两个值不同,得到的轨迹曲线也就不一样
a_max=0.5;
t_c=v_max/a_max;
q_c=0.5*a_max*t_c^2;
t_f=(qf-2*q_c)/v_max+2*t_c;
t=t0:0.1:t_f;
n=size(t);
qt=zeros(n(2),1);
i=1;
if t_f-2*t_c>0 % 达到最大速度,也即速度曲线为梯形
for t=t0:0.1:t_f
if t<=t_c
qt(i)=q0+0.5*a_max*t*t;
elseif t_c<t && t<=t_f-t_c
qt(i)=q0+0.5*a_max*t_c*t_c+a_max*t_c*(t-t_c);
else
qt(i)=qf-0.5*a_max*(t_f-t)^2;
end
i=i+1;
end
else % 未达到最大速度,也即速度曲线为三角形
t_c=sqrt( (qf-q0)/a_max);
t_f=2*t_c;
for t=t0:0.1:t_f
if t<=t_c
qt(i)=q0+0.5*a_max*t*t;
else
qt(i)=qf-0.5*a_max*(t_f-t)^2;
end
i=i+1;
end
end
figure(1)
plot(qt);
legend('位置曲线')
figure(2)
plot(diff(qt))
legend('速度曲线')
figure(3)
plot(diff(diff(qt)))
legend('加速度曲线')
end