CN112991436B - 基于物体尺寸先验信息的单目视觉slam方法 - Google Patents

Abstract

本发明公开了一种基于物体尺寸先验信息的单目视觉SLAM方法，通过目标检测方法获取物体语义信息，并利用先验物体尺度信息降低单目视觉SLAM过程中的尺度不确定性，利用帧间物体信息的轨迹平滑模块在前端检测到新物体时构建局部地图来减少局部地图漂移。结合物体尺度先验信息的集束调整位姿优化框架通过增加物体尺度先验这一约束，联合路标点和相机位姿同时优化，提高系统整体建图精度。该方案仅利用单目相机作为唯一传感器，成本低，且系统可移植性强。

Description

基于物体尺寸先验信息的单目视觉SLAM方法

技术领域

本发明涉及智能机器人技术领域，尤其涉及一种基于物体尺寸先验信息的单目视觉SLAM方法。

背景技术

随着科技的发展，机器人与人们生活的关系越来越密切。同步定位与建图(Simultaneous localization and mapping,SLAM)技术是机器人进行自主环境探索与导航的核心技术。单目视觉SLAM系统是将单目相机作为主要的外部传感器的SLAM系统。相比于双目相机和RGB-D相机，单目相机具有价格便宜，外形紧凑以及安装方式灵活的特点，因此普遍应用于当前的视觉SLAM系统中。单目视觉SLAM近年来受到了广泛关注并得到了很大的发展。然而，单目视觉SLAM中由于传感器自身特性导致无法直接从图片中得到场景深度，即单目SLAM的尺度不确定性问题。同时，视觉SLAM的里程计通常通过相邻帧之间的帧间匹配来估计出粗略的相机运动，为后端提供较好的初始值。但是里程计会不可避免的出现误差累计，导致构建的地图在长时间内是不准确的，即单目视觉SLAM的尺度漂移问题。而场景的绝对几何尺度的准确感知对于移动机器人探索环境和执行任务具有重要意义。

因此，现有的单目视觉SLAM依然存在的问题有：

1)尺度恢复方法适用范围受限，泛化性低，存在移植性低的问题。

2)现有的基于物体的SLAM系统计算负担大，实时性难以保证，无法满足实际应用中机器人对场景准确尺度感知的需求。

发明内容

本发明的目的是提供一种基于物体尺寸先验信息的单目视觉SLAM方法，充分考虑机器人运行的实时性与高精度的要求，实现了高精度的实时在线尺度恢复，适用面广，可移植性强。

本发明的目的是通过以下技术方案实现的：

一种基于物体尺寸先验信息的单目视觉SLAM方法，包括：

对于单目相机采集的图像，通过目标检测方法获得物体语义信息，以及通过基于SLAM系统的里程计，获得SLAM系统产生的随机地图尺度下的单目相机位姿与路标点空间坐标；利用物体边框信息提取位于物体表面的路标点空间坐标，并结合物体先验尺寸，计算初始地图真实尺度，完成SLAM系统地图尺度的初始化；

利用相机位姿、路标点空间坐标以及地图尺度构建基于物体语义信息的后端集束调整优化框架，并对相机位姿、路标点空间坐标以及初始地图真实尺度进行联合优化后进行同步定位与建图。

由上述本发明提供的技术方案可以看出，通过目标检测获取物体边框信息，并利用先验物体尺度信息降低单目视觉SLAM过程中的尺度不确定性，利用帧间物体信息的轨迹平滑模块在前端检测到新物体时构建局部地图来消除局部地图漂移。结合物体尺度先验信息的集束调整位姿优化框架通过增加物体尺度先验这一约束，联合路标点和相机位姿同时优化，提高系统整体建图精度。该方案仅利用单目相机作为唯一传感器，成本低，且系统可移植性强。

附图说明

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

图1为本发明实施例提供的一种基于物体尺寸先验信息的单目视觉SLAM方法的流程图；

图2为本发明实施例提供的目标检测结果示意图；

图3为本发明实施例提供的车辆建模结果示意图；

图4为本发明实施例提供的SLAM系统的后端集束调整框架的优化过程示意图。

具体实施方式

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

为了克服以单目相机作为唯一传感器的移动机器人在场景探索中尺度感知方面存在的不足，本发明实施例提供一种基于物体尺寸先验信息的单目视觉SLAM方法，该方案充分考虑机器人运行的实时性与高精度的要求，实现了高精度的实时在线尺度恢复，同时适用面广，可移植性强；如图1所示，该方法主要包括：

