路径规划方法、装置、机械臂控制器和可读存储介质与流程

本申请涉及机器人技术领域，特别是涉及一种路径规划方法、装置、机械臂控制器和可读存储介质。

背景技术：

机器人可以广泛应用于老人陪护、医疗手术协助、娱乐家居以及工业生产等诸多领域，而机械臂的路径规划是机器人研究内容的核心之一。路径规划是指对于确定的多自由度的机械臂，当给定起点和终点时，计算出满足约束的路径代价较小的路径，使得机械臂可以在从起点无碰撞地移动到终点。

一种机械臂的路径规划方法是：在对机械臂进行参数化建模并建立正逆运动学模型之后，采用aabb(axis-alignedboundingbox)包围盒法建立机械臂及环境中障碍物的aabb包围盒(用于快速的进行碰撞检测或者进行精确的碰撞检测之前进行过滤)，然后使用rrt(rapidlyexploringrandomtree，快速探索随机树)算法在约束条件下规划出无碰撞的路径。

然而，当起点和终点之间存在凹形障碍物等障碍物时，上述路径规划方法难以搜索出有效路径，导致机械臂难以避开障碍物。

技术实现要素：

基于此，有必要针对上述技术问题，提供一种在起点和终点之间存在凹形障碍物等障碍物时仍能搜索出有效路径的路径规划方法、装置、机械臂控制器和可读存储介质。

第一方面，一种路径规划方法，所述方法包括：

确定机械臂的任务起点在机械臂的关节空间中的起点位形、机械臂的任务终点在所述关节空间中的终点位形，以及确定所述关节空间的采样空间；

根据所述采样空间、起点位形和终点位形，以及预设的第一步长和第二步长，进行随机树生长处理，得到随机树的多个树节点；所述第一步长为新的树节点向采样点生长的步长，所述第二步长为新的树节点向目标树节点生长的步长，所述采样点是从所述采样空间中采样得到的，所述目标树节点为所述起点位形或所述终点位形；

在进行随机树生长处理过程中，进行局部极小状态判断，在判断随机树生长陷入局部极小状态时，调整所述第一步长和所述第二步长，使得所述第一步长大于所述第二步长；

根据所述随机树的多个树节点，得到所述任务起点至所述任务终点在关节空间中的初始路径。

在其中一个实施例中，所述方法还包括：

在判断随机树生长未陷入局部极小状态时，调整所述第一步长和所述第二步长，使得所述第一步长小于所述第二步长。

在其中一个实施例中，所述进行局部极小状态判断，包括：

获取最新的树节点以及所述最新的树节点之前的至少一个树节点，计算所述最新的树节点和所述至少一个树节点之间的至少一个节点距离；

当所述至少一个节点距离均小于或等于预设距离阈值时，确定所述随机树生长陷入局部极小状态；

当所述至少一个节点距离大于预设距离阈值时，确定所述随机树生长未陷入局部极小状态。

在其中一个实施例中，所述方法还包括：

将所述任务起点至所述任务终点在关节空间中的初始路径作为当前路径，进行路径优化处理，得到新的路径；

其中，所述路径优化处理包括：

计算所述当前路径的路径代价；

根据所述路径代价、所述起点位形和所述终点位形，确定一个椭球体空间作为所述关节空间的新的采样空间；所述新的采样空间中各点和所述起点位形、所述终点位形的距离之和小于或等于所述路径代价；

根据所述新的采样空间，进行随机树生长处理，得到所述任务起点至所述任务终点在关节空间中的待定路径，以及当所述待定路径的路径代价小于所述当前路径的路径代价时，将所述待定路径作为新的路径。

在其中一个实施例中，所述方法还包括：

将所述新的路径作为当前路径，继续进行路径优化处理，直至所述新的路径满足预设条件，得到所述最终的新的路径；

其中，确定所述新的路径满足预设条件包括：

获取最新的路径以及所述最新的路径之前的至少一个路径，计算所述最新的路径和所述至少一个路径之间的至少一个路径代价的差值；

当所述至少一个路径代价的差值均小于或等于预设路径代价阈值时，确定所述新的路径满足预设条件。

在其中一个实施例中，所述方法还包括：

根据包裹障碍物法对所述最新的路径进行优化，得到优化后的目标路径。

在其中一个实施例中，所述根据所述采样空间、起点位形和终点位形，以及预设的第一步长和第二步长，进行随机树生长处理，包括：

将所述终点位形作为第一随机树的目标树节点，以及将所述起点位形作为第二随机树的目标树节点；

分别针对所述第一随机树和所述第二随机树，根据所述采样空间、预设的第一步长和第二步长，进行随机树生长处理，直至第一随机树的最新树节点和第二随机树的最新树节点之间的连线满足无碰撞条件。

在其中一个实施例中，所述根据所述采样空间、预设的第一步长和第二步长，进行随机树生长处理，包括：

从所述关节空间的采样空间中采样得到采样点；

遍历所述随机树中各树节点，确定与所述采样点距离最小的树节点作为父节点；所述随机树为所述第一随机树或所述第二随机树；

根据所述父节点、采样点、目标树节点，以及所述第一步长和所述第二步长，确定从所述父节点生长出的子节点；

当所述子节点和所述父节点之间的连线满足无碰撞条件时，将所述子节点作为所述随机树的新的树节点。

