S型速度规划
摘要
梯形速度曲线出现加速度不连续的情形，这样会导致机械系统出现冲击或不可预料的振动效应。因此有必要定义一种更加平滑的运动曲线，例如采用连续的，线性的加速度曲线，如下图所示，速度曲线抛物线过渡的线段组成。由于速度曲线的形状，这个轨迹称为双S速度曲线。这个轨迹有七段加加速度恒定的轨迹组成，因此又叫七段式轨迹。
分析讨论
假设
j m i n = − j m a x , a m i n = − a m a x , v m i n = − v m a x j_{min}=-j_{max},\text a_{min}=-\text a_{max},v_{min}=-\text v_{max} jmin=−jmax,amin=−amax,vmin=−vmax
为了方便分析，首先假设 t 0 = 0 , q 1 > q 0 t_0=0,q_1>q_0 t0=0,q1>q0。那么边界条件为
- 初始速度和终止速度为 v 0 , v 1 v_0,v_1 v0,v1.
- 初始和终止加速度 a 0 , a 1 \text a_0,\text a_1 a0,a1均为0.
曲线可以分为三段
- 加速段， t ∈ [ 0 , T a ] t \in [0,T_a] t∈[0,Ta],加速度从0线性增加到最大，再减小到0.
- 最大速度段， t ∈ [ T a , T a + T v ] t\in [T_a,T_a+T_v] t∈[Ta,Ta+Tv],具有恒定的速度。
- 减速段， t ∈ [ T a + T v , T ] t\in [T_a+T_v,T] t∈[Ta+Tv,T],这里 T = T a + T v + T d T=T_a+T_v+T_d T=Ta+Tv+Td,减速度曲线与加速段相反。
给定最大的加加速度 j m a x j_{max} jmax,加速度和期望位移 h = q 1 − q 0 h=q_1-q_0 h=q1−q0.轨迹的计算按照式（3.30a）-(3.30g)。
第一步是检验轨迹是否真的存在。实际上轨迹根据给定参数需要分几种情况计算。比如，给定位移h太小时，速度无法从 v 0 v_0 v0变化到 v 1 v_1 v1。这样的限制下可能只有加速段（ v 0 < v 1 v_0<v_1 v0<v1）或减速段( v 0 > v 1 v_0>v_1 v0>v1)。
因此首先需要检验是否同时存在加速段和减速段。定义
T j ∗ = min ( ∣ v 1 − v 0 ∣ j m a x , a m a x j m a x ) (3.17) T_j^*=\text {min} \left (\sqrt {\frac {|v_1-v_0|}{j_{max}}},\frac{ \text a_{max} }{j_{max}} \right ) \tag{3.17} Tj∗=min(jmax∣v1−v0∣,jmaxamax)(3.17)
若 T j ∗ = a m a x / j m a x T_j^*=\text a_{max}/j_{max} Tj∗=amax/jmax,加速度可以达到最大值，有可能存在加加速度为0的线段。
如果满足以下条件，则轨迹可行
q 1 − q 0 > { T j ∗ ( v 0 + v 1 ) , if T j ∗ < a m a x j m a x 1 2 ( v 0 + v 1 ) ( T j ∗ + ∣ v 1 − v 0 ∣ a m a x ) , if T j ∗ = a m a x j m a x (3.18) q_1-q_0>\begin{cases} T_j^*(v_0+v_1), \text{if} \ T_j^*<\frac{\text a_{max}}{j_{max}}\\ \frac{1}{2}(v_0+v_1)\left ( T_j^*+\frac{|v_1-v_0|}{\text a_{max}} \right),\text{if}\ T_j^*=\frac{\text a_{max}}{j_{max}} \tag{3.18} \end{cases} q1−q0>{Tj∗(v0+v1),if Tj∗<jmaxamax21(v0+v1)(Tj∗+amax∣v1−v0∣),if Tj∗=jmaxamax(3.18)
若上式满足，则可以计算轨迹的参数，否则给定的参数都无法在指定的位移从初始速度变化到末速度（这时候可以改变初始速度或末速度的值，可另外讨论。这不在本文的讨论范围内）。假设运动中的最大速度为 v l i m = max ( q ˙ ( t ) ) v_{lim}=\text {max}(\dot q(t)) vlim=max(q˙(t)),这时存在两种情况
Case1 : v l i m = v m a x v_{lim}=v_{max} vlim=vmax.
Case2: v l i m < v m a x v_{lim}<v_{max} vlim<vmax.
第二种情形，最大速度不能达到，只有加速段和减速段，没有匀速段，只有计算轨迹参数后才能判断。
当给定位移较小时，在Case1和Case2中有可能都达不到最大加速度。
我们定义
T j 1 T_{j1} Tj1 ：在加速度段加加速度恒定（ j m a x 或 j m i n j_{max}或j_{min} jmax或jmin）的时间；
T j 2 T_{j2} Tj2：在减速段加加速度恒定（ j m a x 或 j m i n j_{max}或j_{min} jmax或jmin）的时间；
T a T_a Ta：加速度段时间；
T v T_v Tv：匀速段时间；
T d T_d Td：减速段时间；
T T T：总时间（ = T a + T v + T d =T_a+T_v+T_d =Ta+Tv+Td）
Case1: v l i m = v m a x v_{lim}=v_{max} vlim=vmax
通过下面的条件判断是否达到最大加速度
if
( v m a x − v 0 ) j m a x < a m a x 2 (3.19) (v_{max}-v_0)j_{max}<\text a^2_{max} \tag{3.19} (vmax−v0)jmax<amax2(3.19)
a m a x \text a_{max} amax达不到
if
( v m a x − v 1 ) j m a x < a m a x 2 (3.20) (v_{max}-v_1)j_{max}<\text a^2_{max} \tag{3.20} (vmax−v1)jmax<amax2(3.20)
若(3.19)满足，则加速段的时间用下式计算
T j 1 = v m a x − v 0 j m a x , T a = 2 T j 1 (3.21) T_{j1}=\sqrt{\frac{v_{max}-v_0}{j_{max}}},T_a=2T_{j1}\tag{3.21} Tj1=jmaxvmax−v0,Ta=2Tj1(3.21)
否则
T j 1 = a m a x j m a x , T a = T j 1 + v m a x − v 0 a m a x (3.22) T_{j1}=\frac{\text a_{max}}{j_{max}},T_a=T_{j1}+\frac{v_{max}-v_0}{\text a_{max}}\tag{3.22} Tj1=jmaxamax,Ta=Tj1+amaxvmax−v0(3.22)
若（3.20）满足，则减速段的时间用下式计算
T j 2 = v m a x − v 1 j m a x , T d = 2 T j 2 (3.23) T_{j2}=\sqrt {\frac{v_{max}-v_1}{j_{max}}},T_{d}=2T_{j2}\tag{3.23} Tj2=jmaxvmax−v1,Td=2Tj2(3.23)
否则
T j 2 = a m a x j m a x , T d = T j 2 + v m a x − v 1 a m a x (3.24) T_{j2}=\frac{\text a_{max}}{j_{max}},T_d=T_{j2}+\frac{v_{max}-v_1}{\text a_{max}}\tag{3.24} Tj2=jmaxamax,Td=Tj2+amaxvmax−v1(3.24)
最后，计算匀速段时间
T v = q 1 − q 0 v m a x − T a 2 ( 1 + v 0 v m a x ) − T d 2 ( 1 + v 1 v m a x ) (3.25) T_v=\frac{q_1-q_0}{v_{max}}-\frac{T_a}{2}(1+\frac{v_0}{v_{max}})-\frac{T_d}{2}(1+\frac{v_1}{v_{max}})\tag{3.25} Tv=vmaxq1−q0−2Ta(1+vmaxv0)−2Td(1+vmaxv1)(3.25)
对 T v T_v Tv进行讨论：
（1）若 T v > 0 T_v>0 Tv>0，则达到最大速度。这时直接可以用式（3.21）-式（3.25）计算轨迹的值。
（2）若 T v < 0 T_v<0 Tv<0，只是意味着 v l i m v_{lim} vlim小于 v m a x v_{max} vmax。这时候考虑Case2.
例子**Example3.9 **：如下面是case1达到最大速度的一个例子，给定的条件如下
边界条件： q 0 = 0 , q 1 = 10 , v 0 = 1 , v 1 = 0 q_0=0,q_1=10,v_0=1,v_1=0 q0=0,q1=10,v0=1,v1=0,
约束条件： v m a x = 5 , a m a x = 10 , j m a x = 30 v_{max}=5,a_{max}=10,j_{max}=30 vmax=5,amax=10,jmax=30.
计算得到轨迹的各段时间为
Tj1: 0.3333Ta: 0.7333Tv: 1.1433Tj2: 0.3333Td: 0.8333T: 2.7100q0: 0q1: 10v0: 1v1: 0vlim: 5alima: 10alimd: -10jmax: 30
轨迹的图形如下图所示
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-jVMAGVRb-1602424996880)(image/Example3.9.png)]
Case2: v l i m < v m a x v_{lim}<v_{max} vlim<vmax
在这种情形下不能达到指定最大速度，所以匀速段的时间（ T v = 0 T_v=0 Tv=0 ）。
若最大加速度和最小加速度都能达到，那么各段的时间可以按下式计算
T j 1 = T j 2 = T j = a m a x j m a x (2.26a) T_{j1}=T_{j2}=T_j=\frac{\text a_{max}}{j_{max}}\tag{2.26a} Tj1=Tj2=Tj=jmaxamax(2.26a)
T a = a m a x 2 j m a x − 2 v 0 + Δ 2 a m a x (2.26b) T_a=\frac{\frac{\text a_{max}^2}{j_{max}}-2v_0+\sqrt{\Delta}}{2\text a_{max}} \tag{2.26b} Ta=2amaxjmaxamax2−2v0+Δ(2.26b)
T d = a m a x 2 j m a x − 2 v 1 + Δ 2 a m a x (2.26c) T_d=\frac{\frac{\text a_{max}^2}{j_{max}}-2v_1+\sqrt{\Delta}}{2\text a_{max}} \tag{2.26c} Td=2amaxjmaxamax2−2v1+Δ(2.26c)
其中
Δ = a m a x 4 j m a x 2 + 2 ( v 0 2 + v 1 2 ) + a m a x ( 4 ( q 1 − q 0 ) − 2 a m a x j m a x ( v 0 + v 1 ) ) (2.27) \Delta=\frac{\text a_{max}^4}{j_{max}^2}+2(v_0^2+v_1^2)+\text a_{max}(4(q_1-q_0)-2\frac{\text a_{max}}{j_{max}}(v_0+v_1))\tag{2.27} Δ=jmax2amax4+2(v02+v12)+amax(4(q1−q0)−2jmaxamax(v0+v1))(2.27)
对 T a , T d T_a,T_d Ta,Td进行讨论
{ T a ≥ 2 T j T d ≥ 2 T j \begin{cases} T_a\ge2T_j\\ T_d\ge 2T_j \end{cases} {Ta≥2TjTd≥2Tj
（1）若上式成立，则加速段和减速段都能达到最大加速度。
例子Example3.10下面是case2加速段和减速段都能达到最大加速度的一个例子。
边界条件： q 0 = 0 , q 1 = 10 , v 0 = 1 , v 1 = 0 , q_0=0,q_1=10,v_0=1,v_1=0, q0=0,q1=10,v0=1,v1=0,
约束条件： v m a x = 10 , a m a x = 10 , j m a x = 30. v_{max}=10,\text a_{max}=10,j_{max}=30. vmax=10,amax=10,jmax=30.
规划得到轨迹的结果为
Tj1: 0.3333Ta: 1.0747Tv: 0Tj2: 0.3333Td: 1.1747T: 2.2494q0: 0q1: 10v0: 1v1: 0vlim: 8.4136alima: 10alimd: -10jmax: 30
最大速度为 8.4136
轨迹的图形如下图所示
（2）否则，加速度段和减速段至少有一段不能达到最大加速度。
这时就不能使用式（3.26a），（3.26b），（3.26c）计算轨迹的各段时间。这种情形（实际上是很少见的）要规划轨迹的参数是相当困难的，而寻找一种近似的方案是很方便的，这种方案不是最优的，但是从计算的角度可以接受。这种方式是逐渐减小 a m a x \text a_{max} amax（假设 a m a x = γ a m a x , 0 < γ < 1 \text a_{max}=\gamma \text a_{max},0<\gamma<1 amax=γamax,0<γ<1）,然后用（3.26a），（3.26b），（3.26c）计算各段时间，直到条件 T a > 2 T j ， T d > 2 T j T_a>2T_j，T_d>2T_j Ta>2Tj，Td>2Tj都成立。但是该方式就有一个缺点，需要循环计算以找到合适的 a m a x \text a_{max} amax，计算量有所增加。如何确定 γ \gamma γ以达到合理的计算量和精度是需要考虑的，本博客提供的程序中取 γ = 0.99 \gamma=0.99 γ=0.99。
例子Example3.11：下面是该情形的一个例子。
边界条件： q 0 = 0 , q 1 = 10 , v 0 = 7 , v 1 = 0 , q_0=0,q_1=10,v_0=7,v_1=0, q0=0,q1=10,v0=7,v1=0,
约束条件： v m a x = 10 , a m a x = 10 , j m a x = 30. v_{max}=10,\text a_{max}=10,j_{max}=30. vmax=10,amax=10,jmax=30.
规划得到轨迹的结果为(注意 γ \gamma γ取不同的值，结果会有所差异，以下结果中取 γ = 0.99 \gamma=0.99 γ=0.99)
Tj1: 0.2321Ta: 0.4666Tv: 0Tj2: 0.2321Td: 1.4718T: 1.9384q0: 0q1: 10v0: 7v1: 0vlim: 8.6329alima: 6.9641alimd: -6.9641jmax: 30
轨迹的图形如下图所示
另外，在计算（3.26a），（3.26b），（3.26c）时可能会出现 T a < 0 T_a<0 Ta<0或 T d < 0 T_d<0 Td<0。这时候只有加速段或只有减速段，这取决于初速度的大小。
当 T a < 0 T_a<0 Ta<0时（只有 v 0 > v 1 v_0>v_1 v0>v1时出现），令 T a = 0 T_a=0 Ta=0，减速段参数按下式计算
T d = 2 q 1 − q 0 v 0 + v 1 (3.28a) T_d=2\frac{q_1-q_0}{v_0+v_1}\tag{3.28a}\\ Td=2v0+v1q1−q0(3.28a)
T j 2 = j m a x ( q 1 − q 0 ) − j m a x ( j m a x ( q 1 − q 0 ) 2 + ( v 1 + v 0 ) 2 ( v 1 − v 0 ) ) j m a x ( v 1 + v 0 ) (3.28b) T_{j2}=\frac{j_{max}(q_1-q_0)-\sqrt{j_{max}(j_{max}(q_1-q_0)^2+(v_1+v_0)^2(v_1-v_0))}}{j_{max}(v_1+v_0)} \tag{3.28b} Tj2=jmax(v1+v0)jmax(q1−q0)−jmax(jmax(q1−q0)2+(v1+v0)2(v1−v0))(3.28b)
当 T d < 0 T_d<0 Td<0时（只有 v 0 < v 1 v_0<v_1 v0<v1时出现），令 T d = 0 T_d=0 Td=0，加速段参数按下式计算
T a = 2 q 1 − q 0 v 0 + v 1 (3.29a) T_a=2\frac{q_1-q_0}{v_0+v_1}\tag{3.29a}\\ Ta=2v0+v1q1−q0(3.29a)
T j 2 = j m a x ( q 1 − q 0 ) − j m a x ( j m a x ( q 1 − q 0 ) 2 + ( v 1 + v 0 ) 2 ( v 1 − v 0 ) ) j m a x ( v 1 + v 0 ) (3.29b) T_{j2}=\frac{j_{max}(q_1-q_0)-\sqrt{j_{max}(j_{max}(q_1-q_0)^2+(v_1+v_0)^2(v_1-v_0))}}{j_{max}(v_1+v_0)} \tag{3.29b} Tj2=jmax(v1+v0)jmax(q1−q0)−jmax(jmax(q1−q0)2+(v1+v0)2(v1−v0))(3.29b)
例子Example3.12：下面是 T a = 0 T_a=0 Ta=0的一个例子。
边界条件： q 0 = 0 , q 1 = 10 , v 0 = 7.5 , v 1 = 0 , q_0=0,q_1=10,v_0=7.5,v_1=0, q0=0,q1=10,v0=7.5,v1=0,
约束条件： v m a x = 10 , a m a x = 10 , j m a x = 30. v_{max}=10,\text a_{max}=10,j_{max}=30. vmax=10,amax=10,jmax=30.
规划得到轨迹的结果为(注意 γ \gamma γ取不同的值，结果会有所差异，以下结果中取 γ = 0.99 \gamma=0.99 γ=0.99)
Tj1: 0Ta: 0Tv: 0Tj2: 0.0973Td: 2.6667T: 2.6667q0: 0q1: 10v0: 7.5000v1: 0vlim: 7.5000alima: 0alimd: -2.9190jmax: 30
轨迹的图形如下图所示
计算 q 1 > q 0 q_1>q_0 q1>q0的轨迹
一旦各段的时间长度和相关参数确定后，就可以使用下面的方程计算S型轨迹。这里假设 t 0 = 0 t_0=0 t0=0。
加速段
a) t ∈ [ 0 , T j 1 ] t\in[0,T_{j1}] t∈[0,Tj1]
{ q ( t ) = q 0 + v 0 t + j m a x t 3 6 q ˙ ( t ) = v 0 + j m a x t 2 2 q ¨ ( t ) = j m a x t q ( 3 ) ( t ) = j m a x (3.30a) \begin{cases} q(t)=q_0+v_0t+j_{max}\frac{t^3}{6}\\ \dot q(t)=v_0+j_{max}\frac{t^2}{2}\\ \ddot q(t)=j_{max}t\\ q^{(3)}(t)=j_{max} \end{cases} \tag{3.30a} ⎩ ⎨ ⎧q(t)=q0+v0t+jmax6t3q˙(t)=v0+jmax2t2q¨(t)=jmaxtq(3)(t)=jmax(3.30a)
b) t ∈ [ T j 1 , T a − T j 1 ] t\in[T_{j1},T_a-T_{j1}] t∈[Tj1,Ta−Tj1]
{ q ( t ) = q 0 + v 0 t + a l i m a 6 ( 3 t 3 − 3 T j 1 t + T j 1 2 ) q ˙ ( t ) = v 0 + a l i m a ( t − T j 1 2 ) q ¨ ( t ) = j m a x T j 1 = a l i m a q ( 3 ) ( t ) = 0 (3.30b) \begin{cases} q(t)=q_0+v_0t+\frac{\text a_{lima}}{6}(3t^3-3T_{j1}t+T_{j1}^2)\\ \dot q(t)=v_0+\text a_{lima}(t-\frac{T_{j1}}{2})\\ \ddot q(t)=j_{max}T_{j1}=\text a_{lima}\\ q^{(3)}(t)=0 \end{cases} \tag{3.30b} ⎩ ⎨ ⎧q(t)=q0+v0t+6alima(3t3−3Tj1t+Tj12)q˙(t)=v0+alima(t−2Tj1)q¨(t)=jmaxTj1=alimaq(3)(t)=0(3.30b)
c) t ∈ [ T a − T j 1 , T a ] t\in[T_a-T_{j1},T_a] t∈[Ta−Tj1,Ta]
{ q ( t ) = q 0 + ( v l i m + v 0 ) T a 2 − v l i m ( T a − t ) − j m a x ( T a − t ) 3 6 q ˙ ( t ) = v l i m + j m i n ( T a − t ) 2 2 q ¨ ( t ) = − j m i n ( T a − t ) q ( 3 ) ( t ) = j m i n = − j m a x (3.30c) \begin{cases} q(t)=q_0+(v_{lim}+v_0)\frac{T_a}{2}-v_{lim}(T_a-t)-j_{max}\frac{(T_a-t)^3}{6}\\ \dot q(t)=v_{lim}+j_{min}\frac{(T_a-t)^2}{2}\\ \ddot q(t)=-j_{min}(T_a-t)\\ q^{(3)}(t)=j_{min}=-j_{max} \end{cases} \tag{3.30c} ⎩ ⎨ ⎧q(t)=q0+(vlim+v0)2Ta−vlim(Ta−t)−jmax6(Ta−t)3q˙(t)=vlim+jmin2(Ta−t)2q¨(t)=−jmin(Ta−t)q(3)(t)=jmin=−jmax(3.30c)
匀速段
a) t ∈ [ T a , T a + T v ] t\in[T_a,T_a+T_v] t∈[Ta,Ta+Tv]
{ q ( t ) = q 0 + ( v l i m + v 0 ) T a 2 + v l i m ( t − T a ) q ˙ ( t ) = v l i m q ¨ ( t ) = 0 q ( 3 ) ( t ) = 0 (3.30d) \begin{cases} q(t)=q_0+(v_{lim}+v_0)\frac{T_a}{2}+v_{lim}(t-T_a)\\ \dot q(t)=v_{lim}\\ \ddot q(t)=0\\ q^{(3)}(t)=0 \end{cases} \tag{3.30d} ⎩ ⎨ ⎧q(t)=q0+(vlim+v0)2Ta+vlim(t−Ta)q˙(t)=vlimq¨(t)=0q(3)(t)=0(3.30d)
减速段
a) t ∈ [ T − T d , T − T d + T j 2 ] t\in[T-T_d,T-T_d+T_{j2}] t∈[T−Td,T−Td+Tj2]
{ q ( t ) = q 1 − ( v l i m + v 1 ) T d 2 + v l i m ( t − T + T d ) − j m a x ( t − T + T d ) 3 6 q ˙ ( t ) = v l i m − j m a x ( t − T + T d ) 2 2 q ¨ ( t ) = − j m a x ( t − T + T d ) q ( 3 ) ( t ) = j m a x = − j m a x (3.30e) \begin{cases} q(t)=q_1-(v_{lim}+v_1)\frac{T_d}{2}+v_{lim}(t-T+T_d)-j_{max}\frac{(t-T+T_d)^3}{6}\\ \dot q(t)=v_{lim}-j_{max}\frac{(t-T+T_d)^2}{2}\\ \ddot q(t)=-j_{max}(t-T+T_d)\\ q^{(3)}(t)=j_{max}=-j_{max} \end{cases} \tag{3.30e} ⎩ ⎨ ⎧q(t)=q1−(vlim+v1)2Td+vlim(t−T+Td)−jmax6(t−T+Td)3q˙(t)=vlim−jmax2(t−T+Td)2q¨(t)=−jmax(t−T+Td)q(3)(t)=jmax=−jmax(3.30e)
b) t ∈ [ T − T d + T j 2 , T − T j 2 ] t\in[T-T_{d}+T_{j2},T-T_{j2}] t∈[T−Td+Tj2,T−Tj2]
{ q ( t ) = q 1 − ( v l i m + v 1 ) T d 2 + v l i m ( t − T + T d ) + a l i m d 6 ( 3 ( t − T + T d ) 2 − 3 T j 2 ( t − T + T d ) + T j 2 2 ) q ˙ ( t ) = v l i m + a l i m d ( t − T + T d − T j 2 2 ) q ¨ ( t ) = − j m a x T j 2 = a l i m d q ( 3 ) ( t ) = 0 (3.30f) \begin{cases} q(t)=q_1-(v_{lim}+v_1)\frac{T_d}{2}+v_{lim}(t-T+T_d)+ \frac{\text a_{limd}}{6}(3(t-T+T_d)^2-3T_{j2}(t-T+T_d)+T_{j2}^2)\\ \dot q(t)=v_{lim}+\text a_{limd}(t-T+T_d-\frac{T_{j2}}{2})\\ \ddot q(t)=-j_{max}T_{j2}=\text a_{limd}\\ q^{(3)}(t)=0 \end{cases} \tag{3.30f} ⎩ ⎨ ⎧q(t)=q1−(vlim+v1)2Td+vlim(t−T+Td)+6alimd(3(t−T+Td)2−3Tj2(t−T+Td)+Tj22)q˙(t)=vlim+alimd(t−T+Td−2Tj2)q¨(t)=−jmaxTj2=alimdq(3)(t)=0(3.30f)
c) t ∈ [ T − T j 2 , T ] t\in[T-T_{j2},T] t∈[T−Tj2,T]
{ q ( t ) = q 1 − v 1 ( T − t ) − j m a x ( T − t ) 3 6 q ˙ ( t ) = v 1 + j m a x ( t − T ) 2 2 q ¨ ( t ) = − j m a x ( T − t ) q ( 3 ) ( t ) = j m a x (3.30g) \begin{cases} q(t)=q_1-v_1(T-t)-j_{max}\frac{(T-t)^3}{6}\\ \dot q(t)=v_1+j_{max}\frac{(t-T)^2}{2}\\ \ddot q(t)=-j_{max}(T-t)\\ q^{(3)}(t)=j_{max} \end{cases} \tag{3.30g} ⎩ ⎨ ⎧q(t)=q1−v1(T−t)−jmax6(T−t)3q˙(t)=v1+jmax2(t−T)2q¨(t)=−jmax(T−t)q(3)(t)=jmax(3.30g)
计算 q 1 < q 0 q_1<q_0 q1<q0的轨迹
当 q 1 < q 0 q_1<q_0 q1<q0，轨迹的参数按照上面的步骤计算，只是需要考虑始末的位置/速度符号相反，并且计算后需要对位置曲线、速度曲线、加速度曲线和加加速度曲线进行反转。
更一般地，设给定的始末位置和速度为 ( q ^ 0 , q ^ 1 , v ^ 0 , v ^ 1 ) (\hat q_0,\hat q_1,\hat v_0,\hat v_1) (q^0,q^1,v^0,v^1)，为了计算轨迹，需要把这些值转化为
q 0 = σ q ^ 0 , q 1 = σ q ^ 1 , v 0 = σ v ^ 0 , v 1 = σ v ^ 1 (3.31) q_0=\sigma \hat q_0,q_1=\sigma \hat q_1,v_0=\sigma \hat v_0,v_1=\sigma\hat v_1 \tag{3.31} q0=σq^0,q1=σq^1,v0=σv^0,v1=σv^1(3.31)
这里 σ = sign ( q ^ 1 − q ^ 0 ) \sigma=\text {sign}(\hat q_1-\hat q_0) σ=sign(q^1−q^0)。相似的，对速度，加速度，加加速度 ( v ^ m a x , v ^ m i n , a ^ m a x , a ^ m i n , j ^ m a x , j ^ m i n ) (\hat v_{max},\hat v_{min},\hat {\text a}_{max},\hat {\text a}_{min},\hat j_{max},\hat j_{min}) (v^max,v^min,a^max,a^min,j^max,j^min)的约束也需要进行转换
{ v m a x = ( σ + 1 ) 2 v ^ m a x + ( σ − 1 ) 2 v ^ m i n v m i n = ( σ + 1 ) 2 v ^ m i n + ( σ − 1 ) 2 v ^ m a x a m a x = ( σ + 1 ) 2 a ^ m a x + ( σ − 1 ) 2 a ^ m i n a m i n = ( σ + 1 ) 2 a ^ m i n + ( σ − 1 ) 2 a ^ m a x j m a x = ( σ + 1 ) 2 j ^ m a x + ( σ − 1 ) 2 j ^ m i n j m i n = ( σ + 1 ) 2 j ^ m i n + ( σ − 1 ) 2 j ^ m a x (3.32) \begin{cases} v_{max}=\frac{(\sigma+1)}{2}\hat v_{max}+\frac{(\sigma-1)}{2}\hat v_{min}\\ v_{min}=\frac{(\sigma+1)}{2}\hat v_{min}+\frac{(\sigma-1)}{2}\hat v_{max}\\ \text a_{max}=\frac{(\sigma+1)}{2}\hat {\text a}_{max}+\frac{(\sigma-1)}{2}\hat {\text a}_{min}\\ \text a_{min}=\frac{(\sigma+1)}{2}\hat {\text a}_{min}+\frac{(\sigma-1)}{2}\hat {\text a}_{max}\\ j_{max}=\frac{(\sigma+1)}{2}\hat j_{max}+\frac{(\sigma-1)}{2}\hat j_{min}\\ j_{min}=\frac{(\sigma+1)}{2}\hat j_{min}+\frac{(\sigma-1)}{2}\hat j_{max}\\ \end{cases} \tag{3.32} ⎩ ⎨ ⎧vmax=2(σ+1)v^max+2(σ−1)v^minvmin=2(σ+1)v^min+2(σ−1)v^maxamax=2(σ+1)a^max+2(σ−1)a^minamin=2(σ+1)a^min+2(σ−1)a^maxjmax=2(σ+1)j^max+2(σ−1)j^minjmin=2(σ+1)j^min+2(σ−1)j^max(3.32)
最后，计算的曲线也需要转换为
{ q ^ ( t ) = σ q ( t ) q ^ ˙ ( t ) = σ q ˙ ( t ) q ^ ¨ ( t ) = σ q ¨ ( t ) q ^ ( 3 ) ( t ) = σ q ( 3 ) ( t ) (3.33) \begin{cases} \hat q(t)=\sigma q(t)\\ \dot{\hat q}(t)=\sigma \dot q(t)\\ \ddot {\hat q}(t)=\sigma \ddot q(t)\\ \hat q^{(3)}(t)=\sigma q^{(3)}(t) \end{cases} \tag{3.33} ⎩ ⎨ ⎧q^(t)=σq(t)q^˙(t)=σq˙(t)q^¨(t)=σq¨(t)q^(3)(t)=σq(3)(t)(3.33)
流程图
综上所述，S型加减速的流程图如下
算法程序实现
前面进行了分类讨论，终于到程序实现这激动人心的时刻了。不过别高兴太早，下面只提供matlab的实现程序，不过该程序是按照c/c++的风格编写，可以轻易改写为c/c++程序。
**欢迎读者对程序进行无情的测试，让bug的暴风雨来得更猛烈些！ **
S型速度规划的算法子函数：CalcFun.m
%{
CalcFun.m
S型速度规划的算法子函数
%}
%%
%子函数声明，可供外部文件调用
function fun=CalcFun
fun.displacement=@displacement;
fun.velocity=@velocity;
fun.acceleration=@acceleration;
fun.jerk=@jerk;
fun.CalcSProfile=@CalcSProfile;
fun.InitialParam=@InitialParam;
end
%%
%子函数定义
%{
根据式(3.30a)-(3.30g)计算任意时刻的位移
%}
function q=displacement(t,Param)
q0=Param.q0;
q1=Param.q1;
v0=Param.v0;
v1=Param.v1;
vlim=Param.vlim;
alima=Param.alima;
alimd=Param.alimd;
jmax=Param.jmax;
Tj1=Param.Tj1;
Ta=Param.Ta;
Tv=Param.Tv;
Tj2=Param.Tj2;
Td=Param.Td;
T=Param.T;
jmin=Param.jmin;
if(t>=0 && t<Tj1)q=q0+v0*t+jmax*t^3/6;
elseif(t>=Tj1 && t<Ta-Tj1) q=q0+v0*t+alima/6*(3*t^2-3*Tj1*t+Tj1^2);
elseif(t>=Ta-Tj1 && t<Ta)q=q0+(vlim+v0)*Ta/2-vlim*(Ta-t)-jmin*(Ta-t)^3/6;
elseif(t>=Ta && t<Ta+Tv)q=q0+(vlim+v0)*Ta/2+vlim*(t-Ta);
elseif(t>=T-Td && t<T-Td+Tj2)q=q1-(vlim+v1)*Td/2+vlim*(t-T+Td)-jmax*(t-T+Td)^3/6;
elseif(t>=T-Td+Tj2 && t<T-Tj2)q=q1-(vlim+v1)*Td/2+vlim*(t-T+Td)+alimd/6*(3*(t-T+Td)^2-3*Tj2*(t-T+Td)+Tj2^2);
elseq=q1-v1*(T-t)-jmax*(T-t)^3/6;
end
%计算（3.33）q=Param.sigma*q;
end%{
根据式(3.30a)-(3.30g)计算任意时刻的速度
%}
function dq=velocity(t,Param)v0=Param.v0;
v1=Param.v1;
vlim=Param.vlim;
alima=Param.alima;
alimd=Param.alimd;
jmax=Param.jmax;
Tj1=Param.Tj1;
Ta=Param.Ta;
Tv=Param.Tv;
Tj2=Param.Tj2;
Td=Param.Td;
T=Param.T;
jmin=Param.jmin;
if(t>=0 && t<Tj1)dq=v0+jmax*t^2/2;
elseif(t>=Tj1 && t<Ta-Tj1) dq=v0+alima*(t-Tj1/2);
elseif(t>=Ta-Tj1 && t<Ta)dq=vlim+jmin*(Ta-t)^2/2;
elseif(t>=Ta && t<Ta+Tv)dq=vlim;
elseif(t>=T-Td && t<T-Td+Tj2)dq=vlim-jmax*(t-T+Td)^2/2;
elseif(t>=T-Td+Tj2 && t<T-Tj2)dq=vlim+alimd*(t-T+Td-Tj2/2);
elsedq=v1+jmax*(T-t)^2/2;
end
%计算（3.33）
dq=Param.sigma*dq;
end%{
根据式(3.30a)-(3.30g)计算任意时刻的加速度
%}
function ddq=acceleration(t,Param)alima=Param.alima;
alimd=Param.alimd;
jmax=Param.jmax;
Tj1=Param.Tj1;
Ta=Param.Ta;
Tv=Param.Tv;
Tj2=Param.Tj2;
Td=Param.Td;
T=Param.T;
jmin=Param.jmin;
if(t>=0 && t<Tj1)ddq=jmax*t;
elseif(t>=Tj1 && t<Ta-Tj1) ddq=alima;
elseif(t>=Ta-Tj1 && t<Ta)ddq=-jmin*(Ta-t);
elseif(t>=Ta && t<Ta+Tv)ddq=0;
elseif(t>=T-Td && t<T-Td+Tj2)ddq=-jmax*(t-T+Td);
elseif(t>=T-Td+Tj2 && t<T-Tj2)ddq=alimd;
elseddq=-jmax*(T-t);
end
%计算（3.33）
ddq=Param.sigma*ddq;
end%{
根据式(3.30a)-(3.30g)计算任意时刻的加加速度
%}
function jt=jerk(t,Param)jmax=Param.jmax;
Tj1=Param.Tj1;
Ta=Param.Ta;
Tv=Param.Tv;
Tj2=Param.Tj2;
Td=Param.Td;
T=Param.T;
jmin=Param.jmin;
if(t>=0 && t<Tj1)jt=jmax;
elseif(t>=Tj1 && t<Ta-Tj1) jt=0;
elseif(t>=Ta-Tj1 && t<Ta)jt=jmin;
elseif(t>=Ta && t<Ta+Tv)jt=0;
elseif(t>=T-Td && t<T-Td+Tj2)jt=jmin;
elseif(t>=T-Td+Tj2 && t<T-Tj2)jt=0;
elsejt=jmax;
end
%计算（3.33）
jt=Param.sigma*jt;
end%{
（1）参数合法性检查
（2）初始化参数结构体
%}
function IniParam=InitialParam(q0,q1,v0,v1,v_max,a_max,j_max)
%参数检查，绝对值小于1.0E-8的数值认为是0.
if(abs(v_max)<1.0E-8)disp('参数错误。最大速度不能为0！程序退出。');return;
end
if(abs(a_max)<1.0E-8)disp('参数错误。最大加速度不能为0！程序退出。');return;
end
if(abs(j_max)<1.0E-8)disp('参数错误。最大加加速度不能为0！程序退出。');return;
end
if(abs(q1-q0)<1.0E-8)disp('参数错误。输入位移不能为0！程序退出。');return;
end
%%
%定义并初始化输入参数的结构体
if(q1-q0>0)sigma=1;
elsesigma=-1;
end
v_min=-v_max;
j_min=-j_max;
a_min=-a_max;
%计算（3.31），
hq0=sigma*q0;
hq1=sigma*q1;
hv0=sigma*v0;
hv1=sigma*v1;
%计算（3.32）
hv_max=(sigma+1)/2*v_max+(sigma-1)/2*v_min;
hv_min=(sigma+1)/2*v_min+(sigma-1)/2*v_max;
ha_max=(sigma+1)/2*a_max+(sigma-1)/2*a_min;
ha_min=(sigma+1)/2*a_min+(sigma-1)/2*a_max;
hj_max=(sigma+1)/2*j_max+(sigma-1)/2*j_min;
hj_min=(sigma+1)/2*j_min+(sigma-1)/2*j_max;
IniParam=struct(...
'q0',{hq0},...
'q1',{hq1},...
'v0',{hv0},...
'v1',{hv1},...
'vmax',{hv_max},...
'vmin',{hv_min},...
'amax',{ha_max},...
'amin',{ha_min},...
'jmax',{hj_max},...
'jmin',{hj_min},...
'sigma',{sigma}...
);
end%{
根据输入参数进行S型速度规划，输出结果Param
%}
function Param=CalcSProfile(q0,q1,v0,v1,v_max,a_max,j_max)%初始化参数
InParam=InitialParam(q0,q1,v0,v1,v_max,a_max,j_max);
%定义并初始化存储规划结果的结构体
Param=struct(...'Tj1',{0},'Ta',{0},'Tv',{0},'Tj2',{0},'Td',{0},'T',{0},...'q0',{0},...'q1',{0},...
'v0',{0},...
'v1',{0},...
'vlim',{0},...
'alima',{0},...
'alimd',{0},...
'jmax',{0},...
'jmin',{0},...
'sigma',{InParam.sigma}...
);
Param.q0=InParam.q0;
Param.q1=InParam.q1;
Param.v0=InParam.v0;
Param.v1=InParam.v1;
Param.jmax=InParam.jmax;
Param.jmin=InParam.jmin;
%%
%计算（3.17）和（3.18）
T1=sqrt(abs(InParam.v1-InParam.v0)/InParam.jmax);
T2=InParam.amax/InParam.jmax;
Tjs=min(T1,T2);
if(T1<=T2)Dq=InParam.q1-InParam.q0;if(Dq<Tjs*(InParam.v0+InParam.v1))disp('位移过小，不存在满足始末速度的轨迹！程序退出。');return;end
elseDq=InParam.q1-InParam.q0;if(Dq<0.5*(InParam.v0+InParam.v1)*(Tjs+abs(InParam.v1-InParam.v0)/InParam.amax))disp('位移过小，不存在满足始末速度的轨迹！程序退出。');return;end
end
%%
%输入参数正确误（即轨迹存在），分类讨论
if((InParam.vmax-InParam.v0)*InParam.jmax<InParam.amax^2)%(3.19)满足,amax不能达到Tj1=sqrt((InParam.vmax-InParam.v0)/InParam.jmax);Ta=2*Tj1;Param.alima=InParam.jmax*Tj1;
else%(3.19)不满足，amax能达到Tj1=InParam.amax/InParam.jmax;Ta=Tj1+(InParam.vmax-InParam.v0)/InParam.amax;Param.alima=InParam.amax;
endif((InParam.vmax-InParam.v1)*InParam.jmax<InParam.amax^2)%(3.20)满足,amin不能达到Tj2=sqrt((InParam.vmax-InParam.v1)/InParam.jmax);Td=2*Tj2;Param.alimd=-InParam.jmax*Tj2;
else%(3.20)不满足,amin能达到Tj2=InParam.amax/InParam.jmax;Td=Tj2+(InParam.vmax-InParam.v1)/InParam.amax;Param.alimd=-InParam.amax;
end%计算（3.25）
Tv=(InParam.q1-InParam.q0)/InParam.vmax-Ta/2*(1+InParam.v0/InParam.vmax)-...Td/2*(1+InParam.v1/InParam.vmax);
if(Tv>0)%case1,最大速度能达到Param.vlim=InParam.vmax;Param.Tj1=Tj1;Param.Ta=Ta;Param.Tj2=Tj2;Param.Td=Td; Param.T=Ta+Tv+Td;Param.Tv=Tv;return;
else% case2,最大速度不能达到Tv=0;Param.Tv=Tv;%计算（3.26a）,(3.27),（3.26b）,（3.26c）Tj=InParam.amax/InParam.jmax;Tj1=Tj;Tj2=Tj;Delta=InParam.amax^4/InParam.jmax^2+2*(InParam.v0^2+InParam.v1^2)+InParam.amax*...(4*(InParam.q1-InParam.q0)-2*InParam.amax/InParam.jmax*(InParam.v0+InParam.v1));Ta=(InParam.amax^2/InParam.jmax-2*InParam.v0+sqrt(Delta))/(2*InParam.amax);Td=(InParam.amax^2/InParam.jmax-2*InParam.v1+sqrt(Delta))/(2*InParam.amax);if(Ta>2*Tj && Td>2*Tj)%加速段和减速段都能达到最大加速度Param.Tj1=Tj1;Param.Tj2=Tj2;Param.Ta=Ta;Param.Td=Td;Param.T=Ta+Tv+Td;Param.alima=InParam.amax;Param.alimd=-InParam.amax;Param.vlim=InParam.v0+(Ta-Tj1)*Param.alima;return;else%至少有一段不能达到最大加速度gamma=0.99;amax=InParam.amax;%逐渐减小最大加速度约束while(Ta<2*Tj || Td<2*Tj)if(Ta>0 && Td>0)amax=gamma*amax;%循环计算（3.26a）,(3.27),（3.26b）,（3.26c）Tj=amax/InParam.jmax;Tj1=Tj;Tj2=Tj;Delta=amax^4/InParam.jmax^2+2*(InParam.v0^2+InParam.v1^2)+amax*...(4*(InParam.q1-InParam.q0)-2*amax/InParam.jmax*(InParam.v0+InParam.v1));Ta=(amax^2/InParam.jmax-2*InParam.v0+sqrt(Delta))/(2*amax);Td=(amax^2/InParam.jmax-2*InParam.v1+sqrt(Delta))/(2*amax); else%出现Ta或Td小于0if(Ta<=0)Ta=0;Tj1=0;%计算（3.28a）Td=2*(InParam.q1-InParam.q0)/(InParam.v0+InParam.v1);%计算（3.28b）num=InParam.jmax*(InParam.q1-InParam.q0)-...sqrt(InParam.jmax*(InParam.jmax*(InParam.q1-InParam.q0)^2+...(InParam.v1+InParam.v0)^2*(InParam.v1-InParam.v0)));den=InParam.jmax*(InParam.v1+InParam.v0);Tj2=num/den;elseif(Td<=0)Td=0;Tj2=0;%计算（3.29a）Ta=2*(InParam.q1-InParam.q0)/(InParam.v0+InParam.v1);%计算（3.29b）num=InParam.jmax*(InParam.q1-InParam.q0)-...sqrt(InParam.jmax*(InParam.jmax*(InParam.q1-InParam.q0)^2-...(InParam.v1+InParam.v0)^2*(InParam.v1-InParam.v0)));den=InParam.jmax*(InParam.v1+InParam.v0);Tj1=num/den;end Param.Tj1=Tj1;Param.Tj2=Tj2;Param.Ta=Ta;Param.Td=Td;Param.T=Ta+Tv+Td;Param.alima=InParam.jmax*Tj1;Param.alimd=-InParam.jmax*Tj2;Param.vlim=InParam.v0+(Ta-Tj1)* Param.alima;return; end endParam.Tj1=Tj1;Param.Tj2=Tj2;Param.Ta=Ta;Param.Td=Td;Param.T=Ta+Tv+Td;Param.alima=InParam.jmax*Tj1;Param.alimd=-InParam.jmax*Tj2;Param.vlim=InParam.v0+(Ta-Tj1)* Param.alima;return; end
endend
算法测试及结果显示程序：SProfileTest.m
%{
%SProfileTest.m
S型速度规划测试程序，2019.02.12，Brian
（1）输入轨迹的边界条件及约束条件；
（2）绘制速度规划的结果。
%}
clc
clear
close('all')
%%
%定义轨迹边界条件及约束条件
q0=10;
q1=0;
v0=-7;
v1=0;
v_max=10;
a_max=10;
j_max=30;
%%
%调用算法子函数，进行速度规划，得到规划结果
fun=CalcFun();
Param=fun.CalcSProfile(q0,q1,v0,v1,v_max,a_max,j_max);
%%
%根据规划结果，调用算法子函数计算轨迹
Ts=0.001;
i=0;
for t=0:Ts:Param.Ti=i+1; time(i)=i*Ts;dis(i)=fun.displacement(t,Param);vel(i)=fun.velocity(t,Param);acc(i)=fun.acceleration(t,Param);jerk(i)=fun.jerk(t,Param);
end
%%
%绘图
figure
%位置
subplot(4,1,1)
plot(time,dis,'r','LineWidth',1.5);
grid on
ylabel('position')
%速度
subplot(4,1,2)
plot(time,vel,'b','LineWidth',1.5);
grid on
hold on
ylabel('velocity')
v_ref1=v_max*ones(1,length(time));
v_ref2=-v_max*ones(1,length(time));
plot(time,v_ref1,'b--','LineWidth',1.5);
plot(time,v_ref2,'b--','LineWidth',1.5);
%加速度
subplot(4,1,3)
plot(time,acc,'g','LineWidth',1.5);
grid on
hold on
ylabel('acceleration')
a_ref1=a_max*ones(1,length(time));
a_ref2=-a_max*ones(1,length(time));
plot(time,a_ref1,'g--','LineWidth',1.5);
plot(time,a_ref2,'g--','LineWidth',1.5);
%加加速度
subplot(4,1,4)
plot(time,jerk,'m','LineWidth',1.5)
grid on
hold on
ylabel('jerk')
j_ref1=j_max*ones(1,length(time));
j_ref2=-j_max*ones(1,length(time));
plot(time,j_ref1,'m--','LineWidth',1.5);
plot(time,j_ref2,'m--','LineWidth',1.5);
%结束
小结
上述的S型速度规划参考文献[1]实现，算法实现的逻辑清晰，实现简单，可以应用在实际的数控系统中。但是其中存在两个缺点：
（1）在case2中出现无法达到给定最大加速度时，通过减小最大加速度约束来规划轨迹，这使得所得的轨迹通常不是最优，即在指定的参数范围内，不是时间最短的轨迹。
（2）在给定的位移较小，无法达到给定的始末速度时，即式（3.18）不成立，该算法不能进行处理。这需要用户进行额外的处理，重新给定参数，使得轨迹存在。
特别是使用该算法进行速度前瞻时要处理好这两个问题，否则可能导致运动控制系统性能和稳定性下降。
关于第（1）个问题，参考文献[2]-[4]有所涉及，综合这几篇文献，是可以解决缺点（1）的，使得规划的S曲线始终是最优，这当然会增加算法逻辑的复杂度。如何解决这两个问题，使得该算法始具有更好的性能和稳定性，且听下回分解。
参考文献
[1]Biagiotti L, Melchiorri C. Trajectory Planning for Automatic Machines and Robots[M]. Springer Berlin Heidelberg, 2009.
[2]Erkorkmaz K, Altintas Y. High speed CNC system design. Part I: jerk limited trajectory generation and quintic spline interpolation[J]. International Journal of Machine Tools & Manufacture, 2001,41(9):1323-1345.
[3]商允舜. CNC数控系统加减速控制方法研究与实现[D]. 浙江大学机械制造及自动化, 2006.
[4]郭新贵, 李从心. S曲线加减速算法研究[J]. 机床与液压, 2002(05):60-62.
[5]张碧陶, 高伟强, 沈列, 等. S曲线加减速控制新算法的研究[J]. 机床与液压, 2009,37(10):27-29.
[6]何均, 游有鹏, 陈浩, 等. S形加减速的嵌套式前瞻快速算法[J]. 航空学报, 2010,31(4):842-851.
