[发明专利]基于拓扑地图和度量地图融合的自主探索方法和导航方法

权利要求书

1.一种基于拓扑地图和度量地图融合的自主探索方法，应用于机器人对未知环境进行探索，其特征在于，包括：

A、采集当前图像，确定候选的探索方向；

B、对每一个候选的探索方向进行探索，驱动所述机器人向所述候选的探索方向行进第一设定距离，并在行进的过程中更新拓扑地图和度量地图，直至当前图像的所有候选的探索方向均探索完毕；

C、根据已创建的拓扑地图和度量地图，驱动所述机器人行进第二设定距离，并返回A，直至探索完当前未知环境。

2.根据权利要求1所述的方法，其特征在于：

对于每一个未知环境，创建一个拓扑地图和至少两个度量地图；

所述拓扑地图和度量地图之间是双向索引的，拓扑地图的每个节点存在一个对应的变量，所述变量指示该节点所位于的度量地图；每个度量地图保存有用于指示所述度量地图所对应的拓扑地图中节点的信息、以及所述节点在度量地图中的位姿。

3.根据权利要求1所述的方法，其特征在于，所述A包括：

采集当前图像，在当前图像中检测可行走区域，并根据可行走区域确定候选的探索方向。

4.根据权利要求1所述的方法，其特征在于，所述B包括：

B1、如果当前图像存在至少两个候选的探索方向，选择其中一个作为当前探索方向，并将其余的候选的探索方向保存在探索方向集合中；

B2、驱动机器人向当前探索方向行进第一设定距离，根据当前图像与拓扑地图中上一个节点所对应的图像的重叠度更新拓扑地图，并通过跟踪当前图像的视觉特征更新度量地图；

B3、从所述探索方向集合中选择一个探索方向作为当前探索方向，执行B2，删除所选择的探索方向，直至探索方向集合为空。

5.根据权利要求4所述的方法，其特征在于：

所述根据当前图像与拓扑地图中上一个节点所对应的图像的重叠度更新拓扑地图包括：计算当前图像与拓扑地图中上一个节点所对应的图像的重叠度；若重叠度小于设定门限，则将当前图像作为一个节点加入到拓扑地图中，并在当前图像对应的节点和上一个节点之间增加一条边，否则，结束对拓扑地图的更新；

所述通过跟踪当前图像的视觉特征更新度量地图包括：提取当前图像的视觉特征，调用当前的局部度量地图对当前图像的视觉特征进行跟踪；如果跟踪成功，则按照视觉特征的重投影误差最小化的原则，优化度量地图中特征的位姿和机器人的位姿，并将当前图像新观测到的视觉特征加到度量地图中，并建立当前图像所对应的度量地图与拓扑地图的双向索引信息；否则，若跟踪失败的帧数大于设定次数，则创建一个新的度量地图。

6.根据权利要求1至5任一项所述的方法，其特征在于：

驱动所述机器人向所述候选的探索方向行进第一设定距离包括：

通过拓扑地图进行粗略定位，在粗略定位的基础上，通过度量地图进行精确定位；

基于定位结果，使用拓扑地图和度量地图进行路径规划；

按照规划的路径驱动机器人行进第一设定距离；

驱动所述机器人行进第二设定距离包括：

通过拓扑地图进行粗略定位，在粗略定位的基础上，通过度量地图进行精确定位；

基于定位结果，使用拓扑地图和度量地图进行路径规划；

按照规划的路径驱动机器人行进第二设定距离。

7.根据权利要求6所述的方法，其特征在于，所述通过拓扑地图进行粗略定位，在粗略定位的基础上，通过度量地图进行精确定位包括：

将当前图像与拓扑地图中的所有节点进行比较，寻找最相似的节点；

计算当前图像与最相似节点的相对位姿；

用所述最相似节点在度量地图中的位姿和所述相对位姿计算当前图像在度量地图中的位姿，并以该位姿作为初值，按照重投影误差最小化的原则调整机器人的位姿。

8.根据权利要求6所述的方法，其特征在于，所述使用拓扑地图和度量地图进行路径规划包括：

在拓扑地图上寻找一个连通路径作为导航的路径，其中，路径的起点是当前图像对应的节点；路径的终点是目标位置对应的节点；

当机器人位于度量地图覆盖的区域时，采用视觉伺服的方式将机器人调整到目标位姿；当机器人将要离开度量地图覆盖的区域时，计算当前节点与目标节点之间的最短路径，使机器人行驶至下一个节点。