在其中一个实施例中，所述根据所述随机树的多个树节点，得到所述任务起点至所述任务终点在关节空间中的路径，包括：

根据所述第一随机树的最新树节点和所述第一随机树中各树节点的父子关联关系，确定所述第一随机树中从所述起点位形至所述最新树节点的路径上的多个第一树节点和所述多个第一树节点的顺序；

根据所述第二随机树的最新树节点和所述第二随机树中各树节点的父子关联关系，确定所述第二随机树中从所述最新树节点至所述终点位形的路径上的多个第二树节点和所述多个第二树节点的顺序；

根据所述多个第一树节点和所述多个第一树节点的顺序，以及所述多个第二树节点和所述多个第二树节点的顺序，确定从所述起点位形至所述终点位形的路径上的多个树节点以及所述多个树节点的顺序，作为所述任务起点至所述任务终点在关节空间中的路径。

第二方面，一种路径规划装置，所述装置包括：

初始化模块，用于确定机械臂的任务起点在机械臂的关节空间中的起点位形、机械臂的任务终点在所述关节空间中的终点位形，以及确定所述关节空间的采样空间；

随机树生长模块，用于根据所述采样空间、起点位形和终点位形，以及预设的第一步长和第二步长，进行随机树生长处理，得到随机树的多个树节点；所述第一步长为新的树节点向采样点生长的步长，所述第二步长为新的树节点向目标树节点生长的步长，所述采样点是从所述采样空间中采样得到的，所述目标树节点为所述起点位形或所述终点位形；

第一步长调整模块，用于在进行随机树生长处理过程中，进行局部极小状态判断，在判断随机树生长陷入局部极小状态时，调整所述第一步长和所述第二步长，使得所述第一步长大于所述第二步长；

初始路径获取模块，用于根据所述随机树的多个树节点，得到所述任务起点至所述任务终点在关节空间中的初始路径。

第三方面，一种机械臂控制器，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现以下步骤：

确定机械臂的任务起点在机械臂的关节空间中的起点位形、机械臂的任务终点在所述关节空间中的终点位形，以及确定所述关节空间的采样空间；

根据所述采样空间、起点位形和终点位形，以及预设的第一步长和第二步长，进行随机树生长处理，得到随机树的多个树节点；所述第一步长为新的树节点向采样点生长的步长，所述第二步长为新的树节点向目标树节点生长的步长，所述采样点是从所述采样空间中采样得到的，所述目标树节点为所述起点位形或所述终点位形；

在进行随机树生长处理过程中，进行局部极小状态判断，在判断随机树生长陷入局部极小状态时，调整所述第一步长和所述第二步长，使得所述第一步长大于所述第二步长；

根据所述随机树的多个树节点，得到所述任务起点至所述任务终点在关节空间中的初始路径。

第四方面，一种可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：

确定机械臂的任务起点在机械臂的关节空间中的起点位形、机械臂的任务终点在所述关节空间中的终点位形，以及确定所述关节空间的采样空间；

根据所述采样空间、起点位形和终点位形，以及预设的第一步长和第二步长，进行随机树生长处理，得到随机树的多个树节点；所述第一步长为新的树节点向采样点生长的步长，所述第二步长为新的树节点向目标树节点生长的步长，所述采样点是从所述采样空间中采样得到的，所述目标树节点为所述起点位形或所述终点位形；

在进行随机树生长处理过程中，进行局部极小状态判断，在判断随机树生长陷入局部极小状态时，调整所述第一步长和所述第二步长，使得所述第一步长大于所述第二步长；

根据所述随机树的多个树节点，得到所述任务起点至所述任务终点在关节空间中的初始路径。

上述路径规划方法、装置、机械臂控制器和可读存储介质，机械臂控制器可以通过确定机械臂的任务起点在机械臂的关节空间中的起点位形、机械臂的任务终点在所述关节空间中的终点位形，以及确定所述关节空间的采样空间；根据所述采样空间、起点位形和终点位形，以及预设的第一步长和第二步长，进行随机树生长处理，得到随机树的多个树节点；所述第一步长为新的树节点向采样点生长的步长，所述第二步长为新的树节点向目标树节点生长的步长；根据所述随机树的多个树节点，得到所述任务起点至所述任务终点在关节空间中的初始路径；最为重要的是，在进行随机树生长处理过程中，进行局部极小状态判断，在判断随机树生长陷入局部极小状态时，调整所述第一步长和所述第二步长，使得所述第一步长大于所述第二步长，使得随机树偏向采样点生长，逃脱出局部极小状态，因此可以确保搜索到任务起点至所述任务终点在关节空间中的路径。

附图说明

图1为一个实施例中路径规划方法的应用环境图；

图2为一个实施例中路径规划方法的流程示意图；

图3a为一个实施例中通过优化采样空间优化路径的流程示意图；

图3b为一个实施例中优化的采样空间的示意图；

图3c为一个实施例中包裹障碍物法优化路径的示意图；

图4为一个实施例中双向随机树生长的流程示意图；

图5a为另一个实施例中双向随机树生长的流程示意图；

图5b为一个实施例中双向随机树生长的示意图；

图6为一个实施例中路径规划方法的流程示意图；

图7为一个实施例中路径规划装置的结构框图；

图8为一个实施例中路径规划装置的结构框图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。

