一种基于地图辅助的自动驾驶视-惯融合定位方法

程君，张立炎，陈启宏

(武汉理工大学，自动化学院，武汉 430070)

0 引言

随着人工智能(Artificial Intelligence,AI)和机器人技术的发展，自动驾驶成为交通领域的热门研究方向。自动驾驶车辆的一个基本要素是具备构建环境地图并在地图上定位的能力。为了导航的安全性，自动驾驶汽车需要建立一个环境的准确模型并具备估计自身位置的能力。传统的定位方法依赖全球定位系统(Global Positioning System,GPS)或全球导航卫星系统(Global Navigation Satellite System,GNSS)或实时动态(Real Time Kinematic,RTK) GPS 系统[1]。然而，由于信号反射、时钟偏差及大气层阻碍等因素，GPS、GNSS 的测量误差通常达到在10 m 左右，尤其是车辆在隧道和城市峡谷区域行驶时会出现定位信号丢失的情况，定位误差对于自动驾驶车辆导航而言显然无法接受。RTK-GPS可通过固定基站的内部校准信号校正这些误差，但是，额外的基站设备增加了定位的成本。

近年来，有许多学者提出不依赖GPS、GNSS、RTK 系统的定位方法。其中，基于激光雷达(Light Detection And Ranging,LiDAR)的定位方法受到了广泛关注，可靠的激光扫描特性保证了构建较为稠密的环境地图[2]。在开源社区，基于机器人操作系统(Robot Operating System,ROS)的自适应蒙特卡罗(Adaptive Monte Carlo Localization,AMCL)方法在二维占据栅格地图定位中取得了良好效果[3]，但是，高额的成本阻碍了激光雷达在自动驾驶领域的普及。WARD等[4]提出一种基于毫米波雷达(Radio Detection And Ranging,RADAR)的定位方法。实验表明，由于路侧存在独特的特征信息，该方法的横向定位误差很小，而纵向特征信息不明显使得该方法在纵向定位上存在较大误差。与激光雷达和毫米波雷达传感器相比，相机传感器的测量数据更加丰富，更容易感知墙壁上的纹理特征，且成本更低。CHENG 等[5]提出一种快速且高精度的视-惯融合定位算法，通过改进非线性优化迭代策略加快了系统的响应速度。然而，该方法只能实现在线建图与定位功能，当相机快速移动时轨迹跟踪容易丢失，且无法获得全局坐标系下的车辆位置信息。为解决自动驾驶车辆存在的诸多定位问题，基于地图的定位方法被广泛关注。RANGAN 等[6]创建了带有视觉特征的局部地图，并将局部地图坐标与GNSS 坐标关联。在定位过程中，该方法将当前相机坐标系与GNSS查询地图的特征子集进行匹配，获取全局坐标系下的位置信息。BRUBAKER 等[7]采用概率转移模型和基于混合高斯的图优化方法，通过公开的街道地图(Open Street Maps,OSM)数据集测试发现该方法收敛速度较慢。此外，除了视觉里程计(Visual Odometry,VO)的测量信息没有其他可用的测量模型，车辆行驶在场景相似的路段时，系统需要很长时间收敛甚至不收敛。MA等[8]尝试提取图像中的语义特征来解决这个问题，例如，道路类型、车道线、路边警示牌等语义特征，但车辆运动过快时，基于图像的语义信息无法充分的提取，从而影响车辆定位精度。由于缺乏闭环检测(Loop Closing，LC)模块，在大规模场景该方法会出现较大的定位偏差。

综上所述，人们尝试采用激光雷达、毫米波雷达或摄像头等传感器感知自动驾驶车辆周围的环境并估计自身位置。离线地图为自动驾驶车辆稳定且高精度的定位提供了良好的解决方案，当地图坐标系与GPS,GNSS 坐标系(即全局坐标系)对齐之后，车辆只需在给定的地图上定位即可获得全局坐标系下的位置。本文通过改进ORB-SLAM2 方法[9-10]完成地图定位过程。为提高地图的精度，在地图构建阶段采用图优化算法实现IMU 对地图误差的校正。具体实施流程为：首先，车辆在低速行驶时启动改进的ORB-SLAM2 方法创建并保存具有环境特征的地图；然后，在第2 次运行时调用预先保存的地图并控制车辆以更快的速度在地图上定位。在KITTI 数据集场景和真实场景进行实验。本文的主要贡献为：

