机器人路径规划方法及装置与流程

本发明涉及路径规划领域，具体而言，涉及一种机器人路径规划方法及装置。

背景技术：

基于采样的运动规划算法在近十几年来得到了广泛发展，由于该方法的高效性，吸引了很多人对其的研究。其中最为熟悉的便是随机搜索树算法(rrt，rapidlyexploringrandomtree)，简称rrt算法。该算法是在1998年被美国的stevenm.lavalle教授提出。在多维空间中，该算法对路径的规划较为有效，除此之外，它也是一张数据结构和算法。

在以往的一些传统算法，如人工势场法、遗传算法、模糊算法、神经网络法、蚁群优化算法等，都是需要对障碍物环境进行模型的建立的，这无形之中增加了算法的运行时间，计算量也随着环境的复杂度增加。越智能的机器人的复杂度越高，自由度也越高，以往的传统算法计算量更大，就不适合智能机器人在高维度、复杂的环境下进行路径规划。

rrt算法不再需要对环境进行建模，大大缩短了路径的搜索时间，对于高维度的路径规划问题效率很高。该算法最大的特点就是能够快速的生成伪随机数，得到随机点，在机器人的工作空间中快速搜索，从而得到规划的路径。该算法适合解决多自由度机器人在复杂和动态环境中的路径规划。rrt算法是概率比较完备的算法，理论上能够在机器人的位姿空间中较快速的搜索到可行路径。但是在搜索过程中撒点过于平均，搜索树会在整个工作空间中随机扩展，从而使搜索效率低下，而且规划的最终路径可能会偏离最短路径。

技术实现要素：

本发明的目的在于提供一种机器人路径规划方法及装置，以改善上述问题。为了实现上述目的，本发明采取的技术方案如下：

第一方面，本发明实施例提供了一种机器人路径规划方法，所述方法包括：获取工作空间图像，所述工作空间图像包括至少一个障碍物；获取伪随机数和偏置变量；判断所述伪随机数是否小于所述偏置变量；若确定所述伪随机数不小于所述偏置变量，获取所述工作空间图像中的目标状态点；基于所述目标状态点，对预设的第一颗树进行搜索扩展并判断得出扩展后的第一颗树与所述障碍物没有发生碰撞，获得所述第一颗树的第一新生结点；基于所述第一新生结点，对预设的第二颗树进行搜索扩展；判断扩展后的第一颗树新扩散的结点与扩展后的第二颗树新扩散后的结点是否相等，若相等，获得规划路径。

第二方面，本发明实施例提供了一种机器人路径规划装置，所述装置包括第一获取单元、第二获取单元、第一判断单元、第一扩展单元、第二扩展单元和第二判断单元。第一获取单元，用于获取工作空间图像，所述工作空间图像包括至少一个障碍物。第二获取单元，用于获取伪随机数和偏置变量。第一判断单元，用于：判断所述伪随机数是否小于所述偏置变量；若确定所述伪随机数不小于所述偏置变量，获取所述工作空间图像中的目标状态点。第一扩展单元，用于基于所述目标状态点，对预设的第一颗树进行搜索扩展并判断得出扩展后的第一颗树与所述障碍物没有发生碰撞，获得所述第一颗树的第一新生结点。第二扩展单元，用于基于所述第一新生结点，对预设的第二颗树进行搜索扩展。第二判断单元，用于判断扩展后的第一颗树新扩散的结点与扩展后的第二颗树新扩散后的结点是否相等，若相等，获得规划路径。

本发明实施例提供的一种机器人路径规划方法及装置，通过获取工作空间图像，所述工作空间图像包括至少一个障碍物；获取伪随机数和偏置变量后，判断所述伪随机数是否小于所述偏置变量；若确定所述伪随机数不小于所述偏置变量，获取所述工作空间图像中的目标状态点；基于所述目标状态点，对预设的第一颗树进行搜索扩展并判断得出扩展后的第一颗树与所述障碍物没有发生碰撞，获得所述第一颗树的第一新生结点；基于所述第一新生结点，对预设的第二颗树进行搜索扩展的方式，进而判断扩展后的第一颗树新扩散的结点与扩展后的第二颗树新扩散后的结点是否相等，若相等，获得规划路径。更加有效。