本申请实施例提供的路径规划方法，可以应用于如图1所示的机械臂中。其中，该机械臂可以为常规意义上的机械臂，也可以是机器人，或者其它需要进行路径规划的机械结构。机械臂一般由多个可动关节和至少一个执行器组成，各关节可以被转动、滑动或者其它驱动方式，因此机械臂具有多自由度，例如4自由度、6自由度等；具体地，机械臂可以包括机械臂控制器，机械臂控制器可以控制电机驱动各关节运动，进行控制执行器从任务起点移动至任务终点。然而，任务起点移动至任务终点中可能存在障碍物、锁死点、自身干涉等无法通过区域，因此需要进行本实施例所述的路径规划方法规划从任务起点至任务终点的路径，机械臂控制器可以根据所述路径驱动机械臂的各关节，实现机械臂的执行器从任务起点向任务终点的无碰撞移动。可以理解的是，机械臂控制器可以体现为计算机设备、单片机控制系统以及其它可以进行数据处理的装置。

在一个实施例中，如图2所示，提供了一种路径规划方法，以该方法应用于图1中的机械臂控制器为例进行说明，包括以下步骤：

s201，确定机械臂的任务起点在机械臂的关节空间中的起点位形、机械臂的任务终点在所述关节空间中的终点位形，以及确定所述关节空间的采样空间。

机械臂控制器可以存储有建立好的机械臂的正逆运动学方程。其中，正运动学是指已知各关节类型、各相邻关节之间的尺寸、相邻关节相对变化量的大小时，求解执行器在基坐标系(固定坐标系)中的位置和方向；逆运动学与正运动学相反，已知各关节类型、各相邻关节之间的尺寸等信息，以及执行器在基坐标系(固定坐标系)中的位置和方向，求解相邻关节相对变化量的大小。因此，上述正逆运动学方程可以表征相邻关节相对变化量的大小(各关节变量参数)与执行器在基坐标系中的位置和方向的关系，一般体现为变换矩阵的形式；若机械臂的执行器的位置和方向参数构成笛卡尔空间，机械臂的各关节变量参数构成关节空间，则正逆运动学方程相当于机械臂的笛卡尔空间和机械臂的关节空间的一种映射关系。

示例性地，通过denavit和hartenberg提出的一种d-h建模方法，可以在机械臂的各个关节上建立关节坐标系，根据多自由度机械臂的dh参数(dh参数表，用来描述机器人各关节坐标系之间的关系)，以及基坐标系，可以确定上述正逆运动学方程。

在一个实施方式中，机械臂控制器可以根据预设的正逆运动学方程，以及输入的任务起点和任务终点，确定所述任务起点在关节空间中的参数，即起点位形，以及确定所述任务终点在关节空间中的参数，即终点位形；示例性地，当所述机械臂为n自由度时，所述起点位形和所述终点位形分别可以是n维参数，每一维参数分别对应某一个关节的变量参数。

当然，上述机械臂控制器也可以直接接受输入的起点位形和终点位形，或者获取已经预设的计算文件中的起点位形或终点位形，本实施例对此并不限制。

上述关节空间的采样空间可以是预设的采样空间，该采样空间仅是限制各关节的变量参数的取值范围，例如某些变量参数为关节的相对变化角度，而且存在锁死角度，因此需要对该变量参数的取值范围进行限定；此外采样空间的设置是为了在有限的空间内进行采样，以减少计算量。

s202，根据所述采样空间、起点位形和终点位形，以及预设的第一步长和第二步长，进行随机树生长处理，得到随机树的多个树节点；所述第一步长为新的树节点向采样点生长的步长，所述第二步长为新的树节点向目标树节点生长的步长，所述采样点是从所述采样空间中采样得到的，所述目标树节点为所述起点位形或所述终点位形。

在随机树生长处理算法中，机械臂控制器可以将关节空间中起点位形或终点位形中的任一点作为随机树的起始树节点，以另一点作为目标树节点，进行单向随机树生长，即从起始树节点开始，在采样空间中采样得到采样点，然后选取随机树中任一树节点作为父节点(最开始为起始树节点)，根据所述第一步长和所述第二步长，计算得到所述父节点的子节点。示例性地，以起点位形xstart作为起始树节点，以终点位形xgoal作为目标树节点，采样点为xrand，父节点为xf，则子节点xnew为：

其中，ρ1表示子节点(即新的树节点)向采样点方向生长的第一步长，ρ2表示子节点向目标树节点方向生长的步长。一般地，上述采样点是随机采样得到的，为随机节点；当然，上述采样也可以是高斯采样等各种采样方法，本实施例对此并不限制。因此，根据上述随机树生长处理，可以将新的树节点加入随机树中得到新的随机树，再根据新的随机树继续进行上述生长，对于单向生长而言，直至新的树节点与所述目标树节点之间的连线满足无碰撞条件即可。

可以理解的是，在上述随机树生长处理过程中，需要对上述子节点和父节点之间的连线进行碰撞检测；当所述子节点和所述父节点之间的连线满足无碰撞条件时，才可以将所述子节点作为所述随机树的新的树节点。具体地，可以将所述子节点和所述父节点之间的连线等分成n个节点，对这n个节点和所述子节点分别进行碰撞检测，若均未发生碰撞，即满足无碰撞条件，则将子节点作为新的树节点加入随机树中，若发生碰撞则丢弃所述子节点。重复上述子节点的获取和碰撞检测过程直到向随机树中添加一个无碰撞的新的树节点。上述碰撞检测可以采用各种包围盒法进行，这里不再赘述。

