（一）双S型速度规划 原理

一、双S型速度规划解决了梯形加速度曲线不连续的问题

梯形速度规划的加速度曲线是不连续的，这对于系统是有害的。从机械系统的角度来说，不连续的加速度会产生不必要的冲击力，使得机械系统产生抖动甚至破坏。双S型速度规划采用两段线性区间组合的方法解决加速度不连续的问题，线性段端点处采用抛物线过渡的方法，保证连接处的加速度曲线连续。若一个系统能够提供足够大的加加速度，双S型速度规划将会等同于梯形速度规划。另外，在相同的距离和期望速度下，双S型速度规划耗费的时间更多[1]，计算也更复杂：

二、双S型速度规划是什么？

双S型速度规划是一种速度规划方式，其目的是为了减少对机械系统造成的影响。也叫铃型规划（Bell）七段S型速度规划。速度曲线如下图所示：

观察上述速度曲线图，可以将整个运动可分为A M D三个过程，分别是：

• 加速阶段（Acceleration Phase, AP）
• 最大速度阶段（Maximum Phase, MP)
• 减速阶段（Deceleration Phase, DP）

AP段和DP段端点处采用抛物线进行拟合，避免了加速度突变。因为AP DP段速度曲线的形状像英文字母中的“S”所以叫做双S型速度规划，事实上，只要存在一个"S"就能称为S型速度规划。

三、规划参数及各阶段特征、规划基本条件

3.1 规划参数

规划参数是对速度轨迹形状的约束，可分为用户参数和系统参数，其中用户参数一般不作修改，为了达到规定的用户参数，系统参数可能会修调。用户参数指的是起始位置及速度：

• 整个规划端点处位移（$q_0$ $q_1$）
• 整个规划端点处速度（$v_0$ $v_1$）

系统参数是指：

• 速度上下限（$v_{max}$ $v_{min}$）
• 最大加速度（$a_{max}$ $a_{min}$）
• 最大加加速度（$j_{max}$ $j_{min}$）

绝大多数时候，系统参数会采用“对称”规划，即：
$$\begin{gathered} j_{min}=-j_{max} \\ {a}_{min}=-a_{max} \\ {v}_{min}=-v_{max}\text{\hspace{1em}(3.1)} \end{gathered}$$
文章的讨论都是基于(3.1)的对称规划进行的讨论。

3.2 各阶段的特征

我们先讨论位移增大的情况($q_1>q_0$)，在文章的最后我们将介绍位移减少情况（$q_1<q_0$），因为我们规定：

• $t\in [0,T_a)$：加速阶段（AP），加速度主要特征是线性变化，大小从$0\to a_{max}\to 0$；

• $t\in [T_a,T_a+T_v)$：匀速阶段（MP），速度的特征是大小保持不变；

• $t\in [T_a+Tv,T]$：减速阶段（DP），加速度主要特征仍然是线性变化，大小从$0\to a_{min}\to 0$，其中$T=T_a+T_v+T_d$ 。

3.3 规划基本条件

不是所有的规划参数都能进行S型规划，在极限情况下，仅存在单一加速或减速段，也就是一正一负加加速度脉冲。规定至少完成一个“S”曲线为规划基本条件，记S型曲线单个脉冲耗时为$T_j^{\ast }$，根据运行过程中加速度能否到达最大加速度有，分为两种情况：

• $T_j^{\ast }=\sqrt{\frac{|v_1-v_0|}{j_{max}}}$，没有达到最大加速度$a_{max}$；
• $T_j^{\ast }=\frac{a_{max}}{j_{max}}$，达到最大加速度；

请注意，这里讨论的是系统参数对称的。

写成一个式子：
T j ∗ = m i n { ∣ v 1 − v 0 ∣ j m a x , a m a x j m a x } (3.2) T_j^*=min\{\sqrt{\frac{|v_1-v_0|}{j_{max}},}\frac{a_{max}}{j_{max}}\}\tag{3.2} Tj∗​=min{jmax​∣v1​−v0​∣​, ​jmax​amax​​}(3.2)

