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

一. 固定加/减速时间和匀速速度的模式

初始速度为$v_0$ = 0, 结束速度为$v_1$ = 0；初始时间$t_0$ = 0。
假设：加速时间、减速时间分别为$T_a$、$T_d$，匀速时速度为$v_v$。

1. 加速阶段

加速时间区间为[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}$$

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

$$\{\begin{array}{l}{a}_0=q_0\\ a_1=0\\ a_2=\frac{v_v}{2T_a}\end{array}$$

• 计算过程中设定$v_0$ = 0

2. 匀速阶段

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

$$\{\begin{array}{l}q(t)=b_0+b_{1}t\\ \dot{q}(t)=b_1\\ \ddot{q}(t)=0\end{array}$$

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

$$\{\begin{array}{l}{b}_1=v_v\\ b_0=q_0-\frac{v_v{T}_a}{2}\end{array}$$

3. 减速阶段

减速时间区间为[$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}$$

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

$$\{\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}$$

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

• q0 = 0, q1 = 30; t0 = 0, t1 = 4; Ta = 1, $v_v$ = 10

关于上面$T_a$和$v_v$的值是怎么确定的，如下推导过程：

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

$$a_a{T}_a=\frac{q_m-q_a}{T_m-T_a}$$

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

$$\{\begin{array}{l}{q}_a=q(t_0+T_a)\\ q_m=\frac{(q_1+q_0)}{2}=q_0+\frac{h}{2}\\ T_m=\frac{(t_1-t_0)}{2}=\frac{T}{2}\end{array}$$

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

$$q_a=q_0+\frac{1}{2}{a}_a{T}_a^2$$

将以上等式、$q_m=\frac{(q_1+q_0)}{2}$、$T_m=\frac{(t_1-t_0)}{2}$代入，可得以下等式：

$$a_a{T}_a^2-a_a(t_1-t_0)T_a+(q_1-q_0)=0$$

同时，因为速度曲线的面积=$q_1-q_0$，由此可得等式：

$$v_v=\frac{q_1-q_0}{t_1-t_0-T_a}=\frac{h}{T-T_a}$$

二. 预先指定加速度的模式

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

$$T_a=\frac{a_a(t1-t0)-\sqrt{a_a^2(t1-t0{)}^2-4a_a(q1-q0)}}{2a_a}$$

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

$$a_a⩾\frac{4(q_1-q_0)}{(t_1-t_0{)}^2}$$

由以上表达式，可知，在此种模式下，可以得到最小加速度为$a_a=\frac{4h}{T^2}$，此时，加速时间为$T_a=\frac{1}{2}(t_1-t_0)$

三. 预先指定加速度和速度的模式

假设：$a_a=a_{max}$，$v_v=v_{max}$

则可以得到如下等式：

$$\{\begin{array}{l}{T}_a=\frac{v_{max}}{a_{max}},加速时间\\ v_{max}(T-T_a)=q_1-q_0=h,位移\\ T=\frac{h{a}_{max}+v_{max}^2}{a_{max}{v}_{max}},总时间\end{array}$$

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

$$q(t)=\{\begin{array}{l}{q}_0+\frac{1}{2}{a}_{max}(t-t_0{)}^2,t_0\le t\le t_0+T_a\\ q_0+a_{max}{T}_a(t-t_0-\frac{T_a}{2}),t_0+T_a\le t\le t_1-T_a\\ q_1-\frac{1}{2}{a}_{max}(t_1-t{)}^2,t_1-T_a\le t\le t_1\end{array}$$

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

$$h⩾\frac{v_{max}^2}{a_{max}}$$

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

$$\{\begin{array}{l}{T}_a=\sqrt{\frac{h}{a_{max}}},加速时间\\ T=2T_a,总时间\\ v_{max}=a_{max}{T}_a=\sqrt{a_{max}h}=\frac{h}{T_a},最大速度\end{array}$$

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

$$q(t)=\{\begin{array}{l}{q}_0+\frac{1}{2}{a}_{max}(t-t_0{)}^2,t_0\le t\le t_0+T_a\\ q_1-\frac{1}{2}{a}_{max}(t_1-t{)}^2,t_1-T_a\le t\le t_1\end{array}$$

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

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

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