大尺度弱纹理场景下多源信息融合SLAM算法

2021-12-13朱叶青赵良玉

宇航学报 2021年10期

朱叶青，金瑞，赵良玉

(北京理工大学宇航学院，北京 100081)

0 引言

近年来，以无人车和无人机为代表的自主机器人数量及需求不断增加[1]。同时定位与地图创建(Simultaneous localization and mapping, SLAM)作为其自主导航的关键技术[2]，允许机器人在未知环境中，依靠自身所带传感器递增式的创建环境地图，并同时给出机器人所在位置，受到了国内外诸多学者的关注和研究。视觉惯性融合的SLAM系统可通过其惯性测量单元(Inertial measurement unit，IMU)测量到的加速度和角速度信息来改善相机因强烈光照变化等引起的问题，其在小范围内定位精度高、不易受外界干扰、信息量获取丰富，成为近年来的热门研究方向[3]。文献[4]提出了一种对比度增强的尺度不变特征变换(Scale-invariant feature transform，SIFT)算法，降低了特征匹配的时间。文献[5]使用点云迭代最近点(Iterative nearest point，ICP)算法获取初始位姿，可有效规避误匹配从而获得更加精准的相对位姿。然而在弱纹理场景中，由于较难提取大量有效特征点，基于特征点法的视觉惯性SLAM性能通常会降低、定位精度会下降，纹理较差的图像序列或由于运动模糊等原因造成系统完全失效。虽然直接法在某种程度上缓解了特征点的依赖问题，但是稠密和半稠密的直接跟踪法计算量较大，无法在一些计算能力有限的平台上使用[6]。

如在城市楼房、街道等人造场景[7]中，通常包含丰富的线性元素，即在缺乏可靠特征点的情况下，弱纹理场景下仍然包含许多可以提取的线特征。线特征相对于点特征而言，是一种更高层次的特征，利用线特征构建的环境地图具有更直观的几何信息，同时也能提高SLAM系统的精度和鲁棒性。对于线特征的表示，文献[8]提出了正交表示法。文献[9]使用线特征研究了视觉惯性里程计(Visual inertial odometry, VIO)的可观测性。文献[10]构造了一种结合点线特征的紧耦合单目VIO系统，但算法只是实现了局部位姿估计,没有考虑全局的定位误差。

在大尺度场景中，为了提高状态估计的精度和鲁棒性，多源信息融合的方法已成为一种主流趋势。根据系统使用的传感器类型不同，将已有的研究工作分为局部定位和全局感知定位。对于局部定位，通常采用相机[11-12]、IMU、激光雷达[13]和RGB-D传感器进行六自由度状态估计。局部定位传感器的不足主要包括：相机在快速移动或者光照发生剧烈变化时特征点容易跟踪丢失、激光雷达对玻璃等物体存在反射、IMU长时间运行会存在累计误差产生漂移等。全局感知定位包含全球导航卫星系统(Global navigation satellite system, GNSS)、磁力计、气压计等。全局感知定位的测量相对于地球框架，独立于起始点，可有效避免累计漂移的问题。然而，全局测量通常含有噪声且采集频率较低，若只有全局传感器不足以实现六自由度状态的实时估计，在做高精度定位时往往无法单独使用。

为了实现精确的全局感知定位，可以将全局传感器与局部传感器融合在一起。文献[14]中引入一种多维观测噪声尺度因子，并在此基础上提出一种基于无迹卡尔曼滤波的自适应SLAM算法。文献[15]通过基于贝叶斯滤波的视觉SLAM方法来估计空间翻滚非合作目标的相对位姿。文献[16]提出一种基于扩展卡尔曼滤波(Extended Kalman filter, EKF)的视觉惯性和全球定位系统(Global positioning system, GPS)测量相融合的无漂移估计算法。文献[17]采用无迹卡尔曼滤波(Unscented Kalman filter, UKF)算法对视觉、激光雷达和GPS测量信息进行融合。然而使用滤波方法对时间同步非常敏感，由于状态传播无法逆向，所以延迟的测量处理较麻烦，需要特殊的排序机制以确保来自多个传感器的所有测量均为有序。与基于滤波的方法相比，基于优化的方法在该方面具有明显优势。其可以在不同的点重复状态向量的线性化，可存储较长一段时间的测量值。文献[18]使用基于优化的框架将局部VIO与GPS测量值融合，定位精度比文献[16]更高。文献[19]提出了一种基于优化的支持多种传感器类型的融合框架，但是该框架没有考虑弱纹理场景下的特征跟踪问题。

