基于视觉的拓扑地图生成方法与流程

本发明属于计算机视觉技术领域，特别是一种基于视觉的拓扑地图生成方法。

背景技术：

无人系统如自动驾驶无人车、无人机等，是利用系统内置的传感器，对系统的行驶状态进行获取和描述，并利用所获得的信息完成自主导航任务的系统。一般无人系统所用到的传感器包括激光、雷达、摄像机、imu等。由于激光、雷达等价格昂贵，摄像机的价格相对较为低廉，基于计算机视觉技术的导航技术得到广泛研究与应用。

即时定位与地图构建(simultaneouslocalizationandmapping，salm)技术，是指搭载有特定传感器的系统，通过运动估计自身的状态，在运动过程中建立包括周围环境信息的地图。当我们主要采用摄像机作为输入传感器时，该系统称为视觉slam系统。例如orb-slam系统，是一套基于单目、双目以及rgb-d的完整方案，可以实现地图重用、回环检测以及重新定位的功能。但是当前的视觉slam方法存在计算量大、内存占用多、对光照等外部条件敏感等问题，使slam系统的整体性能下降。尤其是当系统搭载在移动智能体上使用时，传统的视觉slam技术建立地图时需要存储大量的信息，如大量特征点等，随着工作时间的增长，系统的运算量会越来越大，负载越来越大。现有视觉slam方法不满足移动端计算能力有限、内存占用少的要求。

为了完成无人系统的导航任务，我们首先需要建立出可导航的地图，目前，对环境空间的表示方法大体上可以分为度量地图和拓扑地图两种。度量地图又分为稀疏和稠密两种，但是两种都需要耗费大量的存储空间。相对度量地图，拓扑地图更强调地图元素之间的关系。拓扑地图由边和节点组成，忽略大量详细、繁琐的信息，只考虑节点的连通性，表达更为紧凑、简洁，并且也能够很好的完成节点与节点间的导航。在大环境范围的导航问题中，系统对位置精度的要求比较低，这种情况下，相比度量地图，拓扑地图拥有省计算、省空间的优点。但是在现有方法中，生成拓扑地图缺少有效的逻辑，生成的节点对空间的表示效率也比较低。

在背景技术部分中公开的上述信息仅仅用于增强对本发明背景的理解，因此可能包含不构成在本国中本领域普通技术人员公知的现有技术的信息。

技术实现要素：

针对现有技术中存在的计算量大、占用存储空间多等问题，本发明提出一种基于视觉的拓扑地图生成方法，利用视觉信息建立出精简高效的可导航地图，使拓扑地图的表示效率高、系统运行速度快、占用内存空间小。

本发明的目的是通过以下技术方案予以实现，一种基于视觉的拓扑地图生成方法包括以下步骤：

一种基于视觉的拓扑地图生成方法，所述方法包括以下步骤：

第一步骤中，获取图像信息，基于所述图像信息以预定频率抽取图像帧；

第二步骤中，基于所述图像帧图像预处理得到关键帧，判断当前是否产生回环，若产生回环，矫正当前已有拓扑地图，如果关键帧是本次建图运行过程中的第一帧图像，则在其所在位置设置拓扑节点；

第三步骤中，比较所述图像信息与已有拓扑地图中最新加入的拓扑节点的图像，判断两幅图像的描述符相似度是否低于第一阈值，如果低于第一阈值，则将所述关键帧的图像所在位置作为新的拓扑节点加入拓扑地图，存储当前帧，进入第六步骤，否则，进入第四步骤；

第四步骤中，基于所述关键帧提取水平旋转角度，根据最近若干帧的水平旋转角度，如若干帧连续出现与上一拓扑节点朝向相比，旋转角度大于第二阈值，则判定当前状态为转弯，将当前关键帧所在位置加入拓扑地图，进入第六步骤，否则，进入第五步骤；

第五步骤中，当超过预定最大间隔距离没有新增拓扑节点时，将当前所在位置加入拓扑地图，并保留相应信息，进入第六步骤，否则，进入下一帧图像的处理；

第六步骤中，更新当前最新拓扑节点信息，进入下一帧图像的处理。

所述的方法中，第二步骤中，所述图像帧输入即时定位与地图构建系统slam预处理得到关键帧，即时定位与地图构建系统slam检测当前是否产生回环，将图像帧输入卷积神经网络生成当前图像的描述符。

所述的方法中，描述符归一化公式为：

其中为fi当前输入的描述符，μs和σs为累计均值与方差，fi′为最终被存储的描述符。

