一种改进的地下车库弱纹理下智能汽车即时定位与建图算法

李 辉, 王金刚, 张小俊

(河北工业大学机械工程学院，天津 300130)

在地下车库环境中进行定位无法像在地面等室外环境一样使用全球定位系统(global positioning system，GPS)等卫星信号，只能依靠车辆自身携带的传感器实现定位[1]。传感器定位一般分为使用多传感器融合定位的方法[2-3]和使用单独传感器的定位方法。由于目前市面上的传感器在精度方面都有一定的误差，无法确保定位精度，所以多传感器融合定位成为重点研究方向。

即时定位与建图(simultaneous localization and mapping,SLAM)技术最先由Smith等[4-5]提出，目前已成为解决智能汽车在地下车库环境下导航与定位的有效方法之一，依据采用传感器种类不同主要分为激光SLAM和视觉SLAM。其中，激光SLAM[6]在理论和工程应用中发展较为成熟，但受限于高昂成本，很难在量产车上大规模使用。而视觉SLAM因为其巨大的低成本优势正逐渐被业界所青睐，可预见在未来将有着广阔的应用前景。自21世纪以来，研究者们使用运动恢复结构(structure from motion, SFM)中的方式[7]，基于优化问题来求解SLAM问题，该方法以位姿图的形式描述物体在不同时刻的状态，又称为基于图优化的SLAM，在视觉定位领域中取得了主导地位[8-9]。2015年，ORB-SLAM[10]算法被提出，该算法整合当前各种主流的方式，创新地使用实时跟踪特征点线程、局部束优化线程以及全局位姿图的回环检测与优化线程三线程完成SLAM，能够确保地图与轨迹的全局一致性[11]。然而该算法的建图部分只含有稀疏的地图点，只能满足定位需求，不能用于导航。另外，整个系统都采用特征点进行计算，对于每张图都要计算ORB特征，这使得ORB-SLAM系统的实时性难以满足即时定位的需求。

特征点法在视觉SLAM中依旧占着主流地位，但特征点的提取与描述子的计算非常耗时，且使用特征点时，只用到特征点信息，其他像素点则被忽略。针对这些缺点，出现了直接法视觉SLAM，该法基于灰度不变假设，根据像素来直接计算相机运动。直接法是从光流法[12]演变而来，光流法是描述像素在图像中的运动，而直接法附带一个相机运动模型，算法中空间点成像的灰度值被认为是恒定的，直接根据像素亮度信息，估计相机的运动。直接法完全舍去了特征点提取和描述子计算，不仅避开了特征点法耗时的缺点，而且保留了所有特征信息。

Qin等[13]提出了ROVIO(robust visual inertial odometry)算法，这是一个基于EKF滤波的视觉惯导融合定位(visual inertial odometry，VIO)系统。ROVIO系统新颖的地方在于，基于当前相机坐标系的方向矢量和逆深度来参数化路标点。VINS-Mono[14]是香港科技大学于2017年开源的一种基于非线性优化的紧耦合VIO算法，采用单目和惯性测量单元(inertial measurement unit，IMU)信息恢复尺度，其中使用光流法进行特征点匹配，大大缩短了前端所耗时间。ROVIO的滤波方法相比于VINS-mono的非线性优化方法有一个明显的缺点就是存储量和状态量是平方增长关系，因此不适合大型场景。而现在基于视觉的SLAM方案，路标点数据很大，此时滤波的方法效率非常低。VINS-Mono是目前在VIO方面较为先进的算法，但在特殊的弱特征纹理环境下，如地下车库车位旁的墙面、地面或支撑立柱区域，通过对算法试验，无法准确提取出有效的关键点，导致位姿估计失败。

针对智能汽车在车库弱纹理环境下实现SLAM存在的问题，提出了一种基于VINS-Mono框架的改进视觉惯导融合定位算法。在前端通过提取灰度梯度变化明显的像素点来代替Harris特征点的提取，并进行深度估计，采用非线性优化方法来初步估计相机位姿，通过这种改进使得初始化精度有了提升，SLAM效果大大提高。

1 基于非线性优化的融合算法框架

改进的整个算法流程图如图1所示。在前端像素点提取阶段改进为采用提取像素值陡变点用于像素追踪和视觉定位[15]，同时对IMU做预积分运算，输出两帧图像之间的IMU积分结果，并保证IMU和图像数据的同步；初始化阶段，使用非线性优化建立视觉代价误差函数估计相机运动，经过深度滤波，使得视觉方面得到一个相对运动的情况，结合IMU与积分得到的相对运动情况，两者做视觉惯导松耦合初始化，从而标定出尺度、重力加速度、IMU速度和IMU偏差；后端阶段，把滑动窗口中相邻帧间的IMU产生的残差部分、视觉代价误差函数部分以及从滑动窗口中去掉的位姿和像素点的约束即边缘化残差部分进行联合BA优化，优化完之后得到各个时刻相机的位姿,实现VIO；利用Harris关键点计算描述子进行回环检测；在重定位后，为了确保基于重定位结果对过去的位姿进行全局一致配置。