在确定上述子节点为无碰撞的新的树节点之后，还可以连接所述新的树节点和所述目标树节点；若两点之间的连线不满足无碰撞条件，则说明随机树需要继续生长；若两点之间的连线满足无碰撞条件，则随机树可以停止生长。

需要说明的是，上述父节点可以是上述随机树中任一树节点，也可以是上述随机树中与采样点距离最近的树节点，也可以是随机树中与目标树节点最近的采样点，还可以是随机树中最新的树节点(即最新产生的新的树节点)等；本实施例对此并不限制。

示例性地，第一步长和第二步长可以以d为标尺，作为d的一个小量；例如第一步长可以为0.025d，第二步长为0.05d，d为起点位形和终点位形之间的距离。在本实施例中，距离可以为欧几里得距离，也可以为其它范数。

s203，在进行随机树生长处理过程中，进行局部极小状态判断，在判断随机树生长陷入局部极小状态时，调整所述第一步长和所述第二步长，使得所述第一步长大于所述第二步长。

可以理解的是，当第二步长大于第一步长时，新的树节点会偏向目标树节点方向，第二步长和第一步长的差值越大，新的树节点越偏向目标树节点，此时规划的路径会越接近忽略障碍物后的最短路径；在没有障碍物的理想情况下这是一种很有效的方法，但是现实环境在任务起点与任务终点之间极有可能存在障碍，如果规划算法一直设定第二步长大于第一步长，在凹形障碍物附近将会陷入局部极小，无法规划出可行路径。而一般地，初始设置时一般设置第二步长大于第一步长，以尽可能高效地进行路径规划，但可能会出现上述陷入局部极小的状态。

因此，本实施例的路径规划方法可以在进行随机树生长处理过程中，进行局部极小状态判断，在判断随机树生长陷入局部极小状态时，调整所述第一步长和所述第二步长，使得所述第一步长大于所述第二步长。

可选地，在一种实施方式中，机械臂控制器可以获取所述随机树的树节点的数目，当所述树节点的数目大于预设阈值时，意味着随机树生长可能陷入局部极小，则判断随机树生长陷入局部极小状态。

可选地，在一种实施方式中，如果连续的几个树节点(按照得到的新的树节点的顺序)处于位形空间中的狭小区域内，很可能附近存在一个局部极小值。示例性地，机械臂控制器可以获取最新的树节点以及所述最新的树节点之前的至少一个树节点，计算所述最新的树节点和所述至少一个树节点之间的至少一个节点距离；当所述至少一个节点距离均小于或等于预设距离阈值时，确定所述随机树生长陷入局部极小状态；当所述至少一个节点距离大于预设距离阈值时，确定所述随机树生长未陷入局部极小状态。其中，最新的树节点为最新得到的新的树节点，所述最新的树节点之前的至少一个树节点为根据得到的新的树节点的顺序而言，所述最新的树节点之前的新的树节点。例如，如果对于某些微小的正数εm(预设距离阈值)，存在||xi-xi+1||＜εm，||xi-xi+2||＜εm，以及||xi-xi+3||＜εm，那么可以认为此时随机树生长陷入局部极小状态，其中xi为随机树中的新的树节点。

s204，根据所述随机树的多个树节点，得到所述任务起点至所述任务终点在关节空间中的初始路径。

可以理解的是，在进行上述随机树生长处理后，得到的随机树的多个树节点中存在部分树节点之间的连线组成的从所述任务起点至所述任务终点之间的初始路径。当目标树节点为起点位形时，具体地，因为随机树生长处理的截止条件一般是最新的树节点与目标树节点之间的连线满足无碰撞条件时，因此可以从所述目标树节点和所述最新的树节点开始，将所述目标树节点和所述最新的树节点作为所述初始路径中的前两个路径节点，然后寻找所述最新的树节点的父节点(即与所述最新的树节点存在父子关系的树节点)作为所述初始路径中的第三个路径节点，进而寻找与所述第三个路径节点存在父子关系的树节点作为第四个路径节点，……，如此直至终点位形作为最后一个路径节点加入到所述；因此，从所述第一个路径节点指向所述第二个路径节点的子路径，从所述第二个路径节点指向所述第三个路径节点的子路径，……，由各路径节点组成的各子路径组成所述任务起点至所述任务终点在关节空间中的初始路径。

本实施例的路径规划方法，机械臂控制器通过确定机械臂的任务起点在机械臂的关节空间中的起点位形、机械臂的任务终点在所述关节空间中的终点位形，以及确定所述关节空间的采样空间；根据所述采样空间、起点位形和终点位形，以及预设的第一步长和第二步长，进行随机树生长处理，得到随机树的多个树节点；所述第一步长为新的树节点向采样点生长的步长，所述第二步长为新的树节点向目标树节点生长的步长；根据所述随机树的多个树节点，得到所述任务起点至所述任务终点在关节空间中的初始路径；最为重要的是，在进行随机树生长处理过程中，进行局部极小状态判断，在判断随机树生长陷入局部极小状态时，调整所述第一步长和所述第二步长，使得所述第一步长大于所述第二步长，使得随机树偏向采样点生长，逃脱出局部极小状态，因此可以确保搜索到任务起点至所述任务终点在关节空间中的路径。

