基于自适应鲁棒核的单目视觉惯性SLAM方法与系统

基于自适应鲁棒核的单目视觉惯性slam方法与系统
技术领域
1.本发明属于视觉机器人领域，涉及基于自适应鲁棒核的单目视觉惯性slam方法与系统，尤其涉及单目视觉惯性slam中的单目初始化、后端优化以及回环检测部分的优化。


背景技术：

2.随着自动化集成技术、通讯与电子技术、高精度传感器技术的飞速发展，机器人技术己经成为社会生产生活中不可替代的一部分，并且将在未来更加广泛的影响全球的自动化程度。机器人产业已被世界公认为最重要、发展前景最广阔的高新技术产业之一，但不可否认的是，无论是在机器人还是人工智能领域都还存着许多问题等待着我们去探索。因此涌现出许多专家学者致力于“机器人实现对未知环境的感知和探索以及精准实现自身定位问题”的深入研究。
3.同时定位与建图(simultaneous localization and mapping,slam)作为机器感知和导航领域主要的研究热点，深受研究者们的关注。目前slam技术已经应用于机器人导航、无人驾驶、虚拟现实和增强现实以及三维重建等领域。slam作为智能移动机器人的核心技术之一，在gps失效的室内、水下以及隧道等场景中可以生成环境地图，机器人利用地图可以进行自主定位和导航。在无人驾驶领域，slam技术能够定位车道线，达到厘米级的定位精度，从而辅助gps定位。slam技术能够使虚拟物体与现实场景保持几何上的一致性，使叠加的虚拟物体具有真实感，利用slam技术的地图构建功能，还可以获得物体和场景的三维模型。
4.在slam技术发展初期，主要是围绕激光雷达slam。但精度高的激光雷达价格昂贵，精度稍低的激光雷达存在较大误差，极大的影响slam系统的精度，因此没有被广泛应用。相比于激光雷达传感器，相机具有成本低、体积小、质量小等优点，且图像含有丰富的信息。随着硬件性能的提高和计算机视觉的发展，基于视觉传感器的视觉slam简称vslam(visual slam)问世。因视觉传感器价格较低且包含丰富的纹理信息受到了研究者的广泛关注。尽管vslam技术在一般场景下能够取得不错的效果，但是在快速运动、视点变化引起的图像几何变化、光照变化剧烈甚至是无光的环境中表现很糟糕。为了提高slam系统运行的鲁棒性，研究者们开始将研究方向转移到多传感器融合slam，其中最具有代表性的便是视觉惯性slam。vislam是在视觉slam的基础上引入惯性测量单元，不仅可以弥补纯视觉导航系统的缺陷，而且可以提高特征点的匹配速度并增强算法的鲁棒性。
5.如今，虽然视觉slam的研究已在很多方向有了丰硕的成果，但是依然有许多问题需要优化解决，比如在动态场景、纹理缺失以及复杂环境等场景中，视觉slam很难长久稳定的运行。现有单目视觉研究主要存在如下不足：(1)在单目视觉中，由于需要对平移向量进行归一化处理，导致了单目视觉的尺度不确定性，单目初始化无法从单帧得到深度信息，此外单目初始化过程中仅有纯旋转的情况时，单目将无法初始化。(2)在局部建图线程，虽然降低了由于误匹配等原因造成的整体误差过大，但一些较大误差仍然会对结果造成不可预测的影响，无法突破性地提升精度。(3)在视觉slam前端中，不管是进行位姿估计还是建图，
都是利用相邻帧之间的关系来完成的，这种依赖局部约束的算法，必将导致优化误差逐帧累积，最终产生一个较大的误差漂移。研究一个高鲁棒性并且高准确性的视觉slam系统不但对视觉机器人和计算机视觉等领域的科学研究具有重要价值，而且具有广阔的使用范围理论应用，因此对视觉slam技术作进一步的研究具有重要意义。


技术实现要素：

