CN114310904A - 一种适用于机械臂关节空间路径规划的新型双向rrt*方法 - Google Patents

Abstract

本发明公开了一种适用于机械臂关节空间路径规划的新型双向RRT*方法；为了提高其规划效率并避免逆运动学求解所带来的繁琐计算，设计一种基于人工势场法的拓展策略，用关节空间内发生碰撞的机械臂关节角组来表示相应的障碍物信息，并将其应用于目标偏向的双向快速随机搜索树方法中以此提高机械臂的避障能力；然后提出一种直连策略，在生成新节点前判断当前搜索树的最新节点能否与另一颗搜索树直接相连，以此提高搜索的效率。

Description

一种适用于机械臂关节空间路径规划的新型双向RRT*方法

技术领域

本发明涉及一种关节空间内的新型势能指引双向RRT*方法，主要应用于机械臂的路径规划，属于机器人技术领域。

背景技术

路径规划是保障机械臂安全作业的关键技术之一，它能够在给定机械臂的初始状态与目标状态的前提下，使得机械臂可以自动规划出一条无碰撞的最优路径。路径规划方法一般可以分为以下三类：基于图形的搜索方法、人工势场法和基于采样的规划方法。其中RRT方法是目前应用最广泛的路径规划方法中的一种，它是一种基于采样的全局规划方法，并且具有概率完备性，但其收敛速度较慢，且无法保证规划出的路径是最优解，因此针对上述问题在RRT方法的基础上又衍生出三种方法，分别为目标偏向RRT方法，双向RRT方法与RRT*方法。为了进一步提高规划性能，将三种衍生方法彼此结合，得到目标偏向的双向RRT*方法(简称GB-RRT*)。

机械臂的路径规划既能在任务空间内进行也能在关节空间内进行，但任务空间内的路径规划需要进行逆运动学求解，计算繁琐，效率低，因此本发明在关节空间内采用GB-RRT*方法对机械臂进行路径规划。但GB-RRT*方法未考虑障碍物对运动对象的影响，所以规划过程中会出现许多无效节点，从而影响其规划效率。针对上述问题将人工势场法与RRT*方法相结合，减少无效节点的产生并提高了规划效率。然而，基于人工势场法的GB-RRT*方法(简称PB-RRT*)并不适用于关节空间内的路径规划，因人工势场法需要在障碍物处设置虚拟的排斥力，那么就必须获得障碍物的相关描述信息，但障碍物无法直接在关节空间内进行表述，所以人工势场法无法直接应用于关节空间。

因此如何在关节空间内有效地描述障碍物信息，是PB-RRT*方法应用于关节空间内机械臂路径规划时亟待解决的问题。

发明内容

本发明索要解决的技术问题是：为了实现机械臂的路径规划并提高其效率，提出了一种适用于机械臂关节空间路径规划的新型双向RRT*方法(简称NPB-RRT*)。将人工势场法与GB-RRT*方法相结合，采用发生碰撞的机械臂关节角组来描述关节空间内的障碍物信息，并对关节空间内的NPB-RRT*方法进一步改进，在执行采样操作之前通过直连策略判断两颗搜索树的连通性，以此突破拓展步长的限制从而提高方法的搜索效率。

本发明采用的技术方案是，一种适用于机械臂关节空间路径规划的新型双向RRT*方法，具体按照以下步骤实施：

步骤1，分别在初始关节角q_start与目标关节角q_goal处各建立一颗搜索树T₁与T₂，并将这两组关节角作为各自的根节点；

步骤2，计算搜索树T₁的最新节点q_current与搜索树T₂中所有节点之间的距离，然后挑选出与节点q_current距离最近的节点q_closest，并对两者进行直连判断；若结果显示两者无法直连，则舍弃该局部路径并执行步骤3的操作；反之，则将这两个节点直接相连，并结束整个规划过程；

步骤3，随机选取搜索树T₂中的一个节点作为局部目标点q_localgoal，然后判断随机概率p_rand是否小于给定的阈值概率p_threshold，若小于，则在机械臂关节运动范围内随机选取一组关节角q_rand作为采样点q_sample；反之，则将局部目标点q_localgoal作为采样点q_sample；