本发明的其他特征和优点将在随后的说明书阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明实施例了解。本发明的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为可应用于本发明实施例提供的电子设备的结构框图；

图2为本发明实施例提供的机器人路径规划方法的流程图；

图3为本发明实施例提供的机器人路径规划方中rrt-connect算法的双树扩展过程示意图；

图4为本发明实施例提供的机器人路径规划方法中rrt-connect算法的两树连接示意图；

图5(a)和(b)分别为本发明实施例提供的机器人路径规划方法中rrt-connect算法的搜索过程和搜索路径示意图；

图6(a)和(b)分别为本发明实施例提供的机器人路径规划方法搜索过程和搜索路径示意图；

图7(a)、(b)和(c)分别为本发明实施例提供的机器人路径规划方法中空间1、空间2和空间3地图实例示意图；

图8为本发明实施例提供的机器人路径规划装置的结构框图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本发明的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

图1示出了一种可应用于本发明实施例中的电子设备100的结构框图。如图1所示，电子设备100可以包括存储器102、存储控制器104、一个或多个(图1中仅示出一个)处理器106、外设接口108、输入输出模块110、音频模块112、显示模块114、射频模块116和机器人路径规划装置。

存储器102、存储控制器104、处理器106、外设接口108、输入输出模块110、音频模块112、显示模块114、射频模块116各元件之间直接或间接地电连接，以实现数据的传输或交互。例如，这些元件之间可以通过一条或多条通讯总线或信号总线实现电连接。机器人路径规划方法分别包括至少一个可以以软件或固件(firmware)的形式存储于存储器102中的软件功能模块，例如所述机器人路径规划装置包括的软件功能模块或计算机程序。

存储器102可以存储各种软件程序以及模块，如本申请实施例提供的机器人路径规划方法及装置对应的程序指令/模块。处理器106通过运行存储在存储器102中的软件程序以及模块，从而执行各种功能应用以及数据处理，即实现本申请实施例中的机器人路径规划方法。

存储器102可以包括但不限于随机存取存储器(randomaccessmemory，ram)，只读存储器(readonlymemory，rom)，可编程只读存储器(programmableread-onlymemory，prom)，可擦除只读存储器(erasableprogrammableread-onlymemory，eprom)，电可擦除只读存储器(electricerasableprogrammableread-onlymemory，eeprom)等。

