軌跡規劃 - 梯形速度分佈

梯形速度分佈的軌跡規劃，從本質上來說，是一個分段函數的軌跡規劃，基本的方式是一個先加速，再勻速，再減速的三段函數的過程。當然，當間隔時間太短時，會出現分段函數只有加速和減速，無勻速的情況。或者因爲開始速度和結束速度不相等，出現加速和減速過程不對稱的情況。在梯形速度分佈中，都需要根據實際的狀況，分別的全面考慮所有的情況。

一. 固定加/減速時間和勻速速度的模式

初始速度爲$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是通過給定的加速度和速度計算出來的。

以上三種模式，均需要根據已知的值確定加速、勻速、減速的階段，在確定過程中，可能在某個階段，不是完全按照加速、勻速、減速的方式進行，譬如，沒有勻速階段，直接加速然後就需要減速了。所有的這些情況，在以上三種模式的基本公式中可以計算得到。

更多文章，請關注公衆號：