一种基于避障规划的机器人路径优化方法及系统

1.本发明涉及机器人领域，尤其涉及一种基于避障规划的机器人路径优化方法及系统。


背景技术：

2.如图1所示为机械臂算法的流程图，由图1可知，机械臂的算法由上层至下层可以分为五个部分，分别是路径规划(path planning)、路径优化(path optimization)、轨迹规划(trajectory planning)、轨迹跟随(trajectory tracking)以及电机控制(motor control)。
3.路径规划的过程中，基于采样的规划算法的节点数随着探测空间范围的增大而增加，轨迹点数也随之增加，这种现象在采样空间为关节空间时尤为明显。同时由于基于采样的规划算法的盲目性与随机性，规划的路径点通常呈现锯齿状，在这种情况下机器人的运动会产生一定的冲击，使驱动电机受到速度大幅突变的损害。


技术实现要素：

4.本发明针对现有技术中存在的技术问题，提供一种基于避障规划的机器人路径优化方法及系统，提出一种路径压缩算法对路径规划的结果进行优化，在保留原有曲线特征与避障能力的前提下，剔除冗余的轨迹点，缩短轨迹长度。
5.根据本发明的第一方面，提供了一种基于避障规划的机器人路径优化方法，包括：步骤1，获取原始轨迹序列点，将所述原始轨迹序列点中的第一个和最后一个轨迹序列点添加至优化路径；pi和pj分别表示第i个和第j个轨迹序列点，i和j的初始值分别为1和2；
6.步骤2，判断机械臂执行pi和pj的连线的轨迹的花费是否少于执行对应的原始轨迹的花费，是则执行步骤3，否则执行步骤4；
7.步骤3，检测pi与pj的连线的轨迹是否存在碰撞，是则执行步骤4，否则对j加1后重新执行步骤2；
8.步骤4，将p
j-1
添加至所述优化路径，判断pj是否为原始轨迹序列点中的最后一个点，是则结束该路径优化过程得到优化路径中的各个轨迹序列点，否则令i＝j－1后重新执行步骤2。
9.在上述技术方案的基础上，本发明还可以作出如下改进。
10.可选的，所述步骤2中通过评价函数v计算机械臂执行任意一段轨迹的花费，包括：
11.分别计算机械臂执行第i个与第j个、第i个与第j－1个以及第j－1个与第j个轨迹序列点的连线的轨迹的花费v
i,j
、v
i,j-1
和v
j-1,j
；
[0012]vi,j
≤v
i,j-1
v
j-1,j
时，判定执行第i个与第j个轨迹序列点的连线的轨迹的花费少于执行对应的原始轨迹的花费。
[0013]
可选的，在关节空间规划时，所述评价函数v为关节向量的欧氏距离。
[0014]
可选的，在笛卡尔空间中规划时，所述评价函数v为笛卡尔空间中向量的欧氏距
离。
[0015]
可选的，所述步骤3中检测pi与pj的连线的轨迹是否存在碰撞的过程包括：
[0016]
在pi与pj的连线上进行线性插值得到更加细密的插值点序列，对所述点序列中的每个插值点分别执行碰撞检测，若所有的插值点都不与障碍物碰撞，说明pi与pj的连线的轨迹不存在碰撞。
[0017]
可选的，所述步骤4之后还包括：
[0018]
步骤5，翻转所述优化路径中的轨迹序列点，将翻转后的所述轨迹序列点作为原始轨迹序列点重新执行步骤1－4。
[0019]
可选的，所述步骤5包括：
[0020]
步骤501，设置翻转标志位，所述翻转标志位初始值为0；
[0021]
步骤502，判断所述翻转标志位是否为1，是则将所述优化路径中的轨迹序列点进行翻转后结束，否则执行步骤503；
[0022]
步骤503，翻转所述优化路径中的轨迹序列点，将所述翻转标志位值设为1后，将翻转后的所述轨迹序列点作为原始轨迹序列点重新执行步骤1－4。
[0023]
根据本发明的第二方面，提供一种基于避障规划的机器人路径优化系统，包括：原始轨迹序列点获取模块、路径比较模块、碰撞检测模块和优化路径获取模块；
[0024]
所述原始轨迹序列点获取模块，用于获取原始轨迹序列点，pi和pj分别表示第i个和第j个轨迹序列点，i和j的初始值分别为1和2；
[0025]
所述路径比较模块，用于判断机械臂执行pi和pj的连线的轨迹的花费是否少于执行对应的原始轨迹的花费，是则进入所述碰撞检测模块，否则进入所述优化路径获取模块；
[0026]
所述碰撞检测模块，用于检测pi与pj的连线的轨迹是否存在碰撞，是则进入所述优化路径获取模块，否则对j加1后重新进入所述路径比较模块；
[0027]
所述优化路径获取模块，用于将p
j-1
添加至所述优化路径，判断pj是否为原始轨迹序列点中的最后一个点，是则将所述原始轨迹序列点中的第一个和最后一个轨迹序列点添加至所述优化路径，得到优化路径中的各个轨迹序列点；否则令i＝j－1后重新进入所述路径比较模块。
[0028]
根据本发明的第三方面，提供了一种电子设备，包括存储器、处理器，所述处理器用于执行存储器中存储的计算机管理类程序时实现基于避障规划的机器人路径优化方法的步骤。
[0029]
根据本发明的第四方面，提供了一种计算机可读存储介质，其上存储有计算机管理类程序，所述计算机管理类程序被处理器执行时实现基于避障规划的机器人路径优化方法的步骤。
[0030]
本发明提供的一种基于避障规划的机器人路径优化方法、系统、电子设备及存储介质，通过构造评价函数计算各个轨迹点之间的花费来比较判断哪条路径花费更少，并针对不同的空间规划确定不同的评价函数，通过引入该评价函数使得算法可以应用于任意维度空间中；通过对两个点之间的连线进行线性插值得到更加细密的插值点序列，然后对每个插值点执行碰撞检测来判断两个点形成的路径是否发生碰撞；考虑基于双指针的遍历算法只能对两指针之间的区域进行探测，而对两指针之外的区域没有前瞻性，所以不能找到全局的最优路径，因此再得到优化路径后，翻转优化后的点序列，并再次运行此算法进行路
径压缩，即可得到全局最优解；结合双指针遍历与双向遍历保证找到全局最优解，并且只有线性的时间复杂度，在保留原有曲线特征与避障能力的前提下，剔除冗余的轨迹点，缩短轨迹长度。
附图说明
[0031]
图1为机械臂算法的流程图；
[0032]
图2为本发明实施例提供的路径压缩优化前后的轨迹图
[0033]
图3为本发明提供的一种基于避障规划的机器人路径优化方法的实施例的流程图；
[0034]
图4为本发明实施例提供的单向单向路径压缩无法找到最优解的轨迹示意图；
[0035]
图5为本发明实施例提供的双向路径压缩后的轨迹的示意图；
[0036]
图6为本发明提供的一种测试实施例中bi－rrt算法结果可视化示意图；
[0037]
图7为本发明提供的一种测试实施例中路径压缩结果可视化示意图；
[0038]
图8为本发明提供的一种基于避障规划的机器人路径优化系统的结构框图；
[0039]
图9为本发明提供的一种可能的电子设备的硬件结构示意图；
[0040]
图10为本发明提供的一种可能的计算机可读存储介质的硬件结构示意图。
具体实施方式
[0041]
以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。
[0042]
本发明提供的一种基于避障规划的机器人路径优化方法，提出一种路径压缩算法对路径规划的结果进行优化，如图2所示为本发明实施例提供的路径压缩优化前后的轨迹图，图2中给出的实施例中，假设当前环境中存在两个障碍物，机器人需要在位置1和位置8中规划出一条无碰撞的路径，通过bi-rrt算法规划出来的路径如图中黑色虚线所示；实际上，出于规避障碍物的目的，机器人没有必要严格的按照1-2-3-4-5-6-7-8的点序列执行；例如，机器人可以直接从点1运动到点4，再从点4到点8，这样不仅剔除了冗余点，也使得机器人的运动更加平顺。
[0043]
如图3为本发明提供的一种基于避障规划的机器人路径优化方法的实施例的流程图，如图3所示，该路径优化方法包括：
[0044]
步骤1，获取原始轨迹序列点，将原始轨迹序列点中的第一个和最后一个轨迹序列点添加至优化路径；pi和pj分别表示第i个和第j个轨迹序列点，i和j的初始值分别为1和2。
[0045]
具体实施中，可以通过设置指针的形式实现，设置头指针为i，尾指针为j。
[0046]
步骤2，判断机械臂执行pi和pj的连线的轨迹的花费是否少于执行对应的原始轨迹的花费，是则执行步骤3，否则执行步骤4。
[0047]
步骤3，检测pi与pj的连线的轨迹是否存在碰撞，是则执行步骤4，否则对j加1后重新执行步骤2。
[0048]
步骤4，将p
j-1
添加至优化路径，判断pj是否为原始轨迹序列点中的最后一个点，是则结束该路径优化过程得到优化路径中的各个轨迹序列点，否则令i＝j－1后重新执行步骤2。
[0049]
本发明提供的一种基于避障规划的机器人路径优化方法，提出一种路径压缩算法对路径规划的结果进行优化，在保留原有曲线特征与避障能力的前提下，剔除冗余的轨迹点，缩短轨迹长度。
[0050]
实施例1
[0051]
本发明提供的实施例1为本发明提供的一种基于避障规划的机器人路径优化方法的实施例，结合图3可知，该路径优化方法的实施例包括：
[0052]
步骤1，获取原始轨迹序列点，将原始轨迹序列点中的第一个和最后一个轨迹序列点添加至优化路径；pi和pj分别表示第i个和第j个轨迹序列点，i和j的初始值分别为1和2。
[0053]
步骤2，判断机械臂执行pi和pj的连线的轨迹的花费是否少于执行对应的原始轨迹的花费，是则执行步骤3，否则执行步骤4。
[0054]
在一种可能的实施例方式中，步骤2中每个循环周期中算法对pi、p
j-1
和pj三个点进行探测，通过评价函数v计算机械臂执行任意一段轨迹的花费，包括：
[0055]
分别计算机械臂执行第i个与第j个、第i个与第j－1个以及第j－1个与第j个轨迹序列点的连线的轨迹的花费v
i,j
、v
i,j-1
和v
j-1,j
。
[0056]vi,j
≤v
i,j-1
v
j-1,j
时，判定执行第i个与第j个轨迹序列点的连线的轨迹的花费少于执行对应的原始轨迹的花费。
[0057]
可以理解的是，具体实施中，程序比较v
i,j
与v
i,j-1
v
j-1,j
两者的大小关系，如果v
i,j
≤v
i,j-1
v
j-1,j
，则说明p
j-1
为一个潜在的可以剔除的冗余点，否则p
j-1
不可剔除，需要添加到优化路径中。
[0058]
在一种可能的实施例方式中，在关节空间规划时，评价函数v可以为关节向量的欧氏距离。
[0059]
在一种可能的实施例方式中，在笛卡尔空间中规划时，评价函数v可以为笛卡尔空间中向量的欧氏距离。
[0060]
步骤3，检测pi与pj的连线的轨迹是否存在碰撞，是则执行步骤4，否则对j加1后重新执行步骤2。
[0061]
在一种可能的实施例方式中，步骤3中检测pi与pj的连线的轨迹是否存在碰撞的过程包括：
[0062]
在pi与pj的连线上进行线性插值得到更加细密的插值点序列，对点序列中的每个插值点分别执行碰撞检测，若所有的插值点都不与障碍物碰撞，说明pi与pj的连线的轨迹不存在碰撞。
[0063]
若p
j-1
为一个潜在的可以剔除的冗余点，算法会在pi与pj之间直接连一条线，并进行线性插值得到更加细密的点序列，然后对每个点执行碰撞检测，若所有的点都不与障碍物碰撞，说明pi到pj的局部路径无碰撞，则确信p
j-1
是一个的可以剔除的点，否则添加p
j-1
到处理后轨迹中。
[0064]
步骤4，将p
j-1
添加至优化路径，判断pj是否为原始轨迹序列点中的最后一个点，是则结束该路径优化过程得到优化路径中的各个轨迹序列点，否则令i＝j－1后重新执行步骤2。
[0065]
在一种可能的实施例方式中，步骤4之后还包括：
[0066]
步骤5，翻转优化路径中的轨迹序列点，将翻转后的轨迹序列点作为原始轨迹序列
点重新执行步骤1－4。
[0067]
在一种可能的实施例方式中，步骤5包括：
[0068]
步骤501，设置翻转标志位，翻转标志位初始值为0。
[0069]
步骤502，判断翻转标志位是否为1，是则将优化路径中的轨迹序列点进行翻转后结束，否则执行步骤503。
[0070]
步骤503，翻转优化路径中的轨迹序列点，将翻转标志位值设为1后，将翻转后的轨迹序列点作为原始轨迹序列点重新执行步骤1－4。
[0071]
如图4所示为本发明实施例提供的单向单向路径压缩无法找到最优解的轨迹示意图，图4中，当前环境中存在三个障碍物，路径规划的结果为1－2－3－4－5－6－7－8－9点序列，运行上述单向的路径压缩算法得到的结果为1－4－9，尽管大量的冗余点被成功剔除，但1－4－7－9并不是最优路径，最优路径为1－4－9。出现这种现象的原因是基于双指针的遍历算法只能对两指针之间的区域进行探测，而对两指针之外的区域没有前瞻性，所以不能找到全局的最优路径。对于此问题本发明的解决方案是在上述步骤的基础上，翻转优化后的点序列，并再次运行此算法进行路径压缩，即可得到全局最优解，如图5所示为本发明实施例提供的双向路径压缩后的轨迹的示意图。
[0072]
传统的路径优化方法只考虑了一个方向的遍历，并不能找到全局最优解；或是采用了只适用于笛卡尔空间中的距离计算，不能推广到更高维度的空间中。本文提出的算法结合双指针遍历与双向遍历保证找到全局最优解，并且只有线性的时间复杂度，此外通过引入评价函数使得算法可以应用于任意维度空间中。
[0073]
实施例2
[0074]
本发明提供的实施例2为本发明提供的一种基于避障规划的机器人路径优化方法具体应用的测试实施例，在三维空间中进行实验，使用笛卡尔空间向量的欧式距离作为路径压缩中的评价函数：
[0075]
v(p1,p2)＝‖p
1-p2‖
[0076]
采样空间为：
[0077][0078]
环境中一共存在27个障碍物，其中包括8个正方体障碍物与19个球形障碍物，起点坐标为[10,10,10]，终点坐标为[490,490,490]，rrt的规划结果如图6所示。
[0079]
对以上轨迹序列使用本文提出的路径压缩算法，如图7所示，使用路径压缩率α来评价算法剔除冗余点的效果。
[0080][0081]
经过1000次实验，平均压缩率为12.82％。
[0082]
实施例3
[0083]
本发明提供的实施例3为本发明提供的一种基于避障规划的机器人路径优化系统的实施例，图8为本发明实施例提供的一种基于避障规划的机器人路径优化系统结构图，结
合图8可知，该实施例包括：原始轨迹序列点获取模块、路径比较模块、碰撞检测模块和优化路径获取模块。
[0084]
原始轨迹序列点获取模块，用于获取原始轨迹序列点，pi和pj分别表示第i个和第j个轨迹序列点，i和j的初始值分别为1和2。
[0085]
路径比较模块，用于判断机械臂执行pi和pj的连线的轨迹的花费是否少于执行对应的原始轨迹的花费，是则进入碰撞检测模块，否则进入优化路径获取模块。
[0086]
碰撞检测模块，用于检测pi与pj的连线的轨迹是否存在碰撞，是则进入优化路径获取模块，否则对j加1后重新进入路径比较模块。
[0087]
优化路径获取模块，用于将p
j-1
添加至优化路径，判断pj是否为原始轨迹序列点中的最后一个点，是则将原始轨迹序列点中的第一个和最后一个轨迹序列点添加至优化路径，得到优化路径中的各个轨迹序列点；否则令i＝j－1后重新进入路径比较模块。
[0088]
可以理解的是，本发明提供的一种基于避障规划的机器人路径优化系统与前述各实施例提供的基于避障规划的机器人路径优化方法相对应，基于避障规划的机器人路径优化系统的相关技术特征可参考基于避障规划的机器人路径优化方法的相关技术特征，在此不再赘述。
[0089]
请参阅图9，图9为本发明实施例提供的电子设备的实施例示意图。如图9所示，本发明实施例提了一种电子设备，包括存储器1310、处理器1320及存储在存储器1310上并可在处理器1320上运行的计算机程序1311，处理器1320执行计算机程序1311时实现以下步骤：步骤1，获取原始轨迹序列点，将原始轨迹序列点中的第一个和最后一个轨迹序列点添加至优化路径；pi和pj分别表示第i个和第j个轨迹序列点，i和j的初始值分别为1和2；步骤2，判断机械臂执行pi和pj的连线的轨迹的花费是否少于执行对应的原始轨迹的花费，是则执行步骤3，否则执行步骤4；步骤3，检测pi与pj的连线的轨迹是否存在碰撞，是则执行步骤4，否则对j加1后重新执行步骤2；步骤4，将p
j-1
添加至优化路径，判断pj是否为原始轨迹序列点中的最后一个点，是则结束该路径优化过程得到优化路径中的各个轨迹序列点，否则令i＝j－1后重新执行步骤2。
[0090]
请参阅图10，图10为本发明提供的一种计算机可读存储介质的实施例示意图。如图10所示，本实施例提供了一种计算机可读存储介质1400，其上存储有计算机程序1411，该计算机程序1411被处理器执行时实现如下步骤：步骤1，获取原始轨迹序列点，将原始轨迹序列点中的第一个和最后一个轨迹序列点添加至优化路径；pi和pj分别表示第i个和第j个轨迹序列点，i和j的初始值分别为1和2；步骤2，判断机械臂执行pi和pj的连线的轨迹的花费是否少于执行对应的原始轨迹的花费，是则执行步骤3，否则执行步骤4；步骤3，检测pi与pj的连线的轨迹是否存在碰撞，是则执行步骤4，否则对j加1后重新执行步骤2；步骤4，将p
j-1
添加至优化路径，判断pj是否为原始轨迹序列点中的最后一个点，是则结束该路径优化过程得到优化路径中的各个轨迹序列点，否则令i＝j 1后重新执行步骤2。
[0091]
本发明实施例提供的一种基于避障规划的机器人路径优化方法、系统、电子设备及存储介质，通过构造评价函数计算各个轨迹点之间的花费来比较判断哪条路径花费更少，并针对不同的空间规划确定不同的评价函数，通过引入该评价函数使得算法可以应用于任意维度空间中；通过对两个点之间的连线进行线性插值得到更加细密的插值点序列，然后对每个插值点执行碰撞检测来判断两个点形成的路径是否发生碰撞；考虑基于双指针
的遍历算法只能对两指针之间的区域进行探测，而对两指针之外的区域没有前瞻性，所以不能找到全局的最优路径，因此再得到优化路径后，翻转优化后的点序列，并再次运行此算法进行路径压缩，即可得到全局最优解；结合双指针遍历与双向遍历保证找到全局最优解，并且只有线性的时间复杂度，在保留原有曲线特征与避障能力的前提下，剔除冗余的轨迹点，缩短轨迹长度。
[0092]
需要说明的是，在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详细描述的部分，可以参见其它实施例的相关描述。
[0093]
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
[0094]
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式计算机或者其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0095]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0096]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0097]
尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。
[0098]
显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包括这些改动和变型在内。