CN110515094B - 基于改进rrt*的机器人点云地图路径规划方法及系统 - Google Patents

Abstract

本发明涉及一种基于改进RRT*的机器人点云地图路径规划方法及系统，所述路径规划方法首先对采集到的点云地图数据进行预处理，然后对预处理后的点云地图数据进行地形信息的计算，得到点云地图中各个位置的可行度，最后将各个位置的可行度结合改进的RRT*算法，进行路径搜索，得到三维路径，基于该三维路径控制机器人移动。与现有技术相比，本发明可以直接在三维点云地图上进行机器人的路径规划。

Description

基于改进RRT*的机器人点云地图路径规划方法及系统

技术领域

本发明涉及一种机器人路径控制方法，尤其是涉及一种基于改进RRT*的机器人点云地图路径规划方法及系统。

背景技术

目前，随着机器人应用范围越来越广泛，应用场景越来越复杂，机器人在复杂环境中的导航问题成为研究热点和亟待解决的问题。机器人的路径规划问题就是导航问题其中的一个分支，其目的是依据某个或某些优化准则(如工作代价最小、行走路线最短、行走时间最短等)，在工作空间中找到一条从起始状态到目标状态的能避开障碍物的最优路径。

目前国内存在一些机器人路径规划的专利，大多都是基于二维地图的路径规划方法。传统的路径规划算法有人工市场法、模糊规则法、遗传算法、神经网络、模拟退火算法、蚁群优化算法等。但这些方法都需要在一个确定的空间内对障碍物进行建模，计算复杂度与机器人自由度呈指数关系，不适合解决多自由度机器人在复杂环境中的规划。二维地图上的地形元素只有障碍物和可通行区域两种，其路径规划算法较为成熟，主要分为两类：第一类基于图搜索的方法，例如Dijkstra算法、A*算法等，这种算法主要是在地图上建立图的数据结构，通过每条边的代价值，找出一条代价最低的路径。第二类基于采样的算法，例如随机路图法(PRM)、快速随机搜索树算法(RRT)等，该类算法通过在地图上随机采样路径点，进行路径搜索。以上方法能够使机器人在二维地图中完成简单的导航任务。

但是考虑到随着机器人工作环境的复杂化，机器人不能只局限于二维场景的导航，这就要求机器人能够在三维环境中进行路径规划，需要将规划范围扩展到三维场景。

对于三维场景，地形的元素不再限于二维场景中只有障碍物和可通行区域两种元素，而是加入了多种地形元素，例如阶梯、坡面和崎岖路面等，传统的方法不能够适用，且国内的专利在这方面的发明比较缺乏。综上所述，目前已公开的方法大多只适用于二维地图的路径规划，并不能解决三维场景中的路径规划问题。

发明内容

本发明的目的在于克服上述现有技术存在的缺陷而提供一种基于改进RRT*的机器人点云地图路径规划方法及系统。

本发明的目的可以通过以下技术方案来实现：

一种基于改进RRT*的机器人点云地图路径规划方法，该方法首先对采集到的点云地图数据进行预处理，然后对预处理后的点云地图数据进行地形信息的计算，得到点云地图中各个位置的可行度，最后将各个位置的可行度结合改进的RRT*算法，进行路径搜索，得到三维路径，基于该三维路径控制机器人移动。

进一步地，对所述点云地图数据进行的预处理包括离群点剔除、降采样和网格对齐。

进一步地，利用PCL对所述点云地图数据进行预处理。

进一步地，所述可行度由可行度函数计算获得，所述可行度函数表示为：

其中，R(p)为点p的可行度，z为点p的高度，

为地形平均高度，

分别为点p的x方向和y方向的梯度。

进一步地，利用所述改进的RRT*算法进行路径搜索具体包括：

1)将初始位置作为随机树生长的根节点x_init，设定路径的目标位置x_goal；

