单段时间最优S型速度规划算法

一，背景

在做机械臂轨迹规划的单段路径的速度规划时，除了参考《Trajectory Planning for Automatic Machines and Robots》等文献之外，还在知乎找到了这位大佬 @韩冰 写的在线规划方法：
https://zhuanlan.zhihu.com/p/585253101/editzhuanlan.zhihu.com/p/585253101/edit

从其输入条件和结果来看，在线规划的方法还是有一些精度、特殊条件情况的问题。

另外还有两个库，Reflexxes Motion Libraries 和 Ruckig ，它们都有开源的内容和商业化的内容。但是受限于我的开发平台，很难去调用、复现它们。

而以上文献中的离线规划方法限制性更大，只能实现起始结束条件速度加速度都为0的情况。

本文在以上文献中的离线规划方法的基础上，实现了更广泛的适用条件，并且在求解速度、最优解选择上进行了一定的优化。最终达到了以下效果：
1，可任意设置起始、结束状态的速度、加速度。可以等于或不为0、大于0、小于0、超出限制值等。
2，本文方法的主要思路还是和以上文献里的迭代搜索一样，设定在迭代终止条件匀速段时间小于1ms的情况下，各种测试用例的迭代次数最大12次，总的运算耗时少于20us。
3，迭代搜索实际上会有多个区间，各自有最优值。本文方法并没有求出所有解，但是根据公式特性尽可能的选择了一种最优解。

二，问题描述

可能对“速度规划”感兴趣的同学多少都了解这个问题的定义，但我在这里还是简单描述一下。

**已知：**一段路径有起点、终点（它无关坐标系、也无关路径形状轨迹），起点的位置值Ps，终点的位置值Pe。起点的速度Vs、加速度As，终点的速度Ve、加速度Ae，并限制整段路径的速度绝对值不超过Vlim，加速度绝对值不超过Alim，加加速度的绝对值不超过Jlim。

**求解：**时间最优的速度规划结果。

怎样理解这个“规划结果”呢？按照《Trajectory Planning … 》的内容，其求解结果应当包含7段位置关于时间的分段函数，一般情况如下图所示：

它由7段分段函数构成，如下：

加速区间（加加速段——匀加速段——减加速段）

匀速区间 ( Ta <= t <= Ta + Tv )

减速区间（加减速段——匀减速段——减减速段）



它可以理解为一系列的“时间-位置”插值点。在线规划方法的结果可以是一种计算方法，能够在已知当前点求出下一个周期时刻的位置点。而离线规划方法的结果也是一种计算方法/或数据，它能够用一系列的分段函数来表示位置关于时间的曲线函数。

再次强调下，
1，此类方法中速度规划的输入的位置是一段或多段路径的曲线距离，它无关坐标系、也无关路径形状轨迹。可以认为是一维的曲线距离关于时间的规划函数求解。
2，在插补运动中的应用场景可以参考后文——空间三维插补和速度规划。
3，而路径和速度规划的关系可以理解为：路径是上层确定的输入，在做速度规划时不会改变其路径；路径就像是铁轨，而速度规划则是控制铁轨上的火车时快时慢的沿着铁轨的运动。而无论火车跑得快或慢，都不影响铁轨的形状。

该问题的输入在应用上还有一点注意事项：路径和自由度的不同。
1，自由度可以理解为一个直线电机，它可以在导轨上任意的正向/负向运动，也就是说问题的输入Ps,Pe,Vs,Ve等可以是任意的正负值。
2，但是路径不同，路径可以认为是基于参数从0到100%的描述，这意味着其位置值是在路径的维度里相对0%点的曲线距离。
3，那么在应用上，路径的距离不能为负数，因为从0%往-100%的路径是不确定的，所以负的距离也失去了意义。
4，在大多数应用上，速度也最好没有负数，因为负数的速度意味着运动点将在中间一段路径上折返多次，这对于大多数应用是有害的。

三，梯形速度规划

首先我们简化问题，引入梯形速度规划，也可称为三段式。与如上问题的条件一样，但是不对加加速度进行限制，那么意味着允许加速度可以限制值内任意的快速变化。此部分可以直接参考教科书的3.2节。书中一般的规划结果如下图：