步骤4，在树T₁中搜寻距离采样点q_sample最近的节点q_near；

步骤5，若采样点q_sample为随机点q_rand，则在采样点q_sample与节点q_near的连线上拓展一个新节点q_new；若采样点q_sample为局部目标点q_localgoal，则通过人工势场法确定拓展方向，并在该方向上拓展一个新节点q_new；其中点q_new与节点q_near之间的距离为拓展步长δ；然后对点q_new对应的机械臂的空间状态进行碰撞检测，若机械臂不发生碰撞，则将点q_new作为新节点加入树T₁中；反之，则将该点作为描述障碍物的特征点进行保存，且不将其加入树T₁，然后跳转到步骤9；

步骤6，在点q_new的邻域内重选其父节点，依次将邻域内的各节点当作点q_new的父节点并计算此时的路径代价，再从中选取路径代价最小的节点作为其新的父节点；

步骤7，在点q_new的邻域内进行改线操作，将点q_new作为各节点的父节点并计算此时的路径代价，若其小于原路径代价，则舍弃该节点原有的父节点而将点q_new作为其新的父节点；反之，则保留原有的父节点；

步骤8，检测新节点q_new是否包含在树T₂中或与树T₂的某个节点间的距离小于给定阈值，若满足上述条件，则代表树T₁与树T₂彼此相连，规划过程结束；反之，执行步骤9；

步骤9，判断当前搜索树是T₁还是T₂，若是树T₁，则将操作对象切换至树T₂；若是树T₂，则将操作对象切换至树T₁；然后返回执行步骤2到步骤8的操作；

为了进一步实现本发明的目的，还可以采用以下技术方案：

如上所述的一种适用于机械臂关节空间路径规划的新型双向RRT*方法，所述步骤2中，直连策略对节点q_current与节点q_closest间的关节路径进行离散，检测每个离散点对应的机械臂的末端位置是否落在障碍物的OBB包围盒的边界坐标范围内，如公式(1)所示；若存在离散点落在障碍物的边界坐标范围内，则表示节点q_current与节点q_closest无法直接连接；反之，则对每个离散点对应的机械臂的空间状态进行碰撞检测，若每个离散点均不发生碰撞，则表示节点q_current与节点q_closest能够直接连接；若存在发生碰撞的离散点，则表示节点q_current与节点q_closest无法直接连接；

其中x_p,y_p,z_p―分别代表离散点p在任务空间内的X轴,Y轴和Z轴的坐标；

―分别代表障碍物在任务空间内的X轴,Y轴和Z轴的下边界坐标；

―分别代表障碍物在任务空间内的X轴,Y轴和Z轴的上边界坐标；

如上所述的一种适用于机械臂关节空间路径规划的新型双向RRT*方法，所述步骤5中，关节空间内基于人工势场法的拓展策略在目标节点处虚拟设置一个引力势场，如公式(2)所示；其对当前节点产生一个指向目标节点的吸引力，如公式(3)所示；在每个障碍物处与其发生碰撞的每组关节角均产生一个斥力势场，如公式(4)所示，其对当前节点产生一个背向障碍物的排斥力，如公式(5)所示；则每个障碍物产生的排斥力为与其发生碰撞的每组关节角所产生的排斥力的平均值，如公式(6)所示；环境中所有障碍物产生的总排斥力即为每个障碍物所产生的排斥力的合力，如公式(7)所示；

其中q_near―搜索树T₁中距离局部目标点q_localgoal最近的节点；U_att―局部目标点q_localgoal对节点q_near的引力势场函数；k_att―引力势场系数；

其中F_att―局部目标点q_localgoal对节点q_near的引力；

其中

―与第i个障碍物发生碰撞的第j组关节角；

―关节角

对节点q_near的斥力势场函数；

―第i个障碍物的斥力势场系数；

―关节角

与节点q_near间的欧式距离；

―第i个障碍物产生斥力作用的最大影响距离；l―周边环境中障碍物的个数；m―与第i个障碍物发生碰撞的关节角组数；