(1)在ORB-SLAM2的基础上拓展了地图保存功能。与原始的框架一样，拓展后的系统在慢速驾驶条件下作为SLAM 模块。在快速驾驶条件下可以作为定位模块，实现更加鲁棒的定位性能。

(2)采用基于关键帧的图优化方法实现IMU对视觉地图的误差校正。将IMU 测量误差作为约束项加入到因子图中，构建了一个新的因子图模型。相比原始的ORB-SLAM2方法，本方法具有更高的定位精度。

(3)采用轻量化和模块化的设计思路。改进后的系统可以被打包成为一个文件，方便移植到嵌入式平台中，也可以作为额外的输入包，实现与其他传感器的集成，提高了系统的拓展性能。

1 ORB-SLAM2框架

ORB-SLAM2 是一个基于ORB 特征点的实时纯视觉SLAM系统，在大规模、小规模、室内或室外环境均可以运行。该系统除了对光照变化具有较强的鲁棒性，还支持宽基线的闭环检测和重定位功能，包括全自动初始化功能。ORB-SLAM2主要由3个线程构成：跟踪、局部建图以及回环检测线程。

如图1所示，该系统可接入单目、双目、RGB-D(Red Green Blue Depth)相机，跟踪线程通过寻找特征匹配并最小化两帧之间的重投影误差来估计当前时刻相机的位姿。为节约计算资源，局部建图和回环检测线程并行工作。在局部建图线程中采用捆集调整(Bundle Adjustment,BA)方法优化关键帧和关键点。回环检测线程主要负责检测车辆是否回到之前访问的位置，它分为检测和校正两个步骤，其中检测环节需完成数据库查询和李群向量(SE3)的计算，校正环节包含了回环融合和本征图优化过程，并调用g2o(general graph optimization)优化库[11]优化消除累积误差。完成上述位姿优化之后，系统启动第4 个线程实现全局BA 以及对整个地图和轨迹进行优化。全局优化是ORB-SLAM2系统必不可少的部分。即使很小的误差也会随着时间累积，通过回环检测和全局优化可以很大程度上消除累积的误差。

图1 ORB-SLAM2系统概述Fig.1 A general overview of ORB-SLAM2

ORB-SLAM2 从输入的图像中提取ORB(Oriented FAST and Rotated BERIEF)征点[12]并以稀疏的方式表征环境信息。ORB 特征点在旋转性和缩放性方面表现良好，同时，可以快速地进行特征提取和匹配，保证系统的实时性。ORB-SLAM2通过调用DBoW2(Distributed Bag of Words version2)模型[13]识别之前访问的位置。如果当前帧和DBoW2 数据库的匹配相似度达到了一定的得分值，则该区域被认为是一个闭环。在定位模式下，停用局部建图和回环检测线程，系统只使用跟踪线，并在之前建好的地图中跟踪相机轨迹。

2 地图辅助的视-惯融合定位方法

尽管ORB-SLAM2具备定位功能，但是此功能只适用于在线模式。用户可以自行在图形界面开启或关闭此功能。当SLAM 进程结束之后地图会被删除。本文在ORB-SLAM2 的基础上拓展了地图存储功能，改进的版本可以在SLAM进程结束后保存1个离线的视觉特征地图。

2.1 地图保存功能设计

原始的ORB-SLAM2 系统建图和定位同步进行，改进的版本将两者拆分成两个单独的模块，即建图模块和定位模块，其中，建图模块需要提供1个包含ORB-SLAM2 类的地图文件。该文件包含地图、地图点、关键帧、关键帧数据库以及每个帧的DBoW2 矢量和特征矢量信息。在建图进程结束后，地图将被保存成一个二进制(.bin)的文件。通过更改系统配置文件(.yaml)的参数，指定是否希望加载地图。其中，参数设置为0 表示不加载地图；参数设置为1 表示加载地图。一旦决定使用之前保存的地图，系统启动时将调用地图文件并加载地图和关键帧数据库，同时，系统将被设置为定位模式，后者自动完成且无需在线手动更改系统的运行模式。改进后的系统将被打包成1个文件，可以被移植到其他基于ROS 的无人系统中，也可以代替以激光雷达为基础的AMCL包，或者作为额外的输入实现更复杂的定位算法。改进的系统流程如图2所示。

