基于GPS和视觉同步定位与建图的无人驾驶车辆定位方法研究＊

高扬 徐永贵 刘江

（长安大学，西安 710054）

主题词：无人驾驶车辆 视觉同步定位与建图 角点均匀分布策略 GPS

1 前言

定位是在环境中寻找相对于参考系的自我位置的任务[1]，是无人驾驶汽车感知的重要一环。GPS 定位技术是常用的绝对定位技术，其定位精度可达10～15 m[2]，且没有累积误差，但在城市环境中，其定位精度受到环境影响，甚至可能无法定位[3]。尽管实时动态载波相位差分技术能够在大范围环境中提供厘米级高精度定位[4]，但在GPS 信号薄弱甚至缺失的环境下同样无法正常工作。同步定位与建图（Simultaneous Localization and Mapping，SLAM）技术主要采用地图匹配实现定位，自行绘制地图，不依赖于先验地图信息，且定位精度可以达到毫米级。然而，其需要同时完成定位与绘图，计算量较大，且在大范围移动场景中存在累积误差[5]。视觉同步定位与建图（Visual Simultaneous Localization and Mapping，VSLAM）技术是SLAM 技术的一种，依赖视觉传感器作为主要感知手段，具有成本低、能实现三维位姿估计与建图等优点。

GPS 与SLAM 的定位方法具有各自的局限性，但两者之间也具备鲜明的互补性。事实上，GPS 与VSLAM技术融合定位在许多研究中都有涉及。Markus Schreiber[6]提出一种GPS 与VSLAM 紧耦合的无人驾驶汽车低成本定位方案，在GPS信号差的区域依然能保持系统的鲁棒性，但其定位精度不高。Mutz[7]引入GPS 信号帮助SLAM 算法进行闭环检测，减少SLAM 算法的计算量，但其未融合二者的定位信息，GPS 定位信息利用率低。许豪[8]提出一种GPS 与地图匹配的组合定位方法，结合了二者的优点，能够为无人驾驶汽车在室外环境提供较高精度的定位，但只适用于提前构建好地图的区域。Chen[9]等提出一种基于GPS与ORB-SLAM[10]紧耦合的无人驾驶定位方法，可以实现无人驾驶汽车在室外大范围环境中的定位，但其在动态环境中鲁棒性不足。

目前，GPS 与VSLAM 系统融合的方案大多对GPS定位信息利用率低或系统鲁棒性不高，进而无法应用于无人驾驶汽车的定位。因此，本文提出一种GPS 与VSLAM 技术融合的无人驾驶汽车定位方法，首先基于VINS-Mono 算法提出了改进算法VINS-FAST，将已有的Shi-Tomasi 角点检测替换为鲁棒性更强的FAST 角点。同时，针对FAST 角点检测所识别的特征存在集中度较高的问题，本文进一步提出一种FAST 角点均匀分布策略，使得在纹理弱的区域也能检测到FAST角点，最后提出一种基于GPS与VINS-FAST融合的定位算法。

2 基于VINS-Mono改进的VINS-FAST算法

VINS-Mono 是香港科技大学沈劭劼团队提出的一种VSLAM 算法[11]，具有在室内环境中定位精度高等优点。本文在VINS-Mono的基础上进行改进，提出VINSFAST 算法，运行框架如图1 所示，可以输出相机的6 自由度位姿和地图三维点。

图1 VINS-FAST算法流程

VSLAM 系统中常用的角点包括Shi-Tomasi 角点、尺度不变特征转换（Scale-invariant feature transform，SIFT）和FAST（Features from Accelerated Segment Test）角点。FAST角点检测算法于2006年提出，以检测速度快著称[12]。2015年，R.Mur-Artal进一步改进了FAST角点，添加了尺度和旋转的描述，进而使FAST角点具有尺度不变性和方向不变性[10]。由于Shi-Tomasi 角点无法描述图像尺度信息，仅对图像方向信息进行了描述，而FAST 角点同时描述了尺度信息与方向信息，从而大幅提升了FAST角点在不同图片中进行光流跟踪时的鲁棒性。因此，本文首先针对VINS-Mono 算法进行改进，将其采用的Shi-Tomasi角点替换为鲁棒性更强的FAST角点，提出一种VINS-FAST视觉同步定位与建图算法，其流程如图2所示。