所述的方法中，第三步骤中，两幅图像的描述符相似度为计算两图像描述符之间的余弦距离，余弦距离越小则相似度越大，描述符相似度公式为：其中fi和fj分别代表图像i和图像j的描述符，上式的取值范围为[-1，1]。

所述的方法中，第三步骤中，第一阈值为0，当前帧图像与最新节点的描述符相似度小于0时，则认为当前帧为新节点，将当前帧的相关信息存入新的拓扑节点。

所述的方法中，即时定位与地图构建系统slam预处理时预测位姿为：pose：{{posiztion：x，y，z}，{orientation：x，y，z，w}}，其中，pose.orientation.w代表智能体当前朝向与上一节点位置朝向的角度差值，当这一差值大于第二阈值时，则认为当前系统状态为正在转弯，设置转弯标志位为1，设当前帧为t时刻，设置三个标志位分别代表t-2时刻、t-1时刻、t时刻的转弯状态，没有进行转弯操作时取0，否则取1，当连续出现三个转弯标志位都为1时，则将当前t时刻系统所在位置作为新的拓扑节点加入拓扑地图中，并存入当前帧。

所述的方法中，第六步骤中，更新当前最新拓扑节点信息时，在地图中对应坐标加入一个拓扑节点noden，当n＞1时，与上一个最新的拓扑节点noden-1进行连接，方向指向noden。

所述的方法中，第一步骤中，从摄像机传感器获取图像信息，获得的图像帧进行校正处理。

所述的方法中，第四步骤中，第二阈值的设定为20～30°。

所述的方法中，第五步骤中，预定最大间隔距离大小使得拓扑点间的导航漂移不超过导航点间路径长度的5％。

有益效果

本发明实现了基于视觉信息，利用神经网络获取图像描述符来建立拓扑地图。描述符之间的相似度能够很好反应图像之间的相似度，由此为拓扑地图提供了一条添加节点的准则。利用神经网络获取图像描述符的速度比较快，得到的描述符所占用的内存小，准确率又比较高，对于移动端智能体而言效果更好。基于本方法提取的描述符不仅对图像具有较好的表示，并且占用内存少。对于相似的输入图像，神经网络得到的图像描述符，应当具有较高的相似度，或者说两幅图像的描述符向量之间具有较短的距离。本发明实现了拓扑地图的回环检测。本发明结合slam算法，在建立拓扑地图的过程中同时进行slam建图，由slam帮助拓扑地图进行漂移较正，实现拓扑地图的回环检测，建立更精确的拓扑地图。在导航时，无需再同时运行slam，也能实现拓扑地图的导航功能，大大减少内存的消耗。

附图说明

通过阅读下文优选的具体实施方式中的详细描述，本发明各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。说明书附图仅用于示出优选实施方式的目的，而并不认为是对本发明的限制。显而易见地，下面描述的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。而且在整个附图中，用相同的附图标记表示相同的部件。

在附图中：

图1是本发明方法流程图；

图2是以orb-slam为例说明典型slam算法的流程图；

图3是拓扑节点判定流程图；

图4是实验结果示意图。

以下结合附图和实施例对本发明作进一步的解释。

具体实施方式

下面将参照附图1至图4更详细地描述本发明的具体实施例。虽然附图中显示了本发明的具体实施例，然而应当理解，可以以各种形式实现本发明而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本发明，并且能够将本发明的范围完整的传达给本领域的技术人员。

需要说明的是，在说明书及权利要求当中使用了某些词汇来指称特定组件。本领域技术人员应可以理解，技术人员可能会用不同名词来称呼同一个组件。本说明书及权利要求并不以名词的差异来作为区分组件的方式，而是以组件在功能上的差异来作为区分的准则。如在通篇说明书及权利要求当中所提及的“包含”或“包括”为一开放式用语，故应解释成“包含但不限定于”。说明书后续描述为实施本发明的较佳实施方式，然所述描述乃以说明书的一般原则为目的，并非用以限定本发明的范围。本发明的保护范围当视所附权利要求所界定者为准。

为便于对本发明实施例的理解，下面将结合附图以具体实施例为例做进一步的解释说明，且各个附图并不构成对本发明实施例的限定。

为了更好地理解，图1为一个基于视觉的拓扑地图生成方法工作流程图，如图1所示，一种基于视觉的拓扑地图生成方法包括以下步骤：

第一步骤s1中，获取图像信息，基于所述图像信息以预定频率抽取图像帧；

第二步骤s2中，基于所述图像帧图像预处理得到关键帧，判断当前是否产生回环，若产生回环，矫正当前已有拓扑地图，如果关键帧是本次建图运行过程中的第一帧图像，则在其所在位置设置拓扑节点。其中，关键帧的选取依据当前帧与上一关键帧之间是否有足够的视差，回环检测基于图像描述符相似性与特征点匹配综合判定；