对于单目相机采集的图像，通过目标检测方法获得物体语义信息，以及通过基于多视图几何的SLAM系统的里程计，获得SLAM系统产生的随机地图尺度下的单目相机位姿与路标点空间坐标；利用物体边框信息提取位于物体表面的路标点空间坐标，并结合物体先验尺寸，计算初始地图真实尺度，完成SLAM系统地图尺度的初始化；利用相机位姿、路标点空间坐标以及地图真实尺度构建基于物体语义信息的后端集束调整优化框架，并对相机位姿、路标点空间坐标以及初始地图真实尺度进行联合优化后进行同步定位与建图。

相较于现有技术中的单目视觉SLAM系统仅利用特征点坐标以及相机位姿构建重投影误差函数，通过最小化重投影误差来得到优化后的相机位姿以及空间点坐标的方案而言，本发明实施例上述方案引入了地图尺度这一新变量，通过重新构造误差函数来实现对地图真实尺度，相机位姿以及空间点坐标的共同优化，从而提高了视觉SLAM系统的整体精度。

为了便于理解，下面针对整个方案的各个步骤做详细的介绍。

一、目标检测线程。

本发明实施例中，所述目标检测方法可以通过目标检测网络实现，物体语义信息包括：物体边框的几何信息以及物体所属种类。

本发明实施例中，以基于多视图几何的视觉SLAM系统为基础，如ORB-SLAM2，在其前端融合了目标检测网络来提取图片中的物体语义信息。

示例性的，可以采用目标检测框架YOLO进行目标检测，它是一种快速紧凑的开源对象检测模型，其在保证目标检测准确率的前提下同时能保证单目视觉SLAM系统的实时性。图2所示即为YOLO在KITTI 07里程计序列上输入单帧图片的检测结果。图片输入YOLO网络进行目标检测后，输出图片中的检测到的物体在像素坐标系下边框(bounding box)左上角坐标、边框的宽和高(物体边框的几何信息)、以及物体边框所属种类(例如，Car、person)。

二、跟踪线程。

将单目相机采集的图像转换为灰度图，提取ORB特征点，在当前SLAM系统产生的随机地图尺度下，通过帧间特征点匹配以及对极几何和对特征点三角化求得单目相机位姿与路标点空间坐标。利用位于物体表面的路标点与物体的bounding box(边框)在同一平面这一约束，结合物体先验尺寸，恢复出地图的绝对尺度，完成地图初始化，其原理如下：

单目ORB-SLAM2初始过程中会随机给出一个地图尺度，该随机地图尺度和地图真实尺度通常差距较大，本发明在初始化过程中通过利用场景中物体尺度先验信息为SLAM系统初始化提供一个相对精确的地图尺度。具体来说，利用物体边框信息提取位于物体表面的路标点空间坐标，计算出该物体在随机地图尺度下的物体高度h_orb，结合预先建模的物体先验尺寸h_prior，计算初始地图尺度s：

根据初始始地图尺度s完成对地图真实尺度的初始化，即将s作为初始地图真实尺度。

本发明实施例中，所述单目相机可以为鱼眼摄像头，利用棋盘格标定法对鱼眼摄像头进行相机内参标定，获取摄像头的畸变系数(可用于对图像进行畸变校正)，同时储存下鱼眼摄像头的内部参数。

本发明实施例中，通过预先对物体尺度建模来提供物体尺寸先验，具体的，可以统计场景中可利用物体的尺寸，通过拟合尺寸的分布建模物体尺度。以KITTI里程计数据集为例，可以观察到无论市区，乡村，还是高速公路来说，车辆该物体在场景中会经常出现，图3是通过对KITTI里程计数据集中的小型车辆长度统计建模得到的结果，通过图3可以看到，小型车辆的长度满足高斯分布，均值μ_l＝2.26，方差

同理车辆高度的分布满足均值μ_h＝0.78，方差σ_h ²＝0.04的高斯分布，宽度的分布满足均值μ_w＝0.91，方差

的高斯分布。

三、后端优化框架。

本发明实施例中，将地图尺度这一变量融入至SLAM系统的后端集束调整框架(Bundle Adjustment，BA)中，利用场景物体尺寸约束通过构建图优化模型来对以上三个变量进行联合优化，通过海森矩阵的稀疏性加速计算，减少计算资源。

具体来说，结合这个三个变量构建代价函数，通过最小化代价函数来对单目相机位姿，路标点空间坐标以及初始地图真实尺度进行优化，从而整体提高视觉SLAM的定位与建图精度。

本发明实施例中，将任一路标点P_i的空间坐标记为P_i＝[X_i,Y_i,Z_i]^T，由下式投影到相机像素平面：