可以理解的是，如果规划算法一直设定第一步长大于第一步长，则新的树节点会一直偏向采样点，这会导致路径规划算法低效的问题。本实施例可以动态地设置第一步长和第二步长，首先假设任务起点与任务终点之间没有障碍物，设定第二步长大于第一步长，使得随机树偏向目标树节点方向持续扩展，直到判断随机树生长陷入了局部极小，然后设置第一步长大于第二步长，使得新的树节点偏向采样点的方向生长来逃离局部极小状态；当判断随机树生长逃离局部极小状态时，可以再动态设置第二步长大于第一步长。因此在本实施例中，机械臂控制器可以在判断随机树生长未陷入局部极小状态时，调整所述第一步长和所述第二步长，使得所述第一步长小于所述第二步长。这种方法既保留了灵活避障的优点又增强了搜索的效率。

在一个实施例中，涉及通过优化采样空间实现路径优化的过程，具体可以包括：将所述任务起点至所述任务终点在关节空间中的初始路径作为当前路径，进行路径优化处理，得到新的路径；其中，参照图3a所示，所述路径优化处理包括如下过程：

s301，计算所述当前路径的路径代价。

路径代价可以为评估路径的指标，可以是任意有效的指标；例如，路径代价可以为路径长度，具体地，为构成路径的各子路径的长度之和，各子路径的长度为所述子路径对应的两个路径节点之间的距离，即该路径代价表征了根据该路径控制机械臂导致的关节变化量；当路径代价最小时，关节变化量最小，关节磨损最小，可以延长机械臂寿命。当然，路径代价也采用其它评价标准。

s302，根据所述路径代价、所述起点位形和所述终点位形，确定一个椭球体空间作为所述关节空间的新的采样空间；所述新的采样空间中各点和所述起点位形、所述终点位形的距离之和小于或等于所述路径代价。

这个优化后的新的采样空间可以用下式来表示：

xf*＝{x∈xfree|||xstart-x||2+||x-xgoal||2≤cmax}

这正是椭球体(超椭球体)的表达式，所以又可以称将优化空间约束在由当前路径构造的椭球体内，xf*表示优化后的采样空间，xfree表示无碰撞的采样空间，即所述关节空间的采样空间，cmax表示当前路径的路径代价。

参照图3b所示的优化后的采样空间的二维平面图；cmin为起始树节点与目标树节点之间忽略障碍物后的直线距离，起始树节点xstart和目标树节点xgoal分别为椭球体的两个焦点，cmax为椭球体的长轴，为椭球体的短轴。

s303，根据所述新的采样空间，进行随机树生长处理，得到所述任务起点至所述任务终点在关节空间中的待定路径，以及当所述待定路径的路径代价小于所述当前路径的路径代价时，将所述待定路径作为新的路径。

根据椭球体的性质可知，在该采样空间内采样得到的采样点分别与起始树节点、目标树节点的距离之和小于路径代价cmax，因此根据该优化后的采样空间的采样点相对原采样空间更优，确保从可能改善当前路径代价的空间内进行采样；因此新的路径的路径代价有很大可能相对于当前路径的路径代价偏小，实现路径优化；也就是说，根据当前路径构建椭球体来优化采样空间，可以确保算法只从能优化当前路径的空间内采样，使得搜索路径趋近于最优，提高了算法的搜索效率。

可选地，所述方法还包括：将所述新的路径作为当前路径，继续进行路径优化处理，直至所述新的路径满足预设条件，得到所述最终的新的路径；其中，确定所述新的路径满足预设条件包括：获取最新的路径以及所述最新的路径之前的至少一个路径，计算所述最新的路径和所述至少一个路径之间的至少一个路径代价的差值；当所述至少一个路径代价的差值均小于或等于预设路径代价阈值时，确定所述新的路径满足预设条件。

可以理解的是，在本实施例中，可以从初始路径开始，将初始路径作为当前路径，根据上述路径优化处理过程得到优化后的新的路径，可以继续优化，实现路径的不断渐进优化。上述优化过程需要注意的是如何判断路径已经趋近于最优，本实施例可以采用如下方法来判断路径是否已经趋近于最优：对于某些微小的正εc(εc的取值与采样空间的大小呈正相关)，存在：

其中，ci表示第i条路径的路径代价，i为大于或等于4的整数；示例性地，当连续的4条路径的路径代价均接近时，那么可以认为路径已经趋近于最优，避免重复运算，因此可以提高运算效率。

可选地，在路径优化趋近最优时，可以根据包裹障碍物法对所述最新的路径进行优化，得到优化后的目标路径。其中，所述最新的路径为所述路径优化处理得到的最优的路径。当通过优化采样空间的方式也难以进行有效优化时，可以采用另一种优化方式：包裹障碍物法，再次进行优化，得到目标路径。