2)在运动空间中选择随机节点x_rand，根据步长step由此随机节点生成邻域中心点x_mid，选取其一定半径r范围内的可行度最高的点作为新的节点x_new；

3)对节点x_new，将其周围半径为δ范围内的已在扩展树上的点归入集合X_near，对X_near内每一点x_near，计算其到起点的代价加到x_near的代价；

4)选取各个x_near中计算出的代价最小的点x_min，令其成为x_new的父节点；

5)对于集合X_near\{x_min}，计算其中各点x_near当前到起点的代价与其到起点的代价加x_new到起点的代价，若前者大于后者，则将该点的父节点更改为x_new，否则保持不变；

6)重复步骤2)到步骤5)直至新节点与目标节点间距离小于设定阈值|x_new-x_goal|≤λ，则认为路径已找到。

本发明还提供一种基于改进RRT*的机器人点云地图路径规划系统，包括：

点云预处理模块，用于对采集到的点云地图数据进行预处理；

可行度分析模块，用于对预处理后的点云地图数据进行地形信息的计算，得到点云地图中各个位置的可行度；

搜索规划模块，用于将各个位置的可行度结合改进的RRT*算法，进行路径搜索，得到三维路径，基于该三维路径控制机器人移动。

进一步地，所述点云预处理模块中，对点云地图数据进行的预处理包括离群点剔除、降采样和网格对齐。

进一步地，所述点云预处理模块中，利用PCL对点云地图数据进行预处理。

进一步地，所述可行度分析模块中，可行度由可行度函数计算获得，所述可行度函数表示为：

其中，R(p)为点p的可行度，z为点p的高度，

为地形平均高度，

分别为点p的x方向和y方向的梯度。

进一步地，所述搜索规划模块利用改进的RRT*算法进行路径搜索的具体步骤包括：

1)将初始位置作为随机树生长的根节点x_init，设定路径的目标位置x_goal；

2)在运动空间中选择随机节点x_rand，根据步长step由此随机节点生成邻域中心点x_mid，选取其一定半径r范围内的可行度最高的点作为新的节点x_new；

3)对节点x_new，将其周围半径为δ范围内的已在扩展树上的点归入集合X_near，对X_near内每一点x_near，计算其到起点的代价加到x_near的代价；

4)选取各个x_near中计算出的代价最小的点x_min，令其成为x_new的父节点；

5)对于集合X_near\{x_min}，计算其中各点x_near当前到起点的代价与其到起点的代价加x_new到起点的代价，若后者大于前者，则将该点的父节点更改为x_new，否则保持不变；

6)重复步骤2)到步骤5)直至新节点与目标节点间距离小于设定阈值|x_new-x_goal|≤λ，则认为路径已找到。

与现有技术相比，本发明具有如下有益效果：

1、该方案基于激光雷达采集的三维点云数据进行路径规划，可以直接在三维点云地图上进行机器人的路径规划，找到一条相对平滑，对于机器人消耗最少的三维路径，可用于机器人在复杂环境下的导航。

2、本发明首先对三维点云数据进行预处理，降低处理计算量，有效提高规划精度和效率。

3、本发明利用各点的可行度进行路径规划，适用于三维点云地图特点，提高机器人路径规划精度。

4、本发明对RRT*算法进行改进，将二维路径搜索算法扩展到三维点云，能够直接在点云地图上规划出适合移动机器人移动的路径。

附图说明

图1为本发明的整体流程示意图；

图2为改进RRT*算法中新节点选取示意图；

图3为改进RRT*算法中父节点选取示意图；

图4为地形一点云和起、终点设定；

图5为地形一下本方法生成的路径；

图6为地形二点云和起、终点设定；

图7为地形二下本方法生成的路径；

图8为地形三点云和起、终点设定；

图9为地形三下本方法生成的路径。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

