[发明专利]一种未知空间自主探索规划方法

权利要求书

1.一种未知空间自主探索规划方法，其特征在于，包括如下步骤：

步骤1：通过三维激光传感器和IMU采用三维SLAM算法获取机器人当前的定位信息和感知信息生成当前环境的三维点云地图；

步骤2：在机器人周围设定一个滑动窗口，包含已探索区域和未知区域，在滑动窗口内以机器人当前位置为起点，以当前环境中的障碍物为离散点生成维诺图，并采样生成Voronoi-RRT树；

步骤3：以机器人所在位置为起点，向Voronoi-RRT树的所有边界点使用RRT算法找到一组可行路径；

步骤4：计算每条可行路径的探索增益，选出探索增益最大的一条作为最优路径；

步骤5：采用路径规划算法执行动态避障，生成无障碍路径；

步骤6：采用DWA算法对无障碍路径进行跟踪，得出速度指令下发到机器人控制平台；

步骤7：判断地下未知空间内的最优路径是否探索结束，即机器人是否到达死胡同；若是则调用全局规划器继续判断出对分支探索效益最高的叶节点，基于步骤2的采样点使用Dijkstra算法搜索到此结点的最短路径，继续对分支进行探索，转置执行步骤1，进入下一个循环；若不是则转置执行步骤8；

步骤8：判断探索是否完成或者电量是否不足以继续探索，满足任意条件则触发回家条件，回溯返回起点的最短路径；

所述维诺图使用体素格式；

所述步骤4的过程如下：

步骤4.1：计算每条可行路径σ_i上每个采样点的体积增益Gv；

步骤4.2：为了避免机器人频繁进入各个分叉路口，引入轨迹点在时间维度上相似性权重s(σ_i,σ_e)用于度量当前可行路径σ_i和新探索出的路径σ_e的轨迹相似性，然后把当前可行路径σ_i上所有采样点的体积增益做加和，计算每条可行路径的探索增益GE(σ_i)，公式如下：

其中，是根节点延路径到每个采样点的累计欧式距离，即为用于衡量路径距离的一个权重函数，参数p、q均为权重系数，m_i为一条路径上的第m个轨迹点，s(σ_i,σ_e)为Dynamic Time Warping方法计算轨迹点在时间维度上相似性权重；

步骤4.3：以探索增益最大的一条路径作为最优路径。

2.根据权利要求1所述的未知空间自主探索规划方法，其特征在于，所述以当前环境中的障碍物为离散点生成维诺图，并采样生成Voronoi-RRT树的过程如下：

S1：在体素格式的地图中将当前环境边界离散化处理，以离散化的边界和离散的障碍物点为顶点构建Delaunary三角网，并对所有离散的点和构建好的三角网内的每个三角形编号；

S2：计算每个三角形的外接圆圆心，并存储每个三角形对应的离散点；

S3：遍历三角网链表，寻找与当前三角形相邻的三个三角形，如果找到则把寻找到的三角形外心与当前三角形外心相连，将连线作为维诺边存入维诺边链表；如果找不到则求出当前三角形中最靠近机器人的边的中垂线存入维诺边链表；

S4：以存储的离散点为中心，在维诺边链表附近进行随机的采样，构建出Voronoi-RRT树。

3.根据权利要求1所述的未知空间自主探索规划方法，其特征在于，所述步骤4.1的过程如下：

步骤4.1.1：每个采样点发射角度均匀离散的固定等长射线模型；

步骤4.1.2：统计穿过射线的未知占用体素n_unknown、已知占用体素n_occupied和已知无障碍体素n_free的个数；

步骤4.1.3：给每种体素设置一个权重值，并将射线模型上的上述三种体素加权求和作为采样点的体积增益，公式如下：

其中，g_unknown为未知占用体素的权重值，g_occupied为已知占用体素的权重值，g_free为已知无障碍体素的权重值。