参照图3c所示的包裹障碍物法的示意图，表示二维平面中通过逼近包裹障碍物来优化路径的过程，图中深色黑线表示原先规划的路径(图(a))，浅色黑线表示二次优化后的路径(图(f))，深色矩形表示障碍物。首先假设通过初始路径p0一共有四个路径节点(图(a))，需要优化的路径节点是除了起点与终点之外的节点，即节点x2和x3，每个节点的优化过程分为两步。第一步：离散节点x2和x3之间的路径，得到n个新的离散节点，将它们记为x2i(i∈(1,n))，然后将这些离散节点依次与x2的前一个节点相连，对这条连线进行碰撞检测，找到发生碰撞检测的第一个节点记为x'2(图(b))；第二步：离散x2的前一个节点与节点x'2之间的路径，得到n个新的离散节点，将它们记为x'2i(i∈(1,n))，然后将这些离散节点依次与x3相连，对这条连线进行碰撞检测，找到发生碰撞检测的第一个节点记为x2new(图(c))；经过这两步处理就可以将节点x2替换成新节点x2new。节点x3的处理过程相同(图(d)，图(e))，将节点x3替换成新节点x3new。根据三角不等式原理，二次处理后的路径p1(x1→x2new→x3new→x4)明显优于初始路径p0(x1→x2→x3→x4)。同样地，可以通过包裹障碍物法对所述最优的路径中的各个路径节点进行再次优化，得到新的优化后的路径节点有向连接构成的目标路径。

可以理解的是，因为包裹障碍物法运算量较大，因此可以选择对s204所得到的初始路径进行包裹障碍物法的优化，然后采用计算量较小的优化采样空间的方式对路径进行不断优化，直至趋紧最优，得到最新的路径，最后再采用包裹障碍物法对最新的路径进行优化，得到目标路径，既避免因过多使用包裹障碍物法增加碰撞检测时间，又提高了算法效率。

在一个实施例中，参照图4所示，本实施例涉及采用双向随机树生长进行路径规划，具体可以包括：

s401，将所述终点位形作为第一随机树的目标树节点，以及将所述起点位形作为第二随机树的目标树节点。

当采用双向随机树生长算法时，可以以起点位形作为第一随机树的起始树节点，以终点位形作为第一随机树的目标树节点，进行第一随机树的生长处理；以终点位形作为第二随机树的起始树节点，以起点位形作为第二随机树的目标树节点，进行第二随机树的生长处理。

s402，分别针对所述第一随机树和所述第二随机树，根据所述采样空间、预设的第一步长和第二步长，进行随机树生长处理，直至第一随机树的最新树节点和第二随机树的最新树节点之间的连线满足无碰撞条件。

当第一随机树的最新树节点和第二随机树的最新树节点之间的连线满足无碰撞条件时，意味着第一随机树和第二随机树生长完成，可以根据第一随机树和第二随机树确定路径。

具体地，参照图5a和图5b所示，所述根据所述采样空间、预设的第一步长和第二步长，进行随机树生长处理，可以包括：

s501，从所述关节空间的采样空间中采样得到采样点；

s502，遍历所述随机树中各树节点，确定与所述采样点距离最小的树节点作为父节点；所述随机树为所述第一随机树或所述第二随机树；

s503，根据所述父节点、采样点、目标树节点，以及所述第一步长和所述第二步长，确定从所述父节点生长出的子节点；

s504，当所述子节点和所述父节点之间的连线满足无碰撞条件时，将所述子节点作为所述随机树的新的树节点。

上述双向随机树生长的过程，可以参照上述单向随机树生长的过程，这里不再赘述。需要说明的是，第一随机树ts中子节点xnews如下表示：

第二随机树tg中子节点xnewg如下表示：

其中，xstart作为起始树节点，xgoal作为目标树节点，采样点为xrand，父节点为xnearest。

可选地，所述根据所述随机树的多个树节点，得到所述任务起点至所述任务终点在关节空间中的路径，可以包括：根据所述第一随机树的最新树节点和所述第一随机树中各树节点的父子关联关系，确定所述第一随机树中从所述起点位形至所述最新树节点的路径上的多个第一树节点和所述多个第一树节点的顺序；根据所述第二随机树的最新树节点和所述第二随机树中各树节点的父子关联关系，确定所述第二随机树中从所述最新树节点至所述终点位形的路径上的多个第二树节点和所述多个第二树节点的顺序；根据所述多个第一树节点和所述多个第一树节点的顺序，以及所述多个第二树节点和所述多个第二树节点的顺序，确定从所述起点位形至所述终点位形的路径上的多个树节点以及所述多个树节点的顺序，作为所述任务起点至所述任务终点在关节空间中的路径。

参照图5b所示的双向随机树生长示意图，其中，矩形区域、三角形区域等均为障碍物区域，第一随机树ts为图中左下角的多个相互连接的树节点组成，第二随机树tg为图中右上角的多个相互连接的树节点组成；可以理解的是，在连接关系的两个树节点之间存在父子关联关系。在图5b中，当第一随机树的最新树节点xnews和第二随机树的最新树节点xnewg之间的连线满足无碰撞条件时，可以根据第一随机树中各树节点的父子关联关系，确定所述第一随机树中从所述最新树节点xnews至所述起点位形的路径上的多个第一树节点和所述多个第一树节点的顺序：xnews→xnews的父节点→……→xstart的子节点→xstart，进而可以反顺序，即从所述起点位形至所述最新树节点xnews的路径上的多个第一树节点和所述多个第一树节点的顺序：xstart→xstart的子节点→……→xnews的父节点→xnews。同样地，可以直接根据第二随机树中各树节点的父子关联关系，确定从所述最新树节点至所述终点位形的路径上的多个第二树节点和所述多个第二树节点的顺序：xnewg→xnewg的父节点→……→xgoal的子节点→xgoal。最终可以得到从所述起点位形至所述终点位形的路径上的多个树节点以及所述多个树节点的顺序：xstart→xstart的子节点→……→xnews的父节点→xnews→xnewg→xnewg的父节点→……→xgoal的子节点→xgoal，作为所述任务起点至所述任务终点在关节空间中的路径。

