[发明专利]基于改进RRT*的机器人点云地图路径规划方法及系统

说明书

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

技术领域

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

背景技术

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

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

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

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

发明内容

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

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

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

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

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

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

其中，R(p)为点p的可行度，z为点p的高度，为地形平均高度，分别为点p的x方向和y方向的梯度。

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

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