2021年第6期
【摘要】为提高无人驾驶车辆的定位精度，提出一种基于GPS 与视觉同步定位与建图（VSLAM ）相融合的定位算法，在 VINS-Mono 基础上进行改进，提出了VINS-FAST 算法进行角点提取，并将VINS-FAST 与GPS 技术相结合，提出了GPS 与VSLAM 相融合的VINS-GPS 算法。试验结果表明：VINS-GPS 在KITTI 数据集07和09子集上的均方根误差较VINS-Mono 分别降低了18.16%和33.08%，改善了VSLAM 在大范围环境中表现较差的问题，同时提高了在GPS 信号薄弱或无GPS 信号环境中的定位精度。 主题词：无人驾驶车辆视觉同步定位与建图角点均匀分布策略GPS
中图分类号：TP24.6
文献标识码：A
DOI:10.19620/jki.1000-3703.20200839
Research on Localization Method for Unmanned Vehicle Based on GPS
and VSLAM
Gao Yang,Xu Yonggui,Liu Jiang
（Chang ’an University,Xi ’an 710054）
【Abstract 】In order to improve the localization accuracy of unmanned vehicles,a localization algorithm is proposed based on GPS and Visual Simultaneous Localization and Mapping (VSLAM)fusion.To improve the algorithm based on VINS-Mono,a corner extraction algorithm is proposed,namely the VINS-FAST.On this basis,through the VINS-FAST combined with GPS technology,a VINS-GPS algorithm that fuses GPS and VSLAM is proposed.The experimental results show that the root mean square error of VINS-GPS in KITTI 07data set and 09subsets is reduced by 18.16%and 33.08%
respectively compared with VINS-Mono,which improves the performance of VSLAM in a wide range of environments,and also improves the localization accuracy in the weak or no GPS signal environment.
Key words:Unmanned Vehicle,VSLAM,Corner uniform distribution strategy,GPS
高扬
徐永贵
刘江
（长安大学，西安710054）
*基金项目：长安大学研究生科研创新实践项目（300103703031）。
基于GPS 和视觉同步定位与建图的无人驾驶车辆
定位方法研究*
汽车技术·Automobile Technology
1前言
定位是在环境中寻相对于参考系的自我位置的任务[1]，是无人驾驶汽车感知的重要一环。GPS 定位技术是常用的绝对定位技术，其定位精度可达10~15m [2]，且没有累积误差，但在城市环境中，其定位精度受到环境影响，甚至可能无法定位[3]。尽管实时动态载波相位差分技术能够在大范围环境中提供厘米级高精度定位[4]，但在GPS 信号薄弱甚至缺失的环境下同样无法正常工
作。同步定位与建图（Simultaneous Localization and Mapping ，SLAM ）技术主要采用地图匹配实现定位，自行绘制地图，不依赖于先验地图信息，且定位精度可以达到毫米级。然而，其需要同时完成定位与绘图，计算量较大，且在大范围移动场景中存在累积误差[5]。视觉同步定位与建图（Visual Simultaneous Localization and Mapping ，VSLAM ）技术是SLAM 技术的一种，依赖视觉传感器作为主要感知手段，具有成本低、能实现三维位姿估计与建图等优点。
【引用格式】高扬，徐永贵，刘江.基于GPS 和视觉同步定位与建图的无人驾驶车辆定位方法研究[J].汽车技术,2021(6):19-24.
GAO Y,XU Y G,LIU J.Research on Localization Method for Unmanned Vehicle Based on GPS and VSLAM[J].Automobile Technology,2021(6):19-24.
-
-19
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的基础上进行改进，提出VINS-FAST算法，运行框架如图1所示，可以输出相机的6自由度位姿和地图三维点。
图1VINS-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所示。
图2VINS-FAST特征检测与追踪流程
通过FAST角点检测算法检测到得的角点容易集聚
在纹理、亮度特征变化明显的区域。如图3所示，图中
类圆形阴影表示检测到的FAST角点。此时检测出的FAST角点大多聚集在特征明显的位置，而在纹理弱的区域几乎没有检测到角点。这导致很多FAST角点描述的环境信息过于单调，未能充分代表周围环境，不利于后续匹配、定位等任务的实现。
图3FAST角点检测集中现象
因此，本文提出一种FAST角点均匀分布策略，利用四叉树结构改进FAST角点检测方法，具体流程如图4所示，其中，L为图像金字塔中图像的层数，W ima为图像的宽度，H ima为图像的高度。该策略在提取FAST角点时，需提前设定FAST阈值N a，即整张图像提取FAST角点的数量，最后基于四叉树均匀，按照Harris响应品质的优劣选取数量为N a的FAST角点。
算法设计了2个角点检测阈值：V ini为FAST角点检测的初始阈值，即每个格子中的初始角点数量；V min为最小阈值，即每个格子中的最少角点数量。如果每个格子中的角点数量小于V ini，则认为该区域纹理较弱，无法检测到足够数量的角点，此时，按照V min检测角点。因此，V min的设置应保证在图片中纹理弱的区域也能提取到FAST角点。针对分辨率为640像素×480像素的图片（如图3），取每格边长P=30像素。对图3采用FAST角点均匀分布策略后的角点检测结果如图5所示。
特征检测与追踪相机
IMU IMU预积分
滑动窗口紧耦合非线性优化
特征检索关键帧
初始化
否
是
否
是
闭环检测
系统初始化
6自由度位姿
地图三维点
Shi-Tomasi
角点提取
LK光流跟
踪
参考帧
当前帧
计算基础
矩阵
PnP
三角化
恢复运动
与地图点
构建
全局BA
优化
-
-20
汽车技术
2021年
第6期
图4FAST 角点均匀分布策略流程
图5
FAST 角点均匀分布效果
3基于GPS 与VINS-FAST 融合的VINS-GPS
为进一步消除SLAM 算法中累计误差的影响，提出一种GPS 与VINS-FAST 算法融合的VINS-GPS 算
法。GPS 与VINS-FAST 融合步骤如图6所示，将GPS 的定位结果与VINS-FAST 经过后端位姿优化的定位结果进行融合，最后采用非线性优化的方式求解融合后的位姿。
图6GPS 与VINS-FAST 融合框架
假设已经将VINS-FAST 与GPS 坐标系转换至同一坐标系，构造非线性优化的目标函数：
min F ()R gv ,T gv =∑i
N
P g i -()
R gv ·P v i +T gv （1）
式中，R gv 、T gv 分别为VINS-FAST 坐标系到GPS 坐标系转换的旋转矩阵和平移矩阵；N 为在VINS-FAST 坐标系与
GPS 坐标系下可用于融合的无人驾驶汽车位姿估计结果数量；P g
i 与P v
i 分别为i 时刻无人驾驶汽车在GPS 坐
标系和VINS-FAST 坐标系下的坐标。通过求解式（1）可以求得R gv 和T gv ，获得GPS 与VINS-FAST 的坐标系转换关系。
本文将融合定位算法VINS-GPS 的优化目标函数
设为：
F ()χ=χ-∑i =0N
∑j =0
M
E v
()i ,j ,k i ,k j -w g ∑i =0
N
E g
()
i （2）
式中，χ=[ξ1,…,ξN ]T 为待优化的变量；E v (i ,j ,k i ,k j )为VINS-
FAST 的约束；k i 、k j 分别为i 时刻和j 时刻VINS-FATS 估计的位姿；E g (i )为i 时刻GPS 的约束；ξi 为i 时刻无人驾驶汽车的位姿；w g 为GPS 定位结果进行融合时的权重；M 、
N 分别为融合时VINS-FAST 输出的定位结果数量和GPS 定位结果数量。
无人驾驶汽车i 时刻位姿估计值为：
ξi =R gv i ×ξv i +T gv i
（3）
式中，ξv
i 为由VINS-FAST 得到的i 时刻无人驾驶汽车
位姿估计结果；R gv i 、T gv i 分别为i 时刻VINS-FAST 坐标系到GPS 坐标系转换的旋转矩阵和平移矩阵。
本文将E v (i ,j ,k i ,k j )定义为：
E v ()i ,j ,k i ,k j =
z ()i ,j ,k i ,k j -z ()
i ,j ,k i ,k j 2
（4）
式中，z (i ,j ,k i ,k j )为直接由VINS-FAST 得到的i 时刻到j 时刻无人驾驶汽车位姿估计结果变化量；
z ()i ,j ,k i ,k j 为i 时刻到j 时刻无人驾驶汽车真实位姿位姿结果变化量。
对式（4）中z (i ,j ,k i ,k j )和z ()i ,j ,k i ,k j 进行展开，进一步得到：
E v ()i ,j ,k i ,k j =
ξv j -ξv i +ξj -ξi
2
（5）
没有GPS 定位结果时，本文设E g (i )=0，有GPS 定位
结果时，E g (i )定义为：
E g ()i =
p g i -[]
ξi
2
（6）
式中，p g
i 为通过GPS 得到的i 时刻无人驾驶汽车位姿；
[]ξi
为i 时刻无人驾驶汽车位姿估计值。
GPS 定位的几何精度因子（Geometric Dilution Of
Precision ，GDOP ）是衡量GPS 定位精度的重要参数，其值越小，表示GPS 定位精度越高，通常，该值可由GPS 信号接收机给出。因此，本文采用w g 作为定位结果融合权重设置的依据：
w g
=M N ·1
κ
（7）
式中，κ为几何精度因子。
4试验结果分析
为了验证本文所提出算法的有效性，首先在室内数据集Euroc [13]上验证了VINS-FAST 在室内等遮挡环境的定位效果，并在自动驾驶数据集KITTI 上验证了VINS-GPS 融合定位算法在室外大范围环境下的定位效果[14]。本文所有试验均基于ROS 系统kinetic 版进行，
GPS 6自由度位姿
非线性优化
6自由度位姿
将每一个根节点分成4个子节
点，若子节点中只有1个角点，停止分裂该子节点，若子节点中没有角点，则丢弃该子节点计算四叉树根节点的数量n =W /H （四舍五入）
基于四叉树均匀选取FAST 角点
相机图像
构建图像金字塔按照等比数列计算每一层图
像提取的FAST 角点
L >8
是
否
将该层图像划分P ×P 的格子
对每个格子首先按照V 检测FAST 角点检测不到角点的格子按照V 检测
FAST 角点将每个格子检测到的FAST 角点存
入四叉树中
L 的值增加1
继续将满足要求的子节点分裂成4个子节点，直到节点的数量大于或者等于FAST 阈值选取每个节点中性能最好的
FAST 角点得到均匀分布的角点
-
-21
硬件设备为戴尔游匣G3，处理器为i5-7300HQ。
4.1VINS-FAST角点检测测试
本文选取Euroc数据集中的MH04_difficlut和MH05_difficlut子集作为测试数据。试验参数设置为：V ini=15个，V min=5个，图像金字塔比例系数为1.2，FAST角点检测数量N a=150个。图7所示为系统启动阶段在MH04_difficlut和MH05_difficlut子集上的角点检测效果。
（a）改进前MH04_difficlut检测效果
（b）改进后MH04_difficlut检测效果
（c）改进前MH05_difficlut检测效果
（d）改进后MH05_difficlut检测效果
图7MH04_difficlut和MH05_difficlut子集中角点检测效果不难看出，图7a和图7c中提取所得角点存在明显的聚集现象，且多集中于特征明显的区域，而图7b和图7d中角点检测分布更加均匀，证明了本文角点分散策略的有效性。
4.2VINS-GPS在室外大范围环境中的测试
为测试本文提出的VINS-GPS定位算法在室外大范围环境中的表现，采用KITTI数据集07和09子集进行了测试。KITTI数据集07和09子集的环境如图8所示，是典型的室外环境。
图8KITTI数据集07和09子集部分试验环境由于KITTI数据集只提供了差分GPS定位结果，本文在该结果的基础上分别添加均值为3m的高斯白噪声以模拟无差分GPS定位结果。图9所示为09子集中GPS、差分GPS定位结果与车辆真实轨迹（Ground Truth）对比结果，其局部放大图如图10所示。由图10
不难看出，差分GPS定位结果比较接近真值，而无差分GPS定位结果与真值相差较大，且含有抖动。
图9GPS、差分GPS定位结果与真实轨迹对比结果
图10GPS、差分GPS定位结果和真实轨迹局部放大图
采用与4.1节相同的参数设置，作为对比，引入VINS-Mono算法进行了定位试验。设置Shi-Tomasi 角点检测阈值为30个，Shi-Tomasi角点检测数量为150个。本试验采用KITTI自带的评测工具KITTI_odometry_evaluation_tool评测2个系统的定位精度。
图11给出了VINS-Mono算法所得车辆轨迹与真实轨迹对比结果。由图11不难看出，VINS-Mono在起始
-150-5050150250350
x/m
600
500
400
300
200
100
-100
gps组合y
/
m
差分GPS
真实轨迹
起始点
GPS
140180220260300340
x/m
500
450
400
350
300
250
200
150
y
/
m
差分GPS
真实轨迹
GPS
-
-22
汽车技术
2021年
第6期
阶段位姿估计结果与真值相差较小，定位精度较高。然而，随着移动距离增加，系统累积误差逐渐增大，导致VINS-Mono 的定位精度下降。
（a ）07子集
（b ）09子集
图11
VINS-Mono 算法定位结果与真实轨迹对比结果
图12给出了采用VINS-GPS 定位算法所得车辆轨迹与真实轨迹的对比结果。
（a ）07子集
（b ）09子集
图12VINS-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所示。
表1VINS-GPS 各模块运行时间和频率
不难看出，VINS-GPS 的角点检测与跟踪可以稳定在10Hz 运行，位姿优化与GPS 融合耗时也相对较少。
综上所述，本文提出的VINS-GPS 融合定位算法，不仅能够改善VINS-Mono 在室外大范围环境表现较差
的问题，同时还可以提高GPS 的定位精度。
5结束语
本文基于VINS-Mono 提出一种GPS 与VINS-FAST 算法融合的VINS-GPS 算法，改善了FAST 角点检测集中的现象，使得在纹理弱的区域也能检测到FAST 角点，同时进一步消除了SLAM 算法运行过程
中的累计误差，
能够为无人驾驶汽车在室外大规模复杂环境下提供较高精度的定位。
参
考文
献
[1]KUUTTI S,FALLAH S,KATSAROS K,et al.A Survey of the State-of-the-Art Localization Techniques and Their Potentials for Autonomous Vehicle Applications[J].IEEE Internet of Things Journal,2018,5(2):829-846.
[2]DHONGADE A P,KHANDEKAR M A.GPS and IMU Integration on an Autonomous Vehicle Using Kalman Filter (LabView Tool)[C]//2019International Conference on Intelligent Computing and Control Systems (ICCS),2019.[3]YURTSEVER E,LAMBERT J,CARBALLO A,et al.A
Survey of Autonomous Driving:Common Practices and Emerging Technologies[J].IEEE Access,2020,8:58443-58469.
[4]GIANLUCA F,MARCO P,GIANLUCA M.Loose and Tight
GNSS/INS Integrations:Comparison of Performance
Assessed in Real Urban Scenarios[J].Sensors,2017,17(2):27.
[5]YURTSEVER E,LAMBERT J,CARBALLO A,et al.A
-200-150-100-50
x /m
150100500-50
-100
y /m
差分GPS
真实轨迹
起始点
-200
200
400
x /m
6005004003002001000
-100
y /m
差分GPS
真实轨迹
起始点-200-150-100
-
500
x /m
150100500-50
-100
y /m
差分GPS
真实轨迹
起始点
-200
0200
400
x /m
6005004003002001000
-100y /m
真实轨迹差分GPS 起始点
模块
改进后的FAST 角点检测
位姿优化GPS 融合
耗费时间/s 49.2444529.9809016.63865
频率/Hz 10
-
-23
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议