CN109727269B - 一种基于单目视觉和道路地图的匹配定位方法 - Google Patents

Abstract

本发明公开一种基于单目视觉和道路地图的匹配定位方法及系统，该方法包括：根据道路地图构建拓扑图，对所述道路地图上的道路结构进行描述；通过节点表示道路地图上的道路，通过节点之间的连接关系表示道路之间的连接关系；获取载体的运动信息；通过粒子滤波器对所述载体的运动信息和所述拓扑图中的道路结构的一致性进行匹配输出载体的当前位置。该方案解决了浪费存储资源、定位时间长的问题，实现了降低了存储量，缩短了定位时间，提高了载体匹配定位的鲁棒性和快速性。

Description

一种基于单目视觉和道路地图的匹配定位方法

技术领域

本发明涉及导航定位技术领域，尤其是一种基于单目视觉和道路地图的匹配定位方法及系统。

背景技术

定位系统是无人载体顺利完成任务的重要保障。目前，在无人载体中广泛使用的定位方式是“惯性+卫星（无线电）”组合导航。卫星导航和无线电导航信号极易被干扰，过分依赖于卫星或无线电导航对无人载体的导航系统存在巨大的潜在风险。因此，利用自身携带的传感器，实现自主定位，对无人载体具有重要的意义和广泛的应用前景。

视觉信息可以作为无人载体实现自主定位的重要信息源。一方面载体可以通过图像流，进行航迹递推，估计自身位置；另一方面载体可将实时拍摄的图像与图像库中的图像进行匹配，判断是否处于图像库中某一已知的地标位置。然而，视觉信息受环境光照、季节等因素的影响，基于视觉的定位系统鲁棒性有待进一步提升；此外，利用图像进行匹配定位需要存储整个导航环境中的视觉场景，图像库所需的存储空间随着导航环境的增大呈线性增长，存储开销也将随之增大。

道路地图也可以为无人载体提供匹配定位的参考信息。道路地图由矢量线段组成，所需的存储量小。相较于视觉场景，道路地图也更加稳定，只有当道路发生变化时其几何结构才会有变化。基于道路结构的匹配定位方法通常是将里程计测量得到的轨迹与道路结构进行对比，估计载体位置。为了区分相似的道路结构，载体需要行使足够长的距离才能确定自身位置，所需的定位时间长。

发明内容

本发明提供一种基于单目视觉和道路地图的匹配定位方法及系统，用于克服现有技术中地面无人载体存储量大，道路地图匹配定位所需时间长等缺陷，根据道路地图匹配提升载体匹配定位的鲁棒性和快速性，同时降低定位系统所需的参考图像。

为实现上述目的，本发明提出一种基于单目视觉和道路地图的匹配定位方法，包括以下步骤：

步骤1，根据道路地图构建拓扑图，对所述道路地图上的道路结构进行描述；通过节点表示道路地图上的道路，通过节点之间的连接关系表示道路之间的连接关系；

步骤2，获取载体的运动信息；

步骤3，通过粒子滤波器对所述载体的运动信息和所述拓扑图中的道路结构的一致性进行匹配，输出载体的当前位置。

为实现上述目的，本发明还提供一种基于单目视觉和道路地图的匹配定位系统，包括处理器和存储器，所述存储器存储有基于单目视觉和道路地图的匹配定位程序，所述处理器在运行基于单目视觉和道路地图的匹配定位程序时，执行上述方法的步骤。

本发明提供的基于单目视觉和道路地图的匹配定位方法及系统，通过拓扑图对已知的道路地图的道路结构进行描述，在载体移动过程中获取载体的运动信息，通过粒子滤波器对所述载体的运动信息和所述拓扑图中的道路结构的一致性进行匹配，完成几何相似性匹配，符合设定的条件则认为粒子的当前位置就是载体的当前位置，作为定位结果输出；相对于现有技术，存储的道路地图和图像库（包括若干图片，不需要导航的全程场景视频），因此大大降低了存储量，此外，由于将道路结构描述在拓扑图中，通过节点表示道路，大大缩短了道路地图匹配定位所需的时间，最后，利用单目视觉图像与图像库中的导航全程中参考地标的图像进行图像匹配，提升了载体匹配定位的鲁棒性和快速性，并降低了定位系统所需的参考图像的要求，适应不同稀疏程度的参考图像库。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。