处理器106可以是一种集成电路芯片，具有信号处理能力。上述处理器可以是通用处理器，包括中央处理器(centralprocessingunit，简称cpu)、网络处理器(networkprocessor，简称np)等；还可以是数字信号处理器(dsp)、专用集成电路(asic)、现场可编程门阵列(fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。其可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

所述外设接口108将各种输入/输出装置耦合至处理器106以及存储器102。在一些实施例中，外设接口108、处理器106以及存储控制器104可以在单个芯片中实现。在其他一些实例中，他们可以分别由独立的芯片实现。

输入输出模块110用于提供给用户输入数据实现用户与电子设备100的交互。所述输入输出模块110可以是，但不限于，鼠标和键盘等。

音频模块112向用户提供音频接口，其可包括一个或多个麦克风、一个或者多个扬声器以及音频电路。

显示模块114在电子设备100与用户之间提供一个交互界面(例如用户操作界面)或用于显示图像数据给用户参考。在本实施例中，所述显示模块114可以是液晶显示器或触控显示器。若为触控显示器，其可为支持单点和多点触控操作的电容式触控屏或电阻式触控屏等。支持单点和多点触控操作是指触控显示器能感应到来自该触控显示器上一个或多个位置处同时产生的触控操作，并将该感应到的触控操作交由处理器106进行计算和处理。

射频模块116用于接收以及发送电磁波，实现电磁波与电信号的相互转换，从而与通信网络或者其他设备进行通信。

电子设备100还包括摄像模块。摄像模块用于采集工作空间图像。摄像模块可以为摄像头。

可以理解，图1所示的结构仅为示意，电子设备100还可包括比图1中所示更多或者更少的组件，或者具有与图1所示不同的配置。图1中所示的各组件可以采用硬件、软件或其组合实现。

于本发明实施例中，电子设备100可以为pc(personalcomputer)电脑、平板电脑、手机、笔记本电脑、智能电视、机顶盒、车载终端等设备。

请参阅图2，本发明实施例提供了一种机器人路径规划方法，所述方法包括：步骤s200、步骤s210、步骤s220、步骤s230、步骤s240、步骤s250和步骤s260。

步骤s200：获取工作空间图像，所述工作空间图像包括至少一个障碍物。

在本实施例中，工作空间图像划分为障碍物空间和自由空间，为了更好的表示障碍物空间与自由空间，读取工作空间图像后直接将图像转换成二值图，其中1代表白色，0代表黑色。

步骤s210：获取伪随机数和偏置变量。

在本实施例中，经实验证明偏置变量可以取0-0.5之间，速度差不多，优选地，例如偏置变量可以取0.5，速度是最快的。

步骤s220：判断所述伪随机数是否小于所述偏置变量。

步骤s230：若确定所述伪随机数不小于所述偏置变量，获取所述工作空间图像中的目标状态点。

步骤s240：基于所述目标状态点，对预设的第一颗树进行搜索扩展并判断得出扩展后的第一颗树与所述障碍物没有发生碰撞，获得所述第一颗树的第一新生结点。

步骤s240可以包括：

设置所述第一颗树以所述目标状态点为随机点；

查找所述第一颗树的第一扩散路径图中距离所述随机点最近的第一最近点；

在所述工作空间图像中向所述随机点的方向移动第一预设距离，获得第一新生结点；

若判断得出所述第一最近点到所述第一新生结点的第一路径与所述障碍物没有发生碰撞，且所述第一路径在所述工作空间图像内，将所述第一新生结点添加到所述第一扩散路径图的点集中以及将所述第一路径添加到所述第一扩散路径图的边集中。

步骤s250：基于所述第一新生结点，对预设的第二颗树进行搜索扩展。

步骤s250可以包括：

查找所述第二颗树的第二扩散路径图中距离所述第一新生结点最近的第二最近点；

在所述工作空间图像中向所述第一新生结点的方向移动第二预设距离，获得第二新生结点；

若判断得出所述第二最近点到所述第二新生结点的第二路径与所述障碍物没有发生碰撞，且所述第二路径在所述工作空间图像内，将所述第二新生结点添加到所述第二扩散路径图的点集中以及将所述第二路径添加到所述第二扩散路径图的边集中；

在所述工作空间图像中向所述第一新生结点的方向移动第三预设距离，获得第三新生结点；

若判断得出所述第三新生结点到所述第二新生结点的第三路径与所述障碍物没有发生碰撞，且所述第三路径在所述工作空间图像内，将所述第三新生结点添加到所述第二扩散路径图的点集中以及将所述第三路径添加到所述第二扩散路径图的边集中。

步骤s260：判断扩展后的第一颗树新扩散的结点与扩展后的第二颗树新扩散后的结点是否相等，若相等，获得规划路径。

在判断扩展后的第一颗树新扩散的结点与扩展后的第二颗树新扩散后的结点是否相等之后，在获得规划路径之前，所述方法还可以包括：

若判断得出扩展后的第一颗树新扩散的结点与扩展后的第二颗树新扩散后的结点相等，若判断得出扩展后的第一颗树新扩散的结点数与扩展后的第二颗树新扩散后的结点数不相等，执行交换第一颗树和第二颗树的扩散次序且选择对第一颗树和第二颗树中较小的一颗树进行继续搜索扩展。

进一步地，在获取所述工作空间图像中的目标状态点之后，在基于所述目标状态点，对预设的第一颗树进行搜索扩展之前，所述方法还包括：

获取所述工作空间图像中的起始状态点和设置第一颗树，所述第一颗树包括第一扩散路径图；

将所述起始状态点添加于所述第一扩散路径图的点集中并初始化所述第一扩散路径图的边集为空集；

设置第二颗树，所述第二颗树包括第二扩散路径图；

将所述目标状态点添加于所述第二扩散路径图的点集中并初始化所述第二扩散路径图的边集为空集。

在判断所述伪随机数是否小于所述偏置变量之后，所述方法还可以包括：

若确定所述伪随机数小于所述偏置变量，在所述工作空间图像内随机扩散点。

为了简化描述，在本实施例中提供的机器人路径规划方法简记为rrt-connect*算法。在rrt-connect算法中，也要考虑算法随机撒点的问题。当然在这样的算法下，需要进行的是以对方为目标方向撒点。在概率论和统计学中，随机分布的种类有很多种，要在工作空间中随机均匀撒点，即要采用均匀分布，它是对称概率分布，在相同长度间隔的分布概率是等可能的。均匀分布由两个参数a和b定义，它们是数轴上的最小值和最大值，通常缩写为u(a，b)。均匀分布的概率密度函数为：

累积的分布函数为：

其中a＝0，b＝1时，为标准分布记u(0，1)。

统计学中，当使用p值作为简单零假设的检验统计量，并且检验统计量的分布是连续的，则如果零假设为真，则p值均匀分布在0和1之间。通常编程语言能够根据标准均匀分布生成有效分布的伪随机数，在rrt-connect算法中，用matlab编写程序，调用其中的rand函数即可生成均匀分布的伪随机数。

检测计算点q1到点q2的角度，首先要知道两点的坐标，然后可以通过四象限反正切函数θ＝atan2(δy，δx)，得到角度θ。在相同方向移动一定的距离δ，则可根据反正切函数求得到角度θ，求得：

qnew≈q+δ·[sinθcosθ](3)

整个算法的最终目的是要找到一条从起始状态点到目标状态点的路径，为计算路径长度，采用欧式距离函数d(q1，q2)＝||q1-q2||(4)。

rrt-connect算法与之前基本的rrt相比，主要的突破点就是在目标点处也生成随机树，两棵树一起扩展搜索。算法一开始在不同的环境中进行搜索时，算法进行迭代，树的扩展步骤与之前基本的rrt算法一样，都是先按随机概率分辨撒点的方向进行撒点，再判断路径是否与障碍物碰撞或者随机撒的点是否在障碍物内，判断后再进行扩展第一棵树，得到新节点qnew并把它作为以目标点为起始点扩展的树的扩展方向。这棵树的扩展过程如图3所示。

在此基础上，第二棵随机树的扩展的方式与之前的rrt算法的步骤有点不同。一开始第二棵树，会以第一棵树的qnew结点为扩展方向扩展第一步得到q’new，如果与障碍物没有发生碰撞而且q’new是不在障碍物上的可行点，则继续沿着之前相同的方向扩展第二条树枝，一直扩展到失败或者两棵树扩展的新结点q’new＝qnew，表示第二棵树与第一棵树相互连接了，即像双树rrt算法命名一样connect了，整个rrt-connect算法结束。在工作空间的整个搜索过程中，如果出现起始点与目标点不在工作环境内(或者在障碍物内的情况)，就会搜索失败，找寻不到路径。最后连接的图如图4所示。

在整个算法当中，还需要考虑两棵树的扩展的平衡问题，即两棵随机树扩展的节点的数目的多少(也可以考虑两棵随机树扩展的总共花费的路径长度的长短)，当出现两棵树的节点数不一致(或者两棵随机树扩展的总共花费的路径长度有差异)的情况，就交换次序选择“小”的那棵随机树进行继续扩展。

本发明实施例提供的机器人路径规划方法即rrt-connect*算法的主要设计流程如下rrt-connect*算法所示，对应步骤1-28，％后面的文字内容表示解释前面语句。

rrt-connect算法将两棵树的扩展方向都趋向于对方的方向，避免两棵树都随机无方向的乱搜索的现象的发生，最终有效的提高路径搜索的效率。它是概率比较完备的算法，理论上能够在机器人的位姿空间中较快速的搜索到可行路径。但是在搜索过程中撒点过于平均，搜索树会在整个工作空间中随机扩展，从而使搜索效率低下。因此，在这种情况下，改进随机撒点算法。根据启发式算法解最优解的定义，可以使在一定范围内的概率的随机点向目标状态点的方向扩展蔓延，从而使得随机树的扩展方向整体上趋向于目标状态点。其具体流程是：先生成随机点，再从0到1选定一个偏置变量bias，随后生成一个伪随机数rand，如果0<rand<bias，则随机扩散点；否则就随机采样点就等于目标状态点。经实验证明偏置变量可以取0-0.5之间，速度差不多，这里取偏置变量bias＝0.5，如下改进撒点算法所示。

在获得规划路径之后，所述方法还包括：

基于所述规划路径，获得所述起始状态点到所述目标状态点之间的路径长度。

在本实施例中，基于规划路径和公式(4)，获得起始状态点到所述目标状态点之间的路径长度。

为了更进一步地体现本发明实施例提供的机器人路径规划方法即rrt-connect*算法的有益效果，根据rrt-connect*算法，在matlab中进行程序编写调试，给出不同的障碍物的工作环境，验证算法的可行性，在自定义的环境中，rrt-connect*算法的仿真结果在下表1中给出，测试了10个工作空间图像即map1-map10，曲线表示路径，黑色图形表示障碍物。在这些环境中，都是500*500规格的大小，起始状态点都在[10,10]的位置，目标状态点都在[490,490]的位置，针对每个mapi,i＝1,…,10，实施rrt-connect*算法，得到对应的搜索路径、规划路径、路径长度和处理时间。

表1本发明提供的机器人路径规划方法在不同障碍物的环境下的情况

rrt-connect*算法在rrt算法的基础上，生成两棵树，从起始点与目标点同时扩散搜索，它是rrt算法的改进。改进撒点算法后，rrt-connec*t算法的扩展方向更加明确。rrt算法中撒点算法未改进之前在障碍物空间搜索的过程如图5(a)所示的曲线。最终得到的搜索路径如图5(b)所示的曲线。rrt-connect*算法中的撒点函数改进之后的路径搜索过程如图6(a)所示的曲线。最终的搜索路径如图6(b)所示的曲线。从中可看出改进后的算法目标性更强，搜索的结点数更加的趋向于对方方向。

下面在以下几个地图中，进行测试对比rrt-connect*算法与rrt-connect算法。这种双向的rrt技术具有良好的搜索特性，比原始rrt算法的搜索速度、搜索效率有了显著提高，被广泛应用。rrt-connect算法与之前的算法相比较，在扩展的步长上更长，并且有两棵树同时扩展，使得树的生长蔓延的更快；两棵树不断朝向对方的方向交替扩展，而不是单纯的直接采用随机扩展的方式，特别当起始状态点和目标状态点处在约束区域时，两棵树可以通过朝向对方快速扩展的方式而逃离各自的约束区域。这种带有启发性的扩展使得树的扩展更加贪婪和明确，使得双树rrt算法较之单树rrt算法更加有效。rrt-connect算法是在rrt算法的基础上，生成两棵树，从起始点与目标点同时扩散搜索，它是rrt算法的改进。改进撒点算法后，rrt-connect算法的扩展方向更加明确。

请参阅图7，图7(a)、(b)和(c)的3个地图分别对应空间1、空间2和空间3，黑色图形表示障碍物，进行测试对比rrt-connect*算法与rrt-connect算法。根据上述算法，在matlab中进行程序编写调试，给出不同的障碍物的工作环境，验证算法的可行性，在自定义的环境中，rrt-connect算法的仿真结果在下表2-表4中给出。在这些环境中，都是选择500*500的规格大小，起始状态点都在[5,5]的位置，目标状态点都在[485,485]的位置。

表2在空间1的环境下

从表2-表4中数据可看出，在相同的环境中，本实施例提供的机器人路径规划方法即rrt-connect*算法比rrt-connect算法处理时间快，最终的搜索路径也相对来说较短。rrt-connect*算法随机树的扩展目标性更强，两棵树都是一直以对方为扩展方向，加快了搜索速度，一定程度上缩小了搜索得到的路径长度。虽然在随机撒点上进行了改进，仍然可能因为生成的伪随机数的随机性，出现往回搜索的现象导致整个路径变长。从上面的表2-表4结果数据，可以看出rrt-connect*算法在速度上比rrt算法快，效率高。由于双树rrt算法有两棵树同时向对方的方向扩展，速度有了很大的提高，两棵树都是以对方的新生的结点的方向为随机点方向，避免在整个空间均匀搜索以消耗时间。同时，如果起始点和目标点出现在周围是障碍物里的情况，两棵树会因为向对方扩散而快速找到出路，继续搜索。rrt-connect*算法的目的性更强，使得搜索更高效。

请参阅图8，本发明实施例提供了一种机器人路径规划装置800，所述装置800包括第一获取单元810、第二获取单元820、第一判断单元830、第一扩展单元840、第二扩展单元850和第二判断单元860。

第一获取单元810，用于获取工作空间图像，所述工作空间图像包括至少一个障碍物。

第二获取单元820，用于获取伪随机数和偏置变量。

第一判断单元830，用于：判断所述伪随机数是否小于所述偏置变量；若确定所述伪随机数不小于所述偏置变量，获取所述工作空间图像中的目标状态点。

第一判断单元830，还用于若确定所述伪随机数小于所述偏置变量，在所述工作空间图像内随机扩散点。

第一扩展单元840，用于基于所述目标状态点，对预设的第一颗树进行搜索扩展并判断得出扩展后的第一颗树与所述障碍物没有发生碰撞，获得所述第一颗树的第一新生结点。

所述第一扩展单元840，还用于：设置所述第一颗树以所述目标状态点为随机点；查找所述第一颗树的第一扩散路径图中距离所述随机点最近的第一最近点；在所述工作空间图像中向所述随机点的方向移动第一预设距离，获得第一新生结点；若判断得出所述第一最近点到所述第一新生结点的第一路径与所述障碍物没有发生碰撞，且所述第一路径在所述工作空间图像内，将所述第一新生结点添加到所述第一扩散路径图的点集中以及将所述第一路径添加到所述第一扩散路径图的边集中。

所述第一扩展单元840，还用于：获取所述工作空间图像中的起始状态点和设置第一颗树，所述第一颗树包括第一扩散路径图；将所述起始状态点添加于所述第一扩散路径图的点集中并初始化所述第一扩散路径图的边集为空集；设置第二颗树，所述第二颗树包括第二扩散路径图；将所述目标状态点添加于所述第二扩散路径图的点集中并初始化所述第二扩散路径图的边集为空集。

第二扩展单元850，用于基于所述第一新生结点，对预设的第二颗树进行搜索扩展。

所述第二扩展单元850，还用于：查找所述第二颗树的第二扩散路径图中距离所述第一新生结点最近的第二最近点；在所述工作空间图像中向所述第一新生结点的方向移动第二预设距离，获得第二新生结点；若判断得出所述第二最近点到所述第二新生结点的第二路径与所述障碍物没有发生碰撞，且所述第二路径在所述工作空间图像内，将所述第二新生结点添加到所述第二扩散路径图的点集中以及将所述第二路径添加到所述第二扩散路径图的边集中；在所述工作空间图像中向所述第一新生结点的方向移动第三预设距离，获得第三新生结点；若判断得出所述第三新生结点到所述第二新生结点的第三路径与所述障碍物没有发生碰撞，且所述第三路径在所述工作空间图像内，将所述第三新生结点添加到所述第二扩散路径图的点集中以及将所述第三路径添加到所述第二扩散路径图的边集中。

第二判断单元860，用于判断扩展后的第一颗树新扩散的结点与扩展后的第二颗树新扩散后的结点是否相等，若相等，获得规划路径。

所述第二判断单元860，还用于：若判断得出扩展后的第一颗树新扩散的结点与扩展后的第二颗树新扩散后的结点相等，若判断得出扩展后的第一颗树新扩散的结点数与扩展后的第二颗树新扩散后的结点数不相等，执行交换第一颗树和第二颗树的扩散次序且选择对第一颗树和第二颗树中较小的一颗树进行继续搜索扩展。

所述第二判断单元860，还用于基于所述规划路径，获得所述起始状态点到所述目标状态点之间的路径长度。

以上各单元可以是由软件代码实现，此时，上述的各单元可存储于存储器102内。以上各单元同样可以由硬件例如集成电路芯片实现。

本发明实施例提供的机器人路径规划装置400，其实现原理及产生的技术效果和前述方法实施例相同，为简要描述，装置实施例部分未提及之处，可参考前述方法实施例中相应内容。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和框图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本发明各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read-onlymemory)、随机存取存储器(ram，randomaccessmemory)、磁碟或者光盘等各种可以存储程序代码的介质。需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。