在此引入3段式速度规划，是因为它与7段式的问题有很多相似相关的地方，三段式的求解方式大致是：
1，假定规划的最大速度是否能达到速度限制值，若能达到则可根据理想公式（梯形模型）求解。
2，若不能达到则加速段的结尾速度应该等于减速段的起始速度，即最大速度，可根据三角形模型求解。

**相似性：**可以认为这个最大速度是一个未知数，计算出其他所需的物理量，得到代数方程，再求解这个方程即可。区别只是梯形规划的方程很简单，而S型规划的方程较复杂，但是整个流程是可以借鉴的。

**相关性：**S型规划可以分为3大段（加速段-匀速段-减速段），而其中的加速大段或减速大段每段里面又是3小段，在“速度——加速度——加加速度”层面的“加速度增大——加速度恒定——加速度减小”的过程，类似于三段式“位置——速度——加速度”层面的“速度增大——速度恒定——速度减小”的对应关系。计算公式也很类似。

三段式求解
按照以上求解方式，计算流程中主要存在分支[1]：按梯形模型计算 或者 按三角形模型计算。实际应用上还要考虑分支[2]：梯形/三角的开口方向是向上的还是向下的。开口方向并不是显然的根据距离的正负而决定的，例如以下两个例子：

例1：PosEnd - PosStart = 10.0; 结果如下，蓝色曲线为位置关于时间的变化，绿色曲线为速度关于时间的变化。

例2：PosEnd - PosStart = 5.0; 其他参数和例1完全相同，结果如下：

可以看到例1例2的距离都是正数，但是速度曲线的三角型开口一个朝上一个朝下。

我们可以用以下这种方式来判别速度梯形/三角形的开口方向：
1，先只做速度关于时间的规划，假设以最大加速度从起始速度规划到结束速度(速度曲线是一条直线，斜率为最大加速度)，可以计算出此规划的运动距离。
2，若假设的运动距离 < 结束位置-起始位置，则说明还存在速度为正的区间，而为了时间最优，这个正速度应当尽量的大，即应当等于速度三角型模型的尖角速度或梯形模型的平台速度，同时也说明梯形/三角模型的开口应该朝下。
3，若假设的运动距离 > 结束位置-起始位置，则说明还存在速度为负的区间，而为了时间最优，这个负速度绝对值应当尽量的大，即应当等于速度三角型模型的尖角速度或梯形模型的平台速度，同时也说明梯形/三角模型的开口应该朝上。

整体解算过程如下：
1，首先判断1：若 VelStart > VelEnd，则预设加速度 Acc’ 为 -LimAcc，且；否则预设加速度 Acc’ 为 LimAcc。
2，然后只做从 VelStart 到 VelEnd 的匀变速规划，根据以下公式 1 计算预设的运动距离 Dist：
$$Dist=\frac{{VelEnd}^2-{VelStart}^2}{2Acc}\text{\hspace{1em}(1)}$$

3，判断2-1：若 Dist <= PosEnd - PosStart，则梯形/三角模型开口朝下，接下来判断是梯形模型还是三角模型.假设是梯形模型，根据以下公式 2 代入 Acc’ = -LimAcc, Vel’ = LimVel 计算匀速段耗时：
$$t2=\frac{PosEnd-PosStart}{Ve{l}^{\prime }}-\frac{{Ve{l}^{\prime }}^2-VelEn{d}^2}{2Ve{l}^{\prime }\ast Ac{c}^{\prime }}-\frac{{Ve{l}^{\prime }}^2-VelStar{t}^2}{2Ve{l}^{\prime }\ast Ac{c}^{\prime }}\text{\hspace{1em}(2)}$$

4，判断3-1：若 t2 >=0.0 ，则确定模型就是梯形模型。若 t2 < 0.0，则确定模型为三角模型。需要使用以下公式 3 进一步计算三角型的尖峰速度：
$$MaxVel=\sqrt{Ac{c}^{\prime }(PosStart-PosEnd)+\frac{1}{2}({VelEnd}^2+{VelStart}^2)}\text{\hspace{1em}(3)}$$

5，经过以上分支的计算，轨迹的其他信息都可以简单的求得。

