CN113865581B - 基于多层级地图的封闭场景定位方法 - Google Patents

Abstract

本发明提供了一种基于多层级地图的封闭场景定位方法，首先获取封闭场景所有节点的图像信息和轨迹信息，再制作多层级地图，包括BOW特征层、场景结构层和轨迹信息，获取待定位点的车载相机拍摄图像，匹配最近的地图节点，计算待定位点距离最近地图节点的相对坐标，结合节点轨迹信息完成高精度定位；本发明将BOW与视觉局部特征相结合，分别利用地图的视觉局部特征实现节点级粗匹配，再利用场景结构实现度量级精细定位，达到高精度定位的效果。

Description

基于多层级地图的封闭场景定位方法

技术领域

本发明属于计算机视觉与路径规划技术领域，具体涉及基于多层级地图的封闭场景定位方法。

背景技术

随着汽车智能化的不断发展，高精度定位的重要性愈加凸显，实现高精度定位，是获取交通状况、感知道路环境的基础，对智能汽车决策与路径规划起到主导作用。目前在开放交通场景下，智能汽车可在差分GPS/北斗结合惯导辅助下获得较好的定位效果。然而在一些复杂交通场景下，如高架桥、密集的高大建筑群内、地下停车场等，较为封闭的场景导致GPS卫星丢失，从而引起漂移误差。特别是在地下停车场内，完全封闭的区域导致GPS盲区，高精度惯导如NovAtel SPAN-IGM-S1在20秒内就会失效。地下停车场是交通场景的延伸，是交通的重要组成部分，也是智能汽车定位的“最后一公里”问题。如何在此场景下保证定位精度是智能汽车高精度定位亟待解决的难题。由于地下停车场存在光线较暗、遮挡较多、场景重复度高等特点，“停车难”、“找车难”一直是地下停车场存在的问题，不仅花费了车主大量寻找时间，也影响到车辆流转效率。

发明内容

针对现有技术中存在不足，本发明提供了一种基于多层级地图的封闭场景定位方法，实现高精度定位。

本发明是通过以下技术手段实现上述技术目的的。

一种基于多层级地图的封闭场景定位方法，具体包括步骤：

S1，获取封闭场景所有节点的图像信息和轨迹信息；

S2，制作多层级地图，包括BOW特征层、场景结构层和轨迹信息

对双目相机的左右目图像分别提取视觉局部特征，并将左目局部特征与BOW模型结合，构成BOW特征层；

利用左右目局部特征匹配得到封闭场景中局部特征在左右目图片中的像素坐标，计算出空间特征基于当前节点的空间坐标系坐标，得到多层级地图的场景结构信息；

S3，获取待定位点的车载相机拍摄图像，匹配最近的地图节点，计算待定位点距离最近地图节点的相对坐标，结合节点轨迹信息完成高精度定位。

上述技术方案中，所述匹配最近的地图节点，具体为：向视觉特征词典输入待定位点的图片，利用BOW特征层的视觉特征词典生成图像计算图像的词频直方图与描述子后，比较词频描述子与多层级地图所有节点对应的词频描述子的Hamming距离，即可得到待定位点最近的地图节点。

上述技术方案中，所述计算待定位点距离最近地图节点的相对坐标，具体为：提取匹配最近的地图节点左右目图像特征，进行首次特征匹配，提取待定位点图像特征与首次特征匹配结果进行二次特征匹配，运用PnP模型进行待定位点相对最近的地图节点的位置关系计算。

上述技术方案中，所述PnP模型为：

式中：u_i、v_i为二次特征匹配的特征点图像坐标；A为双目相机内参矩阵；R、t分为车载相机位置相对地图节点的旋转和平移变化矩阵，R为3×3矩阵，t为1×3矩阵；X_i、Y_i、Z_i为基于地图节点的左目相机坐标系坐标，f_x、f_y为车载相机的焦距参数，C_x、C_y为左目主点在节点图像中的坐标；

根据公式计算待定位点相对最近的地图节点的位置关系，式中(X_r，Y_r，Z_r)为待定位点基于地图节点相机坐标系的坐标。

上述技术方案中，所述左目局部特征与BOW模型结合包括生成视觉特征词典和根据视觉特征词典生成直方图。