参照图6所示，从另一个角度示出了一种基于双向随机树生长的路径规划方法的示意图，可以包括：

s601，在采样空间内随机采样得到第一随机树的采样点，然后执行s602；

s602，遍历第一随机树得到距离s601中第一随机树的采样点最近的点作为父节点，然后执行s603；

s603，判断第一随机树的生长是否陷入局部极小状态；若否，则执行s604；若是，则执行s605；

s604，根据预设的非局部极小状态下的步长调节机制，使得第一步长小于第二步长，并根据第一步长和第二步长得到s602中父节点的子节点，然后执行s606；

s605，根据预设的局部极小状态下的步长调节机制，使得第一步长大于第二步长，并根据第一步长和第二步长得到s602中父节点的子节点，然后执行s606；

s606，对所述子节点和所述父节点之间的连线进行碰撞检测，判断是否发生碰撞；若是，则执行s601；若否，则执行s607；

s607，将所述子节点加入第一随机树中，作为一个新的树节点，然后执行s608和s613；

s608，在采样空间内随机采样得到第二随机树的采样点，然后执行s609；

s609，遍历第二随机树得到距离s608中第二随机树的采样点最近的点作为父节点，然后执行s610；

s610，判断第二随机树的生长是否陷入局部极小状态；若否，则根据预设的非局部极小状态下的步长调节机制，使得第一步长小于第二步长，若是，则根据预设的局部极小状态下的步长调节机制，使得第一步长大于第二步长；根据第一步长和第二步长得到s609中父节点的子节点，然后执行s611；

s611，对所述子节点和所述父节点之间的连线进行碰撞检测，判断是否发生碰撞；若是，则执行s608；若否，则执行s612；

s612，将所述子节点加入第二随机树中，作为一个新的树节点，然后执行s613；

s613，连接所述第一随机树的新的树节点和第二随机树的新的树节点，然后执行s614；

s614，对所述第一随机树的新的树节点和第二随机树的新的树节点的连线进行碰撞检测，判断是否发生碰撞；若是，则执行s601；若否，则执行s615；

s615，判断是否已经得到初始路径，若否，则执行s616；若是，则执行s619；

s616，根据第一随机树和第二随机树，以及s614中未发生碰撞的第一随机树的新的树节点和第二随机树的新的树节点，生成初始路径p0，然后执行s617；

s617，对初始路径进行包裹障碍物优化，得到p1，然后执行s618；

s618，将p1作为当前路径，根据当前路径的路径代价，优化采样空间，然后执行s601；

s619，判断优化采样空间进行路径优化的过程是否趋于最优，若否，则执行s620；若是，则执行s621；

s620，根据当前路径的路径代价，优化采样空间，然后执行s601；

s621，对当前路径进行包裹障碍物优化，得到目标路径。

应该理解的是，虽然图2,3a,4,5a,6的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图2,3a,4,5a,6中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

在一个实施例中，如图7所示，提供了一种路径规划装置，包括：初始化模块71、随机树生长模块72、第一步长调整模块73和初始路径获取模块74，其中：

初始化模块71，用于确定机械臂的任务起点在机械臂的关节空间中的起点位形、机械臂的任务终点在所述关节空间中的终点位形，以及确定所述关节空间的采样空间；

随机树生长模块72，用于根据所述采样空间、起点位形和终点位形，以及预设的第一步长和第二步长，进行随机树生长处理，得到随机树的多个树节点；所述第一步长为新的树节点向采样点生长的步长，所述第二步长为新的树节点向目标树节点生长的步长，所述采样点是从所述采样空间中采样得到的，所述目标树节点为所述起点位形或所述终点位形；

第一步长调整模块73，用于在进行随机树生长处理过程中，进行局部极小状态判断，在判断随机树生长陷入局部极小状态时，调整所述第一步长和所述第二步长，使得所述第一步长大于所述第二步长；

初始路径获取模块74，用于根据所述随机树的多个树节点，得到所述任务起点至所述任务终点在关节空间中的初始路径。

可选地，参照图8所示，所述装置还可以包括：

第二步长调整模块75，用于在判断随机树生长未陷入局部极小状态时，调整所述第一步长和所述第二步长，使得所述第一步长小于所述第二步长。

可选地，所述进行局部极小状态判断，包括：获取最新的树节点以及所述最新的树节点之前的至少一个树节点，计算所述最新的树节点和所述至少一个树节点之间的至少一个节点距离；当所述至少一个节点距离均小于或等于预设距离阈值时，确定所述随机树生长陷入局部极小状态；当所述至少一个节点距离大于预设距离阈值时，确定所述随机树生长未陷入局部极小状态。

可选地，参照图8所示，所述装置还可以包括：

第一路径优化模块76，用于将所述任务起点至所述任务终点在关节空间中的初始路径作为当前路径，进行路径优化处理，得到新的路径；其中，所述路径优化处理包括：计算所述当前路径的路径代价；根据所述路径代价、所述起点位形和所述终点位形，确定一个椭球体空间作为所述关节空间的新的采样空间；所述新的采样空间中各点和所述起点位形、所述终点位形的距离之和小于或等于所述路径代价；根据所述新的采样空间，进行随机树生长处理，得到所述任务起点至所述任务终点在关节空间中的待定路径，以及当所述待定路径的路径代价小于所述当前路径的路径代价时，将所述待定路径作为新的路径。

