一种基于动态候选池引导和碰撞节点启发的机械臂快速路径规划方法及系统

本发明涉及机械臂路径规划，具体而言，涉及一种基于动态候选池引导和碰撞节点启发的机械臂快速路径规划方法及系统。

背景技术：

1、机械臂具有高精度和高效率作业优势，广泛应用于装备加工制造、大型设施维护、水果自动采摘等生产活动中。机械臂路径规划是指在机械臂的初始构型和目标构型之间规划生成一条由一系列离散节点(每个节点对应一组机械臂关节构型)组成的无碰撞路径，使得机械臂沿着该路径就可以无碰撞地从初始状态运动到目标状态以完成作业任务。机械臂的路径规划通常在高维关节空间中进行，便于处理关节角状态约束和避免机械臂逆运动学解的存在性和多解性问题。然而，由于笛卡尔空间与高维关节空间之间的多维耦合和强非线性映射关系，导致无法在高维关节空间中显式描述机械臂运动的无障碍空间，进而使得dijkstra和a*等依赖于显式栅格地图的规划算法难以高效适配机械臂系统。此外，随着机械臂关节空间维数和任务复杂度的增加，路径规划的计算复杂度和时间成本呈指数增长，这使得机械臂的快速路径规划仍然是一个具有挑战性的问题。

2、为了解决机械臂高维关节空间路径规划问题，国内外研究人员提出了基于随机采样的快速探索随机树(rapidly-exploring random trees，简称rrt)算法，通过在机械臂关节空间中随机采样扩展新节点和增量式生长随机树来规划机械臂从初始构型运动到目标构型的无碰撞路径，避免了对机械臂无障碍关节空间区域的显式表达，因而对高维机械臂的运动规划问题有很好的适用性。然而，rrt算法由于关节空间采样探索过程的强随机性和盲目性，使得路径节点扩展效率较低、随机树生长速度较慢，进而导致机械臂路径规划效率低、耗时较长。

3、近年来，针对制造业自动化对机械臂快速路径规划的实际需求，国内外学者相继提出了rrt-connect、gbi-rrt等从rrt算法改进而来的变体算法(用rrts表示)并验证了其在机械臂路径规划任务中的有效性。然而，rrts算法为了加快随机树向目标节点的生长，将采样点概率性地设置为与目标节点重合。通过最小化由欧氏距离决定的代价，从随机树中已有的节点中选择距离目标节点最近的节点，然后生成一个新的节点并进行碰撞检测。扩展过程将迭代执行，直到获得新的无碰撞节点。然而，事实证明，朝向目标节点扩展新节点失败的最邻近节点，其再次朝向目标节点扩展也会因发生碰撞而失败。因此，随机树将重复向目标节点生成无效的新节点，导致额外时间消耗。这严重限制了机械臂在对时间消耗极端敏感的作业任务中的实际应用。此外，在现有的研究中，如果新节点遇到障碍物，从最近邻近节点向采样点扩展的新节点将被丢弃。然后，在采样空间中向新的采样点重新生成一个新节点并进行碰撞检测。上述过程将迭代执行，直到成功获取新的无碰撞节点或达到迭代极限。然而，由于rrts算法的随机性，新的采样点可能位于采样空间的任何位置，从障碍物附近的节点向新的采样点扩展的新节点仍然有很高的碰撞概率，造成低效避障探索和过多时间消耗，限制了rrts算法在多障碍物环境中的应用。

技术实现思路

1、本发明要解决的技术问题是：

2、为了解决现有机械臂路径规划算法因概率性目标偏执扩展方式造成的随机树多次重复扩展失败和路径规划时间成本高的问题。

3、本发明为解决上述技术问题所采用的技术方案：

4、本发明提供了一种基于动态候选池引导和碰撞节点启发的机械臂快速路径规划方法，包括以下步骤：

5、s100、建立地面惯性坐标系、机械臂基座坐标系和机械臂各关节坐标系，根据机械臂作业任务需求，确定机械臂初始构型θstart和目标构型θgoal；构建环境障碍物的碰撞包围盒模型，并确定所有障碍物的碰撞包围盒模型的三维尺寸及位姿坐标；

6、s200、在机械臂关节空间中同时创建两棵相互吸引的随机树etree1和随机树etree2，随机树etree1以机械臂的初始构型θstart为起始节点，以目标构型θgoal为目标节点；随机树etree2以机械臂的目标构型θgoal为起始节点，以初始构型θstart为目标节点；随机树etree1和随机树etree2交替扩展新节点；