其中

―关节角

对节点q_near的斥力；

其中

―第i个障碍物对节点q_near的总斥力；

其中F_rep―周边环境中所有障碍物对节点q_near的总斥力；

本发明采用以上技术方案，具有以下技术效果：

1.本发明采用发生碰撞的机械臂关节角组来描述关节空间内的障碍物信息，设计了一种适用于关节空间的人工势场法，并将其与GB-RRT*方法相结合，提出一种适用于机械臂的关节空间内的NPB-RRT*方法。；

2.本发明在执行采样操作之前通过直连策略判断两颗搜索树的连通性，以此突破拓展步长的限制从而提高方法的搜索效率；

3.本发明在规划机械臂路径时无需计算其繁琐的逆运动学，从而提高了方法的执行效率。

附图说明

图1为本发明一种适用于机械臂关节空间路径规划的新型双向RRT*方法的流程图；

图2为本发明实施案例在仿真环境中的规划结果。

具体实施方式

下面详细描述本发明的实施方式。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

本发明的目的是为了提高机械臂路径规划的效率，提出了一种适用于机械臂关节空间路径规划的新型双向RRT*方法。用关节空间内发生碰撞的机械臂关节角组来表示相应的障碍物信息，将人工势场法与GB-RRT*方法相结合，并采用直连策略提高搜索的效率。

如图1所示，本实施例公开一种适用于机械臂关节空间路径规划的新型双向RRT*方法，包括以下步骤：

步骤1，分别在初始关节角q_start与目标关节角q_goal处各建立一颗搜索树T₁与T₂，并将这两组关节角作为各自的根节点；

步骤2，计算搜索树T₁的最新节点q_current与搜索树T₂中所有节点之间的距离，然后挑选出与节点q_current距离最近的节点q_closest，并对两者进行直连判断；若结果显示两者无法直连，则舍弃该局部路径并执行步骤3的操作；反之，则将这两个节点直接相连，并结束整个规划过程；

步骤3，随机选取搜索树T₂中的一个节点作为局部目标点q_localgoal，然后判断随机概率p_rand是否小于给定的阈值概率p_threshold，若小于，则在机械臂关节运动范围内随机选取一组关节角q_rand作为采样点q_sample；反之，则将局部目标点q_localgoal作为采样点q_sample；

步骤4，在树T₁中搜寻距离采样点q_sample最近的节点q_near；

步骤5，若采样点q_sample为随机点q_rand，则在采样点q_sample与节点q_near的连线上拓展一个新节点q_new；若采样点q_sample为局部目标点q_localgoal，则通过人工势场法确定拓展方向，并在该方向上拓展一个新节点q_new；其中点q_new与节点q_near之间的距离为拓展步长δ；然后对点q_new对应的机械臂的空间状态进行碰撞检测，若机械臂不发生碰撞，则将点q_new作为新节点加入树T₁中；反之，则将该点作为描述障碍物的特征点进行保存，且不将其加入树T₁，然后跳转到步骤9；

步骤6，在点q_new的邻域内重选其父节点，依次将邻域内的各节点当作点q_new的父节点并计算此时的路径代价，再从中选取路径代价最小的节点作为其新的父节点；

步骤7，在点q_new的邻域内进行改线操作，将点q_new作为各节点的父节点并计算此时的路径代价，若其小于原路径代价，则舍弃该节点原有的父节点而将点q_new作为其新的父节点；反之，则保留原有的父节点；

步骤8，检测新节点q_new是否包含在树T₂中或与树T₂的某个节点间的距离小于给定阈值，若满足上述条件，则代表树T₁与树T₂彼此相连，规划过程结束；反之，执行步骤9；

步骤9，判断当前搜索树是T₁还是T₂，若是树T₁，则将操作对象切换至树T₂；若是树T₂，则将操作对象切换至树T₁；然后返回执行步骤2到步骤8的操作；

