一种未知空间自主探索规划方法与流程

1.本发明涉及路径规划技术领域，尤其涉及一种未知空间自主探索规划方法。


背景技术：

2.近年来，机器人技术飞速发展，自主移动机器人在探索人类不适宜居住、工作的环境如矿井，隧道等方面的研究越来越多。由于地下未知环境的复杂性，比如复杂深邃的地形，通信不发达导致不能直接对机器人进行远程的控制，以及不能利用gps进行导航等。在这种复杂环境下，自主移动机器人需要是高度自主化、鲁棒、可靠和适应在复杂环境下运动的。自主探索技术指导机器人在未知环境下自主运动，依靠各种传感器感知未知环境，完成搜索、救援、探测等具体任务。尽管众多研究者在硬件系统、软件算法上都取得了重大进展，但复杂的地下未知环境仍然对机器人的自主探索能力造成巨大的挑战。尤其是对地下未知多分支环境的探索，这种环境的特性是通常规模很大，分支路径长，狭窄分支多。此外，它们的地形通常非常粗糙，碎石等障碍物较多，对机器人的避障性能要求非常高。
3.目前，移动机器人自主探索中使用的主流方法之一是基于边界的探索方法。机器人获取到传感器感知范围内的环境信息，在感知到的已知环境内生成快速搜索随机树，用聚类算法对快速搜索随机树的边界点进行聚类得到质心点；构建得到的质心点收益函数，计算每个质心点的收益值；选择收益值最大的质心点作为目标点，使用a*全局路径规划算法在已知的环境中快速规划出一条由机器人当前位置到目标点的路径，引导机器人向目标点移动；重复以上步骤直至没有侯选的质心点，结束探索。
4.然而这种算法效率和避障效果并不高。它需要在局部地图内随机撒点，由于地下未知环境中障碍物较多，很多采样点撒在障碍物上是无效的；在狭窄分支下，有效的采样点主要集中在狭窄分支的路径上，这也造成了大量采样点无效，对这些采样点进行搜索计算浪费了大量时间。并且由于随机撒点导致某些目标点距离障碍物或者井道较近，对机器人造成了潜在的碰撞风险。此外，由于地下未知空间多分支的特性，目前的探索方法应用于这种环境会随机进入各个分支，探索效率较低。
5.另外，一种基于简化广义voronoi图的机器人自主探索方法也得到了应用，这种基于形态学方法构建简化广voronoi拓扑地图，找到最佳前沿点，并规划机器人当前位置到最佳前沿点的全局路径，沿全局路径将机器人导航至最佳前沿点。
6.然而，单纯基于voronoi图生成拓扑地图，在拓扑地图里找到前沿点，再利用dijkstra算法搜索到最佳前沿点的最短路径，这种仅基于原始voronoi图找前沿点的方法应用于隧道环境会忽略某些探索增益大的狭小分支，造成探索不完整，并且由于地下未知空间多分支的特性，这种探索方法应用于多分支环境会随机进入各个分支，探索效率较低。


技术实现要素：