进一步地，参照图8所示，所述装置还可以包括：

优化控制模块77，用于将所述新的路径作为当前路径，继续进行路径优化处理，直至所述新的路径满足预设条件，得到所述最终的新的路径；其中，确定所述新的路径满足预设条件包括：获取最新的路径以及所述最新的路径之前的至少一个路径，计算所述最新的路径和所述至少一个路径之间的至少一个路径代价的差值；当所述至少一个路径代价的差值均小于或等于预设路径代价阈值时，确定所述新的路径满足预设条件。

进一步地，参照图8所示，所述装置还可以包括：

第二路径优化模块78，用于根据包裹障碍物法对所述最新的路径进行优化，得到优化后的目标路径。

可选地，参照图8所示，所述随机树生长模块72可以包括：

双向随机树设置单元721，用于将所述终点位形作为第一随机树的目标树节点，以及将所述起点位形作为第二随机树的目标树节点；

双向随机树生长单元722，用于分别针对所述第一随机树和所述第二随机树，根据所述采样空间、预设的第一步长和第二步长，进行随机树生长处理，直至第一随机树的最新树节点和第二随机树的最新树节点之间的连线满足无碰撞条件。

可选地，双向随机树生长单元722具体用于从所述关节空间的采样空间中采样得到采样点；遍历所述随机树中各树节点，确定与所述采样点距离最小的树节点作为父节点；所述随机树为所述第一随机树或所述第二随机树；根据所述父节点、采样点、目标树节点，以及所述第一步长和所述第二步长，确定从所述父节点生长出的子节点；当所述子节点和所述父节点之间的连线满足无碰撞条件时，将所述子节点作为所述随机树的新的树节点。

进一步地，参照图8所示，所述初始路径获取模块74可以包括：

第一路径获取单元741，用于第一根据所述第一随机树的最新树节点和所述第一随机树中各树节点的父子关联关系，确定所述第一随机树中从所述起点位形至所述最新树节点的路径上的多个第一树节点和所述多个第一树节点的顺序；

第二路径获取单元742，用于根据所述第二随机树的最新树节点和所述第二随机树中各树节点的父子关联关系，确定所述第二随机树中从所述最新树节点至所述终点位形的路径上的多个第二树节点和所述多个第二树节点的顺序；

初始路径获取单元743，用于根据所述多个第一树节点和所述多个第一树节点的顺序，以及所述多个第二树节点和所述多个第二树节点的顺序，确定从所述起点位形至所述终点位形的路径上的多个树节点以及所述多个树节点的顺序，作为所述任务起点至所述任务终点在关节空间中的路径。

关于路径规划装置的具体限定可以参见上文中对于路径规划方法的限定，在此不再赘述。上述路径规划装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。

在一个实施例中，提供了一种机械臂控制器，包括存储器和处理器，存储器中存储有计算机程序，该处理器执行计算机程序时实现以下步骤：

确定机械臂的任务起点在机械臂的关节空间中的起点位形、机械臂的任务终点在所述关节空间中的终点位形，以及确定所述关节空间的采样空间；

根据所述采样空间、起点位形和终点位形，以及预设的第一步长和第二步长，进行随机树生长处理，得到随机树的多个树节点；所述第一步长为新的树节点向采样点生长的步长，所述第二步长为新的树节点向目标树节点生长的步长，所述采样点是从所述采样空间中采样得到的，所述目标树节点为所述起点位形或所述终点位形；

在进行随机树生长处理过程中，进行局部极小状态判断，在判断随机树生长陷入局部极小状态时，调整所述第一步长和所述第二步长，使得所述第一步长大于所述第二步长；

根据所述随机树的多个树节点，得到所述任务起点至所述任务终点在关节空间中的初始路径。

在一个实施例中，提供了一种可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现以下步骤：

确定机械臂的任务起点在机械臂的关节空间中的起点位形、机械臂的任务终点在所述关节空间中的终点位形，以及确定所述关节空间的采样空间；

根据所述采样空间、起点位形和终点位形，以及预设的第一步长和第二步长，进行随机树生长处理，得到随机树的多个树节点；所述第一步长为新的树节点向采样点生长的步长，所述第二步长为新的树节点向目标树节点生长的步长，所述采样点是从所述采样空间中采样得到的，所述目标树节点为所述起点位形或所述终点位形；

在进行随机树生长处理过程中，进行局部极小状态判断，在判断随机树生长陷入局部极小状态时，调整所述第一步长和所述第二步长，使得所述第一步长大于所述第二步长；

根据所述随机树的多个树节点，得到所述任务起点至所述任务终点在关节空间中的初始路径。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(rom)、可编程rom(prom)、电可编程rom(eprom)、电可擦除可编程rom(eeprom)或闪存。易失性存储器可包括随机存取存储器(ram)或者外部高速缓冲存储器。作为说明而非局限，ram以多种形式可得，诸如静态ram(sram)、动态ram(dram)、同步dram(sdram)、双数据率sdram(ddrsdram)、增强型sdram(esdram)、同步链路(synchlink)dram(sldram)、存储器总线(rambus)直接ram(rdram)、直接存储器总线动态ram(drdram)、以及存储器总线动态ram(rdram)等。

以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。