速度规划之：起点速度和终点速度不为零的非对称梯形速度规划

一、引言

在工业自动化和控制系统设计中，速度规划是确保运动平滑、高效和精确的关键环节。梯形速度规划作为一种简单而常用的速度控制策略，广泛应用于各类伺服系统和控制系统中。然而，传统的梯形速度规划通常假设初速度和末速度为零，且加速和减速过程对称。在实际应用中，往往需要处理起点速度和终点速度不为零，且加速与减速过程非对称的情况，因此起点速度和终点速度不为零且加速与减速过程非对称的梯形速度规划更为通用。

二、理论基础

1. 梯形速度规划概述

梯形速度规划是一种分段的速度控制策略，如图所示，包括加速、匀速和减速三个阶段。在非对称情况下，加速和减速阶段的加速度、时间长度以及最大速度可能不同。

2.数学建模

- 变量定义

• $v_s$：初速度
• $v_{max}$：匀速阶段速度（最大速度限制）
• $t$： 时刻
• $t_1$： 加速结束时间点
• $t_2$： 匀速结束时间点
• $t_{all}$：总时间
• $a$： 加速阶段加速度
• $d$： 减速阶段加速度
• $S_a$ ： 加速阶段路程
• $S_v$： 匀速阶段路程
• $S_d$： 减速阶段路程
• $S$ ： 总路程

- 约束关系

