地方建立网站做SEM,网页设计个人主页模板图片,深圳市企业网络推广平台,郑州的建设网站有哪些手续费一#xff0c;背景
在做机械臂轨迹规划的单段路径的速度规划时#xff0c;除了参考《Trajectory Planning for Automatic Machines and Robots》等文献之外#xff0c;还在知乎找到了这位大佬 韩冰 写的在线规划方法#xff1a; https://zhuanlan.zhihu.com/p/585253101/e…一背景
在做机械臂轨迹规划的单段路径的速度规划时除了参考《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]梯形/三角的开口方向是向上的还是向下的。开口方向并不是显然的根据距离的正负而决定的例如以下两个例子
例1PosEnd - PosStart 10.0; 结果如下蓝色曲线为位置关于时间的变化绿色曲线为速度关于时间的变化。
例2PosEnd - PosStart 5.0; 其他参数和例1完全相同结果如下
可以看到例1例2的距离都是正数但是速度曲线的三角型开口一个朝上一个朝下。
我们可以用以下这种方式来判别速度梯形/三角形的开口方向 1先只做速度关于时间的规划假设以最大加速度从起始速度规划到结束速度(速度曲线是一条直线斜率为最大加速度)可以计算出此规划的运动距离。 2若假设的运动距离 结束位置-起始位置则说明还存在速度为正的区间而为了时间最优这个正速度应当尽量的大即应当等于速度三角型模型的尖角速度或梯形模型的平台速度同时也说明梯形/三角模型的开口应该朝下。 3若假设的运动距离 结束位置-起始位置则说明还存在速度为负的区间而为了时间最优这个负速度绝对值应当尽量的大即应当等于速度三角型模型的尖角速度或梯形模型的平台速度同时也说明梯形/三角模型的开口应该朝上。
整体解算过程如下 1首先判断1若 VelStart VelEnd则预设加速度 Acc’ 为 -LimAcc且否则预设加速度 Acc’ 为 LimAcc。 2然后只做从 VelStart 到 VelEnd 的匀变速规划根据以下公式 1 计算预设的运动距离 Dist D i s t V e l E n d 2 − V e l S t a r t 2 2 A c c (1) Dist \frac {{VelEnd}^2-{VelStart}^2} {2{Acc}} \tag {1} Dist2AccVelEnd2−VelStart2(1)
3判断2-1若 Dist PosEnd - PosStart则梯形/三角模型开口朝下接下来判断是梯形模型还是三角模型.假设是梯形模型根据以下公式 2 代入 Acc’ -LimAcc, Vel’ LimVel 计算匀速段耗时 t 2 P o s E n d − P o s S t a r t V e l ′ − V e l ′ 2 − V e l E n d 2 2 V e l ′ ∗ A c c ′ − V e l ′ 2 − V e l S t a r t 2 2 V e l ′ ∗ A c c ′ (2) t2 \frac {PosEnd-PosStart} {Vel} - \frac {{Vel}^2 - VelEnd^2} {2 Vel * Acc} -\frac {{Vel}^2 - VelStart^2} {2 Vel * Acc} \tag {2} t2Vel′PosEnd−PosStart−2Vel′∗Acc′Vel′2−VelEnd2−2Vel′∗Acc′Vel′2−VelStart2(2)
4判断3-1若 t2 0.0 则确定模型就是梯形模型。若 t2 0.0则确定模型为三角模型。需要使用以下公式 3 进一步计算三角型的尖峰速度 M a x V e l A c c ′ ( P o s S t a r t − P o s E n d ) 1 2 ( V e l E n d 2 V e l S t a r t 2 ) (3) MaxVel \sqrt {Acc {(PosStart-PosEnd) } \frac{1}{2} ({{VelEnd}}^2{{VelStart}}^2)} \tag {3} MaxVelAcc′(PosStart−PosEnd)21(VelEnd2VelStart2) (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 A 0 t 2 2 J t 3 6 (4) P(t)P_{0}V_{0} t \frac {A_{0} {t}^2} {2} \frac {J {t}^3} {6} \tag {4} P(t)P0V0t2A0t26Jt3(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)的分段组合它有4x416种具体的组合形式且在一具体函数种同时最多存在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[n1]求得此点的函数值 y[n1]再代入 x[n] 和 x[n1] 不断迭代直至精度满足需求。
搜索优化 牛顿法存在不一定收敛的情况除开函数本身可能无解迭代搜索过程中的计算也可能导致不收敛。牛顿下山法是对牛顿法的一种改进它确保每一步迭代的范围都是缩小的。
在以上割线法中也有类似不收敛的问题也可以采用类似下山法的思路进行优化具体实现查看附件代码里的方法 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] 内。也可以超出超出要另外处理。2temp1、temp3、temp4、temp6、temp8、temp9 等是后续的迭代计算中的一些不变量 为了减少每次迭代计算量而做的化简。3VpeakStart、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。求使得S20的解即是最优的结果。
函数本身是连续的且搜索的上下界一正一负所以各种搜索方法应当都可以搜索得到一个解。
这里采用割线法且排除掉了割线范围变大的情况。结果效果比较好迭代收敛速度快。
五 按时间插值位置 VP_S7_Time2Position为按时间插值位置的函数即输入时间输出位置等。即7段式的7段不同函数。
注意看下规划的结果、此函数的输入 规划结果基本包含了所有信息以减少插值函数的计算量。包括各段的时间 t1~t7, 加加速度 jerk 、加速度、速度、位置。
七后文
所有解 以上数值方法求解中由提到“…分段组合它有4x416种具体的组合形式且在一具体函数种同时最多存在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[i1] )。 3每一段内的加速曲线仍然是3段式的(梯形加速度曲线)只用判断梯形模型/三角模型的临界情况即可算得实际能加速的最大V1. 4若 TargetV[i] TargetV[i1] 则用以上的加速计算确定结束速度V[i1]若 TargetV[i] TargetV[i1]则不用计算暂时设V[i1] TargetV[i1]. 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)所构成的路径可以变为一维的曲线距离按照一维进行速度规划。但是有时涉及多个独立维度的时间同步速度规划这时需要处理的一个基本问题是两组/多组速度规划曲线如何时间同步或者说确定时间的速度规划。
一般来说可以通过以上“所有解”的方式确定所有可行的区间从ScF(Vc)来说它是自变量Vc的定义域区间同时也是按Vc求得的整段耗时t 的值域区间。多个自由度的可行耗时 t 区间如下图所示 若是多个自由度需要进行时间同步则如上图所示按照可行区间的边界值从小到大进行判定是否在所有可行区间内灰色为不可行的实时间区间最终得到粗黑线T3a为这三个自由度的最小可同步时间。这样来确定最小的同步时间
若是单个自由度需要进行确定时间的规划则先看设定的耗时是否在可行区间内不在则没有解若在则可以在区间内通过数值迭代的方式求解或通过确定的(以上所有解中16种组合之一)解析式直接求解。
以上只是单段的多自由度时间同步在解决以上这些基础问题后还要考虑的是连续多段的多维度同步规划如何让整体的时间更优这才是真正的难点。