图1为发明实施例一提供的基于单目视觉和道路地图的匹配定位方法的流程图；

图2为图1中载体在道路上的状态示意图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明，本发明实施例中所有方向性指示（诸如上、下、左、右、前、后……）仅用于解释在某一特定姿态（如附图所示）下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。

另外，在本发明中如涉及“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

在本发明中，除非另有明确的规定和限定，术语“连接”、“固定”等应做广义理解，例如，“固定”可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接，还可以是物理连接或无线通信连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

另外，本发明各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。

本发明提出一种基于单目视觉和道路地图的匹配定位方法及系统。

实施例一

请参照图1、图2，本发明提供一种基于单目视觉和道路地图的匹配定位方法，包括以下步骤：

步骤1，根据道路地图构建拓扑图，对所述道路地图上的道路结构进行描述；通过节点表示道路地图上的道路，通过节点之间的连接关系表示道路之间的连接关系；

以单目相机为测量单元，以道路地图和参考图像库为先验信息，采用粒子滤波器，融合运动信息与道路结构的一致性和视觉观测与参考图像的相似性对载体位置进行估计，道路地图，用于描述导航环境中的可行驶路径，可以将载体位置约束在道路地图上，道路地图以拓扑图形式进行表达。参考图像库，用于描述导航环境中的视觉特征，具有位置标签，可为载体提供位置参考。粒子滤波器，用于信息融合，估计载体位置。

所述步骤1包括：

步骤11，对已知的道路地图进行预处理，提取可行使道路的信息；

步骤12，将每条道路的信息用一条矢量线段描述；矢量线段通过下面步骤中的节点表示；

步骤13，构建拓扑节点，每个节点表示每条道路，节点之间的边表示道路在道路地图上的连接关系，节点的具体表达方式如下：

其中表示道路节点编号；表示该道路节点的起始坐标；表示道路节点的长度；表示道路节点的朝向，表示道路方向与东向的夹角；表示该道路节点是否与多个节点相连，即该道路节点是否处于一个交叉口，每一个节点对应一个边向量，其中包含了所有与该节点相连的节点编号；

参见图2，通过构建拓扑节点，将载体在道路地图上的状态表示为：

其中表示载体所在的道路节点编号，表示距离节点起始点的距离；表示载体与道路节点的夹角。

步骤2，获取载体的运动信息和沿途单目视觉图像；

利用单目视觉传感器（或其它运动测量方法）获取载体的运动信息以及沿途的视觉特征。采用粒子滤波器对载体位置进行估计，该滤波器通过融合运动信息与道路结构的一致性以及视觉观测与参考图像的相似性对载体位置进行估计。提出的粒子滤波器还能通过筛选视觉观测，适应不同稀疏程度的参考图像库。

步骤3，通过粒子滤波器对所述载体的运动信息和所述拓扑图中的道路结构的一致性进行匹配，对所述单目视觉图像和预先存储的参考图像库中的图像的相似性进行匹配；融合上述匹配结果，输出载体的当前位置。

通过以上两个步骤实现对载体位置的估计。与现有技术相比，本发明具有以下优点：

1.结合了运动信息与道路结构的一致性以及视觉观测与参考地标的相似性进行定位。

2.可适应呈稀疏分布的参考地标库。

3.可以采用以下三种工作模式：

模式一，仅使用运动信息与道路结构的一致性实现定位。

模式二，仅使用视觉观测与参考图像的相似实现定位。

模式三，同时使用运动信息与道路结构的一致性和视觉观测与参考图像的相似实现定位。