为此，本文提出一种视觉惯性与全球导航卫星系统多源信息融合的SLAM算法。首先利用基于点线的特征跟踪算法，解决了弱纹理场景相邻帧图像之间运动估计精度不高的问题。同时对于具有重复线特征的弱纹理场景，将线特征表示为直线端点的线性约束，整合到基于特征点的算法中，有效改善了算法的鲁棒性。最后为实现大尺度弱纹理场景下的精准状态估计，通过视觉惯性信息和GNSS测量信息融合的算法，实现了局部精确和全局无漂移的位姿估计。

1 系统整体框架

系统整体框架如图1所示，主要包括前端点线特征的跟踪及相机与IMU的数据预处理、后端视觉惯性SLAM位姿图联合优化、GNSS位姿融合3个部分。

图1 系统整体框架Fig.1 System framework

在预处理阶段，惯性测量单元的数据采用IMU预积分的形式[20]，在短期内提供相邻帧之间的相对位姿估计。相邻图像帧之间采用光流法进行前端数据关联。利用惯性测量数据优化视觉测量的尺度因子，并对齐两个传感器的位姿估计结果，实现视觉惯性联合位姿校准。

在后端视觉惯性SLAM位姿图联合优化部分，基于视觉惯性里程计滑动窗口优化方法，将新增的关键帧依次通过滑动窗口进行优化。在全局位姿图优化时，联合优化位移、偏航角、俯仰角和滚转角，实现六自由度的位姿图优化，最终将得到的局部位姿与GNSS全局位姿进行融合，实现鲁棒、高精度的位姿估计。

2 点线特征跟踪算法

2.1 点线特征表示及观测模型

基于点特征提取和描述的视觉SLAM系统目前已经比较成熟，由于特征点位置在图像中比较好定位并加以区分，降低了数据关联的难度，目前大部分视觉惯性融合的SLAM算法都是基于点特征。线特征的优点是在较大的视角变化下比较稳定，并且具有较高的光照不变性；在弱光照下线段检测比点特征检测具有更强的鲁棒性。其缺点是缺少有效的描述匹配方法，线段的检测较不稳定，使得基于线特征的SLAM实现起来具有一定的难度。

相比于霍夫直线变换等经典的直线检测算法，LSD(Line segment detector, LSD)算法是一种局部提取直线的方法[21]，其运行速度更快，能在线性时间内得出亚像素级精度的检测结果。LSD算法以灰度图像为输入，其输出值为一系列检测出的线段。该算法在设计过程中被设定为在任何数字图像中都无需参数调节，并且可以自动控制误检的数量，是一种鲁棒性好、精度高的直线检测算法，使用LSD直线检测算法和LBD(Line band descriptor, LBD)描述子[22]，对旋转与尺度缩放具有较高的鲁棒性，可以达到更优的匹配效果，计算速度更快。

对于直线表示方法，文献[10]在初始化、坐标变换和重投影时使用普吕克坐标表示，后端优化采用正交表示。本文对于具有重复图案或弱纹理的场景，引入一种线性误差，将其表示为直线端点上的线性约束，该约束可整合到两种先进的基于特征点算法的线性公式OPnP[23]和EPnP[23]中。此外，由于遮挡或误检，直线可能只被部分观测，本文线特征参数化方式可将其集成到EPnP和OPnP算法中。

图2 线特征的重投影误差Fig.2 Re-projection error of line feature

定一个平面，其单位法向量li,k定义为[24]：

(1)

(2)

(3)

其中:投影点xi,j=[xi,j,yi,j]T。

(4)

(5)

其中:Op和Oq分别表示Pw,k与Qw,k对应的相机坐标，可通过式得到，οpx,οpy,οpz与οqx,οqy,οqz分别表示Op与Oq在相机坐标系中x,y,z轴上的分量。式(6)中Riw和tiw分别表示世界坐标系到相机坐标系的旋转和平移：

(6)

(7)

2.2 IMU预积分及观测模型

(8)

(9)

其中:

(10)

其中:αbibj、βbibj、qbibj分别为预积分得到的位置、速度和旋转值。式(11)为连续时间下未考虑偏置和噪声的预积分模型，而现实中IMU测量值通常含有噪声，且为离散时间形式。在离散时间条件下，采用中值积分的方式可得到IMU测量值，将帧k和帧k+1之间的IMU预积分表示为：

(11)

(12)

(13)

则IMU的测量残差可写为式(14)：

(14)

其中:[·]xyz是以四元数形式表示的旋转误差的实部。

3 视觉惯性SLAM位姿图联合优化

