CN114358133B

CN114358133B - 一种基于语义辅助双目视觉slam检测回环帧的方法

Info

Publication number: CN114358133B
Application number: CN202111495371.8A
Authority: CN
Inventors: 张红娟; 陈志军; 李文卓; 钱闯
Original assignee: Hulian Intelligent Wuhan Technology Co ltd
Current assignee: Hulian Intelligent Wuhan Technology Co ltd
Priority date: 2021-12-09
Filing date: 2021-12-09
Publication date: 2023-04-18
Anticipated expiration: 2041-12-09
Also published as: CN114358133A

Abstract

本申请公开了一种基于语义辅助双目视觉SLAM检测回环帧的方法，包括：通过双目相机对同一场景图像数据进行采集，利用YOLO学习框架检测目标；提取检测目标中的特征点；根据所述特征点中的描述子，采用颜色直方图将图像的像素在颜色空间中的分布提取出来；通过目标跟踪与图像检索为所述特征点匹配提供更强的约束，使得动点和误匹配最大限度筛除；利用回环检测判断相机是否经过同一场景，对全局的相机位姿与地图点进行优化。本申请实施列能够使得SLAM在复杂场景下鲁棒性和精度提高且能绘制含有语义信息的地图。

Description

一种基于语义辅助双目视觉SLAM检测回环帧的方法

技术领域

本发明涉及双目视觉SLAM领域，尤其涉及一种基于语义辅助双目视觉 SLAM检测回环帧的方法。

背景技术

随着导航技术的发展，高精度定位技术成为目前人们研究的一大热门，它有着巨大的研究价值和广阔的市场前景，而高精度地图是定位技术的刚性需求。高精度地图对地图细节的要求是传统地图无法相比的，而传统制图技术中大量细节的标识需要人工完成，这使得高精度地图的制作难度与成本都非常高。传统的制图手段效率低下，而借助相机、GNSS接收机、IMU、激光雷达等传感器采集的数据，结合SLAM技术进行高效精确的自动化制图才是大势所趋。经典SLAM 算法的目标是定位与建图，即求解载体自身的位置与视野中的地物位置。而在双目视觉SLAM中，摄像头是唯一的传感器，一方面负责观测地物；另一方面，通过一些数学的手段，摄像头也可以测量运动。经典SLAM算法包括以下步骤：首先找到同一帧中左右目像素点的对应关系，从而计算出对应的空间点相对于相机的位置，同时采集同一帧中的场景信息；然后找到前后帧之间的像素点对应关系，通过使用迭代最近点(Iterative Closest Point,ICP)算法^[1]，计算出相机的在两帧之间的运动；最后利用场景信息，检测相机是否经过同一地点，如果是，就可得到一个约束，依据这个约束，可沿用后端优化中的方法，构造最小二乘问题，对全局的相机位姿与地图点进行优化，从而限制误差累积。

目前常用的回环检测技术是基于特征点的词袋技术，词袋技术是通过对大量的特征点描述子进行聚类，来对描述子进行分类，如“车”“狗”“人”，那么所有的描述子就被分为这三类。训练的结果，是得到一个可以帮我们判断描述子类型的字典。得到字典后，我们利用它来描述一张图片。如果一张图中，恰好有一辆车、一只狗、一个人，车上有两个特征点，狗上有一个特征点，人上有一个特征点，则该图可以用描述向量(2,1,1)表示，通过计算描述向量间的距离，作为图像相似性的指标。当相机观测到的当前帧，与先前的某一帧的相似性小于某个阈值，就可以认为检测到了回环，并在当前帧和与匹配的回环帧之间建立约束，进行优化。

但经典SLAM算法难以处理复杂场景下的任务，比如视野中若存在动态物体，就会对SLAM造成极大的影响，从而不利于高精度定位。此外基于特征点的经典视觉SLAM所得到的地图往往是稀疏的点云，几乎无法从中得到任何地物信息。

[1]ARUN K S,HUANG T S,BLOSTEIN S D.Least-squares fitting of two 3-dpoint sets[J].Ieee Transactions on Pattern Analysis and Machine Intelligence,1987, 9(5):699-700.

发明内容

本发明要解决的技术问题在于，针对现有技术的上述缺陷，提供一种基于语义辅助双目视觉SLAM方法和装置，使得SLAM在复杂场景下鲁棒性提高，且能绘制含有语义信息的地图。本发明利用语义对经典双目视觉SLAM框架作出改进，包括采用目标检测技术辅助特征提取，采用目标跟踪辅助传统的帧间特征匹配，采用图像检索辅助传统的帧间匹配，以及利用图像检索取代传统的基于词袋的回环检测。

第一方面，本申请提供了一种基于语义辅助双目视觉SLAM检测回环帧的方法，该方法包括：

步骤1，通过双目相机对同一场景图像数据进行采集，利用YOLO学习框架检测目标，目标检测结果主要包含信息有：包围目标的边界框的中心坐标(b_x,b_y)、边界框的宽度b_w和高度b_h、以及边界框的类型；

步骤2，提取边界框中的特征点；根据所述特征点中的周围像素计算得到描述子，采用颜色直方图将图像的像素在颜色空间中的分布Hist提取出来，利用标签转换函数将直方图矩阵转换为标签sig(Hist)；

步骤3，匹配当前帧与前一帧中目标的边界框，关联成功的前帧中目标的边界框标记为相应的跟踪目标；否则，当前帧中目标的边界框为新出现的目标，将其标记为新的跟踪目标；

步骤4：利用图像检索方法将当前帧或当前帧及前一帧中左右目相机采集到的目标的边界框进行匹配找到匹配对；