可规划位移约束条件，不等式成立是因为将加加速度看成无穷大，即一开始就达到最大加速度：
$$q_1-q_0>\{\begin{array}{rl}& T_j^{\ast }(v_0+v_1)\\ & \\ & \frac{1}{2}(v_0+v_1)[T_j^{\ast }+\frac{|v_1-v_0|}{a_{max}}]\end{array}\text{\hspace{1em}(3.3)}$$
文中可能会出现以下参数记号：

• $T_{j1}$：第一组高、低脉冲时间；
• $T_{j2}$：第二组高、低脉冲时间；
• $T_a$：加速段时间（$T_a\ge 2T_{j1}$）；
• $T_d$：减速段时间（$T_d\ge 2T_{j2}$）；
• $T_v$：匀速阶段；
• $T$：规划总时间（$T=T_a+T_d+T_v$）；
• $v_{lim}=max(\dot{q}(t))$ 运行的最大速度，如果有匀速段也称为运行速度；
• $a_{lim}=max(\ddot{q}(t))$运行的最大加速度。

规划完成后，$v_{lim}$ $a_{lim}$这两个参数在最好情况下等于系统最大速度 $v_{max}$和系统最大加速度$a_{max}$；他可能根据用户参数进行调整，实际可能小于系统最大速度 $v_{max}$和系统最大加速度$a_{max}$[3]。

S型规划的最终的目标合理分配A.M.D三个阶段的时间，完成指定用户期望速度。

四、双S曲线时间参数的计算

完成双S型速度规划的关键在于计算时间参数，分别是$T_{j1}$，$T_{j2}$ ，$T_a$，$T_d$，$T_v$。时间参数计算分为两情况：

• $v_{lim}=v_{max}$
• $v_{lim}<v_{vmax}$

4.1 $T_v$段存在，$v_{lim}=v_{max}$

假设$T_v$段存在，计算$T_{j1}$ $T_{j2}$ 。计算这两个参数之前，需要对Jerk作用段（$T_a$ $T_d$）是否达到最大加速度进行判断：

• $T_a$段参数计算
  $$(v_{max}-v_0)j_{max}<a_{max}^2\text{\hspace{1em}(4.1)}$$

（4.1）成立，即没有达到规划的最大加速度，没有匀加速段：
$$T_{j1}=\sqrt{\frac{v_{max}-v_0}{j_{max}}}，T_a=2T_{j1}\text{\hspace{1em}(4.2)}$$
（4.1）不成立，则规划可能达到最大加速度：
$$T_{j1}=\frac{a_{max}}{j_{max}},T_a=T_{j1}+\frac{v_{max}-v_0}{a_{max}}\text{\hspace{1em}(4.3)}$$

• $T_d$段参数计算
  $$(v_{max}-v_1)j_{max}<a_{max}^2\text{\hspace{1em}(4.4)}$$
  （4.4）成立，即规划没有达到最大加速度，没有匀加速段：
  $$T_{j2}=\sqrt{\frac{v_{max}-v_1}{j_{max}}}，T_d=2T_{j2}\text{\hspace{1em}(4.5)}$$
  （4.4）不成立，即规划可能达到最大加速度，：
  $$T_{j2}=\frac{a_{max}}{j_{max}},T_d=T_{j2}+\frac{v_{max}-v_1}{a_{max}}\text{\hspace{1em}(4.6)}$$

这样一来，$T_v$为：
$$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}})\text{\hspace{1em}(4.7)}$$

如果$T_v\ge 0$参数计算完成；若$T_v<0$则说明假设不成功，需要进一步计算参数。

4.2 $T_v$段不存在，$v_{lim}<v_{max}$

这种情况下，$T_v=0$。假设两个阶段的系统最大加速度$a_{max}$都存在，那么对应单脉时间分别为：
$$T_{j1}=T_{j2}=T_j=\frac{a_{max}}{j_{max}}\text{\hspace{1em}(4.8)}$$

$$T_a=\frac{\frac{a_{max}^2}{j_{max}}-2v_0+\sqrt{\Delta }}{2a_{max}}\text{\hspace{1em}(4.9)}$$