图2 系统流程Fig.2 Flowchart of system

2.2 基于图优化算法的IMU校正

地图校正，即建图步骤中的位姿优化过程。本文采用图优化算法实现此优化过程，该算法的原理是将优化变量视为图的顶点，约束项视为图的边。IMU 测量误差与相机的重投影误差构成了包含多个边的因子图模型。其中，IMU 的测量误差有两项，即预积分残差项和随机游走噪声项。在地图校正过程中，IMU 首先提供一个相对准确的初始位姿；然后，根据该位姿将局部地图中的路标点投影到当前图像帧中，并在图像中寻找与之匹配的特征点；最后，采用列文伯格-马夸尔特(Levenberg-Marquardt,L-M)算法最小化该特征点的视觉重投影误差以及IMU 残差之和，求解最佳的路标点位置，完成地图的校正。图3为考虑IMU测量误差的因子图，图中R(.),v(.)和p(.)分别表示某一关键帧(第i或j或j+1 帧)对应的旋转量、速度量和位置量。bg(.),ba(.)分别表示某一关键帧(第i或j或j+1帧)对应的陀螺仪偏移量和加速度计偏移量。

图3 考虑IMU测量误差的因子图Fig.3 Factor graph considering IMU measurement errors

IMU 校正过程主要是在局部建图线程中完成。该过程会对地图进行修改，因此，非线性优化的方式根据局部地图是否修改而不同，本文的校正过程分为如下两种情况。

(1)情况1，局部地图被修改，IMU 提供“关键帧-关键帧”之间的约束。此时，地图中的关键帧和路标点状态均有所调整，考虑当前关键帧j与最近关键帧i之间的IMU约束，优化问题为

式中：X为待估计状态变量，包括第i帧估计的旋转量RWBi，位置pWBi，速度vWBi，陀螺仪偏移bgi，加速度计偏移bai；ej,k为帧j与路标点k之间的重投影误差；Σp为图像特征点位置的协方差；EIMU(i,j)为IMU测量误差，主要由两部分组成，即IMU预积分测量残差eIMU(i,j)和零偏随机游走噪声eb(i,j)。

在计算IMU 预积分测量残差时，本文考虑了零偏修正量以及关键帧i-帧j的状态量。eΔpij,eΔvij,eΔRij和如文献[14]中所定义，相应的IMU预积分测量协方差ΣIMU计算式为

式中：δpij,δvij,δRij近似为零均值高斯噪声，满足N～(0,Σ)。零偏随机游走噪声协方差Σb可以根据Σb=ΔtCov(η)从传感器的器件参数η和时间间隔Δt计算得到。

上述非线性优化得到的结果以及边缘化(Marginalization)得到的海塞(Hessian)矩阵可以作为下一次计算的近似先验分布。

(2)情况2，局部地图没有被修改，IMU提供“关键帧-帧”约束。如果包含下一关键帧j+1 的局部地图没有被修改，那么，同时估计关键帧j和关键帧j+1 的状态，此时，IMU 提供两帧之间的约束，上一次计算所得的先验值也被视为当前帧的约束。优化问题为

式中：ej,m,ej+1,n是两帧之间特征点的视觉重投影误差；m,n为参与叠加计算的重投影误差数量；EIMU(j,j+1)是两帧之间的IMU测量误差。与式(2)类似，先验分布对应的残差Eprior(j)为

式中：(.)prior为上一次的估计值；ep,ev,eR,eb分别为位置、速度、旋转、IMU 偏移的误差量；ΣpriorH为上一次估计后边缘化所获得的Hessian矩阵；Rj,Rpriorj分别为第j关键帧的旋转量估计以及上一帧旋转量的估计值；vj,vpriorj分别为第j关键帧的速度估计以及上一帧速度的估计值；bj,bpriorj分别为由第j帧以及上一帧估计的陀螺仪和加速度计的零偏，包含了6 维向量；这里ep的定义形式是单纯的相减pj-ppriorj，其形式可以参考文献[14]，优化完成后帧j将被丢弃，并且按照文献[15]的方式同样计算帧j+1的边缘分布，在地图未被修改之前被视为下一次计算的先验值。地图修改后，此先验值不再适用，需要结合式中“关键帧-帧”的IMU约束来计算。