步骤5：根据步骤4找出的当前帧或当前帧及前一帧左右目相机采集到的目标的边界框的匹配对，计算出目标的边界框的空间点相对于相机的位置，根据步骤3中与当前帧中所述目标的边界框关联成功的前一帧中目标的边界框，使用迭代最近点ICP算法计算出相机的在当前帧与前一帧之间的运动；

步骤6：根据目标的边界框的类型，对当前帧和之前的所有帧所采集的场景图像设置一个描述向量，其维度为种类不同的多种静态目标的边界框的类型，元素值分别为各类目标的边界框的个数；计算当前帧的描述向量与之前的所有帧的描述向量的距离，初筛出距离小于阈值的剩余帧；

步骤7：基于步骤3的方法输出剩余帧中目标的边界框的预测结果和当前帧图像中的目标的边界框的总匹配度best；

步骤8：筛选出剩余帧中best值最大的前几帧作为备选帧，重复步骤4的图像检索方法检查当前帧与几帧备选帧之间的匹配对，并将误匹配最少的一帧作为最终的回环帧，若误匹配数一样则取best最大一帧作为最终的回环帧。

优选地，采用颜色直方图将图像的像素在颜色空间中的分布Hist提取出来的步骤包括：

首先，计算HSV颜色空间下的图像矩阵为I(i,j)，

其中i,j分别为图像矩阵行标和列标，H,S,V分别为像素在HSV颜色空间下的色度分量，饱和度分量以及明度分量；

然后，利用函数in[h，v，I(i,j)]对图像矩阵为I(i,j)进行颜色量化，即将颜色空间划分为若干区间，并统计落在区间中的像素数量；

色度域分为bin_H个区间，饱和度域分为bin_S个区间，h,v为直方图矩阵中的区间下标，1、2代表通道；

最后，计算图像矩阵在色度与饱和度两个维度下的二维直方图矩阵Hist

优选地，所述步骤6中静态目标有两类，即交通灯与交通标志牌。

进一步地，步骤3的匹配步骤包括，重复步骤1～2采集场景图像当前帧中目标的边界框，基于SORT算法对前一帧与当前帧中目标的边界框进行建模：X＝[b_x、 b_y、b_h、s、b′_x、b′_y、b′_h、s′]^T，s为目标的边界框宽高比，X中后四项为前四项相应的变化率，使用卡尔曼滤波对前一帧中目标的边界框在当前帧中的位置进行预测，前一帧的目标经卡尔曼滤波预测的边界框集合为Prediction＝{P₁，P₂，…,P_m}, 当前帧的检测结果在边界框的集合为Detection＝{D₁，D₂，…,D_m}，

以这两个集合为顶点，建立带权二分图，带权二分图对应的邻接矩阵为W，则有：

mdist(Xp_i,Xq_j)为待匹配帧边界框集合中各边界框间的马氏距离，帧间待匹配边界框的下标分别为i,j，马氏距离设置限值为T_mdist，

Σ^-1为边界框的m维协方差矩阵Σ的逆，两帧间目标的边界框的移动距离为 move，动态系数为k，则给出动态约束：

将带权二分图对应的邻接矩阵送入KM算法进行匹配，则完成目标跟踪。

进一步地，步骤4中利用图像检索方法将当前帧或当前帧及前一帧中左右目相机采集到的目标的边界框进行匹配找到匹配对的步骤包括：

建立基于左目与右目中目标的边界框的相似度约束、行对准约束、尺度约束的带权二分图的邻接矩阵，将带权二分图的邻接矩阵送入KM算法进行匹配，得到左目与右目中目标的边界框之间的对应关系，若左目与右目中目标的边界框关联成功，则将该目标的边界框标记为相应的匹配对；

相似度约束：EMD(sig_i，sig_j)≤T_EMD

Hist_i与Hist_j分别为左目与右目中目标的边界框的直方图矩阵，将Hist_i转换得到的标签记作P，P＝sig(Hist_i)，将Hist_j转换得到的标签记作Q，Q＝sig(Hist_j)，

P＝{(p₁，ω_p1)，(p₂，ω_p2)，…，(p_m，ωp_m)}

Q＝{(q₁，ω_q1)，(q₂，ω_q2)，…，(q_n，ω_qn)}

其中p与q分别为P与Q中的特征，是维度相同的向量；ω则表示对应特征的权重，是一个标量，定义一个流动矩阵F，描述P调整到Q需要经历的运输过程，F(ij，)即从p_i到q_j的运输量；

对目标函数的值进行归一化处理，就得到EMD指标：

EMD限值为T_EMD，EMD(PQ)≤T_EMD；

行对准约束：Δb_h＝b_hi-b_hj，Δb_h≤T_Δbh

对于待匹配的左目中的目标i和右目中目标j，设可容忍的高度差为 T_Δbh(0＜T_Δbh≤3)，单位为像素；

尺度约束：先使待匹配的两个边界框左上角重合，再计算交并比IoU，对于待匹配的目标i与j，对齐后的IoU可按下式计算：

设可容忍的IoU下限为T_IoU(0.5＜T_IoU＜1)，IoU(i，j)＞T_IoU，单位为像素；

建立带权二分图，其对应的邻接矩阵为w，则有：

进一步地，筛选出左目和右目的匹配对后，还使用相对关系一致约束，使得动点和误匹配最大限度筛除，相对关系一致约束：设左目和右目的匹配对表示为如下集合Match：

Match＝{(L₁，R₁)，(L₂，R₂)，…，(L_n，R_n)}

其中L为左目中目标的边界框，R为右目中目标的边界框。对于左目和右目中的目标的边界框(L_i，R_i)与每个其他目标的边界框匹配对，如下构建其左目和右目中的相对关系矩阵Rel：