6，判断2-2：若 Dist > PosEnd - PosStart，则梯形/三角模型开口朝上，接下来判断是梯形模型还是三角模型.假设是梯形模型，根据以上公式 2 代入 Acc’ = LimAcc, Vel’ = -LimVel 计算匀速段耗时。

7，判断3-2：在判断2-2的基础上，若 t2 >=0.0 ，则确定模型就是梯形模型。若 t2 < 0.0，则确定模型为三角模型。需要使用以上公式 3 进一步计算三角型的尖峰速度：-MaxVel。

这样就完成了梯形速度规划的求解。

四，S型速度规划编辑此区域

匀速段距离关于速度的函数
以上相似性已经分析过，两者的求解流程相似，可以认为匀速段的速度是一个未知数，计算出其他所需的物理量，得到代数方程，再求解这个方程即可。
1，同样的也是先假设匀速段速度能达到±LimVel，若能达到，则可以直接计算出规划结果，并且匀速段的速度尽可能的大，所以整体耗时会更小。
2，若无法达到则是三角型模型，匀速段的耗时接近于0，且当匀速段的速度尽可能大的时候，整体耗时会更小。所以需要求解以上这个方程，方程的自变量是假设的匀速段速度，需要求解当匀速段耗时=0时绝对值最大的解。
3，而考虑到匀速段速度定义域包含0，时间 = 距离 / 速度，那么匀速段耗时关于匀速段速度的函数将不连续，更一般的情况，建立匀速段距离关于匀速段速度的函数 Sc = F(Vc)。

加速区间/减速区间
以上相关性已经分析过，梯形速度规划的三段式，类似于S型速度规划里的加速区间/减速区间，加速区间/减速区间可以称为三段式梯形加速度规划。其计算方式几乎完全一样。只用将梯形速度规划里的时间，位置，速度，加速度，替换为自变量，因变量，一阶导，二阶导这样一个更通用的数学形式；然后再在S型速度规划的加速区间/减速区间里用物理量再替换回来，即替换回：时间，速度，加速度，加加速度。而通过以上 Sc = F(Vc) 的定义，Vc变成了自变量或者说已知量，所以可以直接使用梯形速度规划的计算方式来求解梯形加速度规划，进一步积分计算得规划中的各位置量。

即，假设已知匀速段速度Vc，可计算得唯一的Sacc和Sdec表示加速区间和减速区间的距离增量。这样也就求出了Sc = PosEnd - PosStart - Sacc(Vc) - Sdec(Vc) = F(Vc)。

而除了位置积分之外不再需要额外的计算公式，位置积分公式如下：
$$P(t)=P_0+V_{0}t+\frac{A_0{t}^2}{2}+\frac{J{t}^3}{6}\text{\hspace{1em}(4)}$$

典型函数图像
一般而言，以上F(Vc) = 0的求解存在多解的情况。尤其是直接在 [-LimVel , LimVel] 搜索可能并不能得到更优的解。

为了优化求解过程，并且进一步理解 Sc = F(Vc) 函数，可以观察一下函数图像。

例1，输入参数：

Sc = F(Vc) 函数图像结果：

上图蓝线为Vc，绿线为Sc，两尖角处Vc为 -47.75 和 125。这两个Vc点也是该组数据计算时的分段函数区间分界点，Vc = -47.75 对应“加速区间”计算时的判断2-1和2-2的临界点，即“加速区间”的梯形或三角形模型是开口朝上还是朝下；而 Vc = 125 对应“减速区间”计算时的判断2-1和2-2的临界点，即“减速区间”的梯形或三角形模型是开口朝上还是朝下.

例2，输入数据：

Sc = F(Vc) 函数图像结果：

上图蓝线为Vc，绿线为Sc，两尖角处Vc为 95.416667 和 141.666667，也是该组输入数据的“加速区间”和“减速区间”开口朝上朝下的临界点。放大细节如下：

红线为匀速段的拟定耗时，正数为有效的结果。

例3，输入数据：

Sc = F(Vc) 函数图像结果：

上图蓝线为Vc，绿线为Sc，两尖角处Vc为 73.333333 和 -85.666667，也是该组输入数据的“加速区间”和“减速区间”开口朝上朝下的临界点。放大细节如下：

