一种基于双向采样和虚拟势场引导的机械臂路径规划方法

本发明涉及机械臂路径规划，尤其是涉及一种基于双向采样和虚拟势场引导的机械臂路径规划方法。

背景技术：

1、随着现代工业和服务领域对自动化技术的迅速需求增加，机械臂已经在各个领域得到广泛应用，如制造业生产线、医疗手术机器人、物流仓储拣选、农业果实采摘等。在实际操作中，机械臂需要考虑复杂环境障碍和特定任务需求等因素， 找到高效无碰撞的机械臂运动路径，以实现高效、精确和安全的操作。

2、传统的机械臂路径规划方法大致可以分为：基于图搜索的路径规划方法、基于群优化的智能仿生算法以及基于采样的路径规划方法。基于图搜索的路径规划方法有dijkstra算法、算法、算法等；基于群优化的智能仿生算法有遗传算法、粒子群算法、蚁群算法等；基于采样的路径规划方法有prm算法、rrt系列算法等。其中，图搜索算法如dijkstra算法要遍历所有节点，效率低下。算法的效率高度依赖于所选的启发式函数，一个不恰当的启发式函数可能导致效率降低或无法找到最优解。基于图搜索的路径规划方法，对于大型图或高维度空间需要考虑大量的节点和边，导致计算复杂性增加，而机械臂一般有6个自由度，运动过程的计算量非常庞大，此类方法不适用。基于群优化的智能仿生算法都是模拟生物和自然过程来解决优化问题，此类方法易陷入局部最优解、高维复杂环境计算成本较高。基于采样的路径规划方法如rrt算法，对状态空间进行随机采样点进行碰撞检测，避免了对空间的建模，适合解决多自由度机器人在高维空间和复杂动态环境下的路径规划问题。但rrt算法生成的路径存在较多冗余节点，且得到的路径不是最优的，机械臂运动做了较多无用运动，且不能精确避障。

技术实现思路

1、针对上述机械臂在路径规划过程中存在的冗余采样点过多、避障随机性高、规划时间较长等技术问题，本发明提出了一种基于双向采样和虚拟势场引导的机械臂路径规划方法，该方法解决了传统采样路径规划算法冗余采样点过多、避障随机性高、规划时间较长的问题，可实现在工作空间中规划一条渐进最优的无碰撞路径。

2、一种基于双向采样和虚拟势场引导的机械臂路径规划方法，包括如下步骤：

3、s1、定义机械臂的工作环境信息的栅格地图，栅格地图中包括障碍物，确定机械臂末端的初始点和目标点在栅格地图中的初始点和目标点；

4、s2、将栅格地图中的初始点和目标点分别设置为初始随机树的根节点和目标随机树的根节点，将栅格地图中的初始点和目标点还分别设置为目标随机树目标点和初始随机树的目标点，预设扩展步长、终止检测阈值以及扩展次数；

5、s3、以初始随机树和目标随机树各自对应的根节点和目标点为参考，采用双向随机树导向策略进行第n次采样，得到两颗随机树第n次采样后各自对应的采样点，基于第n次采样后各自对应的采样点，采用预设的扩展策略对两颗随机树交替进行第n次扩展，并对扩展后产生的节点执行碰撞检测并处理，得到两颗随机树第n次扩展后各自对应的节点，并将第n次扩展后各自对应的节点分别加入对应的随机树中；

6、s4、根据预设的终止检测阈值l以及预设的扩展次数判断两颗随机树第n次扩展后是否满足随机树扩展的终止条件，若不满足终止条件，则执行s3，对两颗随机树进行第n+1次扩展；

7、s5、若满足终止条件，则对两颗随机树终止扩展，输出栅格地图中从初始点到目标点的初始路径，对初始路径进行修剪优化，将优化后路径作为规划出的机械臂路径。

8、优选地，s3中双向随机树导向策略具体为：初始随机树和目标随机树中的任意一颗随机树要进行扩展时，选取另一颗随机树上一次扩展后得到的节点作为采样点。

9、优选地，s3中预设的扩展策略具体为两种，一种是往采样点的方向以预设的扩展步长s进行一次扩展；另一种是往采样点的方向以预设的扩展步长s进行多次重复扩展直至遇到栅格地图中的障碍物。

10、优选地，s3中采用预设的扩展策略对两颗随机树交替进行第n次扩展，具体包括：两棵随机树交替使用两种扩展策略进行扩展，两颗随机树交替扩展一次为一次扩展，一次扩展完成后，两颗随机树交换扩展策略再进行下一次扩展。

11、优选地，s3中对扩展后产生的节点执行碰撞检测并处理，得到两颗随机树第n次扩展后各自对应的节点，具体包括：

12、s31、对第n次扩展后各自产生的节点分别进行碰撞检测，若未发生碰撞，直接将该节点作为第n次扩展后各自对应的节点，即：

13、；

14、其中，为当前随机树第n次扩展后的节点，为当前随机树第n-1次扩展后的节点，为当前随机树在第n次扩展时从另一颗随机树中选取的采样点，为随机树预设的扩展步长。

15、s32、若发生碰撞，将发生碰撞的节点删除，并采用虚拟势场导向策略得到避开障碍物的节点，将采用虚拟势场导向策略得到的避开障碍物的节点作为第n次扩展后各自对应的节点，即：