上述技术方案中，所述结合节点轨迹信息完成高精度定位，具体为：通过视觉局部特征与前一节点的场景结构计算，同时配合数据采集获得轨迹信息，得到所有节点在封闭场景中的高精度定位坐标信息。

上述技术方案中，对高精度定位结果进行检测，在车辆行驶过程中，每一个定位时刻得到的定位坐标与上一个定位时刻坐标计算欧式距离，若欧式距离偏离预测距离超过25％，则判断该点位置定位失败，不将该点记录进行驶轨迹。

本发明的有益效果为：

(1)本发明构建了多层级地图，多层级地图包括BOW特征层、场景结构层和轨迹信息，保证每个节点包含视觉特征、场景结构以及轨迹信息的多维度信息，保证定位精度；

(2)本发明根据所构建的多层级地图，设计由粗到精的多尺度匹配定位方法，分别利用地图的视觉局部特征实现节点级粗匹配，再利用场景结构实现度量级精细定位，达到高精度定位的效果；

(3)本发明将词袋模型(bag of words，BOW)应用于封闭场景定位中，且与视觉局部特征相结合，达到提高定位精度的效果。

附图说明

图1为本发明所述基于多层级地图的封闭场景定位方法流程图；

图2为本发明所述BOW特征层制作示意图；

图3为本发明所述节点级定位示意图；

图4为本发明所述多层级地图及采集示意图；

图5为本发明所述节点级定位结果示意图；

图6为本发明所述定位结果误差直方图。

具体实施方式

下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。

如图1所示，一种基于多层级地图的封闭场景定位方法，具体包括如下步骤：

步骤(1)，多层级地图数据采集

利用双目相机在封闭场景的道路中每隔两米采集一组双目视觉图像，图像内容包括当前节点所能看到的封闭场景，直至采集节点覆盖整个封闭场景，得到所有节点的图像信息。在每个节点采集视觉图像的同时，利用惯性导航或者激光测距仪等轨迹记录工具，记录当前节点与上一节点的相对位置关系，得到所有节点轨迹信息。整理获得的节点图像信息与各自对应的轨迹信息，完成原始数据集。

步骤(2)，制作多层级地图

多层级地图包括BOW特征层、场景结构层和轨迹信息。

1)BOW特征层

对双目相机的左右目图像分别提取视觉局部特征，并将左目局部特征与BOW模型结合，构成BOW特征层，如图2所示。

所述视觉局部特征提取的过程为：使用ORB算法对节点图像信息进行特征提取，将多层级地图中所有节点的左目图像提取特征并计算描述子，每张图片默认提取500个描述子。

所述将左目局部特征与BOW模型结合的具体过程为：

a)生成视觉特征词典

对所有节点图像的描述子利用k-mean算法进行聚类，默认生成1000个聚类中心，由于ORB算法计算的描述子是二进制，因此利用Hamming距离来计算两个描述子之间的距离。每个聚类中心的描述符的形式，是跟各个描述子形式一致的256位二进制描述子，将聚类中心的描述符称之为视觉单词，这1000个视觉单词的组合，即为BOW建立的视觉特征词典，用W＝{w₁，w₂，w₃，…}来表示。

b)根据视觉特征词典生成直方图

制作完视觉特征词典后，将多层级地图中所有节点左目局部特征点描述子x按照Hamming距离最小的原则投票到各个视觉单词上，具体方法为：

w_i＝map(x，W)＝argmin(x，w_i)，w_i∈W (1)

其中map表示按照Hamming距离最小的原则投票。

将一张图片的所有视觉局部特征点描述子投票完成后，即可得到这张图片基于视觉特征词典的视觉单词出现频率直方图；频率直方图的统计方法为：

其中：B_i指第i个视觉单词，x_j表示一张图片上全部描述子。

完成所有封闭场景节点图像的特征点视觉单词出现频率统计后，得到所有节点图像的词频直方图，生成与各节点一一对应的词频描述子的数据集。到此为止，构成BOW特征层。

(2)场景结构层

将左右目局部特征进行匹配(为现有技术)，并通过三角法计算三维信息构成场景结构层。

