基于核密度估计的城市动态密集场景激光雷达定位 
下载： 1061次
1 引言
定位技术是自动驾驶领域中一项非常关键的基础性工作。基于全球导航卫星系统(GNSS)的定位精度只能达到10 m左右[1],无法达到自动驾驶的要求。基于载波相位差分(RTK)的卫星定位在信号良好的条件下可以保持厘米级的精度,但其信号极易因周围高大物体的遮挡而减弱或产生多径效应,从而导致定位结果的波动和定位精度的下降。将RTK-GNSS与惯性测量单元(IMU)或里程计进行结合的组合定位技术可以在一定程度上改善该问题。但IMU和里程计均存在累积误差,如果要在卫星定位长期不稳定或失效的环境下保持高精度定位,则需要IMU具有很高的精度,而这种成本是非常高的。
为了以较低的成本实现高精度定位,目前主流的方法是使用激光雷达或视觉传感器,结合已构建好的高精度地图,通过匹配实时数据与地图数据,将定位精度控制在可用范围内。通常情况下,基于视觉方法定位的成本低于使用激光雷达定位的成本,但其测距精度低,且易受光线等因素的影响,因此定位精度难以满足实际需求。相比于视觉方法而言,激光雷达的精度和可靠性均较优。
在定位数据方面,激光雷达可以提供周围环境中物体表面的三维位置信息和反射强度信息,二者均可以用来定位。基于反射率的定位[2-5]使用路面上的标志和车道线等平面静态信息可以实现精确定位,该方法的定位精度高,受车辆或行人等动态目标的影响小,但该方法的应用场景比较有限,雨雪天气会导致路面标志模糊不清,从而影响定位精度;在非路口路段,路面缺乏纵向特征,纵向定位精度难以保证;而在乡村、野外等非结构化道路环境中,路面无法提供有效的道路标志信息,从而导致该定位方法完全失效。基于高度信息的定位[6-10]应用场景更加广泛,受天气影响小,且均适用于结构化和非结构化道路,但该方法面临的一个关键问题是需要克服环境中的运动目标(行驶的车辆和行人)、多变的静态目标(城市改造,停靠的车辆,随季节变化的植被、树木)以及由此产生的遮挡对激光雷达定位的影响。Wan等[11]采用将两种信息进行融合的定位方法,根据定位结果的置信度,自适应地调整二者的权重,但当地面标志被污染且周围环境存在大量动态目标时,该方法便很难得到稳定、精确的定位结果。
在定位算法方面,粒子滤波(PF)是移动机器人领域中应用得最广泛的算法之一[12-13]。考虑到无人驾驶的应用场景中,卫星定位系统通常可以将定位误差限制在几米的范围内,Levinson等[3]在PF的基础上提出了直方图滤波(HF)算法。该算法的优点在于不必担心粒子在正确位置附近丢失,具有比PF更高的稳定性。HF和PF都属于贝叶斯滤波,该类方法已成为目前主流的定位方法,能够在多数场景中取得满足自动驾驶需求的精确定位结果,但是对于一些复杂或极端场景,仍存在一些局限。当全球定位系统(GPS)提供的初始定位偏差较大时,定位的搜索空间会增大,HF和PF的效率都会显著下降,并且二者均有收敛到局部最优的风险,特别是在高动态的城市环境中,实时场景与地图历史数据往往存在一定差异,使得该风险进一步增大。
为了解决上述问题,并使基于地图的定位具有更高的稳健性和环境适应性,本文基于目前主流的HF算法,引入了多层次随机采样一致性(ML-RANSAC)算法[14],以克服初始位置误差大、环境干扰多的问题。ML-RANSAC算法的定位精度较低,无法满足自动驾驶的需求,并且在一些特征差异不明显的场景中易出现定位结果波动的问题,而HF算法具有精度高、波动小的优点。因此,可将二者结合起来实现优势互补。随机采样一致性(RANSAC)算法得到的往往是确定性的结果,而HF是一种概率定位方法,为此,本文对ML-RANSAC算法的输出和HF算法的输入分别进行改造,使用分支定界[6]改进的HF算法实现精确定位,并使用高斯核密度估计(KDE)[15]作为中间衔接,将二者统一到同一概率框架中。
2 激光雷达地图的生成
首先使用RTK-GPS和IMU采集地图数据[16],并对采集的数据进行地面分割和动态障碍剔除[14],然后采用Graph-SLAM技术进行三维重建,最终生成二维概率栅格地图,地图中主要包含障碍物和地面反射率的概率分布。文献[ 2]采用高斯分布来表示每个栅格的反射率信息和高度信息,虽然这种方式能够取得良好的定位效果,但事实上,很多栅格中的数据分布并不满足高斯分布,尤其是一些比较关键的信息,如:障碍物和地面交界处的栅格,其高度信息必然不满足高斯分布;在车道线、地面标志的边缘处,高反射率的涂料和低反射率的柏油路组成的栅格的反射率数据也不满足高斯分布。文献[ 6]采用高斯混合模型(GMM)对每个栅格进行建模,相对于高斯分布,混合高斯模型对环境的表示更合理,也更能表达环境的实际情况。
采用含有两个分量的高斯模型分别对每个栅格的反射率和高度分布进行建模。对于高度信息,两个高斯分量分别描述地面和障碍物的高度分布;对于反射率信息,两个分量分别描述地面上的高反射率数据和低反射率数据的分布。此外,每个栅格还统计了包含的扫描点个数。栅格的大小为20 cm×20 cm。
图 1. 利用所提算法构建的高精度地图。(a)高度地图;(b)反射率地图
Fig. 1. High-accuracy maps constructed by proposed algorithm. (a) Height map; (b) reflectivity map
对于随机变量
式中:
3 激光雷达定位
激光雷达定位通过将实时采集的点云数据与先验概率地图的栅格数据进行匹配来获取车辆在世界坐标系下的二维水平位置和航向角。引入一种改进的RANSAC算法,并提出一种基于高斯混合模型的概率框架,将该算法与传统的滤波定位算法高效地融合在一起,使其能够在初始误差大、动态目标多的复杂场景中实现快速、稳定、精确的定位。定位算法流程图如
3.1 基于ML-RANSAC算法的粗定位
ML-RANSAC算法是一种改进的RANSAC配准算法,相比于传统的RANSAC算法,能够适用更大的初始配准误差和更复杂的动态场景。由于PF和HF在采样空间较大时效率下降明显,因此本研究采用ML-RANSAC算法进行粗定位,为滤波定位提供较准确的初始位姿,以确保其始终运行在效率较高的采样空间中,同时将无用的动态目标剔除,为滤波定位提供更加可靠的点云数据。此外,RANSAC算法的每次采样都具有随机性,模型的每次估计结果之间具有独立性。因此,可以利用该算法解决粒子贫化问题, 帮助滤波定位跳出局部最优。
粗定位过程由两个步骤组成。首先需要对采集的实时点云和地图数据分别进行去噪[17-18]、分割和特征提取。采用基于多元几何约束的区域生长算法对激光雷达点云和栅格地图进行同步去噪和分割,并将分割后各目标的重心点作为特征点进行后续的配准。多元几何约束包括栅格内的点云密度和栅格的高度信息。对于实时点云数据,可以通过栅格化的方法统计上述信息;对于栅格地图,则可以根据每个栅格存储中的高斯混合模型和扫描点个数计算得到上述信息。
然后提取点云分割后每个目标的重心点,进行ML-RANSAC计算。在ML-RANSAC计算过程中,每次计算都需要从一帧数据
式中:
在利用ML-RANSAC算法找到变换矩阵后,采用最近邻搜索和匈牙利算法对变换后的点云进行静态目标提取。
3.2 基于分支定界HF的精确定位
PF和HF都是基于马尔可夫原理的贝叶斯滤波器的算法。PF通过从先验分布中随机抽取粒子来近似后验,HF则通过将状态空间分解成多个有限区域来近似后验。在初始位姿误差较小的环境中,HF具有比PF更好的稳定性。与大多数滤波器一样,HF也包含预测和更新两步。本节只分析和改进更新步,而预测步则整合到3.3节的定位框架中。
经典的HF一次性将整个搜索空间分割为固定大小的栅格。例如,定位栅格的目标大小为15 cm×15 cm,则直接使用15 cm的栅格进行划分。本研究将该过程转换为一个迭代的分支定界过程。基于分支定界的HF允许在精确定位起始阶段使用尺寸更大的栅格对搜索空间进行划分,从而减少划分的栅格数,然后通过分支定界进行迭代,在控制栅格数量的同时,通过缩小栅格尺寸来提高定位精度。
在第1次空间划分时使用更大的栅格,本研究使用大小为60 cm×60 cm的栅格。使用(4)式计算每个定位栅格中心的概率,即
式中:
筛选出若干概率较高的定位栅格,并将它们分别按维度二分,取分裂后的栅格中心继续计算定位概率。对该过程进行迭代,直至栅格的尺寸不大于目标栅格的尺寸,取概率最大的位置作为本次激光雷达定位的结果。
3.3 基于高斯核密度估计的概率定位框架
与大多数概率定位框架类似,本研究中的定位框架包括位姿估计和位姿预测两部分,其中高斯核密度估计主要用来在位姿估计部分对基于ML-RANSAC算法的粗定位和基于分支定界HF算法的精确定位进行衔接。
3.3.1 当前时刻的位姿估计
传统的RANSAC算法通常在多次配准结果中选取内点比例最高的点作为粗配准结果,虽然这种做法在大多数情况下是可靠的,但在一些环境相似度比较高的场景中,可能有很多位置具有较高的内点比例,且比例最高的位置并不一定是车辆的实际位置。因此,使用一个区域内的概率分布比使用一个点来描述定位结果更加合理,同时也有助于后续的滤波定位和传感器融合等。
为此,本研究采用高斯混合模型表示车辆位置在一个区域内的概率分布。不同于传统的RANSAC算法只使用内点比例最大的结果,本研究将RANSAC算法中每一次满足约束条件的采样和配准得到的内点比例作为该位置的权重或置信度,使用核密度估计的方法对本次RANSAC计算中的全部定位结果进行聚类和拟合,估计高斯混合模型的参数。
核密度估计是一种用于估计概率密度函数的非参数方法,
式中:
本研究需要对车辆的
在RANSAC算法中,内点距离阈值
在该分布的峰值(概率分布函数可能存在多个峰值)附近使用HF得到若干精确的高概率位置,并依据估计位置的高斯混合模型概率分布和先验概率(上一时刻对当前时刻的位姿预测),计算这些位置在当前时刻的后验概率。以后验概率最大的估计位置作为本次定位的最终位置。
3.3.2 下一时刻的位姿预测
根据马尔可夫原理和贝叶斯法则,利用IMU或里程计输出的运动状态信息,分别预测这些位置对应的下一时刻定位的先验分布,即
式中:
4 结果与分析
实验使用Velodyne HDL-64E型激光雷达进行点云数据的采集,算法在主频为2.40 GHz的四核工控机上运行,内存为8 GB,采用Ubuntu 16.04操作系统。
选取的测试路段如
将所提算法与当前主流的HF定位算法以及文献[ 14]中提出的ML-RANSAC定位算法进行对比。根据初始位置偏差的类型不同,实验分为两部分,即初始位姿误差持续增长的动态场景定位实验(实验1)和初始位姿误差随机跳动的动态场景定位实验(实验2),且均在含有大量动态目标的道路场景中进行实验。
4.1 初始位姿误差持续增长的动态场景定位实验
实验1在不使用GNSS的情况下仅使用一个精度较低的IMU进行车辆位姿的推算,将推算结果作为激光雷达定位的输入,测试算法在有大量车辆和行人存在的高动态城市环境中的定位精度和稳定性。这种由纯推算产生的累积误差是连续缓慢变化的,不存在位置的跳变。
采集基于差分GPS的车辆行驶轨迹,并在此基础上使用离线激光雷达SLAM技术作为车辆位置的真实数据。实验1包括
3种算法在实验1中的定位误差如
图 4. 使用所提算法定位前后的运动轨迹
Fig. 4. Moving trajectories before and after localization by proposed algorithm
图 5. 3种算法在实验1中的定位结果。(a)横向误差;(b)纵向误差;(c)航向角误差
Fig. 5. Comparison of localization results in experiment 1 by three algorithms. (a) Lateral error; (b) longitudinal error; (c) heading angle error
图 6. 路口的定位结果对比。(a)俯视图;(b)三维视图;(c)相机前视图;(d) HF算法;(e) ML-RANSAC算法;(f)所提算法
Fig. 6. Localization results of cross road. (a) Aerial view; (b) 3D view; (c) forward camera view; (d) HF algorithm; (e) ML-RANSAC algorithm; (f) proposed algorithm
图 7. 直道的定位结果对比。(a)俯视图;(b)三维视图;(c)相机前视图;(d) HF算法;(e) ML-RANSAC算法;(f)所提算法
Fig. 7. Localization results of straight road. (a) Aerial view; (b) 3D view; (c) forward camera view; (d) HF algorithm; (e) ML-RANSAC algorithm; (f) proposed algorithm
4.2 初始位姿误差随机跳动的动态场景定位实验
实验2在实验1的基础上进一步提升难度,在存在累积误差的推算结果中增加一个随机偏置作为激光雷达定位的输入,模拟GPS因不完全遮挡和多径效应而产生的定位突变情况,测试在有跳变误差存在的情况下,所提算法的精度和稳定性。由于在长期推算的情况下,定位已产生了很大的累积误差,这时跳变结果可能是正确的,也可能是错误的,因此不能简单地通过运动的连续性来滤除。
实验2(初始位姿误差随机跳动的动态场景定位实验)分为3组,即航向角偏移量在(-5°,5°)内随机波动,而水平位置偏移量分别在2.5,5,10 m的半径范围内随机波动。
图 8. 3种算法在不同初始位姿误差时的水平位置误差分布。(a)散点图;(b)直方图
Fig. 8. Horizontal position error distributions by three algorithms under different initial pose deviations. (a) Scatter diagram; (b) histogram
表 1. 3种算法在不同初始位置偏差时的定位统计结果
Table 1. Localization statistical results by three algorithms under different initial pose deviations
| ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
5 结论
在使用激光雷达点云数据和高精度先验地图进行定位的过程中,针对目前主流的HF算法存在的在大初始误差、高动态干扰环境下定位效果差、易陷入局部最优的问题,提出了一种将ML-RANSAC算法与HF算法相结合的定位算法,并采用高斯核密度估计的方法将两种算法高效地统一到同一概率框架上,实现了两种算法的优势互补。ML-RANSAC算法具有很强的稳健性,能够适应较大的初始定位偏差和较多的动态目标干扰,但算法的精度较低,且在特征相似度较高的场景中易出现定位波动的情况;而HF算法则具有较高的定位精度,但对动态环境的适用能力较差,在运动目标较多的场景中易陷入局部最优,且在初始定位偏差较大时,定位效率会大幅下降。本课题组提出的概率框架不仅有效弥补了上述两种算法各自的缺点,还进一步提升了算法的性能,不仅能在常见的复杂动态场景中实现更加稳定和高效的定位,还能容忍比之前更大的初始位姿误差,有助于解决卫星定位失效而导致的位置出现大幅波动和漂移的问题。
选用车辆和人流密集的城市道路场景进行了实验。结果表明,无论是在连续累积的初始误差情况下,还是在离散随机跳动的初始误差情况下,所提算法均保持了较高的定位精度和较快的定位速度。相比于改进之前的ML-RANSAC算法和HF算法,所提算法在保证定位精度和速度的前提下,进一步提升了定位的稳健性,能够适应更加复杂和极端的场景。
[2] LevinsonJ, MontemerloM, ThrunS. Map-based precision vehicle localization in urban environments[C]//Burgard W, Brock O, Stachniss C.Proceedings of the 3rd Conference on Robotics: Science and Systems, June 27-30, 2007, Georgia Institute of Technology, Atlanta, Georgia, USA. Cambridge: The MIT Press, 2007: 352.
[3] LevinsonJ, ThrunS. Robust vehicle localization in urban environments using probabilistic maps[C]// Proceedings of IEEE International Conference on Robotics and Automation, May 3-7, 2010, Anchorage, AK, USA. New York: IEEE, 2010: 4372- 4378.
[6] Wolcott RW, Eustice RM. Fast LIDAR localization using multiresolution Gaussian mixture maps[C]// Proceedings of IEEE International Conference on Robotics and Automation, May 26-30, 2015, Seattle, WA, USA. New York: IEEE, 2015: 2814- 2821.
[7] Wolcott R W, Eustice R M. Robust LIDAR localization using multiresolution Gaussian mixture maps for autonomous driving[J]. The International Journal of Robotics Research, 2017, 36(3): 292-319.
[9] SchlichtingA, Brenner C. Vehicle localization by LiDAR point correlation improved by change detection[J]. ISPRS-International Archives of the Photogrammetry, RemoteSensing and Spatial InformationSciences, 2016, XLI-B1: 703- 710.
[11] Wan GW, Yang XL, Cai RL, et al. Robust and precise vehicle localization based on multi-sensor fusion in diverse city scenes[C]//Proceedings of IEEE International Conference on Robotics and Automation, May 21-25, 2018, Brisbane, QLD, Australia. New York: IEEE, 2018: 4670- 4677.
[12] Thrun S. Probabilistic robotics[J]. Communications of the ACM, 2002, 45(3): 52-57.
[13] DoucetA, de FreitasN, MurphyK, et al. Rao-Blackwellised particle filtering for dynamic Bayesian networks[C]//Proceedings of the 16 th Conference on Uncertainty in Artificial Intelligence, June 30-July 3, 2000, San Francisco, CA, USA. San Francisco: Morgan Kaufmann Publishers Inc. , 2000: 176- 183.
[14] 王任栋, 徐友春, 齐尧, 等. 一种鲁棒的城市复杂动态场景点云配准方法[J]. 机器人, 2018, 40(3): 257-265.
[15] Lu WJ, SeignezE, Rodriguez F S A, et al. Lane marking based vehicle localization using particle filter and multi-kernel estimation[C]//Proceedings of the 13 th International Conference on Control, Automation, Robotics and Vision, December 10-12, 2014, Marina Bay Sands, Singapore. New York: IEEE , 2014: 601- 606.
[16] 韩栋斌, 徐友春, 王任栋, 等. 基于多对点云匹配的三维激光雷达外参数标定[J]. 激光与光电子学进展, 2018, 55(2): 022803.
[17] 赵凯, 徐友春, 李永乐, 等. 基于VG-DBSCAN算法的大场景散乱点云去噪[J]. 光学学报, 2018, 38(10): 1028001.
[18] 熊风光, 霍旺, 韩燮, 等. 三维点云中关键点误匹配剔除方法[J]. 光学学报, 2018, 38(2): 0210003.
Article Outline
王任栋, 李华, 赵凯, 徐友春. 基于核密度估计的城市动态密集场景激光雷达定位[J]. 光学学报, 2019, 39(5): 0528003. Rendong Wang, Hua Li, Kai Zhao, Youchun Xu. Robust Localization Based on Kernel Density Estimation in Dynamic Diverse City Scenes Using Lidar[J]. Acta Optica Sinica, 2019, 39(5): 0528003.
PDF全文