16、；

17、其中，为当前随机树采用虚拟势场导向策略得到的第n次扩展后的节点，为当前随机树第n-1次扩展后的节点，为当前随机树采用虚拟势场导向策略在第n次扩展时的虚拟势场力。

18、优选地，s32中虚拟势场导向策略具体为：在当前随机树的目标点周围构建引力势场，在当前随机树第n次扩展时遇到的障碍物周围构建斥力势场，将在第n次扩展时引力势场的引力和斥力势场的斥力形成的合力作为当前随机树在第n次扩展时的虚拟势场力，虚拟势场力用于指引当前随机树在第n次扩展遇到障碍物后的生长方向。

19、优选地，虚拟势场力的计算公式具体为：

20、；

21、式中，；

22、；

23、其中，为当前随机树采用虚拟势场导向策略在第n次扩展时的虚拟势场力，为当前随机树的目标点的引力势场对当前随机树第n次扩展时的引力，为当前随机树第n次扩展时遇到的障碍物的斥力势场对当前随机树第n次扩展时的斥力，为目标点引力尺度因子，为当前随机树第n-1次扩展后的节点，为当前随机树在第n次扩展时从另一颗随机树中选取的采样点，为障碍物斥力尺度因子，当前随机树中第n-1次扩展后的节点距离当前随机树在第n次扩展时遇到的障碍物的最小距离，为障碍物斥力影响距离。

24、优选地，s4中根据预设的终止检测阈值l以及预设的扩展次数判断两颗随机树第n次扩展后是否满足随机树扩展的终止条件，具体如下：

25、s41、计算每颗随机树第n次扩展后得到的节点与采样点之间的欧式距离；

26、s42、将欧式距离和预设的终止检测阈值、当前扩展次数和预设的扩展次数分别进行比较；

27、s43、若欧式距离大于或等于终止检测阈值，且当前扩展次数小于预设的扩展次数，则不满足随机树扩展的终止条件；

28、s44、若欧式距离小于终止检测阈值，或当前扩展次数等于预设的扩展次数，则满足随机树扩展的终止条件。

29、优选地，s41中计算每颗随机树第n次扩展后得到的节点与采样点之间的欧式距离，欧式距离的计算公式具体为：

30、；

31、其中，为第n次扩展后得到的节点与采样点之间的欧式距离，和分别为第n次扩展后得到的节点的坐标，和分别为第n次扩展时选取的采样点的坐标。

32、优选地，s5中对初始路径进行修剪优化，将优化后路径作为规划出的机械臂路径，具体包括：

33、s51、从初始路径的初始点开始，往前数两个点，判断第一个点和第三个点之间的连线之间是否存在障碍；

34、s52、如果第一个点和第三个点之间的连线之间不存在障碍，则第二个点为冗余节点，从初始路径中将其删除，反之则保留第二个点；

35、s53、从第三个点开始，再往前数两个点，重复步骤s51至s52，直到连接到初始路径上的目标点，得到优化后的路径；

36、s54、将优化后路径作为规划出的机械臂路径。

37、上述一种基于双向采样和虚拟势场引导的机械臂路径规划方法，定义机械臂的工作环境信息的栅格地图，并确定机械臂末端的初始点和目标点在栅格地图中的初始点和目标点，将栅格地图中的初始点和目标点分别设置为初始随机树的根节点和目标随机树的根节点，对两颗随机树采用双向随机树导向策略进行采样，得到各自对应的采样点，基于各自对应的采样点，采用预设的扩展策略对两颗随机树交替进行扩展，并对扩展后产生的节点执行碰撞检测并处理，得到两颗随机树在每次扩展后各自对应的节点，并将每次扩展后各自对应的节点加入对应的随机树中；根据预设的终止检测阈值l以及预设的扩展次数判断是否满足随机树扩展的终止条件，若不满足终止条件，则对两颗随机树进行下一次扩展；若满足终止条件，则终止扩展，输出栅格地图上从初始点到目标点的初始路径并优化，将优化后路径作为规划出的机械臂路径。本发明的技术方案具有如下优点：

38、（1）采用了双向随机树导向策略，当两颗随机树中的一棵随机树要扩展时，选取另一颗随机树上一次扩展后得到的节点作为采样点，以此采样点作为随机树探索的导向，能加快两颗随机树收敛、加快路径生成的速度；

39、（2）在随机树扩展周期中，两颗随机树中的一颗随机树会以另一棵随机树在最近一次扩展后得到的节点为采样点，并以预设的扩展步长重复扩展，直到遇到障碍物，可以使算法加快随机树在无障碍区域的扩展效率，并减少在空白区域的冗余采样点数量；

40、（3）在遇到障碍物时，采用虚拟势场导向策略，将障碍物的斥力与目标点的引力结合，采用形成的合力（也就是虚拟势场力）指引随机树新的扩展方向，并且在检测到陷入局部最小值时，更换随机树采样方式为随机树概率采样，使算法不易陷入局部最优区域，并能精确进行避障, 降低避障随机性；

41、（4）对生成的初始路径进行路径修剪，去除路径中的冗余节点，大大减少路径中的节点数量。