利用左右目局部特征匹配得到封闭场景中局部特征在左右目图片中的像素坐标，利用三角法即可计算出空间特征基于当前节点的空间坐标系坐标，即多层级地图的场景结构信息，计算公式如下：

式中：Q为左右目图像三维重建投影矩阵，d为左右目图像中局部特征点对应图像点水平方向像的素差值，C_x、C_y为左目主点在节点图像中的坐标，T_x表示相机左右目投影中心之间的偏移，C′_x为右目主点在节点图像中的坐标，f为双目相机焦距，X、Y、Z、M为中间量，(x，y)为局部特征点在左目图像的像素横纵坐标，(x_r，y_r，z_r)为局部特征点通过三维重建得到的在左目相机坐标系下的三维坐标。

(3)轨迹信息

轨迹信息表示节点之间的位置关系，可将定位结果的相对位姿转化为绝对位姿，通过视觉局部特征与前一节点的场景结构计算，同时配合数据采集获得轨迹信息，得到所有节点在封闭场景中的高精度定位坐标信息。

步骤(3)，由粗到精的定位实现

获取待定位点的车载相机拍摄图像，匹配最近的地图节点，计算待定位点距离最近地图节点的相对坐标，结合节点轨迹信息完成高精度定位。

匹配最近的地图节点的过程为：向视觉特征词典输入待定位点的图片，利用BOW特征层的视觉特征词典生成图像计算图像的词频直方图与描述子后，比较词频描述子与多层级地图所有节点对应的词频描述子的Hamming距离，即可得到待定位点最近的地图节点，如图3所示，计算Hamming距离的公式如下：

其中，A_i与B_i表示两条词频描述子第i位的值，Hamming距离越小代表两张图像的词频描述子越接近，由此得以找到距离待定位点最近的地图节点。缩小定位范围到地图节点周边，完成粗定位，即节点级定位。

构建透视n点问题模型进行度量级精细定位，获得待定位点的位置与姿态。需要对待定位点处场景图像提取局部视觉特征，使用ORB算法进行局部特征提取和特征匹配。ORB算法是将FAST特征点检测法与BRIEF特征描述子相结合，并在此基础上加以改进与优化的特征提取方法。算法速度上，ORB算法的速度是sift的100倍、是surf的10倍。FAST特征点检测法提取特征点的思路为：对在固定半径圆上的像素进行分割测试，除去大量非候选特征点。本发明采用FAST-12-16算法，选取中心像素点，比较中心像素点与周围16个像素点的灰度值差值，当16个像素点中有连续12个点满足灰度值的差值小于设定阈值(一般设定位中心点灰度值的20％)，则中心像素点可以作为一个特征点。对每一个特征点引用BRIEF特征描述子进行描述，BRIEF是由0和1组成的256维二进制描述子，代表了特征点附近的256组随机点对的灰度值大小关系。

待定位点图像的局部特征与已匹配最近的地图节点图像进行局部特征匹配，结合节点在多层级地图中的场景结构层信息，可以得到待定位点图像中局部特征的场景三维信息。同时利用局部特征的三维信息与其基于待定位图像点的二维像素坐标信息，利用PnP模型即可构建透视n点问题模型，计算精细的位姿信息。

PnP模型可用于计算已知部分物体坐标的场景下的相机位姿。在封闭场景定位中，基于地图节点的左右目图像可以通过特征提取匹配两张图像中相同像素点，实现三维重建。PnP在5米范围内具有较高的定位精度。在完成封闭场景内节点级定位后，利用待定位点图像与匹配最近的地图节点左右目三维重建的结果，利用PnP模型完成待定位点度量级定位。

基于PnP模型的度量级定位共分三步实现：

①提取匹配最近的地图节点左右目图像特征，进行首次特征匹配；

②提取待定位点图像特征与首次特征匹配结果进行二次特征匹配；

③运用PnP进行待定位点相对最近的地图节点位置关系计算。

具体地，通过提取局部特征点，每张图片提取5000个局部特征点。将两张图片的特征点描述子进行匹配，依据匹配近似程度将局部特征点对降序排列，选取前15％的结果作为基于地图节点的特征点集。从左目图像特征点集中检索出属于左目图像的特征点索引，得到基于左目图像的局部特征点集。