6.发明目的：针对以上现有技术的不足，本发明目的在于提出一种基于自适应鲁棒核的单目视觉惯性slam方法与系统，改善单目初始化过程，采用自适应鲁棒核降低外点的影响，增强优化效果，提高系统的鲁棒性。
7.技术方案：为实现上述目的，本发明采用如下技术方案：
8.一种基于自适应鲁棒核的单目视觉惯性slam方法，主要包括对单目初始化、局部建图和回环检测进行优化，具体包括：
9.在单目初始化时对当前帧与参考帧进行初始特征匹配，并行计算单应矩阵和基础矩阵两个运动模型，并通过几何鲁棒信息含量选择单应矩阵或者基本矩阵，进行模型分解以及光束法平差ba优化；
10.在局部建图线程构建最小化重投影误差的成本函数，采用自适应鲁棒核应用于光束法平差(ba)优化；
11.在回环检测线程利用自适应鲁棒核减少回环校正过程中的累积误差。
12.作为优选，通过几何鲁棒信息含量的模型选择，表示为：
[0013][0014][0015]
其中，i为几何鲁棒信息含量，k表示关键点序列号，l为稳健函数，e表示重投影误差，用于限制重投影误差的大小，β表示模型的大小，其中β＝2为单应矩阵，β＝3为基本矩阵，n为两个关键帧之间跟踪的地图点数量，表示模型中的自由度数，其中表示基本矩阵，表示单应矩阵，θ2表示关键点定位误差的协方差，λ表示数据的维数，两帧之间的二维对应λ＝4。
[0016]
作为优选，自适应鲁棒核的核函数ρ表示为：
[0017][0018]
其中，μ,ω,c为参数，常数c》0用于约束μ＝0附近二次损失区域规模。
[0019]
作为优选，在局部建图线程，最小化重投影误差的成本函数，表示为：
[0020][0021]
其中，r为旋转矩阵，t为平移向量，为所有匹配特征点的集合，ρ为鲁棒核函数，为投影坐标，x
α
为投影点在世界坐标系中的坐标，表示重投影误差的二范数平方和。
[0022]
作为优选，在回环检测线程，自适应鲁棒核函数应用于sim(3)求解，表示为：
[0023][0024]
其中，s
12
为变换矩阵，p
1,i
为关键帧1中的关键点i，p
1,j
为关键帧2中与i匹配的关键点j，p
1,i
为关键点i在世界坐标系的3d坐标，p
2,j
为与关键点i匹配的关键点j在世界坐标系的3d坐标，n为匹配的关键点数量，πi(i＝1,2)表示投影函数。
[0025]
基于相同的发明构思，本发明提供一种基于自适应鲁棒核的单目视觉惯性slam系统，包括：
[0026]
单目初始化模块，用于对当前帧与参考帧进行初始特征匹配，并行计算单应矩阵和基础矩阵两个运动模型，并通过几何鲁棒信息含量选择单应矩阵h或者基本矩阵f，进行模型分解以及光束法平差ba优化；
[0027]
局部建图模块，用于构建最小化重投影误差的成本函数，采用自适应鲁棒核应用于ba优化；
[0028]
以及，回环检测模块，用于利用自适应鲁棒核减少回环校正过程中的累积误差。
[0029]
基于相同的发明构思，本发明还提供一种计算机系统，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述计算机程序被加载至处理器时实现所述的基于自适应鲁棒核的单目视觉惯性slam方法。
[0030]
有益效果：本发明提供的一种基于自适应鲁棒核的单目视觉惯性slam方法，主要针对单目初始化、后端优化以及回环检测部分提出改进方法，主要优点如下：
[0031]
(1)对于单目视觉中的尺度不确定性以及单目相机的纯旋转问题，对单目系统初始化时进行运动估计的方法进行改进，可以解决很少或没有平移时的初始化失败问题，构建初始的点云地图并为之后的计算提供初始值。
[0032]
(2)slam中的很多状态估计都是以最小化重投影误差作为代价函数，一个偏离较大的外点会对估计结果产生巨大的影响，通过采用自适应鲁棒核函数直接优化求解模型参数，通过降低外点的权重，降低错误数据对整体鲁棒性的影响，能够提高系统精度。
[0033]
(3)slam跟踪过程中产生的累积误差，在回环检测线程利用自适应鲁棒核减少回环校正过程中的累积误差。
附图说明
[0034]
图1是本发明实施例的方法流程图。
具体实施方式
[0035]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。
[0036]
如图1所示，本发明实施例提供的一种基于自适应鲁棒核的单目视觉惯性slam方法，首先在图像帧中提取特征点进行特征匹配，寻找初始数据关联，然后对图像进行去畸变处理(只考虑径向畸变)，并行计算单应矩阵模型和基本矩阵模型，通过几何信息含量对运动模型进行选择，然后对选择的运动模型进行外点剔除和分解得到初始运动估计，最后执行全局ba优化初始重建。本发明实施例主要涉及(1)对单目初始化过程进行分析并设计新的模型选择机制；(2)构建最小化重投影误差的成本函数，采用自适应鲁棒核应用于ba优化；(3)利用自适应鲁棒核减少回环校正过程中的累积误差。各步骤具体如下：
[0037]
1.基于自适应鲁棒核的单目视觉惯性slam单目初始化过程，包括步骤：
[0038]
(1)进行初始的特征匹配，即当前帧fc与参考帧fr中的特征匹配：
[0039]
提取当前帧fc中的orb(oriented fast and rotated brief)特征(仅在最精细的比例下)，然后在参考坐标系fr中搜索匹配特征点如果没有找到足够的匹配项，则重置参考坐标系。寻找匹配点，对前两帧图像提取orb特征之后进行特征点匹配，若匹配个数少于100则匹配失败；然后对进行特征匹配的图像帧进行去畸变处理；
[0040]
(2)单应矩阵模型和基础矩阵模型的并行计算，并通过几何鲁棒信息含量选择单应矩阵h或者基本矩阵f；
[0041]
在并行线程中计算一个单应矩阵h
cr
和一个基本矩阵f
cr
：
[0042]
xc＝h
cr
xr#(1)
[0043][0044]
采用几何鲁棒信息含量的模型选择方法，如式(3)所示：
[0045][0046][0047]
其中，g为几何鲁棒信息含量，k表示关键点序列号，l为稳健函数，e表示重投影误差，用于限制重投影误差的大小，β表示模型的大小，其中β＝2为单应矩阵，β＝3为基本矩阵，n为两个关键帧之间跟踪的地图点数量，表示模型中的自由度数，其中表示基本矩阵，表示单应矩阵，θ2表示关键点定位误差的协方差，λ表示数据的维数，两帧之间的二维对应λ＝4。分别计算单应矩阵和基础矩阵的几何鲁棒信息含量，这样，当帧之间很少或没有平移时，选择单应性以更好地表示运动过程，以增强单目初始化过程。
[0048]
(3)对运动模型分解得到初始运动估计；
[0049]
对于单应矩阵h，采用数值法进行分解得到r和t。
[0050]
对于基本矩阵f，使用校准矩阵k将其转换为基本矩阵e：
[0051]
e＝k
t
fk#(5)
[0052]
根据基本矩阵，通过奇异值分解，恢复摄像机运动的旋转矩阵和平移向量：
[0053]
e＝u∑v
t
#(6)
[0054]
其中，u和v为正交矩阵，∑为奇异值矩阵，得到了r和t的四个运动假设，并通过三角剖分得到最佳方案。如果没有足够的解决方案，则初始化失败，并重置参考坐标系。
[0055]
(4)光束法平差；
[0056]
执行ba以优化初始重建。
[0057]
2.基于自适应鲁棒核的单目视觉惯性slam局部建图线程主要过程：
[0058]
(1)处理关键帧；
[0059]
在该过程中，从图像帧序列选取出关键帧，计算关键帧特征点的bow向量，建立关键帧之间的联系，然后将关键帧插入地图。
[0060]
(2)剔除冗余地图点；
[0061]
遍历当前关键帧的所有地图点，对于不满足条件的地图点将其剔除。
[0062]
(3)添加新的地图点；
[0063]
相机运动过程中与相邻关键帧可以通过三角化恢复出一些地图点。
[0064]
(4)如果当前关键帧是关键帧队列中的最后一帧，则进行局部ba。
[0065]
进一步地，得到最小化重投影误差的成本函数：
[0066]
地图点三维位置旋转和关键帧位姿t
iw
∈se(3)，其中se(3)为特殊欧式群，w代表世界坐标系，对匹配关键点的重投影误差进行优化。在关键帧i中观察地图点j的误差项为：
[0067]ei,j
＝x
i,j-πi(t
iw
,x
w,j
)#(7)
[0068]
其中，πi表示投影函数：
[0069][0070]
[x
i,j y
i,j z
i,j
]
t
＝r
i,w
x
w,j
+t
i,w
#(9)
[0071]
式中r
iw
∈so(3)和分别是t
iw
的旋转和平移部分，so(3)为特殊正交群，(f
i,u
,f
i,v
)和(c
i,u
,c
i,v
)是与第i帧相机相关联的焦距和主点，u和v表示对图像进行去畸变处理。最小化重投影误差的成本函数为：
[0072][0073]
式中ρ为huber核函数，为投影坐标，x
α
为投影点在世界坐标系中的坐标，为所有匹配特征点的集合。在全局ba的情况下，优化所有点和关键帧，但第一个关键帧保持固定作为原点的情况除外。在局部ba中，局部区域中包含的所有点都将得到优化，而关键帧的子集是固定的。在“位姿优化”或“仅运动ba”中，所有点都是固定的，并且只优化相机位姿。
[0074]
自适应鲁棒核应用于ba优化：
[0075]
在ba优化中，将最小化误差项的二范数平方和作为目标函数，如果出现较大的误差项会对其他正确值产生影响，鲁棒核函数将原来误差的二范数度量替换成一个增长没有那么快的函数，同时保证的光滑性质，从而使得整个优化结果更为稳健。自适应鲁棒核如下
式：
[0076][0077]
通过对ω的取值进行分析，最终的鲁棒核函数为：
[0078][0079]
其中，μ,ω,c为参数，ω用于控制鲁棒核形状，c一般为常数，c》0是用于约束μ＝0附近二次损失区域规模的规模参数。通过调整ω在一系列鲁棒核之间进行插值。为了通过参数ω自动确定最佳核形状，将ω视为一个额外的未知参数，同时最小化误差：
[0080][0081]
通过选择ω，在不影响模型参数r和t的情况下，将所有残差加权为较小的值，并最小化重投影误差。
[0082]
(5)剔除冗余关键帧，降低系统计算量，之后将选取的关键帧加入到回环检测线程进行回环校正。
[0083]
3.基于自适应鲁棒核的单目视觉惯性slam回环检测线程中自适应鲁棒核应用于回环校正的相似变换sim(3)计算：
[0084]
(1)检测关键帧序列是否产生回环；
[0085]
(2)对检测到的回环候选关键帧计算相似变换矩阵；
[0086]
给定关键帧1和关键帧2，以及关键帧1、2和与关键点关联的3d贴图点中的一组n个匹配关键点，对于每对匹配点，查找以下变换关系：
[0087]
p
2,j
＝srp
1,j
+t#(13)
[0088]
其中s是比例因子，r是旋转矩阵，t是平移向量。事实上，数据不可避免地会有噪声和误差，定义误差e1,e2，找到合适的比例因子组合，旋转和平移使其对所有数据的误差最小。优化相对sim(3)变换矩阵以最小化两幅图像中的重投影误差：
[0089]
e1＝p
1,i-π1(s
12
,p
2,j
)#(14)
[0090][0091]
自适应鲁棒核函数应用于sim(3)和成本函数，以最小化：
[0092][0093]
通过上式得到满足上述关系的比例因子、旋转矩阵、平移向量以及参数ω，从而优
化整个闭环，从而使闭环误差最小化。
[0094]
(3)回环校正。构建当前关键帧的连续组，并根据计算出的相似变换矩阵校正连续组中关键帧的位姿；根据更新后的相机位姿(相似变换矩阵)将相邻关键帧的所有地图点都重新计算地图点世界坐标，然后进行地图点融合，进行全局ba优化。
[0095]
本发明与orb-slam3在kitti数据集的实验结果对比的标准差(std/m)如下表，通过对比实验结果可以看到，本发明定位精度优于orb-slam3。
[0096][0097][0098]
基于相同的发明构思，本发明实施例提供一种基于自适应鲁棒核的单目视觉惯性slam系统，包括：单目初始化模块，用于对当前帧与参考帧进行初始特征匹配，并行计算单应矩阵和基础矩阵两个运动模型，并通过几何鲁棒信息含量选择单应矩阵h或者基本矩阵f，进行模型分解以及光束法平差ba优化；局部建图模块，用于构建最小化重投影误差的成本函数，采用自适应鲁棒核应用于ba优化；以及，回环检测模块，用于利用自适应鲁棒核减少回环校正过程中的累积误差。各模块的具体实现参考上述方法实施例，此处不赘述。
[0099]
基于相同的发明构思，本发明还提供一种计算机系统，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述计算机程序被加载至处理器时实现前述的基于自适应鲁棒核的单目视觉惯性slam方法。