第三步骤s3中，比较所述图像信息与已有拓扑地图中最新加入的拓扑节点的图像，判断两幅图像的描述符相似度是否低于第一阈值，如果低于第一阈值，则将所述关键帧的图像所在位置作为新的拓扑节点加入拓扑地图，存储当前帧，进入第六步骤，否则，进入第四步骤；s2中的关键帧，也是当前正在处理的关键帧，如果当前处理的关键帧图像与上一次建立的节点的图像比较，相似度较低，则我们认为场景有了较大改变，需要设立一个新的节点。

第四步骤s4中，基于所述关键帧提取水平旋转角度，根据最近若干帧的水平旋转角度，如若干帧连续出现与上一拓扑节点朝向相比，旋转角度大于第二阈值，则判定当前状态为转弯，将当前关键帧所在位置加入拓扑地图，进入第六步骤，否则，进入第五步骤，其中，第二阈值是基于机器人角速度和方向稳定性确定，进一步，若干帧为三帧。

第五步骤s5中，当超过预定最大间隔距离没有新增拓扑节点时，将当前所在位置加入拓扑地图，并保留相应信息，进入第六步骤，否则，进入下一帧图像的处理；

第六步骤s6中，更新当前最新拓扑节点信息，进入下一帧图像的处理。

所述的方法的优选实施方式中，第二步骤s2中，所述图像帧输入即时定位与地图构建系统slam预处理得到关键帧，即时定位与地图构建系统slam检测当前是否产生回环，将图像帧输入卷积神经网络生成当前图像的描述符。

所述的方法的优选实施方式中，描述符归一化公式为：

其中为fi当前输入的描述符，μs和σs为累计均值与方差，fi′为最终被存储的描述符。

所述的方法的优选实施方式中，第三步骤s3中，两幅图像的描述符相似度为计算两图像描述符之间的余弦距离，余弦距离越小则相似度越大，描述符相似度公式为：其中fi和fj分别代表图像i和图像j的描述符，上式的取值范围为[-1，1]。

所述的方法的优选实施方式中，第三步骤s3中，第一阈值为0，当前帧图像与最新节点的描述符相似度小于0时，则认为当前帧为新节点，将当前帧的相关信息存入新的拓扑节点。

所述的方法的优选实施方式中，即时定位与地图构建系统slam预处理时预测位姿为：pose：{{posiztion：x，y，z}，{orientation：x，y，z，w}}，其中，pose.orientation.w代表智能体当前朝向与上一节点位置朝向的角度差值，当这一差值大于第二阈值时，则认为当前系统状态为正在转弯，设置转弯标志位为1，设当前帧为t时刻，设置三个标志位分别代表t-2时刻、t-1时刻、t时刻的转弯状态，没有进行转弯操作时取0，否则取1，当连续出现三个转弯标志位都为1时，则将当前t时刻系统所在位置作为新的拓扑节点加入拓扑地图中，并存入当前帧。

所述的方法的优选实施方式中，第六步骤s6中，更新当前最新拓扑节点信息时，在地图中对应坐标加入一个拓扑节点noden，当n＞1时，与上一个最新的拓扑节点noden-1进行连接，方向指向noden。

所述的方法的优选实施方式中，其中，第一步骤s1中，从摄像机传感器获取图像信息，获得的图像帧进行校正处理。

所述的方法的优选实施方式中，第四步骤s4中，第二阈值的设定为20～30°。

所述的方法的优选实施方式中，第五步骤s5中，预定最大间隔距离大小使得拓扑点间的导航漂移不超过导航点间路径长度的5％。

为了进一步理解本发明，在一个实施例中，包括以下步骤：

步骤1，从摄像机传感器获取图像信息，以预定频率抽取图像帧经过校正后送入系统；

步骤2，将校正后的图像输入系统，进行图像的预处理，判断当前是否产生回环，若产生回环，矫正当前已有拓扑地图。并且，如果是本次建图运行过程中的第一帧图像，则自动在其所在位置设置一个拓扑节点；

步骤3，根据上一步提取到的图像信息，与已有拓扑地图中最新加入的一个节点的图像进行比较，判断两幅图像描述符的相似度是否低于阈值。如果低于阈值，则将所述关键帧的图像所在位置作为新的拓扑节点加入拓扑地图，存储当前帧的相关信息，进入步骤6。否则，进入步骤4；