7、s300、给定目标偏向基准概率βgoal∈[0,1]，按均匀概率产生随机数βrand＝rand(1,1)，若随机数βrand小于基准概率βgoal，则在关节空间中随机生成θrand作为采样点θsample，并基于最小化加权欧几里德距离从随机树etree1中找到与θsample最邻近的节点θnearest；若随机数βrand大于或等于基准概率βgoal，则选择目标节点θgoal作为采样点θsample，并基于最小化加权欧几里德距离成本，从候选池eunexplored的现有节点中选择最接近采样点θsample的最邻近节点θnearest；

8、s400、在步骤s300获取采样点θsample的最邻近节点θnearest后，从θnearest出发沿θnearest和θsample的连线方向θdir按照预定步长θstep扩展生成新节点θnew，判断θnearest与θnew的连线是否与障碍物发生碰撞，若不发生碰撞则把新节点θnew添加到随机树etree1中；若发生碰撞则基于原扩展方向θdir转向得到新扩展方向θver_unit，然后让最邻近节点θnearest沿新扩展方向θver_unit扩展得到新节点θnew，并再次判断θnearest与θnew的连线是否与障碍物发生碰撞，若再次发生碰撞则舍弃θnew并跳转执行步骤s300，若不发生碰撞则把成功生成的新节点θnew添加到随机树etree1中；

9、s500、当随机树etree1成功扩展得到新节点θnew后，将新节点θnew与随机树etree2上的所有节点逐一配对，按照距离由近及远进行捷径连接，并在连线上按照给定步长θstep_collision进行插值点碰撞检测，若未检测到捷径连接线上所有插值点都无碰撞的配对节点则交换两棵随机树并跳转执行步骤s300；若找到无碰撞的配对节点，则表明两棵随机树etree1和etree2已成功连接，然后按照生长步长θstep在配对节点间补入新节点θadd，并添加到随机树etree1中；

10、s600、在etree1和etree2中分别从连接处的θnew和θother出发逐节点回溯到各自的起始节点，得到一条由一系列离散节点有序连接而成的路径πpath，由此获得机械臂从初始构型θstart运动到目标构型θgoal的无碰撞路径。

11、进一步地，在步骤s300中，当随机数βrand小于基准概率βgoal时，采样点θsample的最邻近节点θnearest为，

12、θsampleθrand，βrand＜βgoal  (1)

13、

14、其中，ωi表示机械臂第i个关节角的权值，θk是随机树etree1中的第k个节点。

15、进一步地，在步骤s300中，当随机数βrand大于或等于基准概率βgoal时，采样点θsample的最邻近节点θnearest为，

16、θsampleθgoal，βrand≥βgoal  (3)

17、

18、eunexplored＝exclude(eunexplored，θnearest)  (5)

19、其中，eunexplored＝exclude(eunexplored，θnearest)表示把候选池eunexplored中被选为目标节点θgoal最邻近节点的θnearest移出候选池eunexplored。

20、进一步地，在步骤s400中，从θnearest出发沿着θnearest和θsample的连线方向θdir按照预定步长θstep扩展生成新节点θnew，即，

21、θsn＝θsample-θnearest  (6)

22、

23、θnew＝θnearest+θstepθdir  (8)

24、其中，θsn表示节点扩展方向，θdir表示沿着节点扩展方向的单位向量。

25、进一步地，在步骤s400中，新扩展方向θver_unit的计算方法为，

26、θver＝rand(1,n) (9)

27、

28、

29、

30、其中，θdir是节点的原扩展方向，θver是转向后的新扩展方向，θver_unit是新扩展方向的单位向量，imax是θdir的所有元素绝对值中的最大值对应的列序号，βflag是用于表征两个节点之间的捷径连接线是否存在碰撞的标志量。

31、进一步地，在步骤s500中，插值点碰撞检测为，

32、

33、

34、其中，函数用于对etree2中的元素按照规则f重新升序排列。

35、进一步地，在步骤s500中，当找到无碰撞配对节点后，将新节点θadd添加到随机树etree1中，

36、θadd＝θnew+k(θnear-θnew)/nadd, k＝1,2,…,nadd-1 (15)

37、

38、一种基于动态候选池引导和碰撞节点启发的机械臂快速路径规划系统，该系统具有与上述步骤对应的程序模块，运行时执行上述的基于动态候选池引导和碰撞节点启发的机械臂快速路径规划方法中的步骤。

39、一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现基于动态候选池引导和碰撞节点启发的机械臂快速路径规划方法的步骤。

40、相较于现有技术，本发明的有益效果是：