图1 算法框架

1.1 算法前端

1.1.1 改进的灰度值陡变像素点提取

现阶段用于前端提取像素点的方法主要分为直接法中的灰度值陡变像素点提取法和特征角点提取法。

目前主要的检测特征点的方式有提取SIFT关键点、FAST关键点、Harris关键点等。VINS-mono算法中在关键点检测阶段采用了Harris角点提取算法。它的基本原理是计算图像中每一点与周围点变化率的平均值，在图像追踪精准度方面要更优于其他角点提取的算法，但对于弱纹理区域依然不能提取足够的特征点来匹配计算位姿，如图2所示，在这种弱纹理区域提取的特征点个数约为30，远不能满足位姿计算的需求。

图2 特征点提取与匹配

直接法中提取的是梯度发生明显变化的像素，相机有时候会拍摄到纹理特征不太明显或缺失特征的区域图像，如地下车库的墙面或其中支撑柱体的白色表面，利用其边缘或边缝直接法依然可以较好地估计出位姿[16]，本文算法在像素提取阶段改用提取灰度值陡变像素点的方案改进算法中的灰度梯度阈值可以根据具体的环境有所调整，运行起始阶段设定阈值threshold大小为70，当提取的像素点过少时，阈值threshold将降低10个灰度值以保证提取到更多的像素点；当像素点提取过多时阈值将提高5个灰度值用于减少计算的时间代价，保证实时性，灰度值陡变像素点提取如图3所示，提取的可用于位姿计算的像素点个数在200以上，可以保证环境信息的充分利用性。

图3 灰度值陡变关键点提取

当前相机运动到参考帧的距离达到一个阈值后，则创建当前帧为关键帧[17]，距离函数为

(1)

式(1)中：W为权重矩阵；ξ为两帧之间的变换矩阵，并且距离阈值根据当前场景的平均逆深度来确定。

根据Horn-Schunck光流进行像素的追踪。在前一步创建的关键帧上提取一定数量Harris关键点，计算描述子用于回环检测，由于在地下车库自主泊车中对回环的需求并不明显，同时为了尽可能的保证实时性，故提取该关键点数量的阈值较小。

1.1.2 IMU预积分

惯性测量单元IMU一般由3个单轴加速度计和3个单轴陀螺仪构成，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺仪检测载体相对于导航坐标系的角速度信号，对这些信号进行处理之后，便可解算出物体的姿态。

对于图像帧k和k+1，体坐标系对应为bk和bk+1，位置、速度和方向状态值p、v、q可以根据[tk,tk+1]时间间隔内的IMU测量值，在世界坐标系下进行传递的。

(2)

(3)

(4)

经过变换可得

(5)

(6)

(7)

式中：α、β、γ为bk+1对bk的相机相对运动量，分别对应量纲为位移，速度和四元数。此时使用一阶泰勒展开来获取近似值。

1.2 初始化-视觉追踪

由于在像素提取阶段创新性地采用了提取陡变像素点的方案，为了保证鲁棒性和可行性，在像素点追踪方面摒弃了原有的运动恢复结构(SFM)方法，转而使用非线性优化的直接法来估计出两帧之间的位姿[18]，其中空间点的深度结合深度滤波器进行估计，所构建的误差函数Ep可表示为

(8)

rp(P,ξji)=Ii(P)-Ij{V[P,Di(P),ξji]}

(9)

(10)

(11)

(12)

式中：Px、Py分别为匹配点P在世界坐标系下的x坐标和y坐标；D为深度的集合；ξji为新帧与关键帧的变换关系矩阵；rp为图像像素灰度值误差；Ii、Ij分别表示第i帧和第j帧的灰度值；P为匹配点；V为从图像向关键帧的一个映射；x′、y′、z′分别表示匹配点的像素坐标；d表示对应点的深度值；δ为误差的衡量标准。

使用ξ作为优化变量，最小化误差的二范数，得到优化问题，可表示为

(13)

式(13)中：N表示匹配点个数，使用李代数下的扰动模型，求误差关于相机运动的导数，最终得到一阶导数为

(14)