其中，K是单目相机内参矩阵(也即前文提到的内部参数)，T_w是单目相机位姿，u和v是P_i在相机坐标系下的坐标，d_i是路标点深度。

根据投影方程得到：

P_i＝K^-1T_wsp_i

其中，s为初始地图真实尺度，p_i是P_i在像素坐标系下的坐标点；

将目标检测方法得到的某一物体边框O的高度与长度分别记为h和l，像素坐标系下物体bounding box的单侧上下两个(左侧上下两个或者右侧上下两个)顶点p_i1与p_i2对应的路标点空间坐标为P_i1，P_i2，根据空间中两点距离公式有：

将第w帧中的路标点空间点记为Ρ＝{P_iw}，P_iw表示在第w帧观测到的第i个路标点，第w帧中单目相机位姿记为Γ＝{T_w}，记第w帧中当前类物体的高度先验为h₀其满足h₀～N(h,σ²)的正态分布，关于物体尺度的误差函数为：

e₂＝h₀-h(Γ,p)

联合上面两个式子，得到：

其中，h_n为第w帧中第n类物体在当前SLAM系统下的高度，p₁与p₂为像素坐标系下第w帧中当前类物体边框单侧上下两个顶点；

联合路标点和相机位姿的重投影误差代价函数e₁，整体的目标优化函数为：

本领域技术人员可以理解，重投影误差代价函数e₁是传统的基于特征点的SLAM的目标误差函数，具体可参照常规技术。

上式中，只有e₂是相对于物体尺寸相关的误差，e₂对单目相机位姿的偏导，根据链式法则有：

利用高斯牛顿法或者L-M法对整体的目标优化函数进行迭代求解，同时利用海森矩阵的稀疏性来加速计算。由于单帧中的物体数量相对路标点数量来说很少，因此在SLAM后端增加物体尺寸这一变量不会带来过多计算负担，保障了SLAM系统的实时性，同时由于增加了物体尺寸信息约束，使得SLAM后端精度得到了整体的提升。图4展示了本方案中前端与后端结合后的新集束优化框架，图4中的CNN为目标检测网络。

四、轨迹平滑。

ORB-SLAM2的轨迹优化部分分为局部BA以及全局BA，其中考虑到全局优化下，关键帧相机位姿以及地图点过多的情况下，为了加速计算，ORB-SLAM2提出了一种较为精简的全局优化框架。但是全局优化只在检测到闭环的情况下才会对整体的对相机位姿以及路标点坐标集中优化，但现实中回环的情况取决于机器人所处的具体环境，因此利用闭环检测对相机位姿进行优化的方法使用场景有限。局部BA过程中，当新的物体信息添加到集束调整框架中时会根据物体尺寸先验对当前帧以及其共视帧的位姿进行局部BA调整，但是该优化并未作用在上一次局部BA中，此时便会出现当前优化后的相机位姿与未被有花的相机位姿之间轨迹不连续的情况，可称之为断点帧。

针对上述情况，本发明提出通过利用基于帧间物体语义信息的轨迹平滑算法消除局部地图漂移以及消除断点帧，保持SLAM系统建图的全局一致性，原理如下：当发现新物体并由基于物体尺寸的局部BA算法得到更新后的相机位姿，路标点空间坐标以及地图真实尺度。通过比较当前帧中局部地图的地图真实尺度与前一帧中局部地图的地图真实尺度，如果差值大于设定的阈值，则当前帧即为断点帧。通过提取断点帧优化前后的相机位姿，得到优化前后的相机位姿差，然后将该误差通过一次位姿图优化(位姿图优化技术是已有的)达到轨迹平滑的效果。

本发明实施例中，局部地图的尺度即为本发明上述方案更新后的地图真实尺度，局部地图是指包含相应物体的地图区域；上述判断所设定的阈值大小可根据实际情况或者经验自行设定，本发明不做数值大小做出限定。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例可以通过软件实现，也可以借助软件加必要的通用硬件平台的方式来实现。基于这样的理解，上述实施例的技术方案可以以软件产品的形式体现出来，该软件产品可以存储在一个非易失性存储介质(可以是CD-ROM，U盘，移动硬盘等)中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述的方法。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书的保护范围为准。

Claims (6)

1.一种基于物体尺寸先验信息的单目视觉SLAM方法，其特征在于，包括：

对于单目相机采集的图像，通过目标检测方法获得物体语义信息，以及通过基于SLAM系统的里程计，获得SLAM系统产生的随机地图尺度下的单目相机位姿与路标点空间坐标；利用物体边框信息提取位于物体表面的路标点空间坐标，并结合物体先验尺寸，计算初始地图真实尺度，完成SLAM系统地图尺度的初始化；

