基于拓扑地图和度量地图融合的自主探索方法和导航方法与流程

本发明涉及机器人导航技术领域，更具体的涉及一种通过对未知环境进行自主探索的方法，建立环境的拓扑地图和度量地图，并基于所创建的拓扑地图和度量地图进行自主导航的方法。

背景技术：

在各类机器人系统中，自主导航系统起着重要的作用。在不需要对环境进行改造的导航方式中，目前的导航方案主要有基于激光雷达的导航和基于视觉传感器的导航。所述的视觉传感器通常包括单目摄像头、双目摄像头、多个摄像头的组合等。

基于激光雷达的导航方案相对比较成熟，其具有精度高、容易实现、不易受光照等外界光源干扰等优点。但是，好的激光雷达的成本较高。而低成本的激光雷达通常使用电机进行360度扫描。这种方式需要电机按照一定速率旋转，一般使用皮带及滑环的方式。这种低成本的激光雷达具有能耗高、噪声大、使用寿命有限等问题。近年来，随着计算机视觉技术的日渐成熟，开始有机器人产品使用基于视觉传感器的导航方案。基于视觉传感器的导航具有价格低、功耗低等优点，但是算法复杂，实现难度较大。

在机器人的导航系统中，地图的表示方式起着非常重要的作用。按照地图的抽象程度从低到高的顺序，地图可以分为度量地图、拓扑地图、语义地图。其中：

度量地图中的构成元素具有明确的空间几何属性，可以方便地推算出各元素之间的距离和角度等。常见的度量地图有特征地图和栅格地图。

拓扑地图则采用图结构来表示环境。其中，空间中的物体或路标地点对应图结构中的节点，节点之间的连通性对应着图结构中的边。

语义地图是通过物体检测与识别等方式，在地图中添加场景和物体的语义信息。

当前，基于激光雷达的导航方案和基于视觉传感器的导航方案大都采用度量地图。具体地，基于激光雷达的导航方案通常使用栅格地图；而基于视觉传感器的导航方案通常使用特征地图。通常情况下，度量地图的定位精度高，而拓扑地图的路径规划效率高。实际导航应用需要同时兼顾路径规划效率和定位精度。

技术实现要素：

本申请针对基于视觉传感器的导航，提出一种基于拓扑地图和度量地图融合的自主探索方法和导航方法，以兼顾路径规划效率和定位精度。

本申请公开了一种基于拓扑地图和度量地图融合的自主探索方法，应用于机器人对未知环境进行探索，包括：

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

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

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

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

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

较佳的，所述a包括：

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

较佳的，所述b包括：

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

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

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

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

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

较佳的，驱动机器人行进设定距离的方式包括：

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

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

按照规划的路径驱动机器人行进。

较佳的，所述通过拓扑地图进行粗略定位，在粗略定位的基础上，通过度量地图进行精确定位包括：

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

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

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

较佳的，所述使用拓扑地图和度量地图进行路径规划包括：

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

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

本申请还公开了一种基于拓扑地图和度量地图的导航方法，包括：

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

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

按照规划的路径驱动机器人行进。

较佳的，所述通过拓扑地图进行粗略定位，在粗略定位的基础上，通过度量地图进行精确定位包括：

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

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

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

较佳的，所述使用拓扑地图和度量地图进行路径规划包括：

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

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

由上述技术方案可见，本申请通过采集当前图像确定候选的探索方向，并通过对每一个候选的探索方向进行自主探索，以及在自主探索的过程中更新拓扑地图和度量地图，再基于拓扑地图和度量地图融合进行自主探索和导航，兼顾了拓扑地图和度量地图的优势，从而能够兼顾路径规划效率和定位精度。

附图说明

图1为本发明一较佳实施例中地板前沿与探索方向的示意图；

图2为本发明一较佳实施例中自主探索方法的流程示意图；

图3为本发明一较佳实施例中地板检测的典型结果示意图；

图4为本发明一较佳实施例中度量地图的构建结果示意图；

图5为本发明一较佳实施例中创建拓扑地图的流程示意图；

图6为本发明一较佳实施例中创建度量地图的流程示意图；

图7为本发明一较佳实施例中定位方法的流程示意图；

图8为本发明一较佳实施例中导航方法的流程示意图。

具体实施方式

为使本申请的目的、技术方案及优点更加清楚明白，以下参照附图并举实施例，对本申请作进一步详细说明。