所述步骤3中的参考图像库，用于描述导航环境中的视觉特征，具有位置标签，为载体提供位置参考；所述步骤3包括：

步骤31，粒子滤波器中的粒子的状态表示为：

其中表示载体在道路地图上的状态；是一个差异角，表示载体朝向与道路节点方向的差异角，这里的差异角表示载体朝向与道路节点朝向之间的夹角；是图像匹配相似性系数，表示观测图像与图像库之间的相似性；表示粒子权重；

步骤32，根据载体的运动信息和道路结构，按照预设条件对进行更新；

当里程计测量得到运动量后，将对粒子状态进行更新，其运动模型表示如下：

其中，表示平移运动量；表示角运动量，为高斯噪声，为移位高斯噪声，为航向高斯噪声，表示里程计测得的位移和航向变化量，。载体状态中的节点编号的更新还需要考虑到道路地图的结构，其更新方法如下：

其中，是一个平滑系数；表示与当前节点相连的节点道路朝向，当前节点相连的节点道路方向与东向的夹角，载体到达道路交叉口时，将转向朝向与载体自身朝向最为相似的道路节点。

所述步骤32包括：

步骤321, 当不在交叉口时，载体到道路节点起始点的距离大于载体所在道路节点的长度（），则将载体更新到与所在道路节点相连接的道路节点上；

步骤322，当在交叉口时，载体到道路节点起始点的距离大于载体所在道路节点长度+平滑距离（），则将载体更新到与所在道路节点相连接且角度差异最小的道路节点上；

步骤323，当载体不在交叉口且距离道路节点起始点的距离小于载体所在道路节点的长度（时），载体更新后的道路节点不变。

步骤33，根据更新后的状态，按照预设条件对进行更新；完成运动量与道路结构之间的一致性匹配；

根据更新后的粒子状态，可以得到粒子朝向与道路节点朝向之间的夹角。然而，在道路交叉口区域载体朝向可能会与道路朝向有较大的差别。因此，对差异角更新进行如下处理：

当粒子不处于是交叉口时，其差异角即为粒子与道路节点间的夹角；

所述步骤33包括：

步骤331，在交叉路口时，差异角保持不变，仍然为；当粒子处于交叉口时，在计算时对后者的平滑处理；

步骤332，不在交叉路口时，将差异角更新到载体与道路节点的夹角相同为。

步骤34，将载体的沿途单目视觉图像与图像库中的图像按照设定的参数进行相似度对比，获得图像匹配相似性系数，对进行更新；完成观测图像与图像库之间的相似性匹配；

所述步骤34包括：

步骤341，对载体的沿途单目视觉图像进行筛选，获得在参考地标附近的单目视觉图像，按照下面的公式筛选：

其中，为将当前拍摄的单目图像与粒子邻近区域内所有参考图像(参见步骤3中关于参考图像的说明)对比的图像匹配相似性系数，为粒子上一更新时刻的图像匹配相似性系数。

步骤342，按照图像的像素点灰度的相似度度量方法将载体在参考地标附近的单目视觉图像与图像库中的参考地标图进行对比，按照下面的公式获得图像匹配相似性系数，按照下面的公式获得图像匹配相似性系数：

其中和表示压缩后的图像（包括单目视觉图像和图像库中的图像）大小；表示实时图像中的像素点灰度；表示库图像的相素点灰度。

本发明可以对视觉观测进行筛选，建立有效匹配对，从而使算法能够适应不同密度的参考图分布。在筛选过程中，粒子首先仅将在参考地标附近拍摄的相机图与参考图进行对比。其次，粒子仅利用在参考地标附近拍摄的与参考图最相似的相机图作为有效观测。

步骤35，按照相似性模型对运动量与道路结构之间的一致性以及观测图像与图像库之间的相似性的权重系数进行分配，对进行更新；

当更新得到差异角及图像相似性后，可对粒子权重进行更新，粒子权重的更新方式表示为

