梯形加减速

我就是我 2022-12-13 12:55 155阅读 0赞

# 梯形加减速 #

## 引例 ##

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

\]这种轨迹分为三个部分。假设位移是正的，即 q 1 > q 0 q\_1>q\_0 q1>q0。若位移是负的，只需改变对应的加速度和速度的符号。第一部分，加速度恒定，速度是时间的线性函数，位移是时间的抛物线函数；第二部分，加速度为0，速度恒定，位移是时间的线性函数；第三部分，加速度为恒定的负值，速度线性减小，位移是时间的二次多项式。一般假设加速段 T a T\_a Ta和减速段时间 T d T\_d Td相等\[1\]。

如果 t 0 = 0 t\_0=0 t0=0，轨迹可由下列表达式描述：

（1）加速段， t ∈ \[ 0 , T a \] t\\in\[0,T\_a\] t∈\[0,Ta\]  
 \{ q ( t ) = a 0 + a 1 t + a 2 t 2 q ˙ ( t ) = a 1 + 2 a 2 t q ¨ ( t ) = 2 a 2 . (3.1) \\begin\{cases\} q(t)=a\_0+a\_1t+a\_2t^2\\\\ \\dot q(t)=a\_1+2a\_2t\\\\ \\ddot q(t)=2a\_2. \\end\{cases\} \\tag\{3.1\} ⎩⎪⎨⎪⎧q(t)=a0\+a1t\+a2t2q˙(t)=a1\+2a2tq¨(t)=2a2.(3.1)  
若初始速度 v 0 v\_0 v0设置为0，最大速度设置为 v v v\_v vv.可以解的系数  
 \{ a 0 = q 0 a 1 = 0 a 2 = v v 2 T a \\begin\{cases\} a\_0=q\_0\\\\ a\_1=0\\\\ a\_2=\\frac\{v\_v\}\{2T\_a\} \\end\{cases\} ⎩⎪⎨⎪⎧a0=q0a1=0a2=2Tavv  
（2）匀速段， t ∈ \[ T a , t 1 − T a \] t\\in \[T\_a,t\_1-T\_a\] t∈\[Ta,t1−Ta\]  
 \{ q ( t ) = b 0 + b 1 t q ˙ ( t ) = b 1 q ¨ ( t ) = 0 (3.2) \\begin\{cases\} q(t)=b\_0+b\_1t\\\\ \\dot q(t)=b\_1\\\\ \\ddot q(t)=0 \\end\{cases\} \\tag\{3.2\} ⎩⎪⎨⎪⎧q(t)=b0\+b1tq˙(t)=b1q¨(t)=0(3.2)  
根据连续性，解得  
 \{ b 1 = v v b 0 = q 0 − v v T a 2 \\begin\{cases\} b\_1=v\_v\\\\ b\_0=q\_0-\\frac\{v\_vT\_a\}\{2\} \\end\{cases\} \{ b1=vvb0=q0−2vvTa  
(3)减速段， t ∈ \[ t 1 − T a , t 1 \] t\\in \[t\_1-T\_a,t\_1\] t∈\[t1−Ta,t1\]  
 \{ q ( t ) = c 0 + c 1 t + c 2 t 2 q ˙ ( t ) = c 1 + 2 c 2 t q ¨ ( t ) = 2 c 2 (3.3) \\begin\{cases\} q(t)=c\_0+c\_1t+c\_2t^2\\\\ \\dot q(t)=c\_1+2c\_2t\\\\ \\ddot q(t)=2c\_2 \\end\{cases\} \\tag\{3.3\} ⎩⎪⎨⎪⎧q(t)=c0\+c1t\+c2t2q˙(t)=c1\+2c2tq¨(t)=2c2(3.3)  
设置末速度为零，那么解的  
 \{ c 0 = q 1 − v v t 1 2 2 T a c 1 = v v t 1 T a c 2 = − v v 2 T a \\begin\{cases\} c\_0=q\_1-\\frac\{v\_vt\_1^2\}\{2T\_a\}\\\\ c\_1=\\frac\{v\_vt\_1\}\{T\_a\}\\\\ c\_2=-\\frac\{v\_v\}\{2T\_a\} \\end\{cases\} ⎩⎪⎨⎪⎧c0=q1−2Tavvt12c1=Tavvt1c2=−2Tavv  
综合起来，考虑一下一般的情况 t ≠ 0 t\\neq 0 t=0,轨迹的位移表示为

t 0 ≤ t < t 0 + T a t\_0\\leq t<t\_0+T\_a t0≤t<t0\+Ta  
 \{ q ( t ) = q 0 + v v 2 T a ( t − t 0 ) 2 , q ˙ ( t ) = v v T a ( t − t 0 ) , q ¨ ( t ) = v v T a . \\begin\{cases\} 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\{cases\} ⎩⎪⎨⎪⎧q(t)=q0\+2Tavv(t−t0)2,q˙(t)=Tavv(t−t0),q¨(t)=Tavv.  
 t 0 + T a ≤ t < t 1 − T a t\_0+T\_a\\leq t<t\_1-T\_a t0\+Ta≤t<t1−Ta  
 \{ q ( t ) = q 0 + v v ( t − t 0 − T a 2 ) , q ˙ ( t ) = v v , q ¨ ( t ) = 0. \\begin\{cases\} q(t)=q\_0+v\_v(t-t\_0-\\frac\{T\_a\}\{2\}),\\\\ \\dot q(t)=v\_v,\\\\ \\ddot q(t)=0. \\end\{cases\} ⎩⎪⎨⎪⎧q(t)=q0\+vv(t−t0−2Ta),q˙(t)=vv,q¨(t)=0.  
 t 1 − T a ≤ t ≤ t 1 t\_1-T\_a\\leq t\\leq t\_1 t1−Ta≤t≤t1  
 \{ q ( t ) = q 1 − v v 2 T a ( t 1 − t ) 2 , q ˙ ( t ) = v v T a ( t 1 − t ) , q ¨ ( t ) = − v v T a . \\begin\{cases\} 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\{cases\} ⎩⎪⎨⎪⎧q(t)=q1−2Tavv(t1−t)2,q˙(t)=Tavv(t1−t),q¨(t)=−Tavv.  
Eample3.2:给定梯形轨迹的条件， q 0 = 0 , q 1 = 30 , t 0 = 0 , t 1 = 4 , T a = 1 , v v = 10 q\_0=0,q\_1=30,t\_0=0,t\_1=4,T\_a=1,v\_v=10 q0=0,q1=30,t0=0,t1=4,Ta=1,vv=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

结果如下图所示  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70_pic_center]

## 通用梯形速度曲线 ##

确定梯形速度曲线的参数 h , v 0 , v 1 , T a , T d , a a , a d , v m a x h,v\_0,v\_1,T\_a,T\_d,a\_a,a\_d,v\_\{max\} h,v0,v1,Ta,Td,aa,ad,vmax,可以指定其中部分参数，其他参数根据轨迹几何关系计算出来。对于一台机器要运动的位移，时间是千变万化的，但是其允许的最大速度，加速度，启动速度一般受到机械部件的限制，是确定的了。因此，实际应用中一般是指定梯形轨迹的位移，初始速度，最大速度，终止速度，最大加速度来确定轨迹。

因此，通用梯形速度曲线为已知初始速度，终止速度，加速度，减速度，最大速度，位移。要确定轨迹，关键是计算出加速度段，匀速段，减速度段对应的时间 T a , T v , T d T\_a,T\_v,T\_d Ta,Tv,Td，然后就可以计算任意时刻 t ∈ \[ 0 , T a + T v + T d \] t\\in\[0,T\_a+T\_v+T\_d\] t∈\[0,Ta\+Tv\+Td\]对应的位移，速度，加速度。

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

（1）根据 h , v 0 , v 1 , a a , a d h,v\_0,v\_1,a\_a,a\_d h,v0,v1,aa,ad,其中 h = q 1 − q 0 h=q\_1-q\_0 h=q1−q0,计算能达到的最大速度 v f v\_f vf。要达到最大速度，只有加速度段和减速段，没有减速段。因此有  
 h ≥ h a + h d = v f 2 − v 0 2 2 a a + v 1 2 − v f 2 2 a d h\\geq h\_a+h\_d=\\frac\{v\_f^2-v\_0^2\}\{2a\_a\}+\\frac\{v\_1^2-v\_f^2\}\{2a\_d\} h≥ha\+hd=2aavf2−v02\+2adv12−vf2  
即  
 v f = 2 a a a d h − a a v 1 2 + a d v 0 2 a d − a a v\_f=\\sqrt\{\\frac\{2a\_aa\_dh-a\_av\_1^2+a\_dv\_0^2\}\{a\_d-a\_a\}\} vf=ad−aa2aaadh−aav12\+adv02

(2)比较 v f v\_f vf与 v m a x v\_\{max\} vmax的大小。

1）若 v f < v m a x v\_f<v\_\{max\} vf<vmax,说明按照给定参数运动无法达到用户设定的最大速度。这时候匀速段速度设置为 v v = v f v\_v=v\_f vv=vf（也是最大速度），实际上这时候没有匀速段，只有加速度段和减速段。

2）若 v f ≥ v m a x v\_f\\geq v\_\{max\} vf≥vmax说明按照给定参数运动可以达到用户设定的最大速度。但是又不允许超过最大速度，因此匀速段的速度设置为 v v = v m a x v\_v=v\_\{max\} vv=vmax.

(3)计算加速段，匀速段和减速段的时间和位移  
 T a = v v − v 0 a a , L a = v 0 T a + 1 2 a a T a 2 T v = h − v v 2 − v 0 2 2 a a − v 1 2 − v v 2 2 a d v v , L v = v v T v , T d = v 1 − v v a d , L d = v v T d + 1 2 a d T d 2 . T\_a=\\frac\{v\_v-v\_0\}\{a\_a\},L\_a=v\_0T\_a+\\frac\{1\}\{2\}a\_aT\_a^2\\\\ T\_v=\\frac\{h-\\frac\{v\_v2-v\_0^2\}\{2a\_a\}-\\frac\{v\_1^2-v\_v^2\}\{2a\_d\}\}\{v\_v\},Lv=v\_vT\_v,\\\\ T\_d=\\frac\{v\_1-v\_v\}\{a\_d\},L\_d=v\_vT\_d+\\frac\{1\}\{2\}a\_dT\_d^2. Ta=aavv−v0,La=v0Ta\+21aaTa2Tv=vvh−2aavv2−v02−2adv12−vv2,Lv=vvTv,Td=adv1−vv,Ld=vvTd\+21adTd2.

（3）计算轨迹的离散点。轨迹可以使用下面的方程描述  
 \{ q ( t ) = q 0 + v 0 t + 1 2 a a t 2 , q ˙ ( t ) = v 0 + a a t , q ¨ ( t ) = a a . t ∈ \[ 0 , T a + T v ) \\begin\{cases\} q(t)=q0+v\_0t+\\frac\{1\}\{2\}a\_at^2,\\\\ \\dot q(t)=v\_0+a\_at,\\\\ \\ddot q(t)=a\_a. \\end\{cases\} t\\in\[0,T\_a+T\_v) ⎩⎪⎨⎪⎧q(t)=q0\+v0t\+21aat2,q˙(t)=v0\+aat,q¨(t)=aa.t∈\[0,Ta\+Tv)

\{ q ( t ) = q 0 + L a + v v ( t − T a ) , q ˙ ( t ) = v v , q ¨ ( t ) = 0. t ∈ \[ T a , T ) \\begin\{cases\} q(t)=q\_0+L\_a+v\_v(t-T\_a),\\\\ \\dot q(t)=v\_v,\\\\ \\ddot q(t)=0. \\end\{cases\} t\\in \[T\_a,T) ⎩⎪⎨⎪⎧q(t)=q0\+La\+vv(t−Ta),q˙(t)=vv,q¨(t)=0.t∈\[Ta,T)

\{ q ( t ) = q 0 + L a + L v + 1 2 a d ( t − T a − T v ) 2 , q ˙ ( t ) = v v + a d ( t − T a − T v ) , q ¨ ( t ) = a d . t ∈ \[ T a + T v , T a + T v + T d \] \\begin\{cases\} 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\{cases\} t\\in \[T\_a+T\_v,T\_a+T\_v+T\_d\] ⎩⎪⎨⎪⎧q(t)=q0\+La\+Lv\+21ad(t−Ta−Tv)2,q˙(t)=vv\+ad(t−Ta−Tv),q¨(t)=ad.t∈\[Ta\+Tv,Ta\+Tv\+Td\]

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

Example3.3:

(a)  t 0 = 1 , q 0 = 5 , q 1 = 20 , v 0 = 50 , v m a x = 150 , v 1 = 20 , a a = 1000 , a d = − 1500 t\_0=1,q\_0=5,q\_1=20,v\_0=50,v\_\{max\}=150,v\_1=20,a\_a=1000,a\_d=-1500 t0=1,q0=5,q1=20,v0=50,vmax=150,v1=20,aa=1000,ad=−1500

(b) t 0 = 1 , q 0 = 5 , q 1 = 30 , v 0 = 50 , v m a x = 150 , v 1 = 20 , a a = 1000 , a d = − 1500 t\_0=1,q\_0=5,q\_1=30,v\_0=50,v\_\{max\}=150,v\_1=20,a\_a=1000,a\_d=-1500 t0=1,q0=5,q1=30,v0=50,vmax=150,v1=20,aa=1000,ad=−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

程序运行结果如下图所示

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70_pic_center 1]

## 小结 ##

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

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

参考文献

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

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

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70_pic_center]: /images/20221123/5387d967459d4057a517a3df5bb5ff73.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70_pic_center 1]: /images/20221123/e532e92412a740ac934d55a5ad98d17a.png