$$T_d=\frac{\frac{a_{max}^2}{j_{max}}-2v_1+\sqrt{\Delta }}{2a_{max}}\text{\hspace{1em}(4.10)}$$
其中
$$\Delta =\frac{a_{max}^4}{j_{max}^2}+2(v_0^2+v_1^2)+a_{max}\left(4(q_1-q_0)-2\frac{a_{max}}{j_{max}}(v_0+v_1))\right)\text{\hspace{1em}(4.11)}$$

公式（4.9 ~ 4.11）完成了在最大加速度存在的情况下的轨迹规划[2]。

• 假设成立，$T_a\ge 2T_{j1}$ 且 $T_d\ge 2T_{j2}$，参数计算完成；
• 假设不成立，$T_a<2T_{j1}$或$T_d<2T_{j2}$，需要进一步计算参数。假设失败意味着至少AP段和DP段至少一个不能到达$a_{max}$，这种情况非常罕见，这需要对降低规划的加速度要求，一种可行的方法是：按比例衰减加速度
  $$a_{max}=\gamma a_{max}0<\gamma <1\text{\hspace{1em}(4.12)}$$
  后，根据公式（4.8 ~ 4.11）计算并重新验证条件是否满足，若满足退出，规划完成，反之继续衰减加速度。

在衰减的过程中，可能出现$T_a<0$或者$T_d<0$，这意味着AP和DP段不存在，即对应的$T_a=0$ $T_b=0$。

在迭代过程中若出现，$T_a<0$，计算方法如下：
$$T_d=2\frac{q_1-q_0}{v_1+v_0}\text{\hspace{1em}(4.13)}$$

$$T_{j_2}=\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)}\text{\hspace{1em}(4.14)}$$

同样的，若$T_d<0$：
$$T_a=2\frac{q_1-q_0}{v_1+v_0}\text{\hspace{1em}(4.15)}$$

$$T_{j_1}=\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)}\text{\hspace{1em}(4.16)}$$

至此，时间参数计算完成。

4.3 计算实际运行参数$v_{max}$ $a_{max}$

至此五个时间参数$T_{j1}$，$T_{j2}$ ，$T_a$，$T_d$，$T_v$均已确定，为了保证S型规划始终可用，可能做了如下修改：

• $v_{max}$降至$v_{lim}$
• $a_{max}$降至$a_{lim}$

其实规划参数$v_{max}$ $a_{max}$理解为期望参数更为合适，降低后的参数$v_{lim}$ $a_{lim}$更像是妥协后的实际运行参数。根据时间参数可以计算实际运行参数$v_{lim}$ $a_{lim}$：
$$a_{li{m}_a}=j_{max}{T}_{j1}{a}_{li{m}_d}=j_{max}{T}_{j2}\text{\hspace{1em}(4.17)}$$

$$v_{lim}=v_0+(T_a-T_{j1})a_{li{m}_a}=v_1-(T_d-T_{j2})a_{li{m}_d}\text{\hspace{1em}(4.18)}$$

五、关于时间参数的分段表达式

时间参数计算完成，关于S型规划就算完成了绝大部分。运用微积分的可求解各部分的分段表达式：