其中，(x_Ln、y_Ln)表示左目中的目标Ln的边界框的中心坐标，(x_Rn、y_Rn)表示右目中的目标Rn的边界框的中心坐标，x_Li-x_Ln，y_Li-y_Ln表示左目中的目标的边界框的中心坐标(x_Li，y_Li)与(x_Ln、y_Ln)的位置关系，x_Ri-x_Rn，y_Ri-y_Rn表示右目中的目标的边界框的中心坐标(x_Ri，y_Ri)与(x_Rn、y_Rn)的位置关系；

将上两个矩阵对应元素相乘，得到用于判断相对关系是否一致的一致性矩阵Con：

则相对关系一致约束可写作下式：

该式等价于Con中元素符号全为正。对于不符合该式的匹配对，将其删去, 剩余的即匹配对，Sgn是数学上的一个符号函数，返回值如果number大于0，则Sgn返回1；等于0，返回0；小于0，则返回-1。

进一步地，所述总匹配度best通过KM算法计算，其过程如下：

设有一带权二分图G(V,E)，顶点集V＝V_x∪V_Y其中V_x＝{v_x1,v_x2,…v_xm}， V_Y＝{v_y1,v_y2,…v_ym}，(m≤n)分别为二分图两个互不相交且内部不存在边相连顶点的顶点集；边集E＝{e_xi,e_yj|1≤i≤m,1≤j≤n}，设所有顶点的标签值为L，边权重值为 ω，V_Y中顶点被匹配所需的最小补偿值为slack，算法描述如下：

步骤S301，初始化L，best，L(v_xi)＝maxω(e_xi,e_yj)，L(v_yj)＝0，best＝0；

步骤S302，为V_xi寻找匹配点，i初值为1；

初始化slack，由于slack为最小补偿值，故令slack(v_yj)＝inf；

遍历V_Y，对V_Y中每一个v_yj，计算gap＝L(v_xi)+L(v_yj)-ω(e_xi,e_yi)；

若gap为0，判断v_yj是否已具有匹配，若否，则匹配成功，退出步骤S302；

若是，则对v_yj的原配V_xk执行步骤S302；

若gap不为0，更新V_yj的补偿值，slack(v_yj)＝min[slack(v_yj)，gap]；

遍历结束，匹配仍不成功，减小V_xi的标签，L(v_xi)＝L(v_xi)-drop，drop＝ min[slack(v_yj)]重新执行步骤S302；

步骤S303，V_xi匹配成功，对i加1，执行步骤S302；

步骤S304，V_x全部匹配完成，逐个检查匹配对间的边权重是否为0；若是，删除该匹配对，若否，将其权重加入best；

步骤S305，输出best，算法退出。

本申请提供的基于语义辅助双目视觉SLAM回环检测的方法，能够使得 SLAM在复杂场景下鲁棒性提高，且能绘制含有语义信息的地图。

应当理解的是，以上一般描述和后文的细节描述仅是示例性的，并不能限制本发明。

本发明选择著名目标检测框架YOLOv3作为目标检测的工具，引入目标跟踪与图像检索的相关技术，为特征点匹配提供了更强的约束，使SLAM能在复杂环境下正常工作。对于SLAM前端，本发明利用目标检测提取出来的目标，结合目标跟踪技术和图像检索技术，为仅依赖特征点的经典SLAM前端增加更多特征点匹配约束，从而将动点和误匹配最大限度地筛除。对于SLAM后端，位姿优化方面本文与传统SLAM并无区别，采取的是一般的非线性优化方法。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并于说明书一起用于解释本发明的原理。

图1是本申请一示例性实施例提供的一种基于语义辅助双目视觉SLAM检测回环帧的方法流程图；

图2是本申请一示例性实施例提供的一种匹配方式算法流程图；

图3是本申请一示例性实施例提供的一种基于语义辅助双目视觉SLAM目标检测图；

图4为SORT算法在百度Apollo Scape数据集中第44帧视频上的表现；

图5为SORT算法在百度Apollo Scape数据集中第90帧视频上的表现；

图6组合导航平面轨迹解算结果；

图7经典双目SLAM平面轨迹解算结果；

图8语义双目SLAM平面轨迹解算结果。

具体实施方式

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

双目视觉，也被称为立体视觉。顾名思义，是仿照人眼的工作原理，使用两个相机对同一场景采集图像数据，从而利用两个相机对同一场景观测之间的差异，完成对场景深度的测量，从而将平面图像中的信息恢复到三维空间中。

SLAM(simultaneous localization and mapping)，即同时定位与建图，是一种常用于机器人自主定位、导航与地图绘制的技术，一般以激光雷达或相机等作为主要传感器。

双目视觉SLAM，即指以双目相机为主要传感器，利用双目相机的三维空间观测能力，来完成对相机在三维空间中运动的测量，进一步推算相机位置与地物位置，从而完成定位、导航与建图的任务。双目视觉SLAM框架一般包含四个模块：视觉里程计、后端优化、回环检测与建图。

请参见图1，图1是本发明实施例中提供的一种基于语义辅助双目视觉SLAM 检测回环帧的方法流程图。该方法步骤包括：

步骤101：通过双目相机对同一场景图像数据进行采集，利用YOLO学习框架检测目标，目标检测结果主要包含信息有：包围目标的边界框的中心坐标、边界框的宽度和高度、以及边界框的类型。其中YOLO学习框架采用YOLOV3在速度与精度上的高水准，大大提高了目标检测的实用性。

所采用的目标检测技术YOLO(YouLookOnlyOnce)是2015年JosephRedmon 等提出的目标检测框架，通过改变传统的框架结构，第一版YOLO极大地提升了目标检测的速度，但由于准确率不高，并未引起注意；2016年，JosephRedmon 等进一步提出YOLOv2，对YOLO作出了部分改进，保持YOLO速度优势的同时，较大地提高了检测的准确率，开始受到研究者的广泛关注；2018年， JosephRedmon等再次提出YOLOv3，这一版的YOLO作出了一些重大改进，维持速度的同时，准确率显著提高。YOLOv3在NvidiaGeForce960M笔记本载GPU 上处理512*512分辨率图片，可达每秒10至15帧，且能正确检出图片中大部分目标。

