基于拓扑图的覆盖路径规划方法和装置

1.本发明涉及路径规划技术领域，具体涉及一种基于拓扑图的覆盖路径规划方法和装置。


背景技术：

2.覆盖未知空间是移动机器人主动探索的基本能力之一。对于完全自主的系统覆盖方法，机器人需要主动规划其路径，以实现环境的全覆盖。规划出能通过设定的特征点同时避开障碍物的路径称为覆盖路径规划，目标是规划一条覆盖指定区域的有效路径。
3.常见的覆盖路径规划方法包括基于边界的覆盖路径规划、下一最佳视点nbv和基于采样的覆盖路径规划。
4.基于边界的覆盖路径规划是早期贡献之一。基于边界的方法识别地图中自由已知空间和未知空间之间的边界，并反复选择一个这样的边界作为目标，最终导致完全覆盖。
5.根据在先前位置获取的数据来计算一系列之后的传感位置被称为下一最佳视点(next-best-view)nbv。下一最佳视点nbv算法通过选择的位置引导机器人导航，以确保对环境的全面覆盖。现有的大多数nbv技术都没有解决导航安全问题。此外，为了应用基于信息增益驱动的覆盖路径规划策略进行探索，通常使用度量网格图/体素图。处理体素图以识别下一个最佳视点是一项计算量很大的任务。计算下一个最佳视点成本高且耗时，为了解决这个问题，下一个最佳视点是随机抽样的。
6.基于采样的覆盖路径规划由可以采样的机器人配置组成，这些配置共同给出了结构边界的完全覆盖，这被称为覆盖采样问题。基于采样的方法通常以下一最佳视点nbv方式对视图配置进行采样。基于采样的方法在计算上比基于边界的方法更有效，这些方法还应用成本效用函数来评估采样节点的探索潜力以获得覆盖路径。
7.基于边界的覆盖路径规划、下一最佳视点nbv和基于采样的覆盖路径规划都需要环境的先验信息，因此它们不适合杂乱、动态的环境；且现有方案大多数专注于使用占用栅格地图规划路径，这些地图不能很好地扩展，计算效率低，并且在较大的传感器范围噪声值下不稳健，在高噪声环境下不具备鲁棒性。


技术实现要素：