本发明提出一种通过自主探索创建拓扑地图和度量地图的方法，以及一种基于拓扑地图和度量地图的视觉导航方法。本发明主要用于机器人在未知环境中的自主探索和导航，使用的传感器可以是视觉传感器，可选地，可以使用辅助定位传感器。在本发明较佳实施例中，采用的视觉传感器为单目摄像头；辅助定位传感器包括码盘和惯性测量单元(inertialmeasurementunit，imu)。需要说明的是，本发明同样适用于其他的视觉传感器，如双目摄像头和多目摄像头等。

本发明方法可以用软件实现，该软件系统主要包括：自主探索模块、地图创建模块、定位模块和路径规划模块。系统在使用时分为两个不同的阶段：一是生成地图阶段；二是导航阶段。在生成地图阶段需要使用所有的4个模块，即自主探索模块、地图创建模块、定位模块、路径规划模块。在导航阶段只需定位模块和路径规划模块工作即可。

在生成地图阶段，自主探索和创建地图是同时进行的。即：机器人一边自主探索，一边创建地图。当自主探索完成后，系统即生成了环境的拓扑地图和度量地图。生成地图阶段主要包含以下3个迭代的步骤：

第一步：自主探索模块确定探索方向。本步骤中，自主探索模块通过采集当前图像，检测可行走区域的方式进行自主探索，根据地板检测的结果生成若干个探索方向。

第二步：机器人从所述若干个探索方向中选择一个探索方向行进，并保存其余的探索方向。在机器人行进的过程中，每获取一幅图像后，使用地图创建模块在线更新拓扑地图和度量地图，直至当前图像的所有候选的探索方向均探索完毕。

第三步：利用已经创建的地图和导航功能将机器人导航至待探索方向的位置，然后返回第一步，直到探索完毕当前未知环境。导航功能的介绍参考接下来的描述。

当系统创建了部分或全部环境的地图之后，系统可以进入导航阶段。导航主要包含以下2个迭代的步骤。

一、定位模块。机器人使用拓扑地图进行粗略定位，然后用度量地图进行精细定位。给定一幅图像，定位模块首先使用拓扑地图进行粗略定位，然后用度量地图进行精细定位。

二、路径规划模块。首先，通过在拓扑地图上寻找一个连通路径作为导航的路径。其中，路径的起点是当前图像对应的节点；路径的终点是目标位置对应的节点。然后用度量地图调整机器人的位姿(pose)；最后，通过运动控制使机器人按照规划的路径前进。

下面通过一个较佳实施例对本申请技术方案进行详细说明。

首先介绍地板上方边界、地板前沿和探索方向的概念，这些概念是自主探索算法的基础。需要说明的是，本实施例主要以室内场景为例进行说明，对于室内场景，可行走区域就是地板区域，因此，所涉及的具体的概念以“地板”为例进行说明。而对于室外的场景，可行走区域通常包括道路、平地等。对于室外场景，本发明相关的定义仍然适用。例如：

“候选的地板前沿”可对应替换为“候选的可行走区域前沿”；

“地板前沿”可对应替换为“可行走区域前沿”。

假设机器人获取的图像的宽度为w个像素，高度为h个像素；图像横坐标的取值从1到w，向右为正；纵坐标的取值从1到h，向上为正；像素的索引(x,y)中，x是横坐标，y是纵坐标。参见图1所示地板前沿与探索方向的示意图，图中所示的长方形区域分为地板区域和非地板区域，通过本发明地板检测算法得到的地板区域蒙板(mask)m定义为：

首先，参见图1，给出如下3个定义：

定义1：候选的地板前沿

候选的地板前沿a定义为：

a＝{(x，u[x])|1≤x≤w}

给定地板区域的蒙板m，u[x]是同时满足以下条件的值：(1)对于任何1≤y≤u[x]，m(x,y)均为1；(2)对于y＝u[x]+1，m(x,y)为0。此外有两种特殊情况：若m(x,1)＝0，则定义u[x]＝0；若第x列的所有元素对应的m取值均为1，则定义u[x]＝h。

定义2：地板前沿

地板前沿是候选的地板前沿的子集，可以记做

b＝{(x，u[x])|l≤x≤r}

任一地板前沿需要满足以下条件：

(1)前沿的宽度(r-l)大于设定的阈值，即r-l≥threshold1；

(2)集合b中的任何u[x]≥threshold2；

(3)l位于图像的最左方或者其左邻居小于threshold2，即l＝1或者u[x-1]<threshold2；

(4)r位于图像的最右方或者其右邻居小于threshold2，即r＝w或者u[x+1]<threshold2。