步骤4，结合步骤2中获得的关键帧信息，从中提取出代表水平旋转角度的数据，根据最近若干帧关键帧的水平旋转角度信息，若近若干帧连续出现与上一拓扑节点朝向相比，旋转角度大于阈值的情况，则判定当前状态为转弯，将当前关键帧所在位置加入拓扑地图，进入步骤6。否则，进入步骤5。其中阈值的设定为20～30°，避免渐变弯道和orb算法误差导致建立过多无效拓扑点；

步骤5，最后我们设定一个最大间隔距离，当超过这个距离没有新增拓扑节点时，将当前所在位置加入拓扑地图，并保留相应信息，进入步骤6。否则，进入下一帧图像的处理。其中，最大间隔距离的判断主要决定于惯性传感器的精度，要求拓扑点间的导航漂移不超过导航点间路径长度的5％。

步骤6，更新当前最新拓扑节点信息。进入下一帧图像的处理。

进一步的，步骤2的实现方法：

步骤2.1，将图像输入slam系统，以orb-slam为例的slam系统流程图如图2所示。orb-slam会对当前帧图像进行处理，提取特征点、关键帧，构建地图。

同时slam会进行回环检测。当slam系统检测到回环时，将会对本地地图进行优化，此时需要对拓扑地图进行相应的优化更新。系统将会检索所建立的拓扑地图中的所有拓扑节点，根据对应的优化后的slam地图，对所有拓扑节点进行更新，校正漂移误差。

步骤2.2，将图像输入给特定的卷积神经网络完成当前图像的描述符生成，我们以基于places365数据集训练得到的alexnet为例，取该网络的第三个卷积层的特征图，经过高斯随机映射进行降维后，可得到长度为4096的向量，作为当前图像的描述符。

在求解描述符的过程中，需要对描述符做归一化，公式为：

其中为fi当前输入的描述符，μs和σs为累计均值与方差，fi′为最终被存储的描述符。

进一步的，步骤3的实现方法：将步骤2中得到的当前图像描述符，与最新的拓扑节点的描述符求相似度。相似度的计算方式为计算两图像描述符之间的余弦距离，余弦距离越小则相似度越大，故用1减去余弦距离作为相似度，公式为：

其中fi和fj分别代表图像i和图像j的描述符。上式的取值范围为[-1，1]，根据实验结果，取0作为阈值。即当前帧图像与最新节点的相似度小于0时，则认为当前帧为新节点，将当前帧的相关信息存入新的拓扑节点。

进一步的，步骤4的实现方法：

在步骤2中，orb-slam会提供预测位姿，预测位姿的形式为：

pose：{{posiztion：x，y，z}，{orientation：x，y，z，w}}

其中，pose.orientation.w代表智能体当前朝向与上一节点位置朝向的角度差值。当这一差值大于一定阈值时，则认为当前系统状态为正在转弯，设置转弯标志位为1。设当前帧为t时刻，设置三个标志位分别代表t-2时刻、t-1时刻、t时刻的转弯状态，没有进行转弯操作时取0，否则取1。当连续出现三个转弯标志位都为1时，则将当前t时刻系统所在位置作为新的拓扑节点加入拓扑地图中，并存入当前帧的相关信息。

进一步的，步骤6的实现方法：当系统确认当前需要加入一个新的拓扑节点时，需要完成的内容包括：在地图中对应坐标加入一个拓扑节点noden，当n＞1时，与上一个最新的拓扑节点noden-1进行连接，方向指向noden。这一部分需要用到数据库，如mongodb，建立拓扑地图。

具体加入节点的方法是，在数据库中加入当前拓扑节点的相关信息。一个拓扑节点的信息可以表示为：

{

node，

enablenode：{node1，node2...noden}，

pose，

keypoints：{kp1，kp2...kpn}，

descriptors

}

node——表示拓扑节点的名称；

enablenode——表示当前拓扑节点能够连通的其他节点名称；

pose——表示拓扑节点的位姿信息，包括世界系坐标和方向；

keypoints——表示当前图像中所有特征点的特征信息；

descriptors——表示当前图像的视觉描述符。

此外，还需要给定节点位姿的偏移容忍度，即位姿在多少差值范围内可认为回到该节点。

这样得到的拓扑地图所占用的存储空间相对较少，能够满足移动智能体的轻量级需求，同时又能够表示足够的信息，可用于导航。实验效果如附图4所示。

尽管以上结合附图对本发明的实施方案进行了描述，但本发明并不局限于上述的具体实施方案和应用领域，上述的具体实施方案仅仅是示意性的、指导性的，而不是限制性的。本领域的普通技术人员在本说明书的启示下和在不脱离本发明权利要求所保护的范围的情况下，还可以做出很多种的形式，这些均属于本发明保护之列。