0 引言
同步定位与建图技术(simultaneous localization and mapping,SLAM)目前是机器人与计算机视觉研究热门方向,它在自动驾驶汽车、移动机器人、增强现实(AR)、无人机等领域有非常广泛的使用,例如自动驾驶汽车对安全稳定性的要求比较高,但仅使用RTK(实时动态载波相位置差分系统)在隧道、高楼密集处等场景定位信号弱甚至容易丢失,必须依赖SLAM技术来进行局部定位,同时SLAM技术还可以提供六自由度的位姿,对车辆的行驶控制也起着关键性的作用。
SLAM分为基于激光雷达的激光SLAM与基于视觉传感器的视觉SLAM,由于激光雷达成本过高,因此视觉SLAM成为研究热点,但仅依靠单目相机作为单一传感器存在尺度模糊等问题,这会限制其应用(如ORB-SLAM2[1]),通过引入惯性测量单元(IMU)能够很好的解决这一问题,二者具有很强的互补性,除了可以确定绝对尺度外还可以准确地估计并修正漂移。将二者进行紧耦合能够获得较好的效果,常见的视觉惯性SLAM算法有基于滤波的MSCKF[2]和基于非线性优化的OKVIS[3]、VINS-Mono[4]、ORB-SLAM3[5]、ICE-BA[6]等。
目前,在视觉惯性SLAM算法领域,VINS-Mono是一个完整的单目视觉惯导系统,有效地解决了单目尺度不确定性和IMU积分漂移的问题,具有极高的研究和应用价值,因此选择以此为基本框架。VINS-Mono前端采用直接法,虽省去了计算特征点与描述子的时间,但由于假设了灰度不变,易受光照影响,且IMU的bias在预积分中持续发散,使系统易在静止或有较大外力作用的情况下产生轨迹漂移,在很多场景易影响整个SLAM系统的鲁棒性和定位精度。针对上述问题,本文中在视觉里程计(前端)与IMU初始化部分进行优化。视觉里程计作为SLAM框架中通过视觉传感器提取数据的部分,对估算相邻图像间的运动有着重要的作用。文献[7]在前端部分通过点特征与线特征融合对SLAM系统参数进行初始化,解决了弱纹理下特征点难提取的问题。文献[8]对ORB特征做出改进,提出了一种对关键帧采用特征点提取,非关键帧采用光流法的算法来节省提取关键点和描述子的时间。文献[9]基于ORB-SLAM2算法改进,通过对运动模型的估计将初始关键点与光流跟踪进行一一对应,并利用BA改进对应关系,但在运行速度上为ORB-SLAM2的2倍。文献[10]提供了一种基于解耦图优化的传感器融合方法用来推断视觉里程计的位姿。文献[11-13]通过引入GNSS来辅助视觉里程计的测量及系统的定位精度。文献[14]提出了一种改进的测量灰度角的方法,从而使得测量问题更容易处理。文献[15]在VINS-Mono的基础上将特征点的三角化进行改善,使其能够支持双目相机从而解决尺度问题。
结合上述文献对视觉里程计的优化及存在的一些问题,本文中算法对加速度零偏和角速度零偏进行修正,能够在更短的时间内精确地计算出陀螺仪和加速度计的偏差,同时引入开源视觉SLAM框架ORB-SLAM2,应用ORB-SLAM2前端特征点法替换VINS-Mono前端直接法。通过计算FAST角点与BRIEF描述子来提取特征点,由于特征点本身对环境敏感程度较低,在某些场景下可以有效的改善系统鲁棒性并提高一定的定位精度。
1 IMU初始化设计
1.1 陀螺仪偏差估计
为了方便描述本体在三维空间的运动。定义几个常用的坐标系如图1所示。定义IMU坐标系与本体坐标系重合,TBC表示由相机坐标系到IMU坐标系的变换矩阵,由RBC以及TBC所组成。TWB表示由本体坐标系到世界坐标系的变化矩阵,其中RWB代表旋转部分,WP代表平移部分。
图1 坐标系定义
Fig.1 Coordinate system definition
陀螺仪的偏差可以仅从2个连续关键帧的已知位置变换估计出来。假设偏差变化可以忽略不计,则对于所有连续的关键帧,可以假设一个恒定不变的陀螺仪偏差bg,使得计算得到的陀螺仪整体偏差和已知位置变换之间的差异最小化,通过相邻两帧的旋转矩阵对应的李代数差值可以解耦出bg的大小,具体计算公式如下:
(1)
式(1)中:N表示关键帧的数量; RWB=RWCRCB,RWC表示从相机坐标系到世界坐标系的旋转变换矩阵,由SLAM系统计算获得,RCB表示从IMU坐标系到相机坐标系的旋转变换矩阵,由相机标定获得;ΔRi,i+1是2个连续关键帧之间的陀螺仪积分。可以利用非线性高斯-牛顿法来求解(1),从而得到bg的值。
1.2 尺度与重力的估计
当我们估计出了陀螺仪偏差bg,可以结合IMU测量的加速度与角速度,将其代入预积分公式,这里不展开阐述IMU预积分原理,计算出相邻两帧之间的旋转角度与位移量,从而构建相邻帧之间的位姿约束,如图2所示。
图2 预积分示意图
Fig.2 Schematic diagram of pre-integrals
由于VINS-Mono系统计算的相机轨迹的尺度具有不确定性,因此在相机坐标系与IMU坐标系之间进行转换时,需要包含一个比例因子sw。其中相机坐标与IMU坐标系转换的公式如下:
WPB=sWPC+RWCCPB
(2)
通过IMU预积分可知,相邻关键帧之间的运动关系公式如下:
(3)
将2个连续关键帧的运动关系方程(3)代入相机与IMU坐标系转换的公式(2),并忽略该点加速度计的偏差,得到:
(4)
式(4)目的是通过求解关于这些变量的线性方程组来估计sw和重力向量gw。由于式中含有速度变量,为了降低计算复杂度,避免求解N个速度,对方程进行如下变换。利用3个连续关键帧的数据代入式(4)可以建立两组线性方程,将相邻关键帧之间的运动关系(3)中的速度关系代入式(4)进行相关替换从而消去速度变量,再将式(4)写成矩阵形式得到表达式(5):
(5)
将关键帧符号i、i+1、i+2用数字1、2、3来替代可得到式(6):
(6)
然后可以将3个连续关键帧关系式(5)组成一个线性系统A3(N-2)×4 x4×1 = B3(N-2)×1,这个系统可以通过奇异值分解(SVD)来求解,从而得到比例因子sw和重力向量gw。另外,由于该线性系统有3(N-2)个方程和4个未知数,因此至少需要4个关键帧才能求解。
1.3 加速度计偏差估计,尺度、重力方向修正
到目前为止,在计算尺度和重力方向时,都还没有考虑加速度计的偏差。若直接在(4)中加入加速度计偏差将极大地增加系统条件不稳定的可能性,因为重力和加速度计偏差很难区分。为了增加可观测性,引入到目前为止没有考虑的新信息,即重力大小G。已知重力方向指向地心,认为惯性坐标系I下的重力方向
再根据已经计算的重力方向
可得到惯性坐标系到世界坐标系的旋转矩阵RWI,计算方法如下:
(7)
重力方向的表达式可表示为
(8)
RWI可以仅需计算绕X轴与Y轴的角度的参数,因为绕Z轴旋转,将Z轴与重力方向对齐,对gW没有影响。这种旋转可以用扰动δθ来进行优化,见式(9):
(9)
用一阶近似式表达见式(10):
(10)
将式(10)代入式(4)并加入加速度计偏差的影响可以得到:
(11)
与式(5)相同,考虑3个连续的关键帧即可消去速度变量,得到如下关系:
(12)
式(12)中λ(i)与式(6)相同,并且φ(i)、ζ(i)和ψ(i)的计算如下:
(13)
其中,(:,1:2)表示矩阵的前两列。与1.2节类似,将3个连续关键帧关系式(12)组成形式为A3(N-2)×6x6×1 = B3(N-2)×1的线性系统,该系统有3(N-2)个方程和6个未知数,所以再次需要至少4个关键帧来求解该系统。通过奇异值分解(SVD)来求解,从而得到校正后的比例因子sw,重力方向δθxy和加速度计偏差ba。另外可以计算条件数(即最大和最小奇异值之比)来检查问题是否合理地解决,如果条件数小于一定的阈值,可以重新线性化(10)并迭代来解决问题。
1.4 速度估计
根据上述小节,我们已经计算出陀螺仪偏差、尺度大小、重力向量、加速度计偏差,但在方程(5)和(12)中利用3个连续关键帧的关系计算时消去了速度变量,因此得到的线性系统没有对应于速度的3N个变量。由于sw、gw、bg、δθxy、ba均是已知的,因此所有关键帧的速度可以代入式(11)计算出来,若计算相近关键帧的速度,使用式(3)中的速度关系即可。
2 改进的视觉里程计
2.1 ORB特征提取
VINS-Mono算法前端采用的直接法受光照、环境等影响,为增强其鲁棒性,本文中将VINS-Mono算法前端改为改进的ORB特征提取法,ORB特征点的组成是FAST关键点和BRIEF描述子,BRIEF描述子为特征点增加了方向,从而可以提取面向多尺度的FAST角点,它们具有极高的计算和匹配速度,同时对视点具有良好的不变性。这使我们能够将它们与宽基线相匹配,提高了BA的准确性。
FAST角点的检测原理是选择图像中的某一像素点P,将它的灰度值假设为I。后设定一个阈值T,用来判断2个像素点灰度值的差异,一般采用I的20%用来自适应不同的图像。以像素P为中心,作半径为3的圆,在圆上选取16个像素点,如图3所示。这16个像素点中有连续的9个点灰度值在阈值范围外则可以确定为关键点,后重复以上步骤即可。
图3 FATS角点提取
Fig.3 FAST corner extraction
在进行图像的特征提取的时候,ORB特征点采用图像金字塔的方法来保证特征点的尺度不变性,如图4所示,在提取ORB特征点的时候,我们会在每一层级的金字塔上来进行特征提取。这样可以保证在任何距离下的相机都能正确的识别出角点,实现尺度的不变性。另外可以采用灰度质心法计算特征点方向来保证每个特征点不受旋转的影响,即特征点的旋转不变性。
图4 图像金字塔
Fig.4 Image pyramid
2.2 BRIEF描述子计算
关键点检测出之后,ORB会利用BRIEF描述子为每个关键点计算相应的特征向量,BRIEF是一种二进制编码的描述子,仅包含0和1向量,计算的步骤是先对图像进行高斯滤波来减少噪声的干扰,以关键点作为中心,选取一定大小的窗口,在窗口内随机选取256对随机点,比较二者像素大小并赋值为0和1,从而得到一个256维的二进制描述子。再利用前面计算的关键点的主方向旋转描述子从而获得旋转不变性。另外计算ORB特征点时可采用上述所示图像金字塔或四叉树进行特征点均匀化,从而提高SLAM系统的整体精度。
3 实验与结果分析
本文实验中选择主流的VIO算法VINS-Mono与本文中改进算法来进行对比实验。实验基于著名公开数据集EuRoc进行精度分析与验证。在EuRoc数据集中评估了本文中提出的IMU初始化方法和以及改进视觉里程计后的SLAM算法。它包含了在2个不同房间和一个工业环境中飞行的微型飞行器(MAV)记录的11个序列。根据光照、纹理、快/慢运动或运动模糊程度,序列分为简单、中等和困难3种。它是测试视觉-惯性SLAM系统的一个常用的数据集。本实验是在一台24 GB RAM的Intel Core i7-4700MQ计算机上进行的,只处理左侧图像。另外,本文中利用已有实验平台进行数据采集,验证算法在真实场景下的鲁棒性。
3.1 实验平台及运行效果
如图5所示,本文中实验平台为一款视觉影像RTK系统,该系统由CMOS图像传感器PS5268、博世SMI130型号IMU及司南卫星导航T300PLUS款RTK模块构成。
图5 实验平台
Fig.5 Experimental platform
实验场景选择上海司南卫星导航公司顶部,将手持设备固定在对中杆上,将终端与设备连接,输入相应的指令开始采集数据,由于相机采集数据早于IMU采集数据时间,所录制数据包早期图片并没有IMU数据,需对数据进行预处理,将图片数据与IMU数据进行对齐,舍弃无IMU数据的图片。实验采集到的场景如图6所示。利用本文中的算法进行实验,可以准确地识别场景信息,系统具有较好的鲁棒性,如图7所示。
图6 上海司南卫星导航公司大楼顶部
Fig.6 The top of the building of Shanghai Sinan Satellite Navigation Company
图7 改进算法系统运行效果
Fig.7 The operation effect of improved algorithm system
3.2 IMU初始化测试
本文中选择EuRoc序列V2_01来评估IMU初始化,每当系统插入关键帧时,从头开始运行初始化。以较低的帧率来运行序列,这样重复的初始化不会干扰系统的正常行为。目标是分析当初始化算法处理更多的关键帧,即更长的轨迹时,变量的收敛性。图8给出了式(12)的条件数,显示在10 s 之后明显下降并开始收敛,表明新的初始化方法需要一定时间,加速度计偏差与陀螺仪偏差也会在10 s后开始逐渐收敛且与估计值接近。图9表示了IMU初始化所需具体时间,近似呈线性增长,系统具有较好的鲁棒性,IMU初始值得到了很好的估计,系统能观性高。
图8 系统条件数
Fig.8 System condition number
图9 IMU初始化时间
Fig.9 IMU initialization time
3.3 SLAM系统测试
为了精确评估改善后SLAM系统的性能,本文中在EuRoC数据集的11个序列中评估了改进前后的VINS-Mono系统的精度。将本地BA的窗口大小设置为10个关键帧,IMU初始化后,系统将执行全BA操作。为测试系统效果,首先在VINS-Mono与改进后的VINS-Mono系统上分别运行MH_01序列,并在Rviz上显示效果,如图10所示。我们能够看到算法改进前后对于特征点提取的对比,改进后算法相比于原算法在场景复杂区域所提取的特征点更加密集且丰富。
图10 改进前后系统特征提取效果对比
Fig.10 Comparison of the feature extraction effect of system before and after improvement
实验采用评估工具evo来对改进前后算法在数据集下的运行效果进行评估,使用绝对轨迹误差(ATE)作为运行轨迹与真实轨迹误差评估指标,利用evo将改进前后算法的轨迹与真实轨迹重合度可视化,如图11所示,可以看出改进后算法误差较原算法误差有一定减小。改进后平移参数与旋转参数误差如图12与图13所示。绝对轨迹误差(ATE)各项指标见图14。实验结果表明,改进后的算法拥有较好的定位精度和较小的轨迹误差。
图11 轨迹可视化
Fig.11 Track visualization
图12 平移参数
Fig.12 Translation parameter
图13 旋转参数
Fig.13 Rotation parameter
图14 绝对轨迹误差
Fig.14 Absolute track error(APE)
另外本文中测试了改进前后VINS-Mono系统对于EuRoc数据集其余10个不同场景、不同难度序列的误差评估,见表1。表1显示了每个序列关键帧轨迹的均方根误差(RMSE),用来评价SLAM系统的精度。从表中可以看出本文中改进的VINS-Mono算法相对于原算法的均方根误差(RMSE)均有一定的减少,平均降低17.6%。
表1 改进前后VINS-Mono性能对比
Table 1 Comparison of Improved VINS-Mono system and VINS-Mono system
EuRoc数据集改进前VINS-monoRMSE/m改进后VINS-monoRMSE/mV1_01_easy0.1200.109V1_02_medium0.1250.110V1_03_difficult0.2010.147V2_01_easy0.1330.102V2_02_medium0.1390.111V2_03_difficult0.2550.224MH_01_easy0.1640.140MH_02_easy0.1860.137MH_03_medium0.1770.155MH_04_difficult0.2740.214MH_05_medium0.2770.235
4 结论
1) 基于效果较好的视觉惯性SLAM系统VINS-Mono进行改进,在视觉里程计(前端)部分引入ORB特征点算法取代原前端直接法。
2) 在IMU初始化步骤中提高了陀螺仪偏差和加速度计偏差的计算速率与准确性。
3) 将VINS-Mono算法与本文中改进后的算法在EuRoc数据集下进行实验与分析,证明了IMU初始化有较好的实现效果,改进前端后的SLAM算法相较于改进前有效地提高了定位精度,均方根误差(RMSE)平均减少了17.6%。
4) 基于单目相机与IMU搭建实验平台,将改进后的算法在采集到的真实场景下进行验证,证实了系统具有较强的鲁棒性。
[1] MUR-ARTAL R,MONTIEL J M M,TARD
S J D.ORB-SLAM:A versatile and accuratemonocular slam system[J].IEEE Transactions on Robotics,2017,31(5):1147-1163.
[2] AIMOURIKIS,SIROUMELIOTIS.A multi-state constraint kalmanfilter for vision-aided inertial navigation[C]//Proceedings 2007 IEEE International Conference on Robotics and Automation.IEEE,2007:3565-3572.
[3] LEUTENEGGER S.OKVIS2:Realtime scalable visual-inertial slam withloop closure[J].arxiv Preprint Arxiv:2202.09199.
[4] TONG Q,LI P,SHEN S.VINS-Mono:A robust and versatile monocular visual-inertialstate estimator[J].IEEE Transactions on Robotics,2017(99):1-17.
[5] CAMPOS C,ELVIRA R,RODR′IGUEZ J J G,et al.ORB-SLAM3:An accurate open-source library for visual,visual-inertial,and multimap slam[J].IEEE Transactions on Robotics,2021.
[6] HLIU,M CHEN,G ZHANG.ICE-BA:Incremental,consistent and efficient bundleadjustment for visual-inertial slam[J].IEEE Conference on Computer Vision and Pattern Recognition,2018,34(3):314-334.
[7] 刘建军,卢大威,胡雪花,等.基于点线特征的快速单目惯性 SLAM 算法[J].国外电子测量技术,2022,41(3):14-19.LIU Jianjun,LU Dawei,HU Xuehua,et al.Fast monocular inertial slam calculation based on point and line features[J].Foreign Electronic Measurement Technology,2022,41(3):14-19.
[8] DIAO Y,CEN R,XUE F,et al.ORB-SLAM2s:A fast ORB-SLAM2 system with sparse optical flowtracking[C]//13th International Conference Onadvanced Computational Intelligence (ICACI),Wanzhou:China,2021:160-165.
[9] FU Q.Fast ORB-SLAM without keypoint descriptors[J].IEEE Transactions on Image Processing,2022,31:1433-1446.
[10] MASCARO R,TEIXEIRA L,HINZMANN T,et al.GOMSF:Graph-optimization based multi-sensor fusion for robust uavpose estimation[C]//IEEE International Conference on Roboticsand Automation (ICRA).IEEE,2018:1421-1428.
[11] YU Y,GAO W,LIU C,et al.A gps-aided omni-directional visual-inertial state estimator in ubiquitous environments[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).IEEE,2019:7750-7755.
[12] 吴有龙,陈帅.惯性辅助的GNSS可区分性分析和粗差探测方法研究[J].兵器装备工程学报,2023,44(5):214-222.WU Youlong, CHEN Shuai.Research on inertial assisted GNSS separability analysis and the outlier detection method[J].Journal of Ordnance Equipment Engineering,2023,44(5):214-222.
[13] CAO S,LU X,SHEN S.Gvins:Tightly coupled GNSS-visual-inertial fusion for smooth and consistent state estimation[J].arxiv Preprint Arxiv:2103,07899:2021.
[14] ROSIN P L.Measuring corner properties[J].Computer Vision and Image Understanding,1999,73(2):291-307.
[15] QIN T,CAO S,PAN J,et al.A general optimization-based framework for global pose estimation with multiple sensors[J].Computer Science,2019,arxiv:1901.03642.
[16] YANG Z,SHEN S.Monocular visual-inertial state estimation with onlineinitiallization and camera-imu extrinsic calibration[J].IEEE Transactions Onautomation Science &Engineering,2017,14(1):39-51.
[17] ZHANG Z,SCARAMUZZA D.A tutorial on quantitative trajectoryevaluation for visual (-inertial) odometry[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).IEEE,2018:7244-7251.
[18] FORSTER C,ZHANG Z,GASSNER M,et al.SVO:Semidirect visual odometry for monocular and multicamerasystems[J].IEEE Transactions on Robotics,2016,33(2):249-265.
[19] LEE W,ECKENHOFF K,GENEVA P,et al.Intermittent gps-aided vio:online initialization and calibration[C]//IEEE International Conference on Robotics and Automation (ICRA).IEEE,2020:5724-5731.