输入图片通过主干网络得到3个尺度的特征图(从上往下：feat1->(256 *52*52),feat2->(512*26*26),feat3->(1024*13*13))，分别在3种尺度上进行检测。 3个特征图经过5层卷积(Conv2D Block)后，分别进入不同的分支，一条分支进行卷积+上采样，得到的特征图与上层的特征图进行通道合并(Concat)，另一条分支通过两层卷积直接输出预测结果。最后一个卷积层为1*1卷积，卷积核尺寸为(B*(5+C))*1*1，B表示一个网格可以预测边界框的数目，C代表C个类别概率，5表示了4个坐标值(t_x，t_y，t_w，t_h)和1个物体置信度。对于coco数据集， C＝80，B＝3。最终3个尺度的检测结果的尺寸分别是255*52*52、255*26*26和 255*52*52。特征图上的一个像素对应原图中的一个网格，每个尺度定义了3种锚框，即每个网格会有3个预测框，每个预测框具有(5+C)个属性。网络在3 个尺度上检测，所以整个网络共检测13*13*3+26*26*3+52*52*3＝10647个边界框。

YOLOv3沿用YOLO9000预测边界框的方法，通过尺寸聚类确定锚框，对于13*13特征图中每个网格点预测3个不同分辨尺寸的边界框，网络为每个边界框预测5个值：t_x，t_y，t_w，t_h，t₀，其中前四个是坐标的偏移值，t₀是置信度，网格距离图像左上角的边距为(c_x,c_y)，p_w和p_h是网格对应的先验框的宽和高，则获得边界框预测：b_x，b_y，b_w，b_h是预测的边界框的中心坐标x，y，边界框的宽度 b_w和高度b_h；

b_x＝σ(t_x)+c_x

b_y＝σ(t_y)+c_y

b_w＝p_w*e^tw

b_h＝p_h*e^th。

步骤102：提取边界框中的特征点；根据所述特征点中的周围像素计算得到描述子，采用颜色直方图将图像的像素在颜色空间中的分布提取出来，利用标签转换函数将直方图矩阵转换为标签sig(Hist)，

标签sig(Hist)由特征对组成，式中特征对前者是特征本身，在直方图矩阵中就是具体的区间坐标；特征对后者则是特征的权重，在直方图矩阵中是相应区间中像素占整幅图像素的比例。

本实施例中特征点的定义指的是图像灰度值发生剧烈变化的点或者在图像边缘上曲率较大的点(即两个边缘的交点)，如FAST角点、Harris角点等等。由于YOLOV3提取出的目标特征是高度抽象的特征高度抽象往往意味着内容的简洁，这使得它在一些场景下是不满足需求的。比如一张图片中提取出来两个交通灯，它们的类型都是交通灯，大小和位置也比较接近，那么YOLOv3提取出来的这些特征就不足以区分它们。因此图像检索技术中采用的颜色直方图是一种不错的选择，它将一副图像的像素在颜色空间中的分布提取出来，能比较好地描述图片的色彩信息。对于目标而言，由于边界框中的图片几乎只含有目标本身，其颜色直方图相当于描述了目标的外观。描述子h(x,y,θ)是对特征点附近邻域内高斯图像梯度统计结果的一种表示，它是一个三维的阵列，但通常将它表示成一个矢量。矢量是通过对三维阵列按一定规律进行排列得到的。特征描述子与特征点所在的尺度有关，因此，对梯度的求取应在特征点对应的高斯图像上进行。因此，考虑到交通灯这类目标由于标准化制造外观高度一致，在计算目标对应的直方图时，会略微扩大边界框的范围，从而加入少量背景信息，以区分外观相似的目标。

采用颜色直方图将图像的像素在颜色空间中的分布提取出来的步骤包括：

首先，计算HSV(Hue,Saturation,Value，色度，饱和度，明度)颜色空间下的图像矩阵为I(i,j)，

其中i,j分别为图像矩阵行标和列标，H,S,V分别为像素在HSV颜色空间下的色度分量，饱和度分量以及明度分量，其取值范围遵循OpenCV中的规定。 HSV颜色空间下的图像矩阵为I(i,j)，可排除光照变化的影响。

如I(I,j,1)就是提取这个像素点的色度，I(I,j,2)就是提取这个像素点的饱和度，h和v分别对应色度域和明度域度，

最后，计算图像矩阵在色度与饱和度两个维度下的二维直方图矩阵Hist。

直方图矩阵是一个描述目标外观的优秀特征。要利用这一特征进行匹配，需要计算它们之间的相似度，这可以通过一些距离指标来实现。但如果按常见的距离定义计算直方图矩阵间的距离，如欧氏距离、相关距离与卡方距离等等，表现并不好。因此，需要采用陆地移动距离(EarthMover’sDistance,EMD)也就是EMD 指标计算直方图矩阵间的相似度。

步骤103：重复步骤102～103采集场景图像当前帧中目标的边界框，基于 SORT算法使前一帧中目标的边界框的预测结果与当前帧中目标的边界框进行匹配，匹配方式采用KM算法，得到边界框的预测结果与当前帧中目标的边界框之间的对应关系，若当前帧中目标的边界框与边界框的预测结果关联成功，则将该当前帧中目标的边界框标记为相应的跟踪目标；否则，当前帧中目标的边界框为新出现的目标，将其标记为新的跟踪目标；

SORT算法是一种目标跟踪方法，匹配帧间的目标，目前主流的目标跟踪方法均是基于目标检测的跟踪方法，即分别在两帧上进行检测，然后对两帧中的目标进行匹配。SORT算法是一种具有代表性的多目标跟踪方法。