3 实验

基于KITTI数据集场景以及真实场景设计，并实现相关实验。实验平台为配备了Ubuntu18.04 ROS melodic,Intel(R) Core(TM) i7-9700 CPU@3.00 GHz 3.00 GHz 以及16 G 内存的台式计算机。具体实验步骤如下：

Step 1 KITTI 数据集场景的地图保存测试。为验证地图保存功能的有效性，以较慢的速度播放数据集并构建地图。为了找到最佳的建图速度，分别测试与评价了小于1 倍速度(如0.1,0.2,…,0.9倍速度)时构建地图的精度。

Step 2 KITTI 数据集场景的地图定位测试。首先，调用预先保存的地图，然后，控制车辆以更快的速度在地图上定位。

Step 3 真实场景的地图定位测试。此部分实验基于自建的自动驾驶小车平台，首先，利用小车在室外环境录制数据集，然后，在ROS环境下加快数据的播放速度，模拟车辆快速驾驶工况。

3.1 数据和评价指标

(1)KITTI数据集

选择无人驾驶KITTI,Raw data 数据集[16]作为测试对象。它在1辆标准的旅行车上配备了2台高分辨率的彩色和灰度相机，1 台360°环视扫描的Velodyne-64 激光雷达，以及1 台高精度的GPS、IMU组合导航设备，其中，GPS、IMU被视为参考系统。数据集包含：城市场景、住宅场景、公路场景、校园场景以及个人场景。系统以10 Hz的频率捕获和同步各个传感器的测量值，主要包括如下信息：

①原始的和经过处理的灰度双目相机图片序列(50万像素，以png格式存储)；

②原始的和经过处理的彩色双目相机图片序列(50万像素，以png格式存储)；

③3D Velodyne 激光点云(每帧100 k 个点，存储为二进制浮点矩阵)；

④3D GPS、IMU 数据(包括：位置、速度、加速度、元信息，存储为文本文件)；

⑤3D 目标跟踪标签(跟踪目标包括：汽车、卡车、有轨电车、行人、自行车，存储为xml文件)。

其中，原始的图片序列表示图像未经过同步和校正处理，此时，图像为索引不对应的原始输入帧；经过处理的图片序列表示图像经过了同步和校正处理，此时，图像帧编号对应所有传感器的数据流。本文选用的“同步+校正”图像数据进行测试。

(2)实际小车数据集

录制数据集的移动小车平台主要由小觅S1030-120 双目摄像头，英伟达JETSON-TX2 嵌入式处理器以及四轮驱动的小车底盘组成。小觅双目摄像头内置1个消费级IMU，并通过硬件电路板(Printed Circuit Board,PCB)与2个摄像头固定在同一平面，该相机实现了IMU 和图像数据的硬件同步。JETSON-TX2 基于NVIDIA PascalTMGPU 架构，内部搭载8 GB 内存，并配备多种标准硬件接口，可实时的处理图像流以及IMU 数据。移动小车如图4所示。

图4 自建的小车硬件平台Fig.4 Platform of self-build mobile vehicle

本文基于移动小车平台，选择武汉理工大学校园内的场景录制相应数据集，其中，部分采集案例如图5所示。

图5 小车室外场景的采集案例Fig.5 Examples from vehicle in outdoor scenes

设置每帧图像提取ORB 特征点数为2000 个，尺度因子为1.2，金字塔层数为8 层，图像帧输出频率为10 Hz。相机和IMU的内、外参以及图像畸变参数采用Kalibr工具箱[17]联合标定得到。标定结果如表1所示，其中，TBC为相机和IMU坐标系之间的转移矩阵；R为旋转矩阵；T为平移矩阵；fx,fy分别为x轴和y轴上的归一化焦距；cx,cy分别为图像的中心像素坐标和图像原点像素坐标之间相差的横向和纵向像素数；k1,k2为径向畸变参数；p1,p2为切向畸变系数。

表1 相机-IMU联合标定Table 1 Camera-IMU joint calibration parameters

(3)评价指标

选择均方根误差(Root Mean Square Errors,RMSE)作为定位精度评价指标，计算式为

