2.1 梯形加减速算法

2.1 梯形加减速算法

简介

本文将详细介绍梯形加减速算法的原理和代码实现。梯形加减速算法是一种常用的运动规划算法，用于实现平滑的加速和减速过程，以达到稳定运动的目的。这种轨迹分为三个部分。梯形加减速规划时，一般只对路径长度L进行规划，不管路径的具体形状，在插补的模块同时根据路径几何规律和加减速规律，计算路径上的插补点。

梯形加减速第一部分，加速度恒定，速度是时间的线性函数，位移是时间的抛物线函数；第二部分，加速度为0，速度恒定，位移是时间的线性函数；第三部分，加速度为恒定的负值，速度线性减小，位移是时间的二次多项式，如下图所示。

梯形加减速算法的原理

给定的参数为位移$L$, 起步速度$v_s$, 匀速速度$v_c$ ，末速度为$v_e$, 加速段加速度为$a=a_{max}$, 减速段加速度为$d=-d_{max}$ 。梯形加减速的速度计算公式为
$$\begin{array}{c}v(t)=\{\begin{array}{ll}{v}_s+at,& 0\le t<t_1\\ v_c,& t_1\le t<t_1+t_2\\ v_c+dt,& t_1+t_2\le t<t_1+t_2+t_3\end{array}\end{array}$$
梯形加减速的位移计算公式为
$$\begin{array}{c}q(t)=\{\begin{array}{ll}{v}_{s}t+\frac{1}{2}a{t}^2,& 0\le t<t_1\\ L_a+v_c(t-t_1),& t_1\le t<t_2\\ L_a+L_v+v_c(t-t_1-t_2)+\frac{1}{2}d(t-t_1-t_2{)}^2,& t_1+t_2\le t<t_1+t_2+t_3\end{array}\end{array}$$
其中，$L_a=v_s{t}_1+\frac{1}{2}a{t}_1^2,L_v=v_c{t}_2$。

为了确定梯形速度曲线，需要指定其中部分参数，其他参数根据轨迹几何关系计算出来。对于一台机器要运动的位移，时间是千变万化的，但是其允许的最大速度，加速度，启动速度一般受到机械部件的限制，是确定的了。因此，实际应用中一般是指定梯形轨迹的位移$L$，初始速度$v_s$，最大速度$v_m$，终止速度$v_e$，加速度$a$，减速度$d$，然后计算出对应的加速时间$t_1$,匀速时间$t_2$和减速时间$t_3$。

用户给定的起始速度，终止速度，加速度，减速度，最大速度，位移参数，不一定都能满足，若给定的参数的轨迹不存在，那么需要修改速度参数，保证必须满足位移条件（在实际的数控系统中常常是这么做），常常修改的是边界速度（常常是末速度），同时保证修改后的速度不会超出用户指定边界的速度。因此，可以按照以下步骤计算时间最优的梯形速度曲线

采用试探法，假设没有匀速段，只有加速度段和减速段，运动中达到的最大速度为$v_f$，则有
$$L=L_a+L_d=\frac{v_f^2-v_s^2}{2a}+\frac{v_e^2-v_f^2}{2d}$$
这是可以解的$v_f$如下
$$v_f=\sqrt{\frac{a{v}_e^2-d{v}_s^2-2adL}{a-d}}$$
需要根据$v_f$与匀速的最大速度$v_c$的关系进行分类讨论

1. 若$v_f>v_c$，有匀速段。说明位移足够长，可以加速到指定的匀速速度，并且能减速到给定的末速度。

   （1）此时对应的时间可以计算出来
   $$\begin{array}{c}\{\begin{array}{l}{t}_1=\frac{v_c-v_s}{a},\\ t_2=\frac{L-\frac{v_c^2-v_s^2}{2a}-\frac{v_e^2-v_c^2}{2d}}{v_c},\\ t_3=\frac{v_e-v_c}{d}\end{array}\end{array}$$

2. 若$v_f\le v_c$,没有匀速段。说明位移较短，无法加速到期望的匀速速度就要开始减速了。此时还需要分3种情况讨论：

   （2）若$v_s<v_f<v_e$，则无法到达给定的末速度，只有加速度段，且末速度必须降低，才能保证位移满足条件。这时末速度为
   $$v_e=\sqrt{v_s^2+2aL}$$
   各段时间为
   $$\begin{array}{c}\{\begin{array}{l}{t}_1=\frac{v_e-v_s}{a},\\ t_2=0,\\ t_3=0\end{array}\end{array}$$
   (3)若$v_e<v_f<v_s$,则无法达到给定的末速度，只有减速段，这时的末速度修正为
   $$v_e=\sqrt{v_s^2+2dL}$$
   各段时间为
   $$\begin{array}{c}\{\begin{array}{l}{t}_1=0,\\ t_2=0,\\ t_3=\frac{v_e-v_s}{d}\end{array}\end{array}$$
   (4)若$v_s<v_f且v_f>v_s$,则存在加速度段和减速段，各段时间为
   $$\{\begin{array}{l}{t}_1=\frac{v_c-v_s}{a},\\ t_2=0,\\ t_3=\frac{v_e-v_c}{d}\end{array}$$
   求出梯形加减速的三段曲线对应的时间后，就可以按照公式(1)和(2)计算对应的位移和速度。

示例演示

梯形加减速算法的原理给出了梯形加减速4种情况的公式推导，这里给出这4种情况的示例和源码（代码附件在文章最后）。

（1）有加速度段、匀速段和减速段。

给定参数为L =10, vs = 5, vmax= 50, ve = 10, acc = 500, dec = 400，速度曲线如下。

该情况下的公式也可以处理起步速度与匀速速度相同，或末速度与起步速度相同，且位移较大的情况。

例如，给定参数为L = 10, vs = 50, vmax= 50, ve = 20, amax= 500, dmax= 400，这是加速段时间为0,，只有匀速段段和减速段。

例如，给定参数为L = 10, vs = 10, vmax= 50, ve = 50, amax= 500, dmax= 400，这是加速段时间为0,，只有加速段和匀速段。

（2）只有加速段

给定参数为L = 1, vs = 10, vmax= 50, ve = 45, amax= 500, dmax= 400,速度曲线如下。

（3）只有减速段。

给定参数为L = 1, vs = 10, vmax= 50, ve = 45, amax= 500, dmax= 400,速度曲线如下。

（4）有加速段和减速段。

给定参数为L = 5, vs = 10, vmax= 50, ve = 20, amax= 500, dmax= 400,速度曲线如下。

总结

梯形加减速算法可以实现平滑的加速和减速过程，从而达到稳定的运动控制效果。

优点：

1. 平滑稳定：梯形加减速算法能够实现平滑的加速和减速过程，减少了机械系统的冲击和震动，提高了运动控制的稳定性。
2. 精确控制：梯形加减速算法可以精确控制运动过程中的加速度和减速度，从而保证了机械系统的运动精度。
3. 简单易用：梯形加减速算法的实现比较简单，代码量相对较小，易于理解和维护。

缺点：

1. 不适用于高速运动：梯形加减速算法在高速运动时，加速和减速时间会变得很短，导致机械系统的冲击和震动加剧，难以实现平滑的运动控制。
2. 不适用于复杂运动：梯形加减速算法只适用于直线运动或简单曲线运动，对于复杂的路径规划，需要使用更加复杂的运动规划算法。
3. 限制运动时间：梯形加减速算法在计算速度曲线时，需要提前确定总运动时间，难以适应需要灵活调整运动时间的应用场景。

综上所述，梯形加减速算法在简单的运动控制场景中具有优越性，但在高速运动和复杂运动控制中存在一定的局限性。需要根据具体应用场景选择适合的运动规划算法。

代码附件

这里给出了以上原理和示例的所有实现代码。

MATLAB测试绘图脚本：

clc
clear
close("all")
set(gcf,'color','white');%白色
%读取C语言写入文件的数据
data=load("tpProfile.txt");
ti=data(:,1);
Li=data(:,2);
vi=data(:,3);
ai=data(:,4);
%绘制位移曲线
subplot(3,1,1);
plot(ti,Li,'-r.');
xlabel('time(s)')
ylabel('position')
%绘制速度曲线
subplot(3,1,2);
plot(ti,vi,'-g.');
xlabel('time(s)')
ylabel('velocity')
%绘制加速度曲线
subplot(3,1,3);
plot(ti,ai,'-b.');
xlabel('time(s)')
ylabel('acceleration')

三个关键函数

function tp = trapProfile(vs, vmax, ve, amax,dmax, L)

    acc=amax;
    dec=-dmax;
	%起步速度和终止速度不能大于匀速速度
	if (vs > vmax)
		vs=vmax;
    elseif (ve > vmax)
		ve = vmax;
    end
	%4种情形
	vf = sqrt((-2.0 * acc * dec * L - vs * vs * dec + ve * ve * acc) / (acc - dec));
	if (vf > vmax)%有匀速段		
		vc = vmax;
		t1 = (vc - vs) / acc;
		t3 = (ve - vc) / dec;
		L1 = vs * t1 + 0.5 * acc * t1 * t1;
		L3 = vc * t3 + 0.5 * dec * t3 * t3;
		L2 = L - L1 - L3;
		t2 = L2 / vc;
		t = t1 + t2 + t3;
	
    else%没有匀速段	
		if (vs < vf && vf < ve)%只有加速段
			ve = sqrt(vs * vs + 2 * acc * L);
			vc = ve;
			t1 = (ve - vs) / acc;
			t2 = 0;
			t3 = 0;
			L1 = vs * t1 + 0.5 * acc * t1 * t1;
			L2 = 0;
			L3 = 0;
			t = t1;
		
        elseif (ve < vf && vf < vs)%只有减速段	
			ve = sqrt(vs * vs + 2 * dec * L);		
			vc = vs;		 
			t1 = 0;
			t2 = 0;
			t3 = (ve - vs) / dec;
			L1 = 0;
			L2 = 0;
			L3 = vc * t3 + 0.5 * dec * t3 * t3;
			t = t3;
		
         else%存在加速段和减速段
		
			vc = vf;
			t1 = (vc - vs) / acc;
			t2 = 0;
			t3 = (ve - vc) / dec;
			L1 = vs * t1 + 0.5 * acc * t1 * t1;
			L2 = 0;
			L3 = vc * t3 + 0.5 * dec * t3 * t3;
			t = t1 + t3;
         end
    end
    tp.vs=vs;
    tp.vc=vc;
    tp.ve=ve;
    tp.acc=amax;
    tp.dec=-dmax;
    tp.t1=t1;
    tp.t2=t2;
    tp.t3=t3;
    tp.L1=L1;
    tp.L2=L2;
    tp.L3=L3;
    tp.L=L;
    tp.t=t;
end

function Lt= trapProfilePos(tp,t)	
    if (tp.t1>0 && t < tp.t1)	
		Lt = tp.vs * t + 0.5 * tp.acc * t * t;
    elseif (tp.t2>0.0 && t < tp.t1 + tp.t2)
		Lt = tp.L1 + tp.vc * (t - tp.t1);
    else
		tmp = t - tp.t1 - tp.t2;
		Lt = tp.L1 + tp.L2 + tp.vc * tmp + 0.5 * tp.dec * tmp * tmp;
    end
end


function  vt= trapProfileVel(tp, t)
    if(t<0.0)
        vt=0.0;
    elseif (tp.t1>0 && t < tp.t1)	
		vt = tp.vs + tp.acc * t;
    elseif (tp.t2> 0.0 &&t < tp.t1 + tp.t2)
		vt = tp.vc;	
    else
		vt = tp.vc + tp.dec * (t - tp.t1 - tp.t2);
    end

end

function acc= trapProfileAcc(tp,t)
	if (t < 0.0)
		acc= 0.0;
    elseif (tp.t1>0 && t < tp.t1)
		acc=tp.acc;
    elseif (tp.t2>0 && t < tp.t1 + tp.t2)
		acc=0.0;
    else %if(tp.t3>0 && t <= tp.t)
		acc=tp.dec;
    end
end