8.针对现有技术中存在不足，本发明提供了一种基于拓扑图的覆盖路径规划方法和装置。
9.本发明是通过以下技术手段实现上述技术目的的。
10.基于拓扑图的覆盖路径规划方法：工控机基于深度扫描点，确定机器人前往终点位置行驶路径的可见边界；控制机器人从当前位置向可见边界点前进，若当前位置与前往的可见边界点之间的距离大于设定的移动距离，则利用当前位置与前往的可见边界点之间的显著特征点进行前进，否则直接前往可见边界点；重复上述过程，直至机器人到达终点位置。
11.进一步的技术方案，所述可见边界的确定过程为：从深度扫描点p1到pn逆时针搜索时，检测不连续性起点，再确定不连续性起点对应的终点；从深度扫描点pn到p1顺时针搜索时，检测不连续性起点，再确定不连续性起点对应的终点；不连续性的起点与对应的终点之间的连线即为可见边界。
12.更进一步的技术方案，所述不连续性包括：
13.两个连续深度扫描点pi和p
i 1
都是障碍点，pi和p
i 1
之间的距离超过fw：||p
i-p
i 1
||》fw；
14.两个连续深度扫描点pi和p
i 1
中至少有一个是非障碍点，非障碍点到机器人之间的距离为传感器的最大扫描范围r
max
；
15.其中fw是机器人可以通过边界的最小宽度。
16.进一步的技术方案，所述不连续性起点对应的终点确定过程为：
17.不连续性起点ps(f)和列表中任一障碍点pk构成的边界从机器人当前位置可见，且满足的障碍点组成所有有效点的集合v(la)，所述集合v(la)中与不连续性起点ps(f)距离最短的点为终点pe(f)；所述列表由逆/顺时针搜索时处于ps(f)之后m个障碍点组成。
18.更进一步的技术方案，所述集合v(la)为空集，将终点pe(f)设置成与起点ps(f)距离为fw的虚拟点，从而确定边界；其中fw是机器人可以通过边界的最小宽度。
19.进一步的技术方案，所述显著特征点的确定过程为：
20.障碍物表面的起点、障碍物表面的终点和机器人坐标形成的三角形质心视为一个显著特征点的候选点，如果候选点与所有深度扫描点的距离均大于fw，则显著特征点将被添加到拓扑图中；其中fw是机器人可以通过边界的最小宽度。
21.一种基于拓扑图的覆盖路径规划装置，包括：
22.可见边界检测模块，用于确定机器人前往终点位置行驶路径的可见边界；
23.显著特征检测模块，用于确定机器人当前位置与前往的可见边界点之间的显著特征点。
24.本发明的有益效果为：
25.(1)本发明的深度传感器仅用于深度扫描，利用深度扫描点检测可见边界和显著特征，检测结果受高噪声环境影响较小；
26.(2)本发明在机器人当前位置实时确定对应的可见边界点和显著特征点，构建拓扑图，实现覆盖路径规划，计算效率高，且拓展性较好。
附图说明
27.图1为本发明所述不连续性检测示意图；
28.图2为本发明所述可见边界搜索示意图；
29.图3为本发明所述显著特征检测示意图；
30.图4为本发明所述拓扑图构建示意图；
31.图5为本发明所述路径点选择示意图；
32.图6为本发明所述构建的拓扑图。
具体实施方式
33.下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。
34.本发明一种基于拓扑图的覆盖路径规划方法和装置的基本思想为：
35.将机器人投放到一个陌生的室内环境中，基于机器人自身携带的深度传感器检测出当前环境中机器人前进过程中的可见边界(visible frontiers)q
vf
和显著特征(salient features)点q
sf
，在工控机中以拓扑图g的形式构建路线图，下面分别介绍可见边界检测和显著特征检测。
36.一、可见边界的检测
37.沿机器人自身顺时针和逆时针方向利用深度传感器进行深度扫描(depth scans)ds，以检测出在机器人当前位置pr传感器可以扫描到的可见边界；深度扫描点列表由ds＝{p1,...,pn}表示，深度扫描点相对于传感器坐标系按逆时针顺序排列；单个深度扫描点pi的极坐标和笛卡尔坐标由(ri,θi)、(xi,yi)表示，其中ri表示深度扫描点pi在机器人坐标系下的极径，θi表示深度扫描点pi在机器人坐标系下的极角，(xi,yi)表示深度扫描点pi在世界坐标系下的横纵坐标，传感器最大扫描范围用r
max
表示。
38.(1)不连续性检测
39.可见边界检测的第一步是深度的不连续性检测，如果满足以下条件之一，则在两个连续深度扫描点(pi,p
i 1
)之间的深度扫描中发生不连续：
40.类型1不连续性：pi和p
i 1
都是障碍点，pi和p
i 1
之间的距离超过fw：||p
i-p
i 1
||》fw；
41.类型2不连续性：pi和p
i 1
至少有一个是非障碍点，非障碍点到机器人之间的距离为传感器的最大扫描范围r
max
；
42.其中fw是机器人可以通过边界的最小宽度，fw的确定基于机器人的形状，例如，如果机器人是圆形的，则fw设置为它的直径 d
safe
；对于矩形机器人，fw设置为其宽度 d
safe
，其中d
safe
是确保机器人安全通过的安全距离。
43.如图1所示，p1、p
10
、p
11
为非障碍点，其余的点都为障碍点(障碍点直接由深度传感器检测出来，机器人周边的深度扫描点远不止这些点，只是为了方便说明，假定这些点为所有的深度扫描点)。由于p5和p6都为障碍点，若它们之间的距离大于fw，则p5和p6之间发生类型1不连续；由于p1为非障碍点，p2为障碍点，所以p1和p2之间发生类型2不连续；由于p6和p7都为非障碍点，所以p6和p7之间发生类型2不连续。
44.不连续的两端用(rs,θs)和(re,θe)表示，逆时针深度扫描时θe≥θs，而顺时针深度扫描时θs≥θe；起点(rs,θs)也称为不连续检测的参考点p
ref
(f)，用于搜索不连续的终点(re,θe)。
45.(2)可见边界搜索
46.首先，在从点p1到pn逆时针搜索检测不连续性的起点，然后找到不连续性起点对应的终点；其次，从点pn到p1顺时针搜索检测不连续性的起点，然后找到不连续性起点对应的终点；不连续性的起点与对应的终点之间的连线即为可见边界。接下来，以逆时针搜索为例进行详细说明：
47.在逆时针方向检查每两个连续深度扫描点(pi,p
i 1
)之间是否存在类型1的不连续性，若存在类型1的不连续性，则将深度扫描点pi作为不连续性的起点ps(f)＝f(rs,θs)，在
不连续性的起点创建其参考点，用p
ref
(f)＝(rs,θs)表示，接下来，寻找不连续性的终点pe(f)＝(re,θe)，方法如下：
48.用la＝{p
i 1
,...,p
i m
}表示逆时针搜索时处于ps(f)之后的m个障碍点的列表，满足逆时针搜索m个障碍点总的旋转角度不超过π；
49.列表la中的每个障碍点(例如pk∈la)是否被标记为有效，首先取决于起点ps(f)和列表中任一障碍点pk创建的边界是否从机器人当前位置pr可见(参见图2，边界ab为在机器人当前位置pr可见，而边界de在机器人当前位置pr不可见)；
50.为了使障碍点pk有效，在满足从机器人当前位置pr可见的情况下，还必须满足以下条件：
[0051][0052]
其中是落在la中的位于ps(f)之后和障碍点pk之前的一系列点的可见角，与每个障碍点pk∈la相关的可见角定义为：
[0053][0054]
其中rs是机器人当前位置pr到起点ps(f)的距离，rk是起点ps(f)到pk的距离，dk是机器人当前位置pr和pk之间的距离。
[0055]
满足从机器人当前位置可见和公式(1)的障碍点组成所有有效点的集合v(la)，v(la)中与ps(f)距离最短的点被选定代表终点pe(f)：
[0056][0057]
ps(f)和pe(f)所连接的线段即表示一个边界，从pe(f)开始继续进行不连续性搜索，寻找下一个边界。
[0058]
在v(la)为空的情况下，将终点pe(f)设置成与起点ps(f)距离为fw的虚拟点，从而确定边界。
[0059]
如图2所示，在逆时针搜索过程中，a点开始出现类型1不连续，因而选定a点为不连续的起点。根据公式(1)选出所有的有效点，比如b点是一个选出的有效点，因为b点的可见角小于ab之间所有其他点(图中b右边的点比如g点)的可见角，再根据公式(3)，确定点c是不连续的终点。因此图2中ac表示在逆时针搜索期间检测到的第一个可见边界，由于ac从机器人当前位置可以看到，所以它被添加到了拓扑图g中；类似的，de表示在逆时针搜索过程中检测到的另一个边界，但边界de没有被添加到拓扑图g中，因为从机器人当前位置pr无法看到该边界。
[0060]
顺时针搜索与逆时针搜索原理相同，参见图2，ef表示在顺时针搜索期间检测到的第一个可见边界，由于ef从机器人当前位置可以看到，所以它被添加到了拓扑图g中；类似的，ag表示在顺时针搜索过程中检测到的另一个边界，但边界ag没有被添加到拓扑图g中，因为从机器人当前位置pr无法看到该边界。
[0061]
(3)利用边界点表示可见边界
[0062]
第i个可见边界的起点与终点构成的线段中点称为边界点(frontier point)，可利用边界点表示可见边界；边界点的计算公式如下：
[0063][0064]
分别表示第i个边界点q
vfi
在世界坐标系下的横纵坐标，和分别表示第i个边界点q
vfi
在世界坐标系下起点的横纵坐标，和分别表示第i个边界点q
vfi
在世界坐标系下终点的横纵坐标。
[0065]
二、显著特征点的检测
[0066]
除了可见边界之外，连续的障碍物表面可确定为环境的显著特征。为了找到显著特征，首先将传感器深度扫描分成几个相等的部分，对于连续的障碍物表面，沿机器人自身逆时针扫描到的第一个点(rs,θs)用于计算障碍物表面的起点(xs,ys)，扫描到的最后一个点(re,θe)用于计算障碍物表面的终点(xe,ye)，两个点的坐标计算如下：
[0067][0068][0069]
其中，(x
robot
,y
robot
)表示机器人在世界坐标下的横纵坐标，yaw
robot
表示机器人在世界坐标系下的朝向，通过添加(x
robot
,y
robot
)和yaw
robot
，将障碍物表面的起点和终点在机器人坐标系下的表示(rs,θs)、(re,θe)转换到了世界坐标系下的表示(xs,ys)、(xe,ye)，示意图如图3所示。
[0070]
然后两个端点(xs,ys)、(xe,ye)和机器人坐标(x
robot
,y
robot
)所形成的三角形质心视为一个显著特征点q
sf
＝(x
sf
,y
sf
)的候选点，计算如下：
[0071][0072]
如果q
sf
与所有深度扫描点的距离均大于fw，则显著特征点q
sf
＝(x
sf
,y
sf
)将被添加到拓扑图g中。
[0073]
三、区域覆盖的拓扑图构建
[0074]
检测到的可见边界和保留的显著特征以图的形式存储，用于近似环境的拓扑。如图4所示，机器人当前位置pr为构建拓扑图提供了基础；在图4中，pr表示机器人当前位置，黑色箭头代表机器人朝向，而4个边界点q
vf1
、q
vf2
、q
vf3
、q
vf4
用于代表检测到的边界，仅保留2个边界点q
vf2
、q
vf3
，因为它们从机器人当前位置可见；线段a、b、c、d、e、f代表机器人周围的深度扫描，a、f对应于边界区域，而b、c、d、e对应于显著特征点q
sf1
、q
sf2
、q
sf3
、q
sf4
，且相邻的显著特征点通过线段相连接，机器人当前位置pr充当拓扑图的根。
[0075]
设定机器人要到达的终点位置，机器人从当前位置pr向着检测的可见边界点前进(在图4中，假设给定的终点位置在机器人右上方，机器人会从当前位置pr往q
vf2
前进，因为
这与机器人去往终点的方向一致)。如果只用边界点构建拓扑图会使得拓扑图过于稀疏，机器人的覆盖路径规划效果很差；显著特征点的加入使得构建的拓扑图变得稠密，可以使机器人的覆盖路径更为平滑，并且对于稠密的地图网络，可以采用成熟的算法(例如dijkstra算法)对机器人进行运动控制。如图5所示，设定移动距离d
fit
(事先设定的)，若机器人当前位置pr与前往的边界点q
vf
之间的距离大于d
fit
，机器人不会直线前往边界点q
vf
，而是选择前往最近的显著特征点，比如q
sf1
；到达q
sf1
后，根据同样的原则，由于机器人当前位置与前往的边界点q
vf
之间的距离大于d
fit
，机器人不会直线前往边界点q
vf
，而是会选择前往最近的显著特征点，比如q
sf2
；重复上述过程，机器人从pr开始依次经过q
sf1
、q
sf2
、q
sf3
、q
sf4
，当机器人到达q
sf4
时，由于机器人当前位置与前往的边界点q
vf
之间的距离小于d
fit
，机器人将会直线前往边界点q
vf
；若机器人当前位置pr与前往的边界点q
vf
之间的距离小于等于d
fit
，则直线前往边界点q
vf
。根据图5所示的方法最终得到的拓扑图如图6所示，实现覆盖路径的规划。
[0076]
一种基于拓扑图的覆盖路径规划装置，包括：
[0077]
可见边界检测模块，用于确定机器人前往终点位置行驶路径的可见边界；
[0078]
显著特征检测模块，用于确定机器人当前位置与前往的可见边界点之间显著特征点。
[0079]
所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。