将待定位点图像与地图节点的特征点集进行二次特征匹配，依据匹配近似程度将点对降序排列，选取前15％的结果作为最近的地图节点特征与待定位点特征匹配的结果。通过两次特征匹配，确定的特征点同时存在于最近的地图节点左右目图像与待定位点图像中。通过多层级地图中场景结构层，可以得到二次特征匹配的各个特征点在当前节点的空间信息，结合二次特征匹配的特征点与待定位点图像中对应点的像素坐标，进行PnP模型计算；PnP模型计算公式如下：

式中：u_i、v_i为二次特征匹配的特征点图像坐标；A为双目相机内参矩阵；R、t分为车载相机位置相对地图节点的旋转和平移变化矩阵，R为3×3矩阵，t为1×3矩阵；X_i、Y_i、Z_i为基于地图节点的左目相机坐标系坐标，f_x、f_y为车载相机的焦距参数。

PnP需要不少于6个特征点的信息输入才能解出正确R、t；计算待定位点基于最近的地图节点的相对位置关系公式如下：

式中：(X_r，Y_r，Z_r)为待定位点基于地图节点相机坐标系的坐标。结合地图节点的轨迹信息得到待定位点在地图中所处坐标，完成度量级定位。

为确保最终结果准确，对高精度定位最终结果进行检测，在车辆行驶过程中，每一个定位时刻得到的定位坐标与上一个定位时刻坐标计算欧式距离，若欧式距离偏离依据车速(封闭环境中车辆多为低速行驶，定位的时间间隔内速度变化可以忽略)的预测距离超过25％，则判断该点位置定位失败，不将该点记录进行驶轨迹。

通过PnP计算出的待定位点位置信息，是基于当前最近的节点为基准。通过多层级地图中轨迹信息中各个节点在地图中的定位信息，将各个节点在地图中的定位信息与PnP计算待定位点基于地图节点的相对位置信息结合，即可完成定位，实现整个封闭场景内的全局高精度定位。

实施例

为验证定位方法可靠性，选取江苏大学汽车工程研究院一楼车库作为实验场景，应用江苏大学智能驾驶测试车辆作为实验数据平台。车辆配备了两台Gige融合工业相机(车载相机)、一台velodyne 64线激光雷达和两台ibeo4线激光雷达(轨迹记录工具)，测试车道总长30米，应用ZED公司生产的ZED1双目相机，双目基线长为120mm，输出左右目图像尺寸均为2208×1242(pixel)。在起点处设置固定标志，使用激光雷达采集实验中地图节点、待定位点与固定标志的距离信息，此距离信息在实验中被视为节点的世界坐标系下的位置信息。从车库一端出发，每2米采集一组双目图像作为地图节点图像，并测量与固定标志的距离。将每两个节点之间的随机五个位置设置待定位点，采集单张左目相机图像并测量与固定标志的距离，采集待定位点的位置信息。记录下所有待定位点位置信息作为当前节点定位结果的真实值。完成一段路径数据采集后，将反向以同样的办法采集对向车道的节点以及待定位点数据。

多层级地图和待定位点的图像数据及位置信息采集完毕后，开始智能汽车定位方法测试，在验证方法之前，需要按顺序完成多层级地图中BOW特征层、场景结构层和轨迹信息内容的填充。

根据本发明的定位方法对多层级地图中每个节点的左目图像进行视觉局部特征提取、生成视觉特征词典、词频统计，生成词频描述子数据集，完成多层级地图的特征层；将待定位点的图像输入视觉特征词典，用生成的词频描述子对多层级地图的描述子数据集进行匹配，得到最近的地图节点。共采集了16个地图节点以及80个待定位点，记录时以节点与待定位点利用激光测距得到的真值距离作为该节点的序列号，对于对向车道的节点与待定位点图像序列号添加“A”作为标记，如图4所示。

完成节点级定位后待定位点的图像将与最近的地图节点左目图像匹配，匹配结果如图5所示。