• 运动过程中速度不能超过$v_{max}$即：
  $$\{\begin{array}{l}v(t)\le v_{max}\\ v_s\le v_{max}\\ v_e\le v_{max}\end{array}$$
• 运动过程中加速度不能超过$a$,减速度不能超过$d$
  $$\{\begin{array}{l}a(t)\le a,加速时间段\\ fabs(a(t))\le fabs(d),减速时间段\end{array}$$

- 公式推导

• 位移表达式
  $$S(t)=\{\begin{array}{l}{v}_{s}t+\frac{1}{2}a{t}^2,0\le t<t_1\\ S_a+v_{max}(t-t_1),t_1\le t<t_2\\ S_a+S_v+v_{max}(t-t_2)-\frac{1}{2}d(t-t_2{)}^2,t_2\le t<t_{all}\end{array}$$
  其中,$t$表示整个规划过程的时刻，$v_s$表示起始速度，$S_a和S_v$分别表示加速距离和匀速运动距离，$a$和$d$分别表示减速度和减速度。

• 速度表达式
  $$v(t)=\{\begin{array}{l}{v}_s+at,0\le t<t_1\\ v_{max},t_1\le t<t_2\\ v_{max}-d(t-t_2),t_2\le t<t_{all}\end{array}$$
  当$t=t_{all}$时，$v_e=v_{max}-d(t_{all}-t_2)$。

• 加速度表达式
  $$a(t)=\{\begin{array}{l}a,0\le t<t_1\\ 0,t_1\le t<t_2\\ d,t_2\le t<t_{all}\end{array}$$
  其中，位置、速度和加速度满足关系
  $$\{\begin{array}{l}v(t)=S^{\prime }(t)\\ a(t)=v^{\prime }(t)=S^{\prime \prime }(t)\end{array}$$
  即速度是位置的一阶导数，加速度是速度的一阶导数，位置的二阶导数。

三、计算过程

在实际的规划过程中，由于距离和起止速度的关系，整个速度变化过程很难像想象中那种先加速、再匀速再减速走完全程。这是我们需要在满足约束的条件下，根据一些临界条件来判断规划过程可能存在的速度段。
具体的流程如图所示：

存在4种阶段：

• 注意：当起点速度等于终点速度($V_s=V_e$)时，可以考虑直接进入是否存在匀速段的判断

1.只存在减速段

当起点速度大于终点速度($V_s>V_e$)时，先计算以减速度$d$直接从起点速度$V_s$减速到终点速度$V_e$所需的减速距离$S_d$:
$$S_d=\frac{(V_s^2-V_e^2)}{2d}$$
减速距离$S_d$和规划路径总长度$S$比较，
如果：
$$S_d\le S$$
说明在路径总长度$S$范围内，不足于或者刚好以减速度$d$直接从起点速度$V_s$减速到终点速度$V_e$，则规划过程只存在减速段，为了保证安全，一般情况下选择重算起点速度$V_s^{\prime }$：
$$V_s^{\prime }=\sqrt{2dS+V_e^2}$$
此时：
$$S(t)=\{\begin{array}{l}0,0\le t<t_1\\ 0,t_1\le t<t_2\\ v_s^{\prime }t-\frac{1}{2}d(t{)}^2,t_2\le t<t_{all}\end{array}$$
$$v(t)=\{\begin{array}{l}{v}_s^{\prime },0\le t<t_1\\ v_s^{\prime },t_1\le t<t_2\\ v_s^{\prime }-dt,t_2\le t<t_{all}\end{array}$$
$$a(t)=\{\begin{array}{l}0,0\le t<t_1\\ 0,t_1\le t<t_2\\ d,t_2\le t<t_{all}\end{array}$$
其中：
$$\{\begin{array}{l}{t}_1=t_2=0\\ t_{all}=\frac{(v_s^{\prime }-v_e)}{d}\end{array}$$
相反。如果：
$$S_d>S$$
则至少存在加速段和减速段，需要进一步判断是否存在匀速段。

2.只存在加速段

当起点速度小于终点速度($V_s<V_e$)时，先计算以加减速度$a$直接从起点速度$V_s$加速到终点速度$V_e$所需的加速距离$S_a$:
$$S_a=\frac{(V_e^2-V_a^2)}{2a}$$
加速距离$S_a$和规划路径总长度$S$比较，
如果：
$$S_a\le S$$
说明在路径总长度$S$范围内，不足于或者刚好以加速度$d$直接从起点速度$V_s$加速到终点速度$V_e$，则规划过程只存在加速段，为了保证安全，一般情况下选择重算终点速度$V’_e：
$$V_e^{\prime }=\sqrt{2aS+V_a^2}$$
此时：
$$S(t)=\{\begin{array}{l}{v}_s^{\prime }t-\frac{1}{2}d(t{)}^{2}0\le t<t_1\\ 0,t_1\le t<t_2\\ 0,t_2\le t<t_{all}\end{array}$$
$$v(t)=\{\begin{array}{l}{v}_s+dt,0\le t<t_1\\ v_e^{\prime },t_1\le t<t_2\\ v_e^{\prime },t_2\le t<t_{all}\end{array}$$
$$a(t)=\{\begin{array}{l}a,0\le t<t_1\\ 0,t_1\le t<t_2\\ 0,t_2\le t<t_{all}\end{array}$$
其中：
$$\{\begin{array}{l}{t}_1=\frac{(v_e^{\prime }-v_s)}{a}\\ t_2=t_1+0=t_1\\ t_{all}=t_2+0=t_1\end{array}$$
相反。如果：
$$S_a>S$$
则至少存在加速段和减速段，需要进一步判断是否存在匀速段。

3.存在加速段和减速段

在确认存在加速段和减速段，可以计算起点速度$V_s$加速到最大速度$V_{max}$再直接减速到终点速度$V_e$的距离，即加速到最大速度$V_{max}$所需的最短距离$S_{min}$:
$$S_{min}=Sa+Sd=\frac{(V_{max}^2-V_a^2)}{2a}+\frac{(V_{max}^2-V_d^2)}{2d}$$
最短距离$S_{min}$和规划路径总长度$S$比较，
如果：
$$S_{min}\le S$$
说明在路径总长度$S$范围内，不足于或者刚好以加速度$a$从起点速度$V_s$加速最大速度$V_{max}$再以减速度$d$减速到终点速度$V_e$，则规划过程不存在匀速段，需最短距离$S_{min}$重算最大速度$V_{max}^{\prime }$：
$$V_{max}^{\prime }=\frac{2ad{S}_{min}+V_a^{2}d+V_d^{2}a}{a+d}$$
此时：
$$S(t)=\{\begin{array}{l}{v}_{s}t+\frac{1}{2}a{t}^2,0\le t<t_1\\ S_a+v_{max}^{\prime }(t-t_1),t_1\le t<t_2\\ S_a+S_v+v_{max}^{\prime }(t-t_2)-\frac{1}{2}d(t-t_2{)}^2,t_2\le t<t_{all}\end{array}$$
$$v(t)=\{\begin{array}{l}{v}_s+at,0\le t<t_1\\ v_{max}^{\prime },t_1\le t<t_2\\ v_{max}^{\prime }-d(t-t_2),t_2\le t<t_{all}\end{array}$$
$$a(t)=\{\begin{array}{l}a,0\le t<t_1\\ 0,t_1\le t<t_2\\ d,t_2\le t<t_{all}\end{array}$$
其中：
$$\{\begin{array}{l}{t}_1=\frac{(v_{max}^{\prime }-v_s)}{a}\\ t_2=t_1+0=t_1\\ t_{all}=t_2+\frac{(v_{max}^{\prime }-v_e)}{d}\end{array}$$
相反。如果：
$$S_{min}>S$$
则说明路径规划过程中存在加速段、匀速段和减速段。

4.存在加速度段、匀速段和减速段

在完整的加速度段、匀速段和减速段三段都存在的情况下，最大速度$V_{max}$可达。此时：
$$S(t)=\{\begin{array}{l}{v}_{s}t+\frac{1}{2}a{t}^2,0\le t<t_1\\ S_a+v_{max}^{\prime }(t-t_1),t_1\le t<t_2\\ S_a+S_v+v_{max}^{\prime }(t-t_2)-\frac{1}{2}d(t-t_2{)}^2,t_2\le t<t_{all}\end{array}$$
$$v(t)=\{\begin{array}{l}{v}_s+at,0\le t<t_1\\ v_{max}^{\prime },t_1\le t<t_2\\ v_{max}^{\prime }-d(t-t_2),t_2\le t<t_{all}\end{array}$$
$$a(t)=\{\begin{array}{l}a,0\le t<t_1\\ 0,t_1\le t<t_2\\ d,t_2\le t<t_{all}\end{array}$$
各个时刻计算，首先可以计算出加速距离和加速时间：
$$\{\begin{array}{l}{S}_a=\frac{(v_{max}^2-v_a^2)}{2a}\\ T_a=\frac{(v_{max}-v_a)}{a}\end{array}$$
减速距离和减速时间：
$$\{\begin{array}{l}{S}_e=\frac{(v_{max}^2-v_e^2)}{2d}\\ T_e=\frac{(v_{max}-v_e)}{d}\end{array}$$
则匀速距离和匀速时间可求：
$$\{\begin{array}{l}{S}_v=S-S_a-S_e\\ T_v=\frac{S_v}{v_{max}}\end{array}$$
最终时刻为：
$$\{\begin{array}{l}{t}_1=T_a=\frac{(v_{max}-v_a)}{a}\\ t_2=t_1+T_v\\ t_{all}=t_2+T_e=t_2+\frac{(v_{max}^{\prime }-v_e)}{d}\end{array}$$

四、仿真实现

用matlab编写梯形速度规划函数，定义系统参数：路径长度、起点速度、终点速度、加速度、减速度等，输入到规划函数中，输出距离、速度和加速度随时间变化的曲线数据，关键参数计算代码如下：

    if(vs>ve)
        sd = (vs^2 - ve^2)/(2.0*d);
        if(sd >= S)
            vs = sqrt((2.0*d*S + ve^2));
            t1 = 0;
            t2 = t1 + 0;
            tall = t2 + (vs-ve)/(1.0*d);
            break;
        end
    elseif(vs<ve)
        se = (ve^2 - vs^2)/(2.0*a);
        if(se >= S)
            ve = sqrt((2.0*a*S + vs^2));
            t1 = (ve-vs)/(1.0*a);
            t2 = t1 + 0;
            tall =  t2 + 0;
            break;
        end 
    end
    % 最大速度可达性判断
    smin = (vmax^2 - ve^2)/(2.0*d) + (vmax^2 - vs^2)/(2.0*a);
    if(smin >= S)%只存在加速段和减速段
        vmax = sqrt((2.0*a*d*S + vs^2*d + ve^2*a)/(a + d));
        t1 = (vmax-vs)/(1.0*a);
        t2 = t1 + 0;
        tall = t2 + (vmax-ve)/(1.0*d);
    else%存在加速段、匀速段和减速段
        t1 = (vmax-vs)/(1.0*a);
        t2 = t1 + (S - (vmax^2 - vs^2)/(2.0*a) - (vmax^2 - ve^2)/(2.0*d))/(1.0*vmax);
        tall = t2 + (vmax-ve)/(1.0*d);
    end

如下所示，分别是四种情况下对应的参数和规划结果：

• 只存在加速段
  输入参数：

%参数
s_i = 3;    %路径长度
v_s = 10;    %起点速度
v_e = 20;    %终点速度
a = 50;      %加速度
d = 40;      %减速度
vmax = 30;	 %最大速度
t_step = 0.0001;    %规划步长
% 调用梯形速度规划接口
[S,V,A,T] = VelocityPlanner_Trap(s_i,v_s,v_e,a,d,vmax,t_step);

仿真结果：

• 只存在减速段
  输入参数：

%参数
s_i = 3;    %路径长度
v_s = 20;    %起点速度
v_e = 10;    %终点速度
a = 50;      %加速度
d = 40;      %减速度
vmax = 30;	 %最大速度
t_step = 0.0001;    %规划步长
% 调用梯形速度规划接口
[S,V,A,T] = VelocityPlanner_Trap(s_i,v_s,v_e,a,d,vmax,t_step);

仿真结果：

• 存在加速段和减速段
  输入参数：

%参数
s_i = 10;    %路径长度
v_s = 20;    %起点速度
v_e = 10;    %终点速度
a = 50;      %加速度
d = 40;      %减速度
vmax = 30;	 %最大速度
t_step = 0.0001;    %规划步长
% 调用梯形速度规划接口
[S,V,A,T] = VelocityPlanner_Trap(s_i,v_s,v_e,a,d,vmax,t_step);

仿真结果：

• 存在加速段、匀速段和减速段
  输入参数：

%参数
s_i = 50;    %路径长度
v_s = 20;    %起点速度
v_e = 10;    %终点速度
a = 50;      %加速度
d = 40;      %减速度
vmax = 30;	 %最大速度
t_step = 0.0001;    %规划步长
% 调用梯形速度规划接口
[S,V,A,T] = VelocityPlanner_Trap(s_i,v_s,v_e,a,d,vmax,t_step);

仿真结果：

五、优缺点

优点

• 规划时间短：在所有速度规划算法中，梯形速度规划通常具有最短的规划时间。这是因为梯形速度规划采用了直接且简单的加速度和减速度变化，使得整个运动过程能够在最短的时间内完成。
• 算法简单、计算量小：梯形速度规划的算法相对简单，计算量较小，这使得它在实时性要求较高的场合下具有较大的优势。例如，在工业自动化、机器人控制等领域，梯形速度规划能够迅速响应指令，完成复杂的运动任务。
• 速度、加速度可控：在梯形速度规划中，速度和加速度是可以根据需要进行精确控制的。这意味着在规划过程中，可以根据实际情况调整加速度和减速度的大小，以满足不同的运动需求。
• 位置平滑：梯形速度规划能够确保位置曲线的平滑性，即在整个运动过程中，物体的位置变化是连续的，没有突变现象。这有助于减少运动过程中的震动和冲击，提高运动的稳定性和可靠性。

缺点

• 加速度跳变：梯形速度规划的加速度在加速和减速阶段存在跳变现象，即加速度的值在瞬间发生较大变化。这种加速度的跳变可能会对受控对象产生冲击，影响其使用寿命和性能。
• 更高阶导数不可控：在梯形速度规划中，虽然速度和加速度可以进行控制，但更高阶的导数（如加加速度）通常是不可控的。这可能导致在运动过程中产生一些不可预测的动态响应，影响运动的精度和稳定性。
• 控制效果较差：由于梯形速度规划存在速度不平滑、加速度跳变等问题，其控制效果相对较差。在需要高速运动或者复杂运动的情形下可能不具体适应性。
• 不适用于高速运动：梯形加减速算法在高速运动时，加速和减速时间会变得很短，导致机械系统的冲击和震动加剧，难以实现平滑的运动控制。
• 不适用于复杂运动：梯形加减速算法只适用于直线运动或简单曲线运动，对于复杂的路径规划，需要使用更加复杂的运动规划算法。

六、总结

非对称S形速度规划是一种有效的速度规划方法，可以在数控机床、自动化生产线设备、机器人运动控制作为基础路径配速算法，实现简单的、速度连续的速度控制，j基本保证运动过程中平稳性。