• 加速阶段 AP
  a）$t\in [0,T_{j1})$
  $$\{\begin{array}{rl}& q(t)=q_0+v_{0}t+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{array}\text{\hspace{1em}(5.1)}$$
  b）$t\in [T_{j1},T_a-T_{j1})$
  $$\{\begin{array}{rl}& q(t)=q_0+v_{0}t+\frac{a_{li{m}_a}}{6}(3t^2-3T_{j1}t+T_{j1}^2)\\ & \dot{q}(t)=v_0+a_{li{m}_a}(t-\frac{T_{j1}}{2})\\ & \ddot{q}(t)=j_{max}{T}_{j1}=a_{li{m}_a}\\ & q^{(3)}(t)=0\end{array}\text{\hspace{1em}(5.2)}$$
  c）$t\in [T_a-T_{j1},T_a)$
  $$\{\begin{array}{rl}& q(t)=q_0+(v_{lim}+v_0)\frac{T_a}{2}-v_{lim}(T_a-t)-j_{min}\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{array}\text{\hspace{1em}(5.3)}$$
  匀速阶段 MP
  $t\in [T_a,T_a+T_v)$
  $$\{\begin{array}{rl}& 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{array}\text{\hspace{1em}(5.4)}$$
  减速阶段 DP
  a）$t\in [T-T_d,T-T_d+T_{j2})$
  $$\{\begin{array}{rl}& 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_{min}=-j_{max}\end{array}\text{\hspace{1em}(5.5)}$$
  b）$T\in [T-T_d+T_{j2},T-T_{j2})$
  $$\{\begin{array}{rl}& q(t)=q_1-(v_{lim}+v_1)\frac{T_d}{2}+v_{lim}(t-T+T_d)+\frac{a_{li{m}_d}}{6}\left(3(t-T+T_d{)}^2-3T_{j2}(t-T+T_d)+T_{j2}^2\right)\\ & \dot{q}(t)=v_{lim}+a_{li{m}_d}(t-T+T_d-\frac{T_{j2}}{2})\\ & \ddot{q}(t)=-j_{max}(t-T+T_d)\\ & q^{(3)}(t)=0\end{array}\text{\hspace{1em}(5.6)}$$
  c）$t\in [T-T_{j2},T]$
  $$\{\begin{array}{rl}& 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{array}\text{\hspace{1em}(5.7)}$$

六、考虑 $q_1<q_0$

为了统一两种情况，需要进行以下处理：
首先，转换初始值参数：
$$q_0=\sigma {\hat{q}}_0{q}_1=\sigma {\hat{q}}_1{v}_0=\sigma {\hat{v}}_0{v}_1=\sigma {\hat{v}}_1\text{\hspace{1em}(6.1)}$$
其中，$$\sigma =sign({\hat{q}}_1-{\hat{q}}_0)\text{\hspace{1em}(6.2)}$$

其次，转换规划参数：
$$\{\begin{array}{rl}& 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}\\ & a_{max}=\frac{(\sigma +1)}{2}{\hat{a}}_{max}+\frac{(\sigma -1)}{2}{\hat{a}}_{min}\\ & a_{min}=\frac{(\sigma +1)}{2}{\hat{a}}_{min}+\frac{(\sigma -1)}{2}{\hat{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{v}}_{min}+\frac{(\sigma -1)}{2}{\hat{v}}_{max}\end{array}\text{\hspace{1em}(6.3)}$$
最后，输出转换：
$$\{\begin{array}{rl}& \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)}=\sigma q^{(3)}(t）\end{array}\text{\hspace{1em}(6.4)}$$

参考文章

[1] 《多轴插补为什么普遍使用梯形速度曲线？》https://www.zhihu.com/question/47474538/answer/106250869
[2] 田军锋, 林浒, 姚壮, et al. 数控系统S型曲线加减速快速规划研究[J]. 小型微型计算机系统, 2013(01):168-172.
[3] Biagiotti, Luigi, Melchiorri, Claudio. Trajectory Planning for Automatic Machines and Robots[J]. 2009.

---

推导提示1：假如系统可以提供无穷大的 $j_{max}$ ，运动的规划将会变成加速度始终为 $a_{max}$ 的匀加速运动，对应的时间 $t_{ideal}=(v_{max}-v_0)/a_{max}$，实际上，假设所有的时间都用来进行加加速，对应的时间为 $t=a_{max}/j_{max}$，因为 $t_{ideal}<t$，所以有：$(v_{max}-v_0)j_{max}<a_{max}^2$

推导提示2： 一个物体的初速度为 $v_0,a_0=0$，以恒定加加速度 $j$ 加速至 $v_1,a_{max}$ ，理论上最短时间为 $\sqrt{\frac{|v_1-v_0|}{j_{max}}}$ 。推导提示：$a_{max}=2\cdot j_{max}\Delta v$，$\Delta v=\frac{1}{2}{a}_{max}\cdot t$。