速度曲线规划 ---- 梯形速度曲线

一、引言

  在伺服系统以及控制系统的加减速动作中，为了让速度更加平滑，可以引入T型速度曲线规划（T-curve velocity profile），T曲线是工业界广泛采用的形式，它是一种时间最优的曲线。一般情况，曲线加速和减速的过程是对称的，设给定速度上限为vmaxv_{max}vmax​。加速度上限为amaxa_{max}amax​，被控对象从A点运动到B点，要求生成的轨迹在这些条件下时间最优。

二、理论分析

在整体系统高速启动，制动的状态下，可以提高整体系统的性能。每当系统完成一个动作的时候，总共包括三个过程，匀加速，匀速，匀减速，具体如下图所示：

根据 v 是否到达Vmax，这里通常要分为两种情况来讨论；

• 第一种：速度到达Vmax，最终速度曲线为梯形；
• 第二种：速度没有到达Vmax​，最终速度曲线为三角形；

下面仅讨论第一种情况；

这里时间使用 t 加脚标来表示，位置量使用 p 来表示，加速度使用 a 来表示

• 设加速时间长度为 ta​：t0 - t1​；
• 因为加速和减速的过程是对称的，所以减速带的时间长度也为 ta：t2 - t3​；
• 最大速度 Vmax 持续的时间长度为 tm​：t1 - t2​；

在实际的系统中，梯形曲线通常需要设置三个参数：

1. 最大速度 Vmax；
2. 加速度 amax​；
3. 最终位置值 Pfinal​，下面简称为 Pf；

所以这三个变量可以看做是已知量来处理。

下面简单推导这三个参数之间的关系：
设加减速区域经过的位置量为Pa,则：Pa=1/2*am*ta²  

设最大区域经过的位置量为 Pm​,则：

所以输出的位置量满足以下关系：

最终可以通过 P(t) 的关系以及①②③④式编写程序可得到梯形速度曲线规划。

三、C语言实现

void sfun_myc_Outputs_wrapper(const real_T *u0,
			                const real_T *u1,
			                const real_T *u2,
			                const real_T *t,
		                	real_T *y0,
		                   	real_T *y1,
			                real_T *y2)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
/* 此示例设置输出等于输入
      y0[0] = u0[0]; 
 对于复杂信号使用: y0[0].re = u0[0].re; 
      y0[0].im = u0[0].im;
      y1[0].re = u1[0].re;
      y1[0].im = u1[0].im;
*/

/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
    int Am = u0[0];
    int Vm = u1[0];
    int Pf = u2[0];
    int T = t[0];
    
    int Ta = Vm/Am;
    int Tm = (Pf - Am*Ta*Ta)/Vm;
    int Tf = 2*Ta+Tm;
    printf("%d\r\n",Tf);    
    //梯形
    if(Tm>0){
        if(T <= Ta){
            y0[0] = 0.5*Am*T*T;
            y1[0] = Am*T;
            y2[0] = Am;
        }else if(T<=(Ta+Tm)){
            y0[0] = 0.5*Am*Ta*Ta + Vm*(T-Ta);
            y1[0] = Vm;
            y2[0] = 0;
        }else if(T<=(Ta+Tm+Ta)){
            y0[0] = 0.5*Am*Ta*Ta + Vm*Tm + 0.5*Am*(T-Ta-Tm)*(T-Ta-Tm);
            y1[0] = Vm-Am*(T-Ta-Tm);
            y2[0] = -Am;
        }
    }else{
    //三角形
        Ta = sqrt(Pf/Am);
        if(T<Ta){
            y0[0] = 0.5*Am*T*T;
            y1[0] = Am*T;
            y2[0] = Am;
        }else{
            y0[0] = 0.5*Am*Ta*Ta + 0.5*Am*(T-Ta)*(T-Ta);
            y1[0] = Am*Ta - Am*(T-Ta);
            y2[0] = -Am;
        }
    }
}

四、总结

T曲线是工业界广泛采用的形式，在运动控制上，相比较S曲线，它算法的复杂度更低，所占用的系统资源更少，但是在恒加速的拐点会出现过冲，这里S曲线就可以减少这种情况的发生。