其中，threshold1的取值反映了构成地板前沿所需的最小宽度；threshold2的取值反映了构成地板前沿所需的最小长度。例如：参见图1，本实施例划分了两个地板前沿：b1＝{(x，u[x])|l1≤x≤r1}和b2＝{(x，u[x])|l2≤x≤r2}。

定义3：探索方向

给定一个地板前沿b，探索方向原则上是该前沿的中心方向。但是，如果前沿b的宽度(r-l)太大，也可以将其进一步划分为n个前沿，每个前沿的中心对应着一个探索方向。具体来说，我们可以将前沿b分为n份，每个前沿的宽度不超过threshold1，即代表不超过u的最大整数。

接下来结合附图介绍本发明系统的主要模块。如前所述，本实施例针对的是室内场景，使用的视觉传感器是单目摄像头，辅助定位传感器包括码盘和惯性测量单元。

一、自主探索模块

自主探索模块主要用于进行自主探索。本发明进行自主探索的流程示意图如图2所示，包含以下步骤：

步骤201：机器人通过视觉传感器采集图像，基于采集的图像检测可行走区域，并按照如前所述的定义计算出地板前沿和探索方向。

本实施例采用基于深度神经网络(deepneuralnetwork)的方法对可行走区域(如前所述，对于室内场景，可行走区域即为地板区域)进行检测。深度神经网络是离线训练的，主要包括两个步骤：首先，人工采集大量的室内图片，并标记图像中的地板区域，然后，训练深度神经网络模型，用来确定每一个像素是否属于地板区域。深度神经网络训练完成后，可以用来检测图像中的地板区域。

检测出地板区域后，依照如前所述的定义计算出地板前沿和探索方向。地板区域检测的典型的结果如图3所示。其中，图3左侧为采集的室内图片，图3右侧为地板检测的结果。

步骤202：判断当前图像是否有未探索的前沿，若当前图像有未探索的前沿，则执行步骤203；否则，若当前图像没有未探索的前沿，则跳到步骤207。

步骤203，选择其中一个前沿的中心作为当前探索方向，其余的前沿保存到待探索的前沿集合中。

步骤204：通过运动控制，驱动机器人沿着探索方向行进。较佳的，可以行进20厘米。

步骤205：机器人采集图像，使用采集的图像进行地图创建，具体请参见地图创建模块中的详细说明。

步骤206：判断对当前探索方向的探索是否结束，若对当前探索方向的探索已结束，返回步骤201，否则返回204。

步骤207：判断待探索前沿的集合中是否还有元素，若有，则执行步骤208，否则，探索结束。

步骤208：从待探索方向集合中选择一个待探索前沿作为当前的探索前沿，并将该探索前沿从待探索前沿集合中删除。

步骤209：利用已经创建的地图，调用导航功能驱动机器人行驶至该探索方向，返回步骤201。

二、地图创建模块

地图创建模块包括拓扑地图创建子模块和度量地图创建子模块，分别用于创建拓扑地图和度量地图。在自主探索的过程中，系统会调用地图创建模块来更新地图。本发明创建的地图是拓扑地图与度量地图的结合。

对于拓扑地图，本发明将不同的地点作为拓扑地图的节点，根据地点之间的连通性构造拓扑地图的边。节点包括该地点的原始图像、图像上提取的视觉特征等。若机器人在探索过程中相继经过某两个节点，说明这两个节点之间是连通的，则在拓扑地图中这两个节点之间增加一条边。

对于度量地图，本发明保存的是环境中特征点和线段的三维空间坐标，如图4所示。

本发明完整的地图包括一个全局的拓扑地图和若干个局部的度量地图。拓扑地图和度量地图之间是可以双向索引的。具体地，拓扑地图的每个节点有一个对应的变量，该变量指示该节点所位于的度量地图。反过来，每个局部的度量地图也保存有一些额外信息，用于指示该度量地图对应拓扑地图的哪些节点、以及这些节点在度量地图中的位姿。

图5为本实施例拓扑地图创建子模块创建拓扑地图的流程示意图，包含以下步骤：

步骤501：计算当前图像与拓扑地图中上一个节点所对应的图像的重叠度。

步骤502：若重叠度小于设定门限，则执行步骤503，否则，执行步骤504。

步骤503：将当前图像作为一个节点加入到拓扑地图中，并在当前图像对应的节点和上一个节点之间增加一条边。

步骤504：如果重叠度较大，则说明当前图像不需要加入到拓扑地图中，结束拓扑地图更新流程。