其中表示测量得到的运动量与道路结构之间的一致性；表示视觉观测与图像库图像之间的相似性。计算方法如下：

其中和均为相似性模型中的定值参数。

所述步骤35之后还包括：

步骤351，根据粒子权重获得有效粒子系数；

步骤352，在有效粒子系数小于设定阈值时，对粒子滤波器进行重采样。

粒子重采样的判断规则为有效粒子系数，其计算公式如下：

当有效粒子系数小于设定阈值时，将对粒子滤波器进行重采样，重采样方法为简单随机重采样算法（simple random resampling algorithm）。

步骤36，在设定所述粒子临近范围内粒子的权重和大于设定阈值时，将所述粒子的位置为作为载体的位置，并输出定位结果。

粒子滤波器的收敛判断依据为邻近粒子的权重和，其计算公式如下：

其中加权函数定义如下：

g表示设定粒子，为所求粒子，j表示设定粒子g的临近范围内的粒子，即仅计算设定粒子的邻近范围内粒子j的权重和。当某一粒子处的权重和大于设定阈值时，则认为该粒子所处的位置即为载体位置。

克服现有技术中地面无人载体视觉匹配定位鲁棒性差、存储量大，道路地图匹配定位所需时间长等缺陷，利用单目视觉信息，融合道路地图匹配和视觉匹配，提升载体匹配定位的鲁棒性和快速性，同时降低定位系统所需的参考图像。

实施例二

基于实施例一，本发明还提供一种基于单目视觉和道路地图的匹配定位系统，包括处理器和存储器，所述存储器存储有基于单目视觉和道路地图的匹配定位程序，所述处理器在运行基于单目视觉和道路地图的匹配定位程序时，执行基于单目视觉和道路地图的匹配定位方法任意实施例的步骤。

本发明提供的基于单目视觉和道路地图的匹配定位方法及系统，通过拓扑图对已知的道路地图的道路结构进行描述，在载体移动过程中获取载体的运动信息和沿途采集的单目视觉图像，通过粒子滤波器对所述载体的运动信息和所述拓扑图中的道路结构的一致性进行匹配，完成几何相似性匹配，对所述单目视觉图像和预先存储的参考图像库中的图像的相似性进行匹配，完成图像相似性匹配，融合几何相似性匹配和图像相似性匹配的结果，符合设定的条件则认为粒子的当前位置就是载体的当前位置，作为定位结果输出；相对于现有技术，存储的道路地图和图像库（包括若干图片，不需要导航的全程场景视频），因此大大降低了存储量，此外，由于将道路结构描述在拓扑图中，通过节点表示道路，大大缩短了道路地图匹配定位所需的时间，最后，利用单目视觉图像与图像库中的导航全程中参考地标的图像进行图像匹配，提升了载体匹配定位的鲁棒性和快速性，并降低了定位系统所需的参考图像的要求，适应不同稀疏程度的参考图像库。

以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。

Claims (9)

1.一种基于单目视觉和道路地图的匹配定位方法，其特征在于，包括以下步骤：

步骤1，根据道路地图构建拓扑图，对所述道路地图上的道路结构进行描述；通过节点表示道路地图上的道路，通过节点之间的连接关系表示道路之间的连接关系；

步骤2，获取载体的运动信息；

步骤3，通过粒子滤波器对所述载体的运动信息和所述拓扑图中的道路结构的一致性进行匹配，输出载体的当前位置。

2.如权利要求1所述的基于单目视觉和道路地图的匹配定位方法，其特征在于，所述步骤1包括：

步骤11，对已知的道路地图进行预处理，提取可行使道路的信息；

步骤12，将每条道路的信息用一条矢量线段描述；

步骤13，构建拓扑节点，每个节点表示每条道路，节点之间的边表示道路在道路地图上的连接关系，节点的具体表达方式如下：