其中，步骤2的具体操作：直连策略对节点q_current与节点q_closest间的关节路径进行离散，检测每个离散点对应的机械臂的末端位置是否落在障碍物的OBB包围盒的边界坐标范围内，如公式(1)所示；若存在离散点落在障碍物的边界坐标范围内，则表示节点q_current与节点q_closest无法直接连接；反之，则对每个离散点对应的机械臂的空间状态进行碰撞检测，若每个离散点均不发生碰撞，则表示节点q_current与节点q_closest能够直接连接；若存在发生碰撞的离散点，则表示节点q_current与节点q_closest无法直接连接；

其中x_p,y_p,z_p―分别代表离散点p在任务空间内的X轴,Y轴和Z轴的坐标；

―分别代表障碍物在任务空间内的X轴,Y轴和Z轴的下边界坐标；

―分别代表障碍物在任务空间内的X轴,Y轴和Z轴的上边界坐标；

步骤5的具体操作：关节空间内基于人工势场法的拓展策略在目标节点处虚拟设置一个引力势场，如公式(2)所示；其对当前节点产生一个指向目标节点的吸引力，如公式(3)所示；在每个障碍物处与其发生碰撞的每组关节角均产生一个斥力势场，如公式(4)所示，其对当前节点产生一个背向障碍物的排斥力，如公式(5)所示；则每个障碍物产生的排斥力为与其发生碰撞的每组关节角所产生的排斥力的平均值，如公式(6)所示；环境中所有障碍物产生的总排斥力即为每个障碍物所产生的排斥力的合力，如公式(7)所示；

其中q_near―搜索树T₁中距离局部目标点q_localgoal最近的节点；U_att―局部目标点q_localgoal对节点q_near的引力势场函数；k_att―引力势场系数；

其中F_att―局部目标点q_localgoal对节点q_near的引力；

其中

―与第i个障碍物发生碰撞的第j组关节角；

―关节角

对节点q_near的斥力势场函数；

―第i个障碍物的斥力势场系数；

―关节角

与节点q_near间的欧式距离；

―第i个障碍物产生斥力作用的最大影响距离；l―周边环境中障碍物的个数；m―与第i个障碍物发生碰撞的关节角组数；

其中

―关节角

对节点q_near的斥力；

其中

―第i个障碍物对节点q_near的总斥力；

其中F_rep―周边环境中所有障碍物对节点q_near的总斥力；

以下用具体的仿真对本发明的实施方式进行详细说明：

在仿真软件中搭建与实际尺寸1:1的仿真平台，然后分别在机械臂末端的初始位置与目标位置处建立搜索树T₁与T₂，然后采用NPB-RRT*方法规划机械臂的路径，规划结束后得到的路径在仿真环境中以线条形式表示，如图2所示，其中矩形物体代表障碍物。在实际运动过程中，机械臂按照图2中的路径进行运动，则能够有效地避开障碍物。此外，经过统计计算，相对于GB-RRT*方法，NPB-RRT*方法的规划耗时缩短了58.85％，其规划效率得到了明显提升。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，聚落如本发明保护范围。

Claims (3)

1.一种适用于机械臂关节空间路径规划的新型双向RRT*方法，其特征在于，包括下列步骤：

步骤1，分别在初始关节角q_start与目标关节角q_goal处各建立一颗搜索树T₁与T₂，并将这两组关节角作为各自的根节点；

步骤2，计算搜索树T₁的最新节点q_current与搜索树T₂中所有节点之间的距离，然后挑选出与节点q_current距离最近的节点q_closest，并对两者进行直连判断；若结果显示两者无法直连，则舍弃该局部路径并执行步骤3的操作；反之，则将这两个节点直接相连，并结束整个规划过程；

步骤3，随机选取搜索树T₂中的一个节点作为局部目标点q_localgoal，然后判断随机概率p_rand是否小于给定的阈值概率p_threshold，若小于，则在机械臂关节运动范围内随机选取一组关节角q_rand作为采样点q_sample；反之，则将局部目标点q_localgoal作为采样点q_sample；

步骤4，在树T₁中搜寻距离采样点q_sample最近的节点q_near；