通过观察以上典型图像，对搜索的区间进行了优化，在一般情况下可以提高计算效率(缩小了搜索范围), 且得到更优时间的解。具体的原理/证明等并不严格，具体实现参考下文“示例代码——三 搜索范围的确定”。

五，数值方法求解

基于以上内容已求得函数 Sc = F(Vc) = PosEnd - PosStart - Sacc(Vc) - Sdec(Vc)。其中Sacc(Vc)和Sdec(Vc)都是4段分段函数组合成的连续函数，其中涉及 根号Vc 的表达式。

则 F(Vc) 也是由分段函数组合成的连续函数，根据Sacc(Vc)和Sdec(Vc)的分段组合，它有4x4=16种具体的组合形式，且在一具体函数种同时最多存在7种组合，其中也包含根号Vc的复杂表达式。直接求解其解析解是较为困难的，结合《Trajectory Planning … 》的内容，可以使用以下这样一种数值计算方法：
1，首先设 Vc1 = LimVel，若求得 Sc1 >= 0 则直接得到了时间最优解，否则下一步；
2，设Vc2 = -LimVel，若求得 Sc2 <= 0 则直接得到了时间最优解，否则下一步；
3，由于函数 F(Vc) 在定义域 [-LimVel , LimVel] 内连续，且存在 F(LimVel) < 0 和 F(-LimVel ) >0，根据中值定理，在区间 [-LimVel , LimVel] 内必定存在一个Vc使得 F(Vc) = 0，也就是要求解的结果。
4，在区间内的求解，最起码的可以采用二分法进行迭代搜索。还可以采用其他经典数值计算的求根方法，加快求解效率。

割线法
割线法，又称弦割法、弦法，是基于牛顿法的一种改进，基本思想是用弦的斜率近似代替目标函数的切线斜率，并用割线与横轴交点的横坐标作为方程式的根的近似。适用于原函数不方便求导的情况。

割线方程：给定函数 f(x) 上两点 x[n-1] , y[n-1] 和 x[n] , y[n]，这两点所在的直线即为割线，方程为：

y - y[n] = (y[n] - y[n-1]) / (x[n] - x[n-1]) * (x - x[n]);

割线法即通过以上两点 x[n-1] 和 x[n] 及其割线方程，求得割线与X轴的交点横坐标作为 x[n+1]，求得此点的函数值 y[n+1]，再代入 x[n] 和 x[n+1] 不断迭代，直至精度满足需求。

搜索优化
牛顿法存在不一定收敛的情况，除开函数本身可能无解，迭代搜索过程中的计算也可能导致不收敛。牛顿下山法是对牛顿法的一种改进，它确保每一步迭代的范围都是缩小的。

在以上割线法中也有类似不收敛的问题，也可以采用类似下山法的思路进行优化，具体实现查看附件代码里的方法 M_Iteration。

其他方法
还有很多其他求根求最值的数值方法。例如 [Boost.Math](Chapter 10. Root Finding & Minimization Algorithms - 1.85.0 (boost.org)) 里的 TOMS Algorithm 748 算法，Brent-Dekker algorithm 布伦特方法，Muller Method 米勒法等等。

六，示例代码

代码见附件 Code_VelPlanS7Single.pdf 文档。代码中的Class_VelPlanS7_Single为规划处理，VP_S7_Time2Position为按时间插值位置，Test为两个单元测试例程。
一 初始化
代码中为 M_Init 方法内容，进行初始化参数、范围判定等。注意以下内容：

1，支持设置 StartAcc, EndAcc。需要在 [-LimAcc , LimAcc] 内。也可以超出，超出要另外处理。

2，temp1、temp3、temp4、temp6、temp8、temp9 等是后续的迭代计算中的一些不变量， 为了减少每次迭代计算量而做的化简。

3，VpeakStart、VpeakEnd 也是由已知条件得到的常量。它们的意义在于，若已知7段式的匀速段速度V，则可以计算出7段式所有的信息，记计算出的匀速段距离为S2，加速段距离为S1，减速段路径为S3. 即把问题理解为一个函数 :

(S1,S2,S3) = function( Vmax )。

也对应代码中的 M_V2S 方法。