其中表示道路节点编号；表示该道路节点的起始坐标；表示道路节点的长度；表示道路节点的朝向，即道路节点方向与东向的夹角；表示该道路节点是否与多个节点相连，即该道路节点是否处于一个交叉口，每一个节点对应一个边向量，边向量包含了所有与该节点相连的节点编号；矢量线段通过节点来表示；

通过构建拓扑节点，将载体在道路地图上的状态表示为：

其中表示载体所在的道路节点编号，表示距离节点起始点的距离；表示载体与道路节点的夹角。

3.如权利要求2所述的基于单目视觉和道路地图的匹配定位方法，其特征在于，所述步骤2中还包括：

在获取载体的运动信息的同时获取沿途单目视觉图像；

所述步骤3包括：

对所述单目视觉图像和预先存储的参考图像库中的图像的相似性进行匹配；融合上述道路结构的一致性匹配结果和图像相似性匹配结果，根据融合后的结果输出载体的当前位置。

4.如权利要求3所述的基于单目视觉和道路地图的匹配定位方法，其特征在于，所述步骤3中的参考图像库，用于描述导航环境中的视觉特征，具有位置标签，为载体提供位置参考；所述步骤3包括：

步骤31，粒子滤波器中的粒子的状态表示为：

其中表示载体在道路地图上的状态；表示载体朝向与道路节点朝向的差异角；是图像匹配相似性系数，表示观测图像与图像库之间的相似性；表示粒子权重；

步骤32，根据载体的运动信息和道路结构，按照预设条件对进行更新；

步骤33，根据更新后的状态，按照预设条件对进行更新；完成运动量与道路结构之间的一致性匹配；

步骤34，将载体的沿途单目视觉图像与图像库中的图像按照设定的参数进行相似度对比，获得图像匹配相似性系数，对进行更新；完成观测图像与图像库之间的相似性匹配；

步骤35，按照相似性模型对运动量与道路结构之间的一致性以及观测图像与图像库之间的相似性的权重系数进行分配，对进行更新；

步骤36，在设定所述粒子临近范围内粒子的权重和大于设定阈值时，将所述粒子的位置为作为载体的位置，并输出定位结果。

5.如权利要求4所述的基于单目视觉和道路地图的匹配定位方法，其特征在于，所述步骤32包括：

步骤321, 当不在交叉口时，载体到道路节点起始点的距离大于载体所在道路节点的长度，则将载体更新到与起始点所在道路节点相连接的道路节点上；

步骤322，当在交叉口时，载体到道路节点起始点的距离大于载体所在道路节点长度+平滑距离，则将载体更新到与起始点所在道路节点相连接的角度差异最小的道路节点上；

步骤323，当载体到道路节点起始点的距离小于载体所在道路节点的长度时，载体更新后的道路节点不变。

6.如权利要求4所述的基于单目视觉和道路地图的匹配定位方法，其特征在于，所述步骤33包括：

步骤331，在交叉路口时，差异角保持不变；

步骤332，不在交叉路口时，将差异角更新到载体与道路节点的夹角相同。

7.如权利要求4所述的基于单目视觉和道路地图的匹配定位方法，其特征在于，所述步骤34包括：

步骤341，对载体的沿途单目视觉图像进行筛选，获得在参考地标附近的单目视觉图像；

步骤342，按照图像的像素点灰度的相似度度量方法将载体在参考地标附近的单目视觉图像与图像库中的参考地标图进行对比，获得图像匹配相似性系数。

8.如权利要求4所述的基于单目视觉和道路地图的匹配定位方法，其特征在于，所述步骤35之后还包括：

步骤351，根据粒子权重获得有效粒子系数；

步骤352，在有效粒子系数小于设定阈值时，对粒子滤波器进行重采样。

9.一种基于单目视觉和道路地图的匹配定位系统，包括处理器和存储器，所述存储器存储有基于单目视觉和道路地图的匹配定位程序，所述处理器在运行基于单目视觉和道路地图的匹配定位程序时，执行所述权利要求1~7任一项所述方法的步骤。