轨迹规划 - 梯形速度分布

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

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

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

1. 加速阶段

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

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

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

$\begin{cases} a_0 = q_0 \\ a_1 = 0 \\ a_2 = \cfrac{v_v}{2T_a} \end{cases}$

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

2. 匀速阶段

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

$\begin{cases} q(t) = b_0 + b_1t \\ \dot{q}(t) = b_1 \\ \ddot{q}(t) = 0 \end{cases}$

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

$\begin{cases} b_1 = v_v \\ b_0 = q_0 - \cfrac{v_vT_a}{2} \end{cases}$

3. 减速阶段

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

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

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

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

根据以上公式，针对两点之间的轨迹规划，使用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_aT_a = \frac{q_m - q_a}{T_m - T_a}$

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

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

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

$q_a = q_0 + \frac{1}{2}a_aT_a^2$

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

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

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

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

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

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

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

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

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

$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_0$到$q_1$的运动总时间T是通过给定的加速度和速度计算出来的。

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

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