7.针对上述现有技术的不足，本发明提供一种未知空间自主探索规划方法。
8.为解决上述技术问题，本发明所采取的技术方案是：一种未知空间自主探索规划
方法，包括如下步骤：
9.步骤1：通过三维激光传感器和imu采用三维slam算法获取机器人当前的定位信息和感知信息生成当前环境的三维点云地图；
10.步骤2：在机器人周围设定一个滑动窗口，包含已探索区域和未知区域，在滑动窗口内以机器人当前位置为起点，以当前环境中的障碍物为离散点生成维诺图，并采样生成voronoi
‑
rrt树；
11.所述维诺图使用体素格式。
12.所述以当前环境中的障碍物为离散点生成维诺图，并采样生成voronoi
‑
rrt树的过程如下：
13.s1：在体素格式的地图中将当前环境边界离散化处理，以离散化的边界和离散的障碍物点为顶点构建delaunary三角网，并对所有离散的点和构建好的三角网内的每个三角形编号；
14.s2：计算每个三角形的外接圆圆心，并存储每个三角形对应的离散点；
15.s3：遍历三角网链表，寻找与当前三角形相邻的三个三角形，如果找到则把寻找到的三角形外心与当前三角形外心相连，将连线作为维诺边存入维诺边链表；如果找不到则求出当前三角形中最靠近机器人的边的中垂线存入维诺边链表；
16.s4：以存储的离散点为中心，在维诺边链表附近进行随机的采样，构建出voronoi
‑
rrt树。
17.步骤3：以机器人所在位置为起点，向voronoi
‑
rrt树的所有边界点使用rrt算法找到一组可行路径；
18.步骤4：计算每条可行路径的探索增益，选出探索增益最大的一条作为最优路径，过程如下：
19.步骤4.1：计算每条可行路径σ
i
上每个采样点的体积增益gv，过程如下：
20.步骤4.1.1：每个采样点发射角度均匀离散的固定等长射线模型；
21.步骤4.1.2：统计穿过射线的未知占用体素n
unknown
、已知占用体素n
occupied
和已知无障碍体素n
free
的个数；
22.步骤4.1.3：给每种体素设置一个权重值，并将射线模型上的上述三种体素加权求和作为采样点的体积增益，公式如下：
[0023][0024]
其中，g
unknown
为未知占用体素的权重值，g
occupied
为已知占用体素的权重值，g
free
为已知无障碍体素的权重值。
[0025]
步骤4.2：为了避免机器人频繁进入各个分叉路口，引入轨迹点在时间维度上相似性权重s(σ
i
,σ
e
)用于度量当前可行路径σ
i
和新探索出的路径σ
e
的轨迹相似性，然后把当前可行路径σ
i
上所有采样点的体积增益做加和，计算每条可行路径的探索增益ge(σ
i
)，公式如下：
[0026]
[0027]
其中，是根节点延路径到每个采样点的累计欧式距离，即为用于衡量路径距离的一个权重函数，参数p、q均为权重系数，m
i
为一条路径上的第m个轨迹点，s(σ
i
,σ
e
)为dynamic time warping方法计算轨迹点在时间维度上相似性权重；
[0028]
所述轨迹点在时间维度上相似性权重s(σ
i
,σ
e
)的计算过程如下：
[0029]
s1：在时间轴上对两个长度不同的路径σ
i
和路径σ
e
进行局部的缩放对齐，将两条轨迹的点进行多对多的映射；
[0030]
s2：再计算两条轨迹的欧式距离，用两条轨迹对应点的空间距离的平均值判断相似性。
[0031]
步骤4.3：以探索增益最大的一条路径作为最优路径。
[0032]
步骤5：采用路径规划算法执行动态避障，生成无障碍路径，过程如下：
[0033]
步骤5.1：路径规划算法以步骤4生成的最优路径为参考生成平滑、无障碍的局部轨迹；
[0034]
步骤5.2：以局部轨迹为中心生成一组候选轨迹；
[0035]
步骤5.3：当有动态障碍物切断中心局部轨迹时则迅速转入其他候选轨迹。
[0036]
步骤6：采用dwa算法对无障碍路径进行跟踪，得出速度指令下发到机器人控制平台；
[0037]
步骤7：判断地下未知空间内的最优路径是否探索结束，即机器人是否到达死胡同；若是则调用全局规划器继续判断出对分支探索效益最高的叶节点，基于步骤2的采样点使用dijkstra算法搜索到此结点的最短路径，继续对分支进行探索，转置执行步骤1，进入下一个循环；若不是则转置执行步骤8；
[0038]
所述调用全局规划器继续判断出对分支探索是效益最高的叶节点的过程如下：
[0039]
s1：使用下式判断每个潜在叶节点v
gi
的探索增益ge
g
；
[0040][0041]
其中，gv(v
gi
)为潜在叶节点v
gi
的体积增益，d(v
gc
,v
gi
)表示机器人当前位置v
gc
,到潜在叶节点v
gi
的距离，参数d为权重系数，目的是惩罚全局规划中较长的路径；
[0042]
s2：得到探索增益最高的潜在叶节点。
[0043]
所述潜在叶节点v
gi
为步骤4中除了探索增益最大的最优路径以外的候选路径的终点。
[0044]
步骤8：判断探索是否完成或者电量是否不足以继续探索，满足任意条件则触发回家条件，回溯返回起点的最短路径。
[0045]
采用上述技术方案所产生的有益效果在于：
[0046]
1、本发明提供的方法基于生成的维诺图路径中心采样生成voronoi
‑
rrt，不需要大范围撒点采样，节省计算时间；
[0047]
2、本发明提供的方法基于生成的维诺图路径中心采样生成的voronoi
‑
rrt得到的路径具有远离障碍物的特性，避免了探索过程中离障碍物或者路径边界太近造成的碰撞风险；
[0048]
3、本发明提供的方法在探索增益中引入dwt动态时间规整法评价路径的相似性，保证先探索主干再探索分支，提高探索效率，避免了反复进入分支的情况；
[0049]
4、本发明提供的方法当机器人进入死胡同，或者机器人电量不足时调用全局规划器，重新定位到之前标记的探索空间边界或返回出发点，克服了当前探索方法随机进入未知空间各个分支导致探索效率较低的问题。
附图说明
[0050]
图1为本发明实施例中提供的未知空间自主探索规划方法的流程图；
[0051]
图2为本发明实施例中生成的voronoi
‑
rrt图的俯视图；
[0052]
图3为本发明实施例中生成的voronoi
‑
rrt图的三维视图；
[0053]
图4为本发明实施例中采样点的等长射线模型示意图；
[0054]
图5为本发明实施例中射线模型穿过的三种体素格式。
具体实施方式
[0055]
下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。
[0056]
本实施例中，由机器人平台搭载激光测距仪和ros操作系统；所述激光测距仪与机器人刚体平台连接，用来构建地图和定位；ros操作系统为用于机器人的后操作系统，为本发明的方法提供优良的开发环境。
[0057]
如图1所示，本实施例中未知空间自主探索规划方法如下所述：
[0058]
步骤1：通过三维激光传感器和imu采用三维slam算法获取机器人当前的定位信息和感知信息生成当前环境的三维点云地图；
[0059]
本实施例中，采用16线三维激光传感器和imu，采用三维slam算法获取机器人当前的定位感知信息生成当前环境的三维点云地图。
[0060]
步骤2：在机器人周围设定一个滑动窗口，包含已探索区域和未知区域，在滑动窗口内以机器人当前位置为起点，以当前环境中的障碍物为离散点生成维诺图，并采样生成voronoi
‑
rrt树；
[0061]
所述维诺图使用体素格式。
[0062]
所述以当前环境中的障碍物为离散点生成维诺图，并采样生成voronoi
‑
rrt树的过程如下：
[0063]
s1：在体素格式的地图中将当前环境边界离散化处理，以离散化的边界和离散的障碍物点为顶点构建delaunary三角网，并对所有离散的点和构建好的三角网内的每个三角形编号；
[0064]
s2：计算每个三角形的外接圆圆心，并存储每个三角形对应的离散点；
[0065]
s3：遍历三角网链表，寻找与当前三角形相邻的三个三角形，如果找到则把寻找到的三角形外心与当前三角形外心相连，将连线作为维诺边存入维诺边链表；如果找不到则求出当前三角形中最靠近机器人的边的中垂线存入维诺边链表；
[0066]
s4：以存储的离散点为中心，在维诺边链表附近进行随机的采样，构建出voronoi
‑
rrt树。
[0067]
本实施例中，生成的voronoi
‑
rrt图的俯视图如图2所示，对应的三维视图如图3所示。
[0068]
步骤3：以机器人所在位置为起点，向voronoi
‑
rrt树的所有边界点使用rrt算法找到一组可行路径；
[0069]
步骤4：计算每条可行路径的探索增益，选出探索增益最大的一条作为最优路径，过程如下：
[0070]
步骤4.1：计算每条可行路径σ
i
上每个采样点的体积增益gv，过程如下：
[0071]
步骤4.1.1：每个采样点发射角度均匀离散的固定等长射线模型；
[0072]
本实施例中，采样点的等长射线模型如图4所示。
[0073]
步骤4.1.2：统计穿过射线的未知占用体素n
unknown
、已知占用体素n
occupied
和已知无障碍体素n
free
的个数；
[0074]
本实施例中，穿过射线的未知占用体素、已知占用体素和已知无障碍体素射线模型穿过的三种体素格式的示意图如图5所示。
[0075]
步骤4.1.3：给每种体素设置一个权重值，并将射线模型上的上述三种体素加权求和作为采样点v
ij
的体积增益，公式如下：
[0076][0077]
其中，g
unknown
为未知占用体素的权重值，g
occupied
为已知占用体素的权重值，g
free
为已知无障碍体素的权重值。
[0078]
步骤4.2：为了避免机器人频繁进入各个分叉路口，引入轨迹点在时间维度上相似性权重s(σ
i
,σ
e
)用于度量当前可行路径σ
i
和新探索出的路径σ
e
的轨迹相似性，然后把当前可行路径σ
i
上所有采样点的体积增益做加和，计算每条可行路径的探索增益ge(σ
i
)，公式如下：
[0079][0080]
其中，是根节点延路径到每个采样点的累计欧式距离，即为用于衡量路径距离的一个权重函数，参数p、q均为权重系数，m
i
为一条路径上的第m个轨迹点，s(σ
i
,σ
e
)为dynamic time warping方法计算轨迹点在时间维度上相似性权重；
[0081]
所述轨迹点在时间维度上相似性权重s(σ
i
,σ
e
)的计算过程如下：
[0082]
s1：在时间轴上对两个长度不同的路径σ
i
和路径σ
e
进行局部的缩放对齐，将两条轨迹的点进行多对多的映射；
[0083]
s2：再计算两条轨迹的欧式距离，用两条轨迹对应点的空间距离的平均值判断相似性。
[0084]
步骤4.3：以探索增益最大的一条路径作为最优路径。
[0085]
步骤5：采用路径规划算法执行动态避障，生成无障碍路径，过程如下：
[0086]
步骤5.1：路径规划算法以步骤4生成的最优路径为参考生成平滑、无障碍的局部轨迹；
[0087]
步骤5.2：以局部轨迹为中心生成一组候选轨迹；
[0088]
步骤5.3：当有动态障碍物切断中心局部轨迹时则迅速转入其他候选轨迹。
[0089]
步骤6：采用dwa算法对无障碍路径进行跟踪，得出速度指令下发到机器人控制平
台；
[0090]
步骤7：判断地下未知空间内的最优路径是否探索结束，即机器人是否到达死胡同；若是则调用全局规划器继续判断出对分支探索效益最高的叶节点，基于步骤2的采样点使用dijkstra算法搜索到此结点的最短路径，继续对分支进行探索，转置执行步骤1，进入下一个循环；若不是则转置执行步骤8；
[0091]
所述调用全局规划器继续判断出对分支探索是效益最高的叶节点的过程如下：
[0092]
s1：使用下式判断每个潜在叶节点v
gi
的探索增益ge
g
；
[0093][0094]
其中，gv(v
gi
)为潜在叶节点v
gi
的体积增益，d(v
gc
,v
gi
)表示机器人当前位置v
gc
,到潜在叶节点v
gi
的距离，参数d为权重系数，目的是惩罚全局规划中较长的路径；
[0095]
s2：得到探索增益最高的潜在叶节点。
[0096]
所述潜在叶节点v
gi
为步骤4中除了探索增益最大的最优路径以外的候选路径的终点。
[0097]
步骤8：判断探索是否完成或者电量是否不足以继续探索，满足任意条件则触发回家条件，回溯返回起点的最短路径。