在视觉惯性里程计后端联合优化部分，通过最小化所有测量残差项之和来优化滑动窗口中的所有状态变量如式(15):

(15)

4 GNSS位姿融合

(16)

(17)

由于视觉惯性局部估计在小范围内是精确的，因此可以利用两帧之间的相对姿态。考虑到两个连续帧t-1帧和t帧，局部因子采用式(18)所示的视觉惯性因子表征:

(18)

(19)

5 仿真校验与分析

5.1 数据集实验

仿真采用开源机器人操作系统(Robot operating system,ROS)Kinetic，使用C++语言基于Ubuntu 16.04环境下的Microsoft Visual Studio Code 2019平台开发，计算机处理器为Intel Xeon CPU E5645，主频2.40 GHz，内存为48 GB。

5.1.1EuRoC数据集

为了验证弱纹理和光照变化较大场景下算法定位精度及鲁棒性，本文选用EuRoC数据集[25]。EuRoC数据集是由苏黎世联邦理工大学采集的微型飞行器(Micro aerial vehicle，MAV)视觉惯性数据集。数据集包括立体图像(Aptina MT9V034全局快门，单色WVGA，相机频率20FPS)，并同步IMU(ADIS 16448, 200 Hz)数据以及精准的地面真实状态(动作捕捉系统VICON，激光追踪器Leica MS50)。数据集提供了所有外部和内部参数。在仿真实验中，只使用左摄像头的图像，与OKVIS[26]以及VINS-mono[27]两种比较先进的单目VIO算法进行比较。OKVIS是基于点特征的滑动窗口优化算法，可应用于单目和立体相机模式。VINS-mono是具有闭环的完整的视觉惯性SLAM系统，在滑动窗口中使用点特征优化IMU状态。

图3所示为EuRoC数据集序列中室内弱纹理场景图。其中图(a1)～图(a7)为数据集中的原始场景图；图(b1)～图(b7)为点特征提取图；图(c1)～图(c7)为线特征提取得到的效果图。可看到在包含较大面积平面的图(a1)、图(a4)、图(a5)光照较暗的图(a2)、图(a3)，以及光照过强而反光的图(a6)、图(a7)等弱纹理场景中，点特征提取较为稀疏，而利用线特征提取算法则可以提取大量有效信息从而更加直观描述几何结构，为后续视觉里程计根据图像信息估计相机运动提供更加丰富且鲁棒的特征信息。

图3 弱纹理场景下特征提取对比Fig.3 Comparison of feature extraction in weak texture scenes

表1 EuRoC数据集上位移与旋转均方根误差比较Table 1 The root mean square error of translation and rotation error on the EuRoC MAV dataset

表1通过不同算法在EuRoC数据集11个序列估计的轨迹和真实轨迹的均方根误差来比较算法的定位性能，图4为位移均方根误差及旋转均方根误差对比柱状图，横坐标为数据集序号。可以看出，由于所提出的算法在点特征提取的基础上加入了线特征提取，所以均方根误差明显低于基于点特征的

图4 位移均方根误差以及旋转均方根误差RMSE对比Fig.4 Comparison of root mean square error (RMSE) for translation and rotation using the EuRoc dataset

VIO算法OKVIS与VINS-Mono。与同样是点线特征提取的PL-VIO相比，后者在V1_02_medium数据集上运行时，出现追踪失败导致严重漂移没有得到最终轨迹，表明本文所提出的算法具有更好的鲁棒性。另外，在快速运动、特征稀疏的数据集V2_03上仍能够保持0.2 m左右的高精度定位，精度提升了19%左右，较未添加线特征和全局定位的VINS-Mono算法提升39%左右。且根据柱状图的分布可知，所提出算法的定位误差及旋转误差的波动更小，定位估计具有更好的一致性，实现了弱纹理环境下鲁棒精准的定位。

图5给出了PL-VIO和所提出算法估计的轨迹，颜色由蓝到红，误差逐渐增大。比较MH_04_difficult、MH_03_medium、V1_03_difficult三个序列上的轨迹，其中图5(a)、(c)、(e)为PLO-VIO的轨迹结果，图5(b)、(d)、(f)为所提出算法的轨迹结果。可以看出：当摄像机快速旋转时，本文所提算法比PL-VIO产生的误差更小。此外，在发生快速旋转的序列上，观测视角会发生很大变化，光照条件对跟踪点特征也非常具有挑战性。为了更加直观的查看不同算法的轨迹对比图，图6给出了EuRoC数据集MH_04上的测试结果轨迹对比图。可以发现，所提出算法的定位轨迹能更接近真实轨迹，定位误差小于PL-VIO、OKVIS和VINS-mono，表明所提出算法的定位更准确。