其中SORT算法包括：对目标边界框进行建模：X＝[b_x、b_y、b_h、s、b′_x、b′_y、b′ _h、s′]^T；使用卡尔曼滤波对前一帧的目标在当前帧中的位置进行预测，然后将预测后的结果与当前帧的检测结果进行匹配，匹配方式采用KM算法。

使用卡尔曼滤波进行目标跟踪的方法记载在文献“BEWLEY A,GE Z,OTT L, etal.Simple online and realtime tracking[C],Proceedings of the 2016IEEEInternational Conference on Image Processing(ICIP),2016.”中。也记载在中国专利CN111640140A的步骤102中。

b_x为目标的边界框中心横坐标，b_y为目标的边界框中心纵坐标，b_h为目标的边界框高度，s为目标的边界框宽高比,目标状态为X，其中，后四项为前四项相应的变化率。前两项描述了目标的位置，后两项描述了目标的尺度，且考虑到实际场景中目标宽度可能会随视角变化而容易发生较大改变，而目标高度在视距确定情况下不会变化，我们在尺度描述中突出了高度h，而将宽度弱化为宽高比s。

本实施例中设SORT算法中前一帧的目标经卡尔曼滤波预测的边界框集合为Prediction＝{P₁，P₂，…,P_m},当前帧的检测结果在边界框的集合为Detection＝{D₁，D₂，…,D_m}。以这两个集合为顶点，建立带权二分图，带权二分图对应的邻接矩阵为W，则有：

mdist(Xp_i,Xq_j)为待匹配帧边界框集合中各边界框间的马氏距离，帧间待匹配边界框的下标分别为i,j，马氏距离设置限值为T_mdist，马氏距离(Mahalanobis Distance)，1930年由Mahalanobis提出，本是描述一个点与一个分布间的距离，由于其天然具有无量纲化的优势，常被用于计算指标中不同数值有数量级差异的相似度。

Σ^-1为边界框的m维协方差矩阵Σ的逆，使得不同随机变量之间的量纲差异被消除。对于之前定义的目标状态X，bx与by作为像素坐标一般值在数百，b_h的值则在数十，s则往往在1附近。如果直接以欧式距离判断目标状态是否相似，位置因素往往会淹没尺度因素，马氏距离则不存在这一问题。本实施例主要跟踪的是静态目标，且采集地图的车辆一般行驶速度为15-20km/h，速度较低，相邻帧之间，载体对同一目标的视角和视距不会发生大的变化，因此状态也基本不变，马氏距离将目标的位置变化与尺度变化的约束放在一起考虑，这导致位置与尺度的约束可能会相互干扰，导致约束失效，如有两个目标尺度相似(这非常常见，比如交通灯)，区分它们需要位置变化约束，但由于检测误差，前帧中第一个目标与后帧中第二个目标尺度完全一致，与后帧中其自身反而略有不同，此时位置约束有可能会淹没在尺度的误差中，导致误匹配。考虑到远处目标的运动较近处目标在图像上更不明显，故将该约束与目标的尺度相关联。设两帧间目标的边界框的移动距离为move，动态系数为k，则给出动态约束：

将带权二分图送入KM算法进行匹配，则完成目标跟踪，输出相邻的前一帧和当前帧图像中的目标的边界框的总匹配度best；

Kuhn-Munkres算法简称KM算法[2]，1955年由H.W.Kuhn提出，1957年由 J.Munkres改进，是一种高效计算带权二分图最大匹配的算法。所谓二分图，即顶点可分为这样两组：同组之内的顶点间不存在边，只有不同组的两点之间可能存在边。如果边有权重，那么这个二分图就是带权二分图。带权二分图的最大匹配，就是将前组顶点与后组顶点匹配，且每个顶点只能匹配一次，要求最终所有匹配对之间的边权重之和最大。

Kuhn-Munkres算法已记载于“[2]MUNKRES J.Algorithms for the Assignmentand Transportation Problems[J].Journal of the Society for Industrial andApplied Mathematics,1957,5(1):32-38.”中得到邻接矩阵后使用Kuhn–Munkres方法进行数据关联，专利CN110310305B中记载了Kuhn–Munkres方法的求解过程：(1) 找到邻接矩阵中每一行值最小的元素，并使每一行的所有元素减去该行的最小值。 (2)找到邻接矩阵中每一列值最小的元素，并使每一列的所有元素减去该列的最小值。(3)用尽可能少的行或列标记覆盖矩阵中的所有0，若此时标记的总数等于邻接矩阵的行数或列数，则完成数据关联，否则执行下一步。(4)找到未被行或列标记覆盖的元素的最小值，所有未被行或列标记覆盖的行元素减去该最小值，所有未被覆盖的列元素加上该最小值，转到(3)。使用Kuhn–Munkres方法进行数据关联即能得到预测的边界框(预测目标)与当前帧的边界框(检测目标)之间的对应关系，若检测目标与预测目标关联成功，则将该检测目标标记为相应的跟踪目标。否则，检测目标为新出现的目标，将其标记为新的跟踪目标，最终实现对目标长期、准确的跟踪。

将待匹配的两组目标视作二分图，利用KM算法进行匹配，KM算法全局准确度和效率更高。由图4-图5，图4是第44帧，图5是第90帧，SORT算法在百度Apollo Scape数据集中某段视频上的表现可以看出，跟踪结果是不错的，通过关注的静态目标在50帧的长度内依然能够正确跟踪。

步骤104：利用图像检索方法将当前帧或当前帧及前一帧中左右目相机采集到的目标的边界框进行匹配找到匹配对；