本发明提供一种基于改进RRT*的机器人点云地图路径规划方法，该方法包括如下步骤：(1)通过激光雷达采集环境的三维点云数据；(2)对采集到的原始点云数据进行处理，包括剔除离群点(点云的滤波)、降采样、网格对齐等操作；(3)将预处理好的点云数据进行地形信息的计算，实现地形的可行性分析，得到地图中各个位置的可行度(可通行程度)信息；(4)应用改进的RRT*算法，结合得到的地形可行度在点云地图中进行路径规划，得到一条平滑的适合机器人移动的三维路径，基于该三维路径对机器人移动过程进行控制。

1、点云数据的预处理

初始得到的点云具有噪声点，并且密度很高，处理起来计算量太大，需要对其进行预处理。本实施例利用PCL来进行点云的预处理，包括滤波、降采样等。PCL是在吸收了前人点云相关研究基础上建立起来的跨平台开源C++编程库，实现了大量点云相关的通用算法和高效数据结构。

1.1、剔除点云数据的离群点

激光扫描通常会产生密度不均匀的点云数据，另外测量中的误差会产生稀疏的离群点。估计局部点云特征时会导致错误的数值。本实施例通过Statistical Out literRemoval滤波器对点云进行离群点的移除。

本发明采用的稀疏离群点移除方法基于在输入数据中对点到临近点的距离分布的计算。对每个点，计算它到它的所有临近点的平均距离，假设得到的结果是一个高斯分布，其形状由均值和标准差决定，平均距离在标准范围之外的点，可被定义为离群点并可以从数据集中去除掉。

该稀疏离群点移除方法步骤如下：

1)对每个点x，求其k邻域的距离均值；

2)对整个数据中的各点的k临近距离均值，求均值和标准差；

3)根据设定的距离阈值与各点的距离均值作比较，若超出该阈值，则认为该点是离群点，将其剔除。

1.2、对点云进行降采样

扫描得到的点云数据量庞大，由于分辨率的关系，在1平方米的面积上就会有数以万计的点云数据，因此在整个环境点云的数据量是非常庞大的，这在计算和处理上就会有一定的难度。所以必须对点云进行降采样处理，才能进行进一步的运算。本实施例选择PCL点云库的Voxel Grid滤波器对点云进行降采样处理。

降采样的步骤如下：

1)通过输入的点云数据创建一个三维体素栅格；

2)在每个体素内用体素中所有点的重心来近似显示体素中的其他点；

3)对所有体素处理后，即可得到过滤后的点云。

经过降采样后，体素中的所有点就用一个重心点来最终表示，这种方法比用体素中心来逼近的方法对于采样点对应的曲面的表示更为准确。

1.3、对点云进行对齐操作

对于杂乱的点云数据，为了能够方便计算其各个位置的地形特征，需要进行对齐操作。首先，找出无序点云数据的边界[X_min,X_max,Y_min,Y_max]，设定步长step，根据边界和步长建立网格：

此时，存在着点云中多个点被重新定位在了同一个x-y平面的位置，这显然是不可行的。为了最大程度地表示出地形的特征，可以选取每个位置的最高点来代表该位置的地形的高度值，而不是对多个z值取平均：z_i＝max(z_ij)(j＝1,2,3...)，这样就得到了一个对齐在网格上的三维点云，便于接下来计算点云的地形信息。

2、点云地图的地形分析

对于三维点云地图，与二维地图不同的是，其不再有明确的障碍区域和非障碍区域。因此，本发明设计了一种计算地图可行性的方法，来判断地图的各个位置是否适合通行。

该方法主要需要求出地形的复杂程度，地形越复杂，函数值越高，地形越平坦，函数值越低，故选择梯度物理量来作为衡量指标之一。对于每个点p，都可以计算出其两个方向的梯度。另一个因素是地形高度对实际导航时的影响，将各点高度和整体平均高度作比较。综上分析，按如下步骤进行地形可行性分析：

1)对于点云内所有p，计算x方向和y方向的梯度

2)计算各点高度z与地形平均高度

之差；

3)定义可行性函数

通过该函数计算出各点的可行性。

