梯形速度分布的轨迹规划，从本质上来说，是一个分段函数的轨迹规划，基本的方式是一个先加速，再匀速，再减速的三段函数的过程。当然，当间隔时间太短时，会出现分段函数只有加速和减速，无匀速的情况。或者因为开始速度和结束速度不相等，出现加速和减速过程不对称的情况。在梯形速度分布中，都需要根据实际的状况，分别的全面考虑所有的情况。
一. 固定加/减速时间和匀速速度的模式
初始速度为
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+a1t+a2t2q˙(t)=a1+2a2tq¨(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=q0a1=0a2=2Tavv
- 计算过程中设定 v 0 v_0 v0 = 0
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+b1tq˙(t)=b1q¨(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=vvb0=q0−2vvTa
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+c1t+c2t2q˙(t)=c1+2c2tq¨(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−2Tavvt12c1=Tavvt1c2=−2Tavv
根据以上公式，针对两点之间的轨迹规划，使用matlab实现的关节角度、关节速度、关节加速度曲线如下所示：
- q0 = 0, q1 = 30; t0 = 0, t1 = 4; Ta = 1, v v v_v vv = 10
关于上面 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} aaTa=Tm−Taqm−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+2hTm=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+21aaTa2
将以上等式、 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 aaTa2−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−Taq1−q0=T−Tah
二. 预先指定加速度的模式
如果指定加速度 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=2aaaa(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=amaxvmax,加速时间vmax(T−Ta)=q1−q0=h,位移T=amaxvmaxhamax+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+21amax(t−t0)2,t0≤t≤t0+Taq0+amaxTa(t−t0−2Ta),t0+Ta≤t≤t1−Taq1−21amax(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⩾amaxvmax2
如果不满足以上限制，则可以得到如下等式：
{ 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=amaxh,加速时间T=2Ta,总时间vmax=amaxTa=amaxh=Tah,最大速度
因为不满足限制条件，此时没有匀速阶段，此时的通用表达式为：
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+21amax(t−t0)2,t0≤t≤t0+Taq1−21amax(t1−t)2,t1−Ta≤t≤t1
通过以上推导可知，如果使用此种模式，从 q 0 q_0 q0到 q 1 q_1 q1的运动总时间T是通过给定的加速度和速度计算出来的。
以上三种模式，均需要根据已知的值确定加速、匀速、减速的阶段，在确定过程中，可能在某个阶段，不是完全按照加速、匀速、减速的方式进行，譬如，没有匀速阶段，直接加速然后就需要减速了。所有的这些情况，在以上三种模式的基本公式中可以计算得到。
更多文章，请关注公众号：
8872
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
