梯形加减速

梯形加减速

引例

获取连续速度曲线的常用的方法是使用带有抛物线过渡的线性轨迹，这就是典型的梯形速度曲线。

]这种轨迹分为三个部分。假设位移是正的，即$q_1>q_0$。若位移是负的，只需改变对应的加速度和速度的符号。第一部分，加速度恒定，速度是时间的线性函数，位移是时间的抛物线函数；第二部分，加速度为0，速度恒定，位移是时间的线性函数；第三部分，加速度为恒定的负值，速度线性减小，位移是时间的二次多项式。一般假设加速段$T_a$和减速段时间$T_d$相等[1]。

如果$t_0=0$，轨迹可由下列表达式描述：

（1）加速段，$t\in [0,T_a]$
$$\{\begin{array}{l}q(t)=a_0+a_{1}t+a_2{t}^2\\ \dot{q}(t)=a_1+2a_{2}t\\ \ddot{q}(t)=2a_2.\end{array}\text{\hspace{1em}(3.1)}$$
若初始速度$v_0$设置为0，最大速度设置为$v_v$.可以解的系数
$$\{\begin{array}{l}{a}_0=q_0\\ a_1=0\\ a_2=\frac{v_v}{2T_a}\end{array}$$
（2）匀速段，$t\in [T_a,t_1-T_a]$
$$\{\begin{array}{l}q(t)=b_0+b_{1}t\\ \dot{q}(t)=b_1\\ \ddot{q}(t)=0\end{array}\text{\hspace{1em}(3.2)}$$
根据连续性，解得
$$\{\begin{array}{l}{b}_1=v_v\\ b_0=q_0-\frac{v_v{T}_a}{2}\end{array}$$
(3)减速段，$t\in [t_1-T_a,t_1]$
$$\{\begin{array}{l}q(t)=c_0+c_{1}t+c_2{t}^2\\ \dot{q}(t)=c_1+2c_{2}t\\ \ddot{q}(t)=2c_2\end{array}\text{\hspace{1em}(3.3)}$$
设置末速度为零，那么解的
$$\{\begin{array}{l}{c}_0=q_1-\frac{v_v{t}_1^2}{2T_a}\\ c_1=\frac{v_v{t}_1}{T_a}\\ c_2=-\frac{v_v}{2T_a}\end{array}$$
综合起来，考虑一下一般的情况$t\ne 0$,轨迹的位移表示为

$t_0\le t<t_0+T_a$
$$\{\begin{array}{l}q(t)=q_0+\frac{v_v}{2T_a}(t-t_0{)}^2,\\ \dot{q}(t)=\frac{v_v}{T_a}(t-t_0),\\ \ddot{q}(t)=\frac{v_v}{T_a}.\end{array}$$
$t_0+T_a\le t<t_1-T_a$
$$\{\begin{array}{l}q(t)=q_0+v_v(t-t_0-\frac{T_a}{2}),\\ \dot{q}(t)=v_v,\\ \ddot{q}(t)=0.\end{array}$$
$t_1-T_a\le t\le t_1$
$$\{\begin{array}{l}q(t)=q_1-\frac{v_v}{2T_a}(t_1-t{)}^2,\\ \dot{q}(t)=\frac{v_v}{T_a}(t_1-t),\\ \ddot{q}(t)=-\frac{v_v}{T_a}.\end{array}$$
Eample3.2:给定梯形轨迹的条件，$q_0=0,q_1=30,t_0=0,t_1=4,T_a=1,v_v=10$. 绘制出轨迹的位移，速度，加速度曲线。

matlab实现代码如下

%{
使用抛物线过渡线性轨迹,example3.2
Brian
2018-12-22,
%}
close('all')
clc
clear
%%
%已知轨迹给定条件
q0=0;
q1=30;
t0=0;
t1=4;
Ta=1;
Vv=10;
%%
%计算轨迹的离散点
i=1;
Ts=0.001;
for t=t0:Ts:t1
    time(i)=t;
    if(t0 <=t && t<t0+Ta)
        q(i)=q0+Vv/(2.0*Ta)*(t-t0)^2;
        dq(i)=Vv/Ta*(t-t0);
        ddq(i)=Vv/Ta;
    end
    if(Ta <=t && t<t1-Ta)
        q(i)=q0+Vv*(t-t0-Ta/2.0);
        dq(i)=Vv;
        ddq(i)=0;      
    end
    if(t1-Ta<=t&&t<=t1)
        q(i)=q1-Vv/(2.0*Ta)*(t1-t)^2;
        dq(i)=Vv/Ta*(t1-t);
        ddq(i)=-Vv/Ta;
    end
    i=i+1;
end

%%
%设置绘图布局
figure
fig1=subplot(3,1,1);
ylabel('position');
grid on
hold on
fig2=subplot(3,1,2);
ylabel('velocity');
grid on
hold on
fig3=subplot(3,1,3);
ylabel('acceleration');
xlabel('time');
grid on
hold on
%绘图
c1=plot(fig1,time,q,'-r','LineWidth',1.5);
c2=plot(fig2,time,dq,'-b','LineWidth',1.5);
c3=plot(fig3,time,ddq,'-g','LineWidth',1.5);
%end

结果如下图所示

通用梯形速度曲线

确定梯形速度曲线的参数$h,v_0,v_1,T_a,T_d,a_a,a_d,v_{max}$,可以指定其中部分参数，其他参数根据轨迹几何关系计算出来。对于一台机器要运动的位移，时间是千变万化的，但是其允许的最大速度，加速度，启动速度一般受到机械部件的限制，是确定的了。因此，实际应用中一般是指定梯形轨迹的位移，初始速度，最大速度，终止速度，最大加速度来确定轨迹。

因此，通用梯形速度曲线为已知初始速度，终止速度，加速度，减速度，最大速度，位移。要确定轨迹，关键是计算出加速度段，匀速段，减速度段对应的时间$T_a,T_v,T_d$，然后就可以计算任意时刻$t\in [0,T_a+T_v+T_d]$对应的位移，速度，加速度。

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

（1）根据$h,v_0,v_1,a_a,a_d$,其中$h=q_1-q_0$,计算能达到的最大速度$v_f$。要达到最大速度，只有加速度段和减速段，没有减速段。因此有
$$h\ge h_a+h_d=\frac{v_f^2-v_0^2}{2a_a}+\frac{v_1^2-v_f^2}{2a_d}$$
即
$$v_f=\sqrt{\frac{2a_a{a}_{d}h+a_a{v}_1^2+a_d{v}_0^2}{a_d+a_a}}$$

(2)比较$v_f$与$v_{max}$的大小。

1）若$v_f<v_{max}$,说明按照给定参数运动无法达到用户设定的最大速度。这时候匀速段速度设置为$v_v=v_f$（也是最大速度），实际上这时候没有匀速段，只有加速度段和减速段。

2）若$v_f\ge v_{max}$说明按照给定参数运动可以达到用户设定的最大速度。但是又不允许超过最大速度，因此匀速段的速度设置为$v_v=v_{max}$.

(3)计算加速段，匀速段和减速段的时间和位移
$$\begin{gathered} T_a=\frac{v_v-v_0}{a_a},L_a=v_0{T}_a+\frac{1}{2}{a}_a{T}_a^2 \\ {T}_v=\frac{h-\frac{v_{v}2-v_0^2}{2a_a}-\frac{v_1^2-v_v^2}{2a_d}}{v_v},Lv=v_v{T}_v, \\ {T}_d=\frac{v_1-v_v}{a_d},L_d=v_v{T}_d+\frac{1}{2}{a}_d{T}_d^2. \end{gathered}$$

（3）计算轨迹的离散点。轨迹可以使用下面的方程描述
$$\{\begin{array}{l}q(t)=q0+v_{0}t+\frac{1}{2}{a}_a{t}^2,\\ \dot{q}(t)=v_0+a_{a}t,\\ \ddot{q}(t)=a_a.\end{array}t\in [0,T_a+T_v)$$

$$\{\begin{array}{l}q(t)=q_0+L_a+v_v(t-T_a),\\ \dot{q}(t)=v_v,\\ \ddot{q}(t)=0.\end{array}t\in [T_a,T)$$

$$\{\begin{array}{l}q(t)=q_0+L_a+L_v+\frac{1}{2}{a}_d(t-T_a-T_v{)}^2,\\ \dot{q}(t)=v_v+a_d(t-T_a-T_v),\\ \ddot{q}(t)=a_d.\end{array}t\in [T_a+T_v,T_a+T_v+T_d]$$

初始时刻$t_0\ne 0$时，把$t$换成$t-t_0$后，上式同样适用。下面给定几组参数绘制出对应的梯形速度曲线

Example3.3:

(a) $t_0=1,q_0=5,q_1=20,v_0=50,v_{max}=150,v_1=20,a_a=1000,a_d=-1500$

(b)$t_0=1,q_0=5,q_1=30,v_0=50,v_{max}=150,v_1=20,a_a=1000,a_d=-1500$

matlab代码如下

%{
一般的抛物线过渡线性轨迹（梯形速度曲线）,example3.3
Brian
2018-12-23,
%}
%  close('all')
clc
clear
%给定的轨迹参数
t0=2;
q0=5;
q1=20;
v0=50;
v1=20;
vmax=150;
aa=1000;
ad=-1500;
%%
%设置绘图布局
fig1=subplot(3,1,1);
ylabel('position');
grid on
hold on
fig2=subplot(3,1,2);
ylabel('velocity');
grid on
hold on
fig3=subplot(3,1,3);
ylabel('acceleration');
xlabel('time');
grid on
hold on
h=q1-q0;
%确定最大速度
vf=sqrt((2*aa*ad*h-aa*v1^2+ad*v0^2)/(ad-aa));
if(vf>vmax)
    Vv=vmax;
else
    Vv=vf;
end
%计算加速和减速段时间
Ta=(Vv-v0)/aa;
La=v0*Ta+1.0/2.0*aa*Ta^2;
Td=(v1-Vv)/ad;
Ld=Vv*Td+1.0/2.0*ad*Td^2;
Tv=(h-(Vv^2-v0^2)/(2.0*aa)-(v1^2-Vv^2)/(2.0*ad))/Vv;
Lv=h-La-Ld;

%计算轨迹的离散点
Ts=0.001;
j=1;
for t=t0:Ts:t0+Ta+Tv+Td
    time(j)=t;
    t=t-t0;
    if(t>=0&&t<Ta)
        q(j)=q0+v0*t+1.0/2.0*aa*t^2;
        dq(j)=v0+aa*t;
        ddq(j)=aa;
    end

   if(t>=Ta &&t<Ta+Tv)
       q(j)=q0+La+Vv*(t-Ta);
       dq(j)=Vv;
       ddq(j)=0;
   end
   if(t>Ta+Tv&&t<Ta+Tv+Td)
       q(j)=q0+La+Lv+Vv*(t-Ta-Tv)+1.0/2.0*ad*(t-Ta-Tv)^2;
       dq(j)=Vv+ad*(t-Ta-Tv);
       ddq(j)=ad;
   end   
   j=j+1;
end
%绘图
c1=plot(fig1,time,q,'--r','LineWidth',1.5);
c2=plot(fig2,time,dq,'--b','LineWidth',1.5);
c3=plot(fig3,time,ddq,'--g','LineWidth',1.5);
clear  q dq ddq  

程序运行结果如下图所示

小结

梯形速度曲线，也称为直线加减速或T型加减速。其算法十分简易，规划周期耗时短，有利于缩减系
统的连续运行时间，从而提高系统的运动控制速度，实施起来比较容易，应用广泛。

但它采用的是匀加减速，使得在加、减速阶段的起点和终点处加速度存在突变，加速度曲线不连续，使其加速、匀速和减速过程不能实现平滑过渡，存在跳跃现象，导致关节电机进行插补时产生较大的冲击，进给驱动系统出现振动，对电机造成损害，影响使用寿命。因而，直线加减速通常运用于低速、低成本的运动控制过程[2]。

参考文献

[1]Biagiotti L, Melchiorri C. Trajectory Planning for Automatic Machines and Robots[M]. Springer Berlin Heidelberg, 2009.

[2]李方书. 多轴运动控制器的速度规划算法研究[D]. 华南理工大学, 2014.