图像检索方法包括建立基于左目与右目中目标的边界框的相似度约束、行对准约束、尺度约束的带权二分图，将带权二分图送入KM算法进行匹配，得到左目与右目中目标的边界框之间的对应关系，若左目与右目中目标的边界框关联成功，则将该目标的边界框标记为相应的匹配对。

相似度约束：EMD(sig_i，sig_j)≤T_EMD

Histi与Hist_j分别为左目与右目中目标的边界框的直方图矩阵，判断左目与右目中目标的边界框的相似度，利用EMD来判断图片相似性，已经是一门被广泛应用的图像检索技术，其过程是对Hist_i的标签找到一种调整方式，使之与Hist_j的标签一致，且转换代价最小。为了书写方便，将Hist_i转换得到的标签记作P， P＝sig(Hist_i)，将Hist_j转换得到的标签记作Q，Q＝sig(Hist_j)写作如下形式：

P＝{(p₁，ω_p1)，(p₂，ω_p2)，…，(p_m，ω_pm)}

Q＝{(q₁，ω_q1)，(q₂，ω_q2)，…，(q_n，ω_qn)}

其中p与q分别为P与Q中的特征，是维度相同的向量；ω则表示对应特征的权重，是一个标量。定义一个流动矩阵F，描述P调整到Q需要经历的运输

过程，F(ij，)即从p_i到q_j的运输量。于是定义如下优化问题：

对目标函数的值进行归一化处理，就得到EMD指标：

EMD指标是一种良好的判断图像相似度的指标。相比于大多数指标，EMD 指标的区分度更为明显。EMD限值为T_EMD，EMD(P，Q)＝EMD(sig_i，sig_j)≤T_EMD的左目与右目中目标的边界框的相似度高。

行对准约束：Δb_h＝b_hi-b_hj，Δb_h≤T_Δbh

尺度约束：先使待匹配的两个边界框左上角重合，再计算交并比IoU，这样 IoU就只描述尺度的相似性了。对于待匹配的目标i与j，对齐后的IoU可按下式计算：

设可容忍的IoU下限为T_IoU(0.5＜T_Iou＜1)，IoU(i，j)＞T_Iou，单位为像素。

图像检索中建立二分图并匹配的过程和帧间匹配同理，建立带权二分图，其对应的邻接矩阵为W，则有：

以上的利用图像检索方法将当前帧中左右目相机采集到的目标的边界框进行匹配找到匹配对，在有的情形下会不够有效，在同一帧中，往往有相当多外观一致、尺度一致且高度一致的目标立体匹配直接关系到我们对地物点的坐标计算，进一步影响相机位姿解算的结果，一旦错误，SLAM结果必然错误，甚至ICP 算法根本无法得出有效解。因此本申请提出了另一种约束，相对关系一致约束，即左目和右目间所有目标的相对关系应当一致。

因此，筛选出左目和右目的匹配对后，还可用相对关系一致约束，使得动点和误匹配最大限度筛除：

具体而言相对关系一致约束：设左目和右目的匹配对可表示为如下集合 Match：Match＝{(L₁，R₁)，(L₂，R₂)，…，(L_n，R_n)}

其中，(x_Ln、y_Ln)表示左目中的目标Ln的边界框的中心坐标，(x_Rn、y_Rn)表示右目中的目标Rn的边界框的中心坐标，x_Li-x_Ln，y_Li-y_Ln表示左目中的目标的边界框的中心坐标(x_Li,y_Li)与(x_Ln、y_Ln)的位置关系，x_Ri-x_Rn，y_Ri-y_Rn表示右目中的目标的边界框的中心坐标(x_Ri,y_Ri)与(x_Rn、y_Rn)的位置关系。

则相对关系一致约束可写作下式：

该式等价于Con中元素符号全为正。对于不符合该式的匹配对，将其删去, 剩余的即匹配对。Sgn是数学上的一个符号函数，返回值如果number大于0，则Sgn返回1；等于0，返回0；小于0，则返回-1。

步骤105：根据步骤104找出的当前帧或当前帧及前一帧左右目相机采集到的目标的边界框的匹配对，计算出目标的边界框的空间点相对于相机的位置，根据步骤103中与当前帧中所述目标的边界框关联成功的前一帧中目标的边界框，使用迭代最近点(Iterative Closest Point,ICP)算法计算出相机的在当前帧与前一帧之间的运动。

步骤106：根据目标的边界框的类型，对当前帧和之前的所有帧所采集的场景图像设置一个描述向量，其维度为种类不同的多种静态目标的边界框的类型，元素值分别为各类目标的边界框的个数；计算当前帧的描述向量与之前的所有帧的描述向量的距离，初筛出距离小于阈值的剩余帧；

本实施例中，提取的静态目标有两类，即交通灯与交通标志牌，它们都是稳定存在于空间中的地物，不易变化，是进行回环检测的理想特征，依据词袋的思想，设置一个二维向量，其中元素值分别为两类目标的个数。虽然在两个不同的场景中，完全有可能具有相同数量的交通灯与标志牌。不过，这种简单描述的好处是匹配所需的运算少，因此作为一种初筛帧序列的手段。尤其是随着SLAM 的运行，帧序列会越来越长，对帧序列进行快速的初筛是非常必要的。通过计算描述向量间的距离，作为图像相似性的指标。当相机观测到的当前帧与先前的某一帧的相似性小于某个阈值，就可以认为检测到了回环。

步骤107：基于SORT算法对剩余帧和当前帧中的边界框进行建模：X＝[b_x、 b_y、b_h、s、b′_x、b′_y、b′_h、s′]^T，使用卡尔曼滤波对剩余帧中目标的边界框在当前帧中的位置进行预测，使剩余帧中目标的边界框的预测结果与当前帧中目标的边界框进行匹配，匹配方式采用KM算法，输出剩余帧中目标的边界框的预测结果和当前帧图像中的目标的边界框的总匹配度best，s为目标的边界框宽高比，X 中后四项为前四项相应的变化率；

