梯形速度分布的轨迹规划，从本质上来说，是一个分段函数的轨迹规划，基本的方式是一个先加速，再匀速，再减速的三段函数的过程。当然，当间隔时间太短时，会出现分段函数只有加速和减速，无匀速的情况。或者因为开始速度和结束速度不相等，出现加速和减速过程不对称的情况。在梯形速度分布中，都需要根据实际的状况，分别的全面考虑所有的情况。

初始速度为 v 0 v_0 v0​ = 0, 结束速度为 v 1 v_1 v1​ = 0；初始时间 t 0 t_0 t0​ = 0。 假设：加速时间、减速时间分别为 T a T_a Ta​、 T d T_d Td​，匀速时速度为 v v v_v vv​。

1. 加速阶段

加速时间区间为[0, T a T_a 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 \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} ⎩⎪⎨⎪⎧​q(t)=a0​+a1​t+a2​t2q˙​(t)=a1​+2a2​tq¨​(t)=2a2​​

表达式中常量参数的公式如下所示：

{ 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 = \cfrac{v_v}{2T_a} \end{cases} ⎩⎪⎪⎪⎨⎪⎪⎪⎧​a0​=q0​a1​=0a2​=2Ta​vv​​​

2. 匀速阶段

匀速时间区间为[ T a T_a Ta​, t 1 − T a t_1 - T_a t1​−Ta​]，因为速度恒定，加速度为0，因此轨迹曲线为一次多项式，在此阶段，位置、速度、加速度的表达式如下所示：

{ q ( t ) = b 0 + b 1 t q ˙ ( t ) = b 1 q ¨ ( t ) = 0 \begin{cases} q(t) = b_0 + b_1t \\ \dot{q}(t) = b_1 \\ \ddot{q}(t) = 0 \end{cases} ⎩⎪⎨⎪⎧​q(t)=b0​+b1​tq˙​(t)=b1​q¨​(t)=0​

表达式中常量参数的公式如下所示：

{ b 1 = v v b 0 = q 0 − v v T a 2 \begin{cases} b_1 = v_v \\ b_0 = q_0 - \cfrac{v_vT_a}{2} \end{cases} ⎩⎨⎧​b1​=vv​b0​=q0​−2vv​Ta​​​

3. 减速阶段

减速时间区间为[ t 1 − T a t_1 - T_a t1​−Ta​, t 1 t_1 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 \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} ⎩⎪⎨⎪⎧​q(t)=c0​+c1​t+c2​t2q˙​(t)=c1​+2c2​tq¨​(t)=2c2​​

表达式中常量参数的公式如下所示：

{ 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 - \cfrac{v_vt_1^2}{2T_a} \\ c_1 = \cfrac{v_vt_1}{T_a} \\ c_2 = -\cfrac{v_v}{2T_a} \end{cases} ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧​c0​=q1​−2Ta​vv​t12​​c1​=Ta​vv​t1​​c2​=−2Ta​vv​​​

根据以上公式，针对两点之间的轨迹规划，使用matlab实现的关节角度、关节速度、关节加速度曲线如下所示：

关于上面 T a T_a Ta​和 v v v_v vv​的值是怎么确定的，如下推导过程：

当初始速度为0时，在t = t 0 + T a t_0 + T_a t0​+Ta​这个点上，加速 T a T_a Ta​后得到的速度和开始匀速时的速度的值是相等的，由此有如下所示的等式：

a a T a = q m − q a T m − T a a_aT_a = \frac{q_m - q_a}{T_m - T_a} aa​Ta​=Tm​−Ta​qm​−qa​​

以上等式中字符的表达式如下所示：

{ q a = q ( t 0 + T a ) q m = ( q 1 + q 0 ) 2 = q 0 + h 2 T m = ( t 1 − t 0 ) 2 = T 2 \begin{cases} q_a = q(t_0 + T_a) \\ q_m = \cfrac{(q_1 + q_0)}{2} = q_0 + \cfrac{h}{2} \\ T_m = \cfrac{(t_1 - t_0)}{2} = \cfrac{T}{2} \end{cases} ⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧​qa​=q(t0​+Ta​)qm​=2(q1​+q0​)​=q0​+2h​Tm​=2(t1​−t0​)​=2T​​

刚刚加速完毕的时刻点，可得到的等式如下所示：

q a = q 0 + 1 2 a a T a 2 q_a = q_0 + \frac{1}{2}a_aT_a^2 qa​=q0​+21​aa​Ta2​

将以上等式、 q m = ( q 1 + q 0 ) 2 q_m = \frac{(q_1 + q_0)}{2} qm​=2(q1​+q0​)​、 T m = ( t 1 − t 0 ) 2 T_m = \frac{(t_1 - t_0)}{2} Tm​=2(t1​−t0​)​代入，可得以下等式：

a a T a 2 − a a ( t 1 − t 0 ) T a + ( q 1 − q 0 ) = 0 a_aT_a^2 - a_a(t_1 - t_0)T_a + (q_1 - q_0) = 0 aa​Ta2​−aa​(t1​−t0​)Ta​+(q1​−q0​)=0

同时，因为速度曲线的面积= q 1 − q 0 q_1 - q_0 q1​−q0​，由此可得等式：

v v = q 1 − q 0 t 1 − t 0 − T a = h T − T a v_v = \frac{q_1 - q_0}{t_1 - t_0 -T_a} = \frac{h}{T - T_a} vv​=t1​−t0​−Ta​q1​−q0​​=T−Ta​h​

如果指定加速度 a a a_a aa​为已知值，那么需要根据此加速度确定加速和减速所需要的时间。

T a = a a ( t 1 − t 0 ) − a a 2 ( t 1 − t 0 ) 2 − 4 a a ( q 1 − q 0 ) 2 a a T_a = \frac{a_a(t1-t0)-\sqrt {a_a^2(t1-t0)^2-4a_a(q1-q0)}}{2a_a} Ta​=2aa​aa​(t1−t0)−aa2​(t1−t0)2−4aa​(q1−q0) ​​

由以上公式可以得到加速度范围的表达式，如下所示：

a a ⩾ 4 ( q 1 − q 0 ) ( t 1 − t 0 ) 2 a_a \geqslant \frac{4(q_1 - q_0)}{(t_1 - t_0)^2} aa​⩾(t1​−t0​)24(q1​−q0​)​

由以上表达式，可知，在此种模式下，可以得到最小加速度为 a a = 4 h T 2 a_a = \frac{4h}{T^2} aa​=T24h​，此时，加速时间为 T a = 1 2 ( t 1 − t 0 ) T_a = \frac{1}{2}(t_1 - t_0) Ta​=21​(t1​−t0​)

假设： a a = a m a x a_a = a_{max} aa​=amax​， v v = v m a x v_v = v_{max} vv​=vmax​

则可以得到如下等式：

{ T a = v m a x a m a x , 加 速 时 间 v m a x ( T − T a ) = q 1 − q 0 = h , 位 移 T = h a m a x + v m a x 2 a m a x v m a x , 总 时 间 \begin{cases} T_a = \cfrac{v_{max}}{a_{max}},\quad\quad\quad 加速时间 \\ v_{max}(T - T_a) = q_1 - q_0 = h,\quad\quad\quad 位移 \\ T = \cfrac{ha_{max} + v_{max}^2}{a_{max}v_{max}},\quad\quad\quad 总时间 \end{cases} ⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧​Ta​=amax​vmax​​,加速时间vmax​(T−Ta​)=q1​−q0​=h,位移T=amax​vmax​hamax​+vmax2​​,总时间​

假设 t 0 t_0 t0​不等于0，因为 t 1 = t 0 + T t_1 = t_0 + T t1​=t0​+T，因此可以得到有加速、匀速、减速三个阶段的通用表达式为：

q ( t ) = { q 0 + 1 2 a m a x ( t − t 0 ) 2 , t 0 ≤ t ≤ t 0 + T a q 0 + a m a x T a ( t − t 0 − T a 2 ) , t 0 + T a ≤ t ≤ t 1 − T a q 1 − 1 2 a m a x ( t 1 − t ) 2 , t 1 − T a ≤ t ≤ t 1 q(t) = \begin{cases} q_0 + \frac{1}{2}a_{max}(t - t_0)^2,\quad\quad\quad t_0 \le t \le t_0 + T_a \\ q_0 + a_{max}T_a(t - t_0 - \cfrac{T_a}{2}),\quad\quad\quad t_0 + T_a \le t \le t_1 - T_a \\ q_1 - \frac{1}{2}a_{max}(t_1 - t)^2,\quad\quad\quad t_1 - T_a \le t \le t_1 \end{cases} q(t)=⎩⎪⎪⎨⎪⎪⎧​q0​+21​amax​(t−t0​)2,t0​≤t≤t0​+Ta​q0​+amax​Ta​(t−t0​−2Ta​​),t0​+Ta​≤t≤t1​−Ta​q1​−21​amax​(t1​−t)2,t1​−Ta​≤t≤t1​​

如果要保证一定有匀速阶段，那么需要满足如下限制：

h ⩾ v m a x 2 a m a x h \geqslant \frac{v_{max}^2}{a_{max}} h⩾amax​vmax2​​

如果不满足以上限制，则可以得到如下等式：

{ T a = h a m a x , 加 速 时 间 T = 2 T a , 总 时 间 v m a x = a m a x T a = a m a x h = h T a , 最 大 速 度 \begin{cases} T_a = \sqrt{\frac{h}{a_{max}}},\quad\quad\quad 加速时间 \\ T = 2T_a,\quad\quad\quad 总时间 \\ v_{max} = a_{max}T_a = \sqrt{a_{max}h} = \frac{h}{T_a},\quad\quad\quad 最大速度 \end{cases} ⎩⎪⎪⎨⎪⎪⎧​Ta​=amax​h​ ​,加速时间T=2Ta​,总时间vmax​=amax​Ta​=amax​h ​=Ta​h​,最大速度​

因为不满足限制条件，此时没有匀速阶段，此时的通用表达式为：

q ( t ) = { q 0 + 1 2 a m a x ( t − t 0 ) 2 , t 0 ≤ t ≤ t 0 + T a q 1 − 1 2 a m a x ( t 1 − t ) 2 , t 1 − T a ≤ t ≤ t 1 q(t) = \begin{cases} q_0 + \frac{1}{2}a_{max}(t - t_0)^2,\quad\quad\quad t_0 \le t \le t_0 + T_a \\ q_1 - \frac{1}{2}a_{max}(t_1 - t)^2,\quad\quad\quad t_1 - T_a \le t \le t_1 \end{cases} q(t)={q0​+21​amax​(t−t0​)2,t0​≤t≤t0​+Ta​q1​−21​amax​(t1​−t)2,t1​−Ta​≤t≤t1​​

通过以上推导可知，如果使用此种模式，从 q 0 q_0 q0​到 q 1 q_1 q1​的运动总时间T是通过给定的加速度和速度计算出来的。

以上三种模式，均需要根据已知的值确定加速、匀速、减速的阶段，在确定过程中，可能在某个阶段，不是完全按照加速、匀速、减速的方式进行，譬如，没有匀速阶段，直接加速然后就需要减速了。所有的这些情况，在以上三种模式的基本公式中可以计算得到。

更多文章，请关注公众号：