利用相机位姿、路标点空间坐标以及地图尺度构建基于物体语义信息的后端集束调整优化框架，并将单目相机位姿、路标点空间坐标以及初始地图真实尺度，融合至SLAM系统进行联合优化；

所述将单目相机位姿、路标点空间坐标以及初始地图真实尺度，融合至SLAM系统进行联合优化包括：

在SLAM系统的后端集束调整框架中融入单目相机位姿、路标点空间坐标以及初始地图真实尺度这三个变量，结合这个三个变量构建代价函数，通过最小化代价函数来对单目相机位姿，路标点空间坐标以及初始地图真实尺度进行优化；

将任一路标点P_i的空间坐标记为P_i＝[X_i,Y_i,Z_i]^T，由下式投影到相机像素平面：

其中，K是单目相机的内部参数，T_w是单目相机位姿，u和v是P_i在相机坐标系下的坐标，d_i是路标点深度；

根据投影方程得到：

P_i＝K^-1T_wsp_i

其中，s为初始地图真实尺度，p_i是P_i在像素坐标系下的坐标点；

将目标检测方法得到的某一物体边框O的高度与长度分别记为h和l，像素坐标系下物体边框的单侧上下两个顶点p_i1 p_i2对应的路标点空间坐标为P_i1，P_i2，根据空间中两点距离公式有：

将第w帧中的路标点空间点记为P＝{P_iw}，P_iw表示在第w帧观测到的第i个路标点，第w帧中单目相机位姿记为Γ＝{T_w}，记第w帧中当前类物体的高度先验为h₀其满足h₀～N(h,σ²)的正态分布，关于物体尺度的误差函数为：

e₂＝h₀-h(Γ,p)

联合上面两个式子，得到：

其中，h_n为第w帧中第n类物体的高度先验，p₁与p₂为像素坐标系下第w帧中当前类物体边框单侧上下两个顶点；

联合路标点和相机位姿的重投影误差代价函数e₁，整体的目标优化函数为：

上式中，只有e₂是相对于物体尺寸相关的误差，e₂对单目相机位姿的偏导，根据链式法则有：

利用高斯牛顿法或者L-M法对整体的目标优化函数进行迭代求解，同时利用海森矩阵的稀疏性来加速计算。

2.根据权利要求1所述的一种基于物体尺寸先验信息的单目视觉SLAM方法，其特征在于，对于单目相机，利用棋盘格标定法对摄像头进行相机内参标定，获取摄像头的本质矩阵以及畸变系数，同时储存下摄像头的内部参数。

3.根据权利要求1所述的一种基于物体尺寸先验信息的单目视觉SLAM方法，其特征在于，所述物体语义信息包括：物体边框的几何信息以及物体所属种类。

4.根据权利要求1所述的一种基于物体尺寸先验信息的单目视觉SLAM方法，其特征在于，所述通过基于SLAM系统的里程计，获得SLAM系统产生的随机地图尺度下的单目相机位姿与路标点空间坐标包括：

将单目相机采集的图像转换为灰度图，提取ORB特征点，在当前SLAM系统产生的随机地图尺度下，通过帧间特征点匹配以及对极几何和对特征点三角化求得单目相机位姿与路标点空间坐标。

5.根据权利要求1所述的一种基于物体尺寸先验信息的单目视觉SLAM方法，其特征在于，所述利用物体边框信息提取位于物体表面的路标点空间坐标，并结合物体先验尺寸，计算初始地图真实尺度，完成SLAM系统地图尺度的初始化包括：

利用物体边框信息提取位于物体表面的路标点空间坐标，计算出在随机地图尺度下的物体高度h_orb，结合预先建模的物体先验尺寸h_prior，计算初始地图真实尺度s，完成SLAM系统地图尺度的初始化：

6.根据权利要求1～5任一项所述的一种基于物体尺寸先验信息的单目视觉SLAM方法，其特征在于，该方法还包括：通过利用基于帧间物体信息的轨迹平滑算法，来保持SLAM系统建图的全局一致性，步骤如下：

当前帧是否为断点帧，判断方式为：对于当前帧中的物体，计算出更新后的相机位姿、路标点空间坐标以及地图真实尺度后，比较当前帧的地图真实尺度与前一帧的地图真实尺度，如果差值大于设定的阈值，则当前帧为断点帧；

如果当前帧为断点帧，则通过提取断点帧优化前后的相机位姿，得到优化前后的相机位姿差，然后将该相机位姿差通过一次位姿图优化实现轨迹平滑。

Images