图5 EuRoC数据集上与PL-VIO的轨迹对比Fig.5 Trajectories comparison of the proposed methods verus PL-VIO using the EuRoc dataset

5.1.2KITTI数据集

为了验证算法在大尺度场景中的定位精度以及鲁棒性，本文选用KITTI数据集[28]。KITTI数据集由德国卡尔斯鲁厄理工学院和丰田美国技术研究院联合记录，是目前最大的自动驾驶场景下的计算机视觉算法评测数据集。KITTI包含市区、乡村和高速公路等场景采集的真实图像数据，有各种程度的遮挡与截断。其中采用的传感器包含立体相机(相机型号：PointGrey Flea 2，单色1382×512像素，相机频率为10PFS)和GPS，地面真实状态由惯性导航系统(OXTS RT 3003)提供。所提出算法从GNSS测量数据中获取纬度、经度和高程。相机用于局部状态估计，在全局优化中，局部结果与GNSS测量值融合。将所提出算法与比较先进的VIO算法融合GPS算法VINS-Fusion[19]进行比较。VINS-Fusion[19]是VINS-Mono的扩展，支持多种视觉惯性传感器类型，

图6 不同算法在EuRoC数据集MH_04_difficult上的轨迹对比Fig.6 Comparison of the estimated trajectories by different algorithms using the EuRoc sequence MH_04_difficult

如单目加IMU，双目相机加IMU以及与GPS的融合。文献[19]在仿真验证中使用Horn方法作为对齐真值，采用表2中Raw Data数据序列对应的KITTI中00-10数据序列的真值进行对齐。

从表2中的均方根误差(Root mean square error, RMSE)可以看出，本文所提算法在所有序列上的性能均优于VINS-Fusion，表明大尺度场景下同样融合GNSS的前提下添加线特征可有效提高估计轨迹的精度，而且为了更加直观的比较轨迹的定位精度,使用KITTI数据集中长度最长(长度为3.7 km)的数据序列10_03_drive_0027，将两种方法估计的轨迹进行对比如图7所示，可以看出所提出算法的轨迹更加贴合真值，尤其在拐角处VINS-Fusion的轨迹与真值相差较大，所提出的算法在大尺度运行场景下鲁棒性更强、定位精度更高。

表2 KITT上位移均方根误差RMSE比较Table 2 The RMSE of translation error on the KITTI raw data

图7 不同算法在KITTI数据集上的定位轨迹对比Fig.7 Comparison of the estimated trajectories by different algorithms using KITTI dataset.

5.1.3AGZ数据集

为了验证算法在含有丰富线性特征的大尺度弱纹理人造场景(如城市楼房、街道等)中的定位精度以及鲁棒性，选用AGZ数据集[7]进行验证。AGZ数据集是由苏黎世联邦理工大学，使用装载GoPro Hero 4相机的微型飞行器在城市街道低海拔(离地5～15 m)进行拍摄，记录图像间隔为30 ms。数据集总长度2 km，包括时间同步的高分辨率图像(1920×1080×24位)、GPS以及IMU传感器数据、地面谷歌街景图像和地面真实数据，因此该数据集适合评估和检测基于图像的定位、单目视觉里程计和在线三维重建环境等算法。

图8是AGZ中部分苏黎世城市街景图像，可以看出，城市户外街景中存在大量弱纹理墙壁及玻璃窗，在地面和墙面上几乎提取不到点特征，而在地面与墙面的交界处，窗户的边缘等却存在丰富的线段特征。图8(a1)、(b1)为提取点特征的场景图，图8(a2)、(b2)对应其提取线特征的对比图，图8(c1)、(c2)、(c3)和图8(d1)、(d2)、(d3)为不同场景下线特征提取的效果图。可看到在包含较大面积墙壁的图8(c1)、(c2)、含有长距离街道的图8(d1)、(d2)，以及大量窗户的图8(c3)、(d3)等场景中，利用线特征提取算法可以提取大量有效信息从而更加直观描述几何结构，为视觉里程计根据图像信息估计相机运动提供更加丰富的特征信息。将所提出算法在AGZ数据集上得到的轨迹与GNSS得到的轨迹在谷歌地图上对比，结果如图9所示，蓝色为GNSS的数据，红色为所提算法的运行轨迹。可以看出在室外大尺度运行过程中，算法可以有效的校正GNSS产生的定位误差，产生的轨迹与地图的路线吻合更好，即算法具有大尺度下姿态估计准确的优点。