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

权利要求书

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对所述点云地图数据进行预处理。