而这个函数是基于两个小函数 S1 = F(Vmax) 和 S3 = G(Vmax)。各自有4种情况，而VpeakStart、VpeakEnd就是这各自4种情况中的2类的分界点，即以上“加速区间”/“减速区间”梯形或三角性模型开口朝上/开口朝下的临界点。

二 极限速度判断
7段式时间最优速度规划的思路是，使得匀速段的速度尽可能的大。所以一开始先假设能达到正负极限速度，看能不能满足条件( 匀速段的时间 > 0)。若满足条件，则直接得到最优结果。若不满足条件，则需要在 ( -LimVel , LimVel ) 之间搜索。由于函数的连续性，在此区间内一定有解。

三 搜索范围的确定
代码 line 18 ~ 108，搜索范围这里根据函数的特性，采用了一定方式进行处理，较快较优的得到一个更合适的范围。

背景是这样一个分段函数 Sc = F(Vc) 函数图像如上图。然后要找函数的零点，且在多个零点中找 V值绝对值更大的。上图只是示意，实际横纵坐标轴可能根据输入条件平移。该方法并没有找出所有的零点，而是根据函数的性质，将所有区间分为3部分 [ -LimVel, Vpeak1 ] , [ Vpeak1 , Vpeak2 ] , [ Vpeak2 , LimVel ]。 先去看看例如Vpeak2~LimVel 里面有没有根。没有就去看看Vpeak1~-LimVel。都没有那就在 [ Vpeak1 , Vpeak2 ] 里面。

也可以直接在 ( -LimVel , LimVel )之间搜索，但数据测试来看以上方法效果更好。

四 迭代搜索
方法 M_Iteration 内为迭代搜索。还是以上说明的函数：(S1,S2,S3) = function( Vmax )

可以认为是单输入单输出的，输入Vmax，输出S2。求使得S2=0的解即是最优的结果。

函数本身是连续的，且搜索的上下界一正一负，所以各种搜索方法应当都可以搜索得到一个解。

这里采用割线法，且排除掉了割线范围变大的情况。结果效果比较好，迭代收敛速度快。

五 按时间插值位置
VP_S7_Time2Position为按时间插值位置的函数，即输入时间，输出位置等。即7段式的7段不同函数。

注意看下规划的结果、此函数的输入：

规划结果基本包含了所有信息，以减少插值函数的计算量。包括各段的时间 t1~t7, 加加速度 jerk 、加速度、速度、位置。

七，后文

所有解
以上数值方法求解中由提到，“…分段组合，它有4x4=16种具体的组合形式，且在一具体函数种同时最多存在7种组合，其中也包含根号Vc的复杂表达式…”。针对具体的16种组合的函数表达式，求出所有的解，再判断解的可行性/时间最优，也是一种规划求解方式。

“背景”中介绍的 Ruckig 基本采用这种思路，它根据Jerk的正负零的情况以及是否达到LimVel/LimAcc/LimDec的情况，将所有的类型分为32种，依次计算每一种，并提到其中最复杂的单一种类可变为6次多项式的求根。它在求出所有类型的解后，用每个解中的每一小段时间是否大于0等条件进行可行性判断，并再在所有可行解中选择时间最优的解。

实际上可以很容易的排除 ±LimVel 的情况，将总数量总32种减少为16种，并且在其中一些涉及根式的计算中，不再设定关于匀速段速度Vc的方程，而是设定关于最大加速度/减速度的方程，可以转为更低次的方程求解。

这种求出所有解的方式能够得到真正的时间最优解，但是在计算时间上并不一定有优势。在一些需要定时规划的应用场景中，可能会更有优势。

连续多段速度规划
单段式速度规划是连续多段速度规划的基础，连续多段速度规划也是轨迹规划中需要解决的问题之一。一般来说都需要采用速度前瞻的处理方式进行规划。