图2 VINS-FAST特征检测与追踪流程

通过FAST角点检测算法检测到得的角点容易集聚在纹理、亮度特征变化明显的区域。如图3 所示，图中类圆形阴影表示检测到的FAST 角点。此时检测出的FAST 角点大多聚集在特征明显的位置，而在纹理弱的区域几乎没有检测到角点。这导致很多FAST角点描述的环境信息过于单调，未能充分代表周围环境，不利于后续匹配、定位等任务的实现。

图3 FAST角点检测集中现象

因此，本文提出一种FAST角点均匀分布策略，利用四叉树结构改进FAST 角点检测方法，具体流程如图4所示，其中，L为图像金字塔中图像的层数，Wima为图像的宽度，Hima为图像的高度。该策略在提取FAST 角点时，需提前设定FAST 阈值Na，即整张图像提取FAST 角点的数量，最后基于四叉树均匀，按照Harris 响应品质的优劣选取数量为Na的FAST角点。

图4 FAST角点均匀分布策略流程

算法设计了2 个角点检测阈值：Vini为FAST 角点检测的初始阈值，即每个格子中的初始角点数量；Vmin为最小阈值，即每个格子中的最少角点数量。如果每个格子中的角点数量小于Vini，则认为该区域纹理较弱，无法检测到足够数量的角点，此时，按照Vmin检测角点。因此，Vmin的设置应保证在图片中纹理弱的区域也能提取到FAST 角点。针对分辨率为640 像素×480 像素的图片（如图3），取每格边长P=30 像素。对图3 采用FAST 角点均匀分布策略后的角点检测结果如图5所示。

图5 FAST角点均匀分布效果

3 基于GPS与VINS-FAST融合的VINS-GPS

为进一步消除SLAM算法中累计误差的影响，提出一种GPS 与VINS-FAST 算法融合的VINS-GPS 算法。GPS与VINS-FAST融合步骤如图6所示，将GPS的定位结果与VINS-FAST经过后端位姿优化的定位结果进行融合，最后采用非线性优化的方式求解融合后的位姿。

图6 GPS与VINS-FAST融合框架

假设已经将VINS-FAST 与GPS坐标系转换至同一坐标系，构造非线性优化的目标函数：

式中，Rgv、Tgv分别为VINS-FAST坐标系到GPS坐标系转换的旋转矩阵和平移矩阵；N为在VINS-FAST坐标系与GPS 坐标系下可用于融合的无人驾驶汽车位姿估计结果数量；分别为i时刻无人驾驶汽车在GPS 坐标系和VINS-FAST坐标系下的坐标。

通过求解式（1）可以求得Rgv和Tgv，获得GPS 与VINS-FAST的坐标系转换关系。

本文将融合定位算法VINS-GPS 的优化目标函数设为：

式中，χ=[ξ1,…,ξN]T为待优化的变量；Ev(i,j,ki,kj)为VINSFAST 的约束；ki、kj分别为i时刻和j时刻VINS-FATS 估计的位姿；Eg(i)为i时刻GPS的约束；ξi为i时刻无人驾驶汽车的位姿；wg为GPS定位结果进行融合时的权重；M、N分别为融合时VINS-FAST 输出的定位结果数量和GPS定位结果数量。

无人驾驶汽车i时刻位姿估计值为：

本文将Ev(i,j,ki,kj)定义为：

式中，z(i,j,ki,kj)为直接由VINS-FAST 得到的i时刻到j时刻无人驾驶汽车位姿估计结果变化量；为i时刻到j时刻无人驾驶汽车真实位姿位姿结果变化量。

没有GPS 定位结果时，本文设Eg(i)=0，有GPS 定位结果时，Eg(i)定义为：

GPS 定位的几何精度因子（Geometric Dilution Of Precision，GDOP）是衡量GPS 定位精度的重要参数，其值越小，表示GPS定位精度越高，通常，该值可由GPS信号接收机给出。因此，本文采用wg作为定位结果融合权重设置的依据：

式中，κ为几何精度因子。

4 试验结果分析