41、①针对现有路径规划方法时间成本高、扩展质量差的问题，本发明提出了基于动态候选池引导的节点选取策略来引导最邻近节点的最佳选择，通过候选池赋予节点记忆性，避免了同一个节点被多次选中为目标节点的最邻近节点，解决了现有算法概率性目标偏执扩展方式造成的随机树多次重复扩展失败问题，使规划进程及时跳出局部最优，实现了随机树朝向目标节点方向快速扩展生成无碰撞节点，进而大大降低路径规划的时间成本。

42、②针对现有路径算法在障碍物附近采样扩展新节点的碰撞概率高，难以快速生成可行新节点的问题，提出了基于碰撞节点启发的避障区域探索策略来调节障碍物附近新节点的扩展方向，使随机树能够以较小的盲目性快速扩展无碰撞新节点以绕过障碍物区域，避免随机树在障碍物附近执行过多的重复探索和无效扩展，从而提高节点质量并缩短路径规划时长。

43、综上，本发明以机械臂为应用对象，在传统路径规划算法的基础上进行改进优化，构建了基于动态候选池引导和碰撞节点启发的机械臂快速路径规划方法，在保证机械臂避障效果的同时，可以更快响应机械臂任务需求，在短时间内规划出无碰撞路径，进而在相同时间内执行更多作业任务，能够更好地满足机械臂快速、低时耗要求，具有较高的工程应用价值。通过与现有路径规划算法的对比实验也可证明，本发明在时间成本、扩展质量、内存成本和探索质量上均有所改善，尤其在实现成本上，均值时间仅为现有算法的约1/7，证明了本算法可有效提高路径规划速度。

技术特征：

1.一种基于动态候选池引导和碰撞节点启发的机械臂快速路径规划方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的一种基于动态候选池引导和碰撞节点启发的机械臂快速路径规划方法，其特征在于：在步骤s300中，当随机数βrand小于基准概率βgoal时，采样点θsample的最邻近节点θnearest为，

3.根据权利要求2所述的一种基于动态候选池引导和碰撞节点启发的机械臂快速路径规划方法，其特征在于：在步骤s300中，当随机数βrand大于或等于基准概率βgoal时，采样点θsample的最邻近节点θnearest为，

4.根据权利要求3所述的一种基于动态候选池引导和碰撞节点启发的机械臂快速路径规划方法，其特征在于：在步骤s400中，从θnearest出发沿着θnearest和θsample的连线方向θdir按照预定步长θstep扩展生成新节点θnew，即，

5.根据权利要求4所述的一种基于动态候选池引导和碰撞节点启发的机械臂快速路径规划方法，其特征在于：在步骤s400中，新扩展方向θver_unit的计算方法为，

6.根据权利要求5所述的一种基于动态候选池引导和碰撞节点启发的机械臂快速路径规划方法，其特征在于：在步骤s500中，插值点碰撞检测为，

7.根据权利要求6所述的一种基于动态候选池引导和碰撞节点启发的机械臂快速路径规划方法，其特征在于：在步骤s500中，当找到无碰撞配对节点后，将新节点θadd添加到随机树etree1中，

8.一种基于动态候选池引导和碰撞节点启发的机械臂快速路径规划系统，其特征在于：该系统具有与上述权利要求1-7任一项权利要求的步骤对应的程序模块，运行时执行上述的基于动态候选池引导和碰撞节点启发的机械臂快速路径规划方法中的步骤。

9.一种计算机可读存储介质，其特征在于：所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现权利要求1-7中任一项所述的基于动态候选池引导和碰撞节点启发的机械臂快速路径规划方法的步骤。

技术总结
本发明提供了一种基于动态候选池引导和碰撞节点启发的机械臂快速路径规划方法及系统，属于机械臂路径规划领域。为了解决现有机械臂路径规划算法因概率性目标偏执扩展方式造成的随机树多次重复扩展失败和路径规划时间成本高的问题。本发明在现有RRTs算法的基础上，提出了一种基于动态候选池引导和碰撞节点启发的机械臂快速路径规划方法，引导最邻近节点的最佳选择和调节碰撞节点的扩展方向，提高机械臂路径规划效率和缩短路径规划时间成本，实现随机树朝向目标节点方向快速扩展生成无碰撞节点，进而大大提高机械臂路径规划效率和降低路径规划的时间成本。

技术研发人员：张立宪,肖广洲,丁一航,董逸飞,朱益民,林泽峰,刘阳光,王越
受保护的技术使用者：哈尔滨工业大学
技术研发日：
技术公布日：2024/12/5