式(14)中：q为空间点在第2个相机坐标系下的坐标；u为它的像素坐标，得到雅克比矩阵为

(15)

最后使用高斯-牛顿方法计算增量，迭代求解。

1.3 后端优化

(16)

优化中的误差状态量为

(17)

结合特征点深度、IMU误差、关键帧姿态三部分，得到优化的残差函数为

(18)

rC=Ii-Ij

(19)

式(19)中：Ii、Ij为相邻两关键帧中同一像素点的灰度值。

IMU约束部分：通过对前面IMU预积分公式的变换，得到IMU残差为

(20)

边缘化约束部分：将滑出窗外的帧与滑窗内的帧的约束使用边缘化的形式保存为先验误差进行后续非线性优化，以保留约束信息。其中根据次新帧是否为关键帧，分为两种边缘化策略，通过对比次新帧和次次新帧的视差量，来决定边缘化掉次新帧或者最老帧，当次新帧为关键帧时，将边缘化掉最老帧，及其看到的路标点和相关联的IMU数据，将其转化为先验信息加到整体的目标函数中；否则，边缘化掉次新帧[20]。

2 实验结果与分析

2.1 数据集测试

改进算法是在VINS-Mono的框架下进行的改进，旨在应对地下车库纹理特征不明显的情境时能够有较好的位姿估计。使用EuRoC数据集对改进算法和VINS-Mono算法的定位精度进行对比评估，其中，该数据集采用Skybotix VI传感器提供一系列的图片，以及与其对应的IMU加速度和角速度测量值，通过硬件计算给科研人员提供了真实轨迹。现分别选取其中的工厂场景(MH_02)、房间1场景(V1_01)和房间2场景(V2_01)3个数据包，分别来对代码进行实验测试，图4为3个数据集的测试效果。

实线表示改进算法的测试轨迹图；虚线表示真实轨迹

图4中，不同区域的颜色表示此处测试轨迹和真实轨迹的距离误差，颜色越靠近右侧色带条上方的红色表示误差越大，越靠近下方的蓝色表示误差越小。根据图4绘制出表1。

表1是两个轨迹的整体均方根误差，可知改进算法在定位精度方面比原算法有了一定的提升，在相比特征纹理不丰富的V1_01和V2_01的场景里更为明显。

表1 不同数据集上绝对轨迹的均方根误差

2.2 地下车库弱纹理场景实验

试验设备及平台：小觅深度相机，ubuntu16.04操作系统，Intel Core i5处理器，ROS操作平台；试验车辆：奇瑞小蚂蚁；试验场景：地下车库中墙面或立柱集中的区域；场景大小：35 m×35 m，如图5所示。

图5 试验设备及试验环境

实验方法为：首先，标记车辆起始位置，在车辆起点处的地面上贴上用于标记的白色胶带。在车辆跑完整个回环时使车辆停到起始的标记位置，以此作为标准来估计运行轨迹在终点和起始点的距离误差。环形轨迹如图6所示。

图6 环形轨迹

以下为原VINS-Mono算法在该实验环境下的轨迹运行结果，如图7所示。改进算法在该实验环境下的轨迹运行结果，如图8所示。

由图7和图8可知，在车库拐角的弱纹理明显的区域，VINS-Mono算法的定位效果图上出现了显而易见的波动，对比VINS-Mono算法，改进的算法形成了较好的运动轨迹，在弱纹理明显的区域曲线过渡更为平缓。改进算法在回环轨迹闭合点处的xy平面内的距离误差约为1 m；原VINS-Mono算法在闭合点处xy平面内的误差为3～4 m；改进算法在z轴上的误差比原算法减小了50%左右。

图7 VINS-Mono实验结果

图8 改进算法实验结果

通过在地下车库弱纹理环境中算法的对比实验，改进算法的轨迹精度比原算法有了提高，其鲁棒性和精确性都得到了验证。

3 结论

提出了一种改进的视觉和IMU融合的算法来针对地下车库中光线不充足、且特征纹理较弱的定位问题，得到如下结论。

(1)改进算法在前端采用提取灰度值陡变的像素点来代替特征点进行位姿估计，使得在弱纹理区域能够提取到足够的特征信息用于满足算法的定位需求，始终保持200个以上用于定位估计的关键点。在后端优化过程中加入了灰度值不变原理所构建的视觉残差，使变量的优化过程更适应地下车库环境，进一步提升了算法的鲁棒性。

(2)通过实验验证了改进算法在定位精度方面优于原VINS-Mono算法。相比原算法虽然在回环检测模块提取了较少的Harris特征点，在闭环精度上有一定的牺牲，但在整体的定位准确度方面并未有明显差别。