本实施例中设SORT算法中剩余帧的目标经卡尔曼滤波预测的边界框集合为E＝{E₁，E₂，…E_m},当前帧的检测结果在边界框的集合为O＝{O₁，O₂，…,O_m}。以这两个集合为顶点，建立带权二分图，带权二分图对应的邻接矩阵为W，则有：

mdist(X_Ei,X_Oj)为待匹配帧边界框集合中各边界框间的马氏距离，帧间待匹配边界框的下标分别为i,j，马氏距离设置限值为T_mdist。

Σ^-1为边界框的m维协方差矩阵Σ的逆，使得不同随机变量之间的量纲差异被消除。设两帧间目标的边界框的移动距离为move，动态系数为k，则给出动态约束：

将带权二分图送入KM算法进行匹配，则完成目标跟踪，输出剩余帧中目标的边界框的预测结果和当前帧的目标的边界框的总匹配度best；

图2是本发明一示例性实施例提供的Kuhn-Munkres算法匹配方式算法流程图，如图2所示：

下面给出KM算法的过程，设有一带权二分图G(V,E)，顶点集V＝V_x∪V_Y其中V_x＝{v_x1,v_x2,…v_xm}，V_Y＝{v_y1,v_y2,…v_ym}，(m≤n)分别为二分图两个互不相交且内部不存在边相连顶点的顶点集；边集E＝{e_xi,e_yj|1≤i≤m,1≤j≤n}，设所有顶点的标签值为L，边权重值为ω，V_Y中顶点被匹配所需的最小补偿值为slack，最终输出的匹配值为best。为了便于讨论，规定V_x中元素数量更少，并假设其中每个元素至少有一条边与之相连(或者对没有边的顶点设一条权重为0的边)，这样 V_x总能够全部匹配，以其为主进行匹配将更容易。算法描述如下：

步骤S301，初始化L，best。L(v_xi)＝maxω(e_xi,e_yj)，L(v_yj)＝0，best＝0；

步骤S302，为V_xi寻找匹配点，i初值为1；

初始化slack。由于slack为最小补偿值，故令slack(v_yj)＝inf；

遍历V_Y。对V_Y中每一个v_yj，计算gap＝L(v_xi)+L(v_yj)-ω(e_xi,e_yi)；

若gap为0，判断v_yj是否已具有匹配。若否，则匹配成功，退出步骤S302；

若是，则对v_yj的原配V_xk执行步骤S302；

若gap不为0，更新V_yj的补偿值。slack(v_yj)＝min[slack(v_yj)，gap]；

遍历结束，匹配仍不成功，减小V_xi的标签。L(v_xi)＝L(v_xi)-drop，drop＝ min[slack(v_yj)]重新执行步骤S302。

步骤S303,V_xi匹配成功。对i加1，执行步骤S302。

步骤S304,V_x全部匹配完成。逐个检查匹配对间的边权重是否为0。若是，删除该匹配对。若否，将其权重加入best。

步骤S305，输出best。算法退出。

步骤108：筛选出剩余帧中best值最大的前几帧作为备选帧，重复步骤104 的图像检索方法检查当前帧与几帧备选帧之间的匹配对，并将误匹配最少的一帧作为最终的回环帧，若误匹配数一样则取best最大一帧作为最终的回环帧；

步骤109：沿用后端优化中的方法，构造最小二乘问题，对全局的相机位姿与地图点进行优化，从而限制误差累积。

图3是本申请一示例性实施例提供的一种基于语义辅助双目视觉SLAM目标检测图。如图2所示本申请所采用的目标检测技术是基于YOLOv3目标检测框架，能够在维持速度的同时，准确率也显著提高，大大提高了目标检测的实用性。YOLOv3在NvidiaGeForce960M笔记本载GPU上处理512*512分辨率图片，可达每秒10至15帧，且能正确检出图片中大部分目标。其中目标检测的结果主要包含的信息有：目标框的位置、大小、以及目标类型。

采用本发明的方法实验时使用一组由双目相机、GNSS接收机以及IMU构成的传感器，在有大量车辆行人的复杂环境下，采集长为900.23米的轨迹数据并进行后处理。后处理过程中，对双目相机数据进行单独解算，然后将GNSS 和IMU组合导航的结果作为参考，来分析双目相机数据的SLAM解算精度，并将不借助语义与借助语义的SLAM结果进行对比。图6中为组合导航的平面轨迹解算结果，图7中为本发明所实现的SLAM框架不借助语义的平面轨迹解算结果，图8中本文所实现的语义SLAM框架的平面轨迹解算结果。定性地看，语义SLAM的结果显然比经典SLAM的结果更平滑。本发明所实现SLAM中为了保证结果的正确性，将明显异常结果视为含有粗差并剔除。经典SLAM结果不如语义SLAM平滑，这说明经典SLAM中异常结果多于语义SLAM，从而被剔除的帧也更多。

以上所述的仅是本发明的实施例，方案中公知的具体结构及特性等常识在此未作过多描述，所属领域普通技术人员知晓申请日或者优先权日之前发明所属技术领域所有的普通技术知识，能够获知该领域中所有的现有技术，并且具有应用该日期之前常规实验手段的能力，所属领域普通技术人员可以在本申请给出的启示下，结合自身能力完善并实施本方案，一些典型的公知结构或者公知方法不应当成为所属领域普通技术人员实施本申请的障碍。应当指出，对于本领域的技术人员来说，在不脱离本发明结构的前提下，还可以作出若干变形和改进，这些也应该视为本发明的保护范围，这些都不会影响本发明实施的效果和专利的实用性。本申请要求的保护范围应当以其权利要求的内容为准，说明书中的具体实施方式等记载可以用于解释权利要求的内容。