通过将点云地图输入上面的函数，可以给每个点计算出其可通行的程度，用于下一步的路径规划。

3、RRT*算法的改进和应用

基于采样的路径规划算法具有效率高的特点，比较适合于机器人的路径规划。基于采样的路径规划算法主要有PRM和RRT算法，本发明采用RRT算法。RRT是一种多维空间中有效率的规划方法，它以一个初始点作为根节点，通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，便可以在随机树中找到一条由从初始点到目标点的路径，主要有以下优点：

①快速扩展随机树是概率完全的，一般情况下都会有解；

②搜索范围很大，能够很大程度的避免陷入到局部的障碍物中而导致搜索不到路径；

③算法相对简单，效率高，容易实现。

3.1、RRT*算法概述

RRT*(rapidly exploring random tree star)算法是一种RRT算法的改进，它是一种渐进最优算法，该算法在原有的RRT算法上，改进了父节点选择的方式，采用代价函数来选取拓展节点领域内最小代价的节点为父节点，同时，每次迭代后都会重新连接现有树上的节点，从而保证计算的复杂度和渐进最优解。与传统RRT算法不同的是，RRT*首先在新生成的节点附近建立临近节点分布图，然后遍历临近的节点，检查是否存在新的路径比现有的路径的代价更低，如果存在则将现有的路径更新为代价更低的路径。

3.2、改进RRT*算法流程

本发明对RRT*算法进行改进，通过可行性算法，计算出点云中各点的可通行性程度，从而将点的选取区域从二维空间扩展到三维空间，使其适用于三维点云上的路径规划，改进RRT*算法的流程如下：

1)将初始位置作为随机树生长的根节点x_init，设定路径的目标位置x_goal。

2)在运动空间中选择随机节点x_rand，根据步长step由此随机节点生成邻域中心点x_mid，选取其一定半径r范围内的可行度最高的点作为新的节点x_new，该可行度由可行性函数R(p)计算得到。

3)对节点x_new，将其周围半径为δ范围内的已在扩展树上的点归入集合X_near，对X_near内每一点x_near，计算其到起点的代价加到x_near的代价。

4)选取各个x_near中计算出的代价最小的点x_min，令其成为x_new的父节点。

5)对于集合X_near\{x_min}，计算其中各点x_near当前到起点的代价与其到起点的代价加x_new到起点的代价，若前者大于后者，则将该点的父节点更改为x_new，否则保持不变。

6)重复步骤2)到步骤5)直至新节点与目标节点间距离小于设定阈值|x_new-x_goal|≤λ，则认为路径已找到。

4、方案效果

为了验证本方法的可行性，在Matlab平台上对多种三维地形进行路径生成。在实验中，首先要设置路径的起点和终点，然后通过可行性分析和路径规划算法，生成路径。

如图4所示为地形一(2601个点)的示意图。为了验证方法的稳定性，进行了10次试验，算法运行的时间和生成路径的长度如表1所示，最终生成路径如图5所示。

表1地形一路径长度和运算时间

如图6所示为地形二(10201个点)的示意图。为了验证方法的稳定性，进行了10次试验，算法运行的时间和生成路径的长度如表2所示，最终生成路径如图7所示。

表2地形二路径长度和运算时间

如图8所示为地形三(5776个点)的示意图。为了验证方法的稳定性，进行了10次试验，算法运行的时间和生成路径的长度如表3所示，最终生成路径如图9所示。

表3地形三路径长度和运算时间

上述实验结果表明，本方案具有较好的稳定性，对于同一地形的不同次试验，都能生成合理的路径，并且路径长度基本相同；本方案也具有较好的鲁棒性，对于不同情况的地形，都能够生成有效的平滑路径。在时间方面，由于RRT*算法的采样随机性，不能保证每次的运算时间都相同，且由数据结果可以看出，运行时间和点的数量呈指数增长关系。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (6)