式中：为x,y,z这3 个方向的估计位姿(位置和姿态)；θpose(xyz)为x,y,z这3 个方向的参考系统的位姿；N为参与计算位姿的总数。

选择失帧率(Frame-Lost-Rate,FLR)和路径丢失率(Path-Lost-Rate,PLR)作为评价系统鲁棒性的指标。测试发现，车辆在快速行驶条件下会出现图像帧丢失和路径丢失的现象。采用EVO工具箱评价输出轨迹，根据终端打印出的不同轨迹帧数和跟踪路径长度计算，公式为

式中：FR,FE分别为参考轨迹总帧数和系统实际跟踪帧数；PR,PE分别为参考轨迹路径长度和系统实际跟踪路径长度。

3.2 KITTI数据集实验与评价

选用KITTI09序列进行测试，该序列包含了多个传感器的测量信息，本文只提取图像和IMU 的测量信息进行实验。

(1)地图保存功能测试

建图模式下，首先，以较低播放速度将数据集中的图像以及IMU数据送入到系统中，然后，运行并保存相应的二进制地图文件。本文通过对比建图模式下输出的轨迹与参考轨迹来判断地图的精度。09 序列车辆行驶速度为8 m·s-1，50%速度(4 m·s-1)建图条件下输出的车辆轨迹和绝对位姿误差(Absolute pose error,APE)曲线如图6所示。

图6 50%速度的建图轨迹和APE曲线Fig.6 Curves of Trajectory and APE in 50%speed

该速度下系统能完整的跟踪车辆轨迹，验证了地图保存功能的有效性。为获取最佳的建图速度，本文测试了不同速度下(0.1～0.9 x)的地图精度，如表2所示。

表2 不同速度下建图的RMSETable 2 RMSE of map in different speed

结果表明，自动驾驶车辆在低速条件下建立的地图具有良好的精度。本文选择40%速度(3.2 m·s-1)建立的地图进一步测试基于地图辅助方法的定位效果。

(2)定位功能测试

为验证基于地图的定位效果，首先，车辆在低速(40%速度)驾驶条件下建立视觉地图，然后，调用该地图并控制车辆以正常或更快的速度在地图中行驶。本文对比了1倍和2倍速度下自动驾驶车辆的定位结果。1 倍速度建图(无地图辅助)模式和1倍速度定位(有地图辅助)模式下跟踪的轨迹与参考轨迹的对比结果如图7所示。

图7 1倍速度(8 m·s-1)下有、无地图辅助轨迹对比Fig.7 Trajectories with or without map aided in 1x speed(8 m·s-1)

由图7可知，这两种方法的轨迹均未丢失，且基于地图辅助方法的跟踪轨迹更加接近参考系统轨迹，定位精度更高。

当车辆速度加倍时，系统出现了跟踪丢失的情况，2倍速度建图(无地图辅助)模式和1倍速度定位(有地图辅助)模式下跟踪的轨迹与参考轨迹的对比结果如图8所示。

由图8可知，无地图辅助方法在后半段路程中出现跟踪丢失情况，而有地图辅助方法能够完整地跟踪车辆轨迹。尽管本文方法的视觉前端采用了鲁棒性较强的ORB 特征检测算法，但是，在2 倍速度且无地图辅助情况下，ORB 特征提取失败导致轨迹无法被完整的跟踪，证明了基于地图辅助的方法在快速驾驶条件下依然能保证精确且稳定的定位性能。

为量化本文所提方法的定位效果，选择失帧率、路径丢失率以及均方根误差这3个指标表示系统的鲁棒性和定位精度。量化结果如表3所示。

在1倍速度建图条件下，跟踪到的帧数为1591帧，跟踪路径长度为1713 m，根据式(7)～式(9)计算得到均方根误差为4.2 m，失帧率为0.6%，路径丢失率为0.7%；基于地图辅助定位方法的均方根误差为3.8 m，失帧率为0.4%，路径丢失率0.5%。在2倍速度建图条件下，跟踪到的帧数为693 帧，跟踪路径长度为778 m，计算得到的失帧率为56.7%，路径丢失率为54.9%；相比而言，基于地图辅助定位方法的RMSE为4.1 m，失帧率下降到了3.8%，路径丢失率下降到了1.4%。

3.3 小车实验与评价