Claims

1.一种基于语义辅助双目视觉SLAM检测回环帧的方法，其特征在于，包括：步骤1，通过双目相机对同一场景图像数据进行采集，利用YOLO学习框架检测目标，目标检测结果主要包含信息有：包围目标的边界框的中心坐标(b_x,b_y)、边界框的宽度b_w和高度b_h、以及边界框的类型；

步骤5：根据步骤4找出的当前帧或当前帧及前一帧左右目相机采集到的目标的边界框的匹配对，计算出目标的边界框的空间点相对于相机的位置，根据步骤3中与当前帧中所述目标的边界框关联成功的前一帧中目标的边界框，使用迭代最近点算法计算出相机在当前帧与前一帧之间的运动；

2.根据权利要求1所述的方法，其特征在于，采用颜色直方图将图像的像素在颜色空间中的分布Hist提取出来的步骤包括：

首先，计算HSV颜色空间下的图像矩阵为I(i,j)，

3.根据权利要求1所述的方法，其特征在于，所述步骤6中静态目标有两类，即交通灯与交通标志牌。

4.根据权利要求1所述的方法，其特征在于，步骤3的匹配步骤包括，重复步骤1～2采集场景图像当前帧中目标的边界框，基于SORT算法对前一帧与当前帧中目标的边界框进行建模：X＝[b_x、b_y、b_h、s、b′_x、b′_y、b′_h、s′]^T，s为目标的边界框宽高比，X中后四项为前四项相应的变化率，使用卡尔曼滤波对前一帧中目标的边界框在当前帧中的位置进行预测，前一帧的目标经卡尔曼滤波预测的边界框集合为Prediction＝{P₁，P₂，…,P_m},当前帧的检测结果在边界框的集合为Detection＝{D₁，D₂，…,D_m}，

Σ-¹为边界框的m维协方差矩阵Σ的逆，两帧间目标的边界框的移动距离为move，动态系数为k，则给出动态约束：

将带权二分图对应的邻接矩阵送入KM算法进行匹配，得到边界框的预测结果与当前帧中目标的边界框之间的对应关系。

5.根据权利要求4所述的方法，其特征在于，步骤4中利用图像检索方法将当前帧或当前帧及前一帧中左右目相机采集到的目标的边界框进行匹配找到匹配对的步骤包括：

相似度约束：EMD(sig_i,sig_j)≤T_EMD

P＝{p_l,ω_p1)，(p₂，ω_p2)，…，(p_m，ω_pm)}

Q＝{(q₁，ω_ql)，(q₂，ω_q2），…，(q_n，ω_qn)}

其中p与q分别为P与Q中的特征，是维度相同的向量；ω则表示对应特征的权重，是一个标量，定义一个流动矩阵F，描述P调整到Q需要经历的运输过程，F(i,j)即从p_i到q_j的运输量；

对目标函数的值进行归一化处理，就得到EMD指标：

EMD限值为T_EMD，EMD(P,Q)≤T_EMD；

行对准约束：Δb_h＝b_hi-b_hj,Δb_h≤T_Δbh

对于待匹配的左目中的目标i和右目中目标j，设可容忍的高度差为T_Δbh(0<T_Δbh≤3)，单位为像素；

设可容忍的IoU下限为T_IoU(0.5<T_IoU<1),IoU(i,j)>T_IoU,单位为像素；

建立带权二分图，其对应的邻接矩阵为W，则有：

6.根据权利要求5所述的方法，其特征在于，筛选出左目和右目的匹配对后，还使用相对关系一致约束，使得动点和误匹配最大限度筛除，相对关系一致约束：设左目和右目的匹配对表示为如下集合Match：

Match＝{(L₁，R₁)，(L₂，R₂)，…，(L_n，R_n)}

其中，(x_Ln、y_Ln)表示左目中的目标Ln的边界框的中心坐标，(x_Rn、y_Rn)表示右目中的目标Rn的边界框的中心坐标，x_Li-x_Ln，y_Li-y_Ln表示左目中的目标的边界框的中心坐标(x_Li,y_Li)与(x_Ln、y_Ln)的位置关系，x_Ri-x_Rn，y_Ri-y_Rn表示右目中的目标的边界框的中心坐标(x_Ri,y_Ri)与(x_Rn、y_Rn)的位置关系；

则相对关系一致约束可写作下式：

该式等价于Con中元素符号全为正，对于不符合该式的匹配对，将其删去,剩余的即匹配对，Sgn是数学上的一个符号函数，返回值如果number大于0，则Sgn返回1；等于0，返回0；小于0，则返回-1。

7.根据权利要求6所述的方法，其特征在于，所述总匹配度best通过KM算法计算，其过程如下，

设有一带权二分图G(V,E)，顶点集V＝V_x∪V_Y其中V_x＝{v_x1,v_x2,…v_xm}，V_Y＝{v_y1,v_y2,…v_ym}，(m≤n)，V_x和V_Y分别为二分图两个互不相交且内部不存在边相连顶点的顶点集；边集E＝{e_xi,e_yj|1≤i≤m,1≤j≤n}，设所有顶点的标签值为L，边权重值为ω，V_Y中顶点被匹配所需的最小补偿值为slack，算法描述如下：

步骤S302，为V_xi寻找匹配点，i初值为1；

初始化slack，由于slack为最小补偿值，故令slack(v_yj)＝inf；

若是，则对v_yj的原匹配顶点V_xk执行步骤S302；

遍历结束，匹配仍不成功，减小V_xi的标签，L(v_xi)＝L(v_xi)-drop，drop＝min[slack(v_yj)]重新执行步骤S302；

步骤S303，V_xi匹配成功，对i加1，执行步骤S302；

步骤S305，输出best，算法退出。