步骤5，若采样点q_sample为随机点q_rand，则在采样点q_sample与节点q_near的连线上拓展一个新节点q_new；若采样点q_sample为局部目标点q_localgoal，则通过人工势场法确定拓展方向，并在该方向上拓展一个新节点q_new；其中点q_new与节点q_near之间的距离为拓展步长δ；然后对点q_new对应的机械臂的空间状态进行碰撞检测，若机械臂不发生碰撞，则将点q_new作为新节点加入树T₁中；反之，则将该点作为描述障碍物的特征点进行保存，且不将其加入树T₁，然后跳转到步骤9；

步骤6，在点q_new的邻域内重选其父节点，依次将邻域内的各节点当作点q_new的父节点并计算此时的路径代价，再从中选取路径代价最小的节点作为其新的父节点；

步骤7，在点q_new的邻域内进行改线操作，将点q_new作为各节点的父节点并计算此时的路径代价，若其小于原路径代价，则舍弃该节点原有的父节点而将点q_new作为其新的父节点；反之，则保留原有的父节点；

步骤8，检测新节点q_new是否包含在树T₂中或与树T₂的某个节点间的距离小于给定阈值，若满足上述条件，则代表树T₁与树T₂彼此相连，规划过程结束；反之，执行步骤9；

步骤9，判断当前搜索树是T₁还是T₂，若是树T₁，则将操作对象切换至树T₂；若是树T₂，则将操作对象切换至树T₁；然后返回执行步骤2到步骤8的操作。

2.根据权利要求1所述的一种适用于机械臂关节空间路径规划的新型双向RRT*方法，其特征在于，所述步骤2中，直连策略对节点q_current与节点q_closest间的关节路径进行离散，检测每个离散点对应的机械臂的末端位置是否落在障碍物的OBB包围盒的边界坐标范围内，如公式(1)所示；若存在离散点落在障碍物的边界坐标范围内，则表示节点q_current与节点q_closest无法直接连接；反之，则对每个离散点对应的机械臂的空间状态进行碰撞检测，若每个离散点均不发生碰撞，则表示节点q_current与节点q_closest能够直接连接；若存在发生碰撞的离散点，则表示节点q_current与节点q_closest无法直接连接；

其中x_p,y_p,z_p―分别代表离散点p在任务空间内的X轴,Y轴和Z轴的坐标；

―分别代表障碍物在任务空间内的X轴,Y轴和Z轴的下边界坐标；

―分别代表障碍物在任务空间内的X轴,Y轴和Z轴的上边界坐标。

3.根据权利要求1所述的一种适用于机械臂关节空间路径规划的新型双向RRT*方法，其特征在于，所述步骤5中，关节空间内基于人工势场法的拓展策略在目标节点处虚拟设置一个引力势场，如公式(2)所示；其对当前节点产生一个指向目标节点的吸引力，如公式(3)所示；在每个障碍物处与其发生碰撞的每组关节角均产生一个斥力势场，如公式(4)所示，其对当前节点产生一个背向障碍物的排斥力，如公式(5)所示；则每个障碍物产生的排斥力为与其发生碰撞的每组关节角所产生的排斥力的平均值，如公式(6)所示；环境中所有障碍物产生的总排斥力即为每个障碍物所产生的排斥力的合力，如公式(7)所示；

其中q_near―搜索树T₁中距离局部目标点q_localgoal最近的节点；U_att―局部目标点q_localgoal对节点q_near的引力势场函数；k_att―引力势场系数；

其中F_att―局部目标点q_localgoal对节点q_near的引力；

其中

―与第i个障碍物发生碰撞的第j组关节角；

―关节角

对节点q_near的斥力势场函数；

―第i个障碍物的斥力势场系数；

―关节角

与节点q_near间的欧式距离；

―第i个障碍物产生斥力作用的最大影响距离；l―周边环境中障碍物的个数；m―与第i个障碍物发生碰撞的关节角组数；

其中

―关节角

对节点q_near的斥力；

其中

―第i个障碍物对节点q_near的总斥力；

其中F_rep―周边环境中所有障碍物对节点q_near的总斥力。

Images