为了验证本文所提出算法的有效性，首先在室内数据集Euroc[13]上验证了VINS-FAST 在室内等遮挡环境的定位效果，并在自动驾驶数据集KITTI 上验证了VINS-GPS 融合定位算法在室外大范围环境下的定位效果[14]。本文所有试验均基于ROS系统kinetic版进行，硬件设备为戴尔游匣G3，处理器为i5-7300HQ。

4.1 VINS-FAST角点检测测试

本文选取Euroc 数据集中的MH04_difficlut 和MH05_difficlut 子集作为测试数据。试验参数设置为：Vini=15个，Vmin=5个，图像金字塔比例系数为1.2，FAST角点检测数量Na=150 个。图7 所示为系统启动阶段在MH04_difficlut和MH05_difficlut子集上的角点检测效果。

图7 MH04_difficlut和MH05_difficlut子集中角点检测效果

不难看出，图7a 和图7c 中提取所得角点存在明显的聚集现象，且多集中于特征明显的区域，而图7b和图7d 中角点检测分布更加均匀，证明了本文角点分散策略的有效性。

4.2 VINS-GPS在室外大范围环境中的测试

为测试本文提出的VINS-GPS 定位算法在室外大范围环境中的表现，采用KITTI 数据集07 和09 子集进行了测试。KITTI 数据集07 和09 子集的环境如图8 所示，是典型的室外环境。

图8 KITTI数据集07和09子集部分试验环境

由于KITTI数据集只提供了差分GPS定位结果，本文在该结果的基础上分别添加均值为3 m 的高斯白噪声以模拟无差分GPS 定位结果。图9 所示为09 子集中GPS、差分GPS 定位结果与车辆真实轨迹（Ground Truth）对比结果，其局部放大图如图10 所示。由图10不难看出，差分GPS 定位结果比较接近真值，而无差分GPS定位结果与真值相差较大，且含有抖动。

图9 GPS、差分GPS定位结果与真实轨迹对比结果

图10 GPS、差分GPS定位结果和真实轨迹局部放大图

采用与4.1 节相同的参数设置，作为对比，引入VINS-Mono 算法进行了定位试验。设置Shi-Tomasi角点检测阈值为30 个，Shi-Tomasi 角点检测数量为150 个。本试验采用KITTI 自带的评测工具KITTI_odometry_evaluation_tool 评测2 个系统的定位精度。

图11给出了VINS-Mono算法所得车辆轨迹与真实轨迹对比结果。由图11 不难看出，VINS-Mono 在起始阶段位姿估计结果与真值相差较小，定位精度较高。然而，随着移动距离增加，系统累积误差逐渐增大，导致VINS-Mono的定位精度下降。

图11 VINS-Mono算法定位结果与真实轨迹对比结果

图12 给出了采用VINS-GPS 定位算法所得车辆轨迹与真实轨迹的对比结果。

图12 VINS-GPS算法定位结果与真实轨迹对比结果

由图12可知，本文提出的VINS-GPS定位算法估计的定位结果基本接近真实轨迹，同时VINS-GPS 在KITTI 数据集07 和09 子集的均方根误差较VINS-Mono分别降低了18.16%和33.08%，较VINS-Mono 定位精度有了很大的提升。这是由于本文提出的VINS-GPS 定位算法融合了VINS-Mono 与GPS 两者的定位优点，能够在室外大范围环境中获得良好表现。本文还统计了VINS-GPS在09子集上各模块的耗费时间和运行频率，如表1所示。

表1 VINS-GPS各模块运行时间和频率

不难看出，VINS-GPS 的角点检测与跟踪可以稳定在10 Hz运行，位姿优化与GPS融合耗时也相对较少。

综上所述，本文提出的VINS-GPS 融合定位算法，不仅能够改善VINS-Mono 在室外大范围环境表现较差的问题，同时还可以提高GPS的定位精度。

5 结束语

本文基于VINS-Mono 提出一种GPS 与VINS-FAST算法融合的VINS-GPS 算法，改善了FAST 角点检测集中的现象，使得在纹理弱的区域也能检测到FAST角点，同时进一步消除了SLAM算法运行过程中的累计误差，能够为无人驾驶汽车在室外大规模复杂环境下提供较高精度的定位。