1.一种基于改进RRT*的机器人点云地图路径规划方法，其特征在于，该方法首先对采集到的点云地图数据进行预处理，然后对预处理后的点云地图数据进行地形信息的计算，得到点云地图中各个位置的可行度，最后将各个位置的可行度结合改进的RRT*算法，进行路径搜索，得到三维路径，基于该三维路径控制机器人移动；

所述可行度由可行度函数计算获得，所述可行度函数表示为：

其中，R(p)为点p的可行度，z为点p的高度，

为地形平均高度，

分别为点p的x方向和y方向的梯度；

利用所述改进的RRT*算法进行路径搜索具体包括：

1)将初始位置作为随机树生长的根节点x_init，设定路径的目标位置x_goal；

2)在运动空间中选择随机节点x_rand，根据步长step由此随机节点生成邻域中心点x_mid，选取其一定半径r范围内的可行度最高的点作为新的节点x_new；

3)对节点x_new，将其周围半径为δ范围内的已在扩展树上的点归入集合X_near，对X_near内每一点x_near，计算其到起点的代价加到x_near的代价；

4)选取各个x_near中计算出的代价最小的点x_min，令其成为x_new的父节点；

5)对于集合X_near\{x_min}，计算其中各点x_near当前到起点的代价与其到起点的代价加x_new到起点的代价，若前者大于后者，则将该点的父节点更改为x_new，否则保持不变；

6)重复步骤2)到步骤5)直至新节点与目标节点间距离小于设定阈值|x_new-x_goal|≤λ，则认为路径已找到。

2.根据权利要求1所述的基于改进RRT*的机器人点云地图路径规划方法，其特征在于，对所述点云地图数据进行的预处理包括离群点剔除、降采样和网格对齐。

3.根据权利要求1所述的基于改进RRT*的机器人点云地图路径规划方法，其特征在于，利用PCL对所述点云地图数据进行预处理。

4.一种基于改进RRT*的机器人点云地图路径规划系统，其特征在于，包括：

点云预处理模块，用于对采集到的点云地图数据进行预处理；

可行度分析模块，用于对预处理后的点云地图数据进行地形信息的计算，得到点云地图中各个位置的可行度；

搜索规划模块，用于将各个位置的可行度结合改进的RRT*算法，进行路径搜索，得到三维路径，基于该三维路径控制机器人移动；

所述可行度分析模块中，可行度由可行度函数计算获得，所述可行度函数表示为：

其中，R(p)为点p的可行度，z为点p的高度，

为地形平均高度，

分别为点p的x方向和y方向的梯度；

所述搜索规划模块利用改进的RRT*算法进行路径搜索的具体步骤包括：

1)将初始位置作为随机树生长的根节点x_init，设定路径的目标位置x_goal；

2)在运动空间中选择随机节点x_rand，根据步长step由此随机节点生成邻域中心点x_mid，选取其一定半径r范围内的可行度最高的点作为新的节点x_new；

3)对节点x_new，将其周围半径为δ范围内的已在扩展树上的点归入集合X_near，对X_near内每一点x_near，计算其到起点的代价加到x_near的代价；

4)选取各个x_near中计算出的代价最小的点x_min，令其成为x_new的父节点；

5)对于集合X_near\{x_min}，计算其中各点x_near当前到起点的代价与其到起点的代价加x_new到起点的代价，若前者大于后者，则将该点的父节点更改为x_new，否则保持不变；

6)重复步骤2)到步骤5)直至新节点与目标节点间距离小于设定阈值|x_new-x_goal|≤λ，则认为路径已找到。

5.根据权利要求4所述的基于改进RRT*的机器人点云地图路径规划系统，其特征在于，所述点云预处理模块中，对点云地图数据进行的预处理包括离群点剔除、降采样和网格对齐。

6.根据权利要求4所述的基于改进RRT*的机器人点云地图路径规划系统，其特征在于，所述点云预处理模块中，利用PCL对点云地图数据进行预处理。

Images