优选地，拓扑地图还可以保存相对位姿。相对位姿包括旋转分量和平移分量。其中，旋转分量可以通过惯性测量单元(imu)中的陀螺仪计算得到。平移分量可以通过码盘、光流传感器等辅助定位装置得到。本实施例用imu和码盘估算机器人在拓扑地图中不同节点之间的相对位姿，并保存在拓扑地图中。

图6为本实施例度量地图创建子模块创建度量地图的流程示意图，包含以下步骤：

步骤601：提取当前图像的视觉特征，例如点特征、线段特征等。

步骤602：调用当前的局部度量地图对当前图像的视觉特征进行跟踪。

步骤603：判断是否跟踪成功，若跟踪成功，则执行步骤604，否则，执行步骤606。这里，可以通过比较当前局部度量地图的视觉特征和当前图像的视觉特征的方式来判断是否跟踪成功，如果当前图像的视觉特征在局部地图中存在足够多的匹配特征，则判定跟踪成功。

步骤604：按照视觉特征的重投影误差最小化的原则，优化度量地图中特征的位姿和机器人的位姿，把当前图像新观测到的视觉特征加到度量地图中。

步骤605：若当前图像被拓扑地图当作节点，则建立一个双向的索引信息。具体地，在度量地图中记录当前图像对应的拓扑地图节点编号；在拓扑地图中记录该图像对应的度量地图编号、以及在该度量地图中的位姿。

步骤606：若连续多帧跟踪失败，则执行步骤607，创建一个新的度量地图。

三、定位模块

本实施例进行定位的流程如图7所示，包括粗略定位和精细定位两个部分，具体包含以下步骤：

步骤701：通过全局的拓扑地图进行粗略定位。

首先进行地点识别，也就是将当前图像与拓扑地图中的所有节点(关键帧)进行比较，寻找最相似的节点。本实施例使用深度神经网络提取图像的特征向量，用特征向量之间的余弦距离来寻找最相似的节点。

步骤702：计算当前图像与最相似节点的相对位姿。具体做法是提取视觉特征点，建立特征点的对应，求解本质矩阵并计算出相对位姿。

步骤703：粗略定位完成之后，使用局部度量地图进行精确定位。在粗略定位阶段已经找到了当前图像对应的最相似节点，系统调出该节点对应的度量地图。

步骤704：在粗略定位阶段已经计算了当前图像与最相似节点的相对位姿，而地图创建阶段保存了该节点在度量地图中的位姿，结合二者可以推算出当前图像在度量地图中的位姿，以该位姿作为初值，按照重投影误差最小化的原则调整位姿。

一旦图像在度量地图中定位成功，接下来获取到新的图像时，可以直接使用精细定位单元进行定位。如果机器人走出了当前度量地图覆盖的范围，精细定位单元无法进行定位，此时需要再次调用粗略定位单元进行粗略定位。

四、路径规划模块

本发明路径规划方法包含以下步骤：

首先，在拓扑地图上寻找一个连通路径作为导航的大致路径。其中，路径的起点是当前图像对应的节点；路径的终点是目标位置对应的节点。作为一个实施例，使用迪杰斯特拉(dijkstra)最短路径算法寻找起点和终点的最短路径作为规划的路径。

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

五、导航功能

当系统创建了部分或全部环境的地图之后，系统可以开启导航功能。导航功能将综合运用定位模块和路径规划模块。需要说明的是：在地图生成阶段，系统也会使用导航功能将机器人导航至未探索的方向。本实施例导航方法的流程图如图8所示，包含以下步骤：

步骤801：获取当前图像。

步骤802：采用粗略定位的方法，将当前图像和拓扑地图中保存的图像进行相似度比较，确定当前图像所对应的节点。

步骤803：若当前图像所对应的节点有关联的度量地图，则调用当前节点对应的度量地图进行精细定位，进一步修正机器人的位姿，并执行步骤804；否则，如果当前图像所对应的节点没有关联的度量地图，则直接执行步骤804。

步骤804：调用路径规划模块进行路径规划。当机器人将要离开度量地图覆盖的区域时，路径规划模块计算当前节点与目标节点之间的最短路径。

步骤805：通过运动控制调整机器人的位姿或者行驶至下一个节点。

步骤806：判断是否到达目标位置，若没有到达，则返回801，否则导航结束。

以上所述仅为本申请的较佳实施例而已，并不用以限制本申请，凡在本申请的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本申请保护的范围之内。