室外场景中的小车行驶路径如图9所示，图中轨迹上的标记为小车行驶的起点。数据集录制过程中控制小车以2 m·s-1的速度正常行驶，上述KITTI数据集实验验证了，慢速条件下本文所提方法可以构建精度较高的地图，在缺乏高精度参考系统的条件下，本文将40%速度(0.8 m·s-1)构建的地图作为参考系统，然后，分别在2 倍(4 m·s-1)，4 倍(8 m·s-1)及8 倍(16 m·s-1)速度条件下测试和比较本文所提方法与ORB-SLAM2 方法的定位性能。基于小车数据集的测试界面如图10所示，图中方格代表当前帧提取的地图特征点，用椭圆包围起来的方格代表定位点，只有定位模式下界面才会出现定位点。2D轨迹如图11所示，3D位置(x,y,z) 变化曲线如图12所示，3D 欧拉角(Roll,Pitch,Yaw)变化曲线如图13所示。

图9 小车行驶路径Fig.9 Traveling path of vehicle

图10 真实数据集测试界面Fig.10 Testing interface of real world dataset

图11 2D轨迹对比曲线Fig.11 Comparison curve of 2D trajectory

图12 3D位置(x,y,z)对比曲线Fig.12 Comparison curves of 3D position (x,y,z)

图13 3D欧拉角(滚转，俯仰，航向)对比曲线Fig.13 Comparison curves of 3D Euler angle(Roll,Pitch,Yaw)

结果表明，快速驾驶条件下ORB-SLAM2方法无法完整地跟踪轨迹，其中，车辆以4 m·s-1(2x)速度行驶在第3个转弯处特征点提取失败，导致轨迹跟踪丢失；以8 m·s-1(4x)和16 m·s-1(8x)速度行驶在第1个转弯前和转完后均出现轨迹跟踪丢失。相比较之下，本文所提方法在4 m·s-1(2x)，8 m·s-1(4x)，16 m·s-1(8x)速度下均能够完整的跟踪轨迹。定量的实验结果如表4所示。

表4 定位量化结果Table 4 Quantitative results of localization

其中，参考系统的跟踪帧数为772 帧，跟踪路径长度为651 m。根据式(7)～式(9)可以分别计算出ORB-SLAM2在4 m·s-1速度下的失帧率和路径丢失率为23.58%和17.50%；在8 m·s-1速度下的失帧率和路径丢失率上升到65.28%和65.75%；在16 m·s-1速度下的失帧率和路径丢失率持续增加到72.41%和70.51%。相比较而言，本文所提方法在4 m·s-1速度下的失帧率为3.76%，路径丢失率为1.38%，RMSE 为2.59 m；在8 m·s-1速度下的失帧率和路径丢失率为3.89%和1.69%,RMSE 为2.61 m；在16 m·s-1速度下的失帧率和路径丢失率为4.27%和1.84%，RMSE为2.73 m。虽然车辆行驶速度加快，但这些指标只是略微的增加，验证了本文所提方法良好的连续定位性能。同时，也验证了该方法在精度和鲁棒性两方面均优于原始的ORBSLAM2方法。

4 结论

本文提出了一种地图辅助的视觉-惯性融合定位方法。该方法在ORB-SLAM2 的基础上做了如下改进：增加了地图保存功能；采用图优化算法融合了IMU 测量信息。和原始ORB-SLAM2 的功能一样，拓展版本可以在低速驾驶场景运行建图(即SLAM)模式，也可以作为定位模块应用于高速驾驶场景中。由于省略了实时建图过程，此系统的定位功能相较于传统的SLAM 方法具有更好的定位精度和鲁棒性。通过KITTI-09 数据集序列测试发现，室外较大场景的低速驾驶条件下构建的地图的定位精度可以保证在5 m 以内。实际自动驾驶小车实验表明，所提方法在4,8,16 m·s-1驾驶速度下的定位精度分别为2.59，2.61，2.73 m，图像失帧率和路径丢失率分别为3.76%和1.38%，3.89%和1.69%，4.27%和1.84%。同样条件下，ORB-SLAM2出现跟踪丢失，失帧率路径丢失率分别为23.58%和17.5%，65.28%和65.75%，72.41%和70.51%，相比较而言，本文方法显著提高了定位精度和鲁棒性，满足自动驾驶定位的地位需求。