待定位点在经过节点级定位匹配后，有3个待定位点未能正确匹配到最近的地图节点，而是匹配到与之相邻的第二接近节点。因为距离匹配节点未超过2m距离，PnP算法高度准确范围为5m以内，因此依然视为正确匹配。因此，与最近的地图节点匹配率为96.25％，与相邻最近的节点匹配结果为100％。

完成节点级定位后，开始进行待测试点度量级定位。利用ORB算法对被选中地图节点的左右目重新开始特征提取及特征匹配。本次实验对每张图片都选取50000个特征点，最终选出前15％的点对作为首次匹配结果，对待定位图像和首次匹配结果进行二次匹配，同样选取前15％的点。利用二次匹配结果点对在节点左右目的对应特征点对进行三维重建，得到点集的空间坐标，利用二次匹配结果点对在待定位图像中对应的点，得到点集的像素坐标。利用本发明方法对待定位点的车辆位置进行PnP计算。得到待定位点车辆与地图节点相对位置关系，通过计算两者距离，将其与激光测距所得真实结果进行对比，误差结果如图6所示，从结果可见，本发明方法定位结果相对于激光测距结果，平均误差为155mm、最高误差为367mm，可以实现封闭场景定位的需要。

所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。

Claims (6)

1.一种基于多层级地图的封闭场景定位方法，其特征在于，具体包括步骤：

S1，获取封闭场景所有节点的图像信息和轨迹信息；

S2，制作多层级地图，包括BOW特征层、场景结构层和轨迹信息

对双目相机的左右目图像分别提取视觉局部特征，并将左目局部特征与BOW模型结合，构成BOW特征层；

利用左右目局部特征匹配得到封闭场景中局部特征在左右目图片中的像素坐标，计算出空间特征基于当前节点的空间坐标系坐标，得到多层级地图的场景结构信息；

S3，获取待定位点的车载相机拍摄图像，匹配最近的地图节点，计算待定位点距离最近地图节点的相对坐标，结合节点轨迹信息完成高精度定位；

所述结合节点轨迹信息完成高精度定位，具体为：通过视觉局部特征与前一节点的场景结构计算，同时配合数据采集获得轨迹信息，得到所有节点在封闭场景中的高精度定位坐标信息。

2.根据权利要求1所述的基于多层级地图的封闭场景定位方法，其特征在于，所述匹配最近的地图节点，具体为：向视觉特征词典输入待定位点的图片，利用BOW特征层的视觉特征词典生成图像计算图像的词频直方图与描述子后，比较词频描述子与多层级地图所有节点对应的词频描述子的Hamming距离，即可得到待定位点最近的地图节点。

3.根据权利要求1所述的基于多层级地图的封闭场景定位方法，其特征在于，所述计算待定位点距离最近地图节点的相对坐标，具体为：提取匹配最近的地图节点左右目图像特征，进行首次特征匹配，提取待定位点图像特征与首次特征匹配结果进行二次特征匹配，运用PnP模型进行待定位点相对最近的地图节点的位置关系计算。

4.根据权利要求3所述的基于多层级地图的封闭场景定位方法，其特征在于，所述PnP模型为：

式中：u_i、v_i为二次特征匹配的特征点图像坐标；A为双目相机内参矩阵；R、t分为车载相机位置相对地图节点的旋转和平移变化矩阵，R为3×3矩阵，t为1×3矩阵；X_i、Y_i、Z_i为基于地图节点的左目相机坐标系坐标，f_x、f_y为车载相机的焦距参数，C_x、C_y为左目主点在节点图像中的坐标；

根据公式计算待定位点相对最近的地图节点的位置关系，式中(X_r，Y_r，Z_r)为待定位点基于地图节点相机坐标系的坐标。

5.根据权利要求1所述的基于多层级地图的封闭场景定位方法，其特征在于，所述左目局部特征与BOW模型结合包括生成视觉特征词典和根据视觉特征词典生成直方图。

6.根据权利要求1所述的基于多层级地图的封闭场景定位方法，其特征在于，对高精度定位结果进行检测，在车辆行驶过程中，每一个定位时刻得到的定位坐标与上一个定位时刻坐标计算欧式距离，若欧式距离偏离预测距离超过25％，则判断该点位置定位失败，不将该点记录进行驶轨迹。