一种连续多段轨迹的速度规划算法原理为：
1，在算法的初始阶段设第一段的起始速度=0，最后一段的结束速度=0；假设相邻段的衔接速度可以不为0，而衔接加速度=0。
2，在每一段内，按照从起始速度V0加速到结束速度V1，计算能够加速到的最大V1。最大V1的目标值 TargetV[i] = min( LimVel[i] , LimVel[i+1] )。
3，每一段内的加速曲线仍然是3段式的(梯形加速度曲线)，只用判断梯形模型/三角模型的临界情况，即可算得实际能加速的最大V1.
4，若 TargetV[i] < TargetV[i+1] ，则用以上的加速计算，确定结束速度V[i+1]；若 TargetV[i] >= TargetV[i+1]，则不用计算，暂时设V[i+1] = TargetV[i+1].
5，按i从小到大的顺序，即正向依次算得所有V[i]。
6，再按照以上类似的方式，按i从大到小的顺序，即反向依次再算一遍V[i]，该过程中 TargetV[i] 为正向计算得到的 V[i]。这样在正向计算中“减速模式”未精确计算的V[i]都得到了计算。所有的V[i]都正向/反向可达且为最大值。
7，这样正向计算+反向计算后，就确定了所有衔接点的速度，且设定加速度=0，这样每一段进行单段式求解的条件就都具备了，按单段式依次求解即可。

若连续多段速度规划不是预先已知所有段的轨迹，而是通过缓存队列的方式动态加载的（有称为DMA功能），则还需要进一步修改算法，并考虑实时插补的运动耗时与前瞻规划处理的计算耗时之间的同步影响。

空间三维插补和速度规划
以上做的都是速度规划，它已知的是一段轨迹/路径的距离，而不用考虑轨迹/路径的形状。如何将速度规划与空间中轨迹规划联系起来呢？

首先通过一个简单的例子，空间中的三维直线路径的插补，来展示它们之间的协作。

空间中的三维直线路径可以有很多种方程表示，在此可以用参数方程的方式表示为：

u是参数从0到1变化，其他的ab为系数，由起点终点计算得到。

xyz(t)的空间三维关于时间的插值问题可以转为u(t)参数关于时间的插值问题，也就是速度规划问题。直线插补的合成最大速度/最大加速度为输入参数，直线距离确定。可以转到参数u，即u的值从0到1，并且等效计算u的最大速度/最大加速度等，这样就可以进行 u(t) 的速度规划了。然后在实时插补中，每一个插补时刻t对应速度规划结果u(t)的一个u值，再通过以上路径的参数方程求出x/y/z，即得到了 xyz(t) 的空间直线插值结果。

其他路径的计算流程也是差不多的，例如空间圆弧路径，通过静态的坐标转换可以表示为某平面内的圆弧，进一步表示为xyz关于圆心角theta的参数方程。

然后也是如上流程：
1,从路径得到整体的路径参数方程，及路径长度，速度限制，加速度限制等；
2,按路径长度进行速度规划，得到路径距离Dist(t)；
3,按照速度规划结果进行实时插值，并由Dist计算得到参数值u；
4,再由参数值计算路径上的坐标值。
这套方法实际上是把空间多维路径归一化到共同的一个参数。它适用于任意空间三维位置路径的插补。一些复杂路径需要额外做一些工作，包括：1 计算复杂路径的欧式空间积分距离；2 在实时速度插值时，已知路径上的距离Dist(t)，求参数u。

多维度时间同步速度规划
空间中的三维位置(x,y,z)所构成的路径，可以变为一维的曲线距离，按照一维进行速度规划。但是有时涉及多个独立维度的时间同步速度规划，这时需要处理的一个基本问题是：两组/多组速度规划曲线如何时间同步，或者说确定时间的速度规划。

一般来说，可以通过以上“所有解”的方式，确定所有可行的区间，从Sc=F(Vc)来说，它是自变量Vc的定义域区间，同时也是按Vc求得的整段耗时t 的值域区间。多个自由度的可行耗时 t 区间如下图所示：

若是多个自由度需要进行时间同步，则如上图所示按照可行区间的边界值从小到大进行判定，是否在所有可行区间内，灰色为不可行的实时间区间，最终得到粗黑线T3a为这三个自由度的最小可同步时间。这样来确定最小的同步时间

若是单个自由度需要进行确定时间的规划，则先看设定的耗时是否在可行区间内，不在则没有解，若在则可以在区间内通过数值迭代的方式求解，或通过确定的(以上"所有解"中16种组合之一)解析式直接求解。

以上只是单段的多自由度时间同步，在解决以上这些基础问题后，还要考虑的是连续多段的多维度同步规划如何让整体的时间更优，这才是真正的难点。