基于三维点云地图和ESKF的无人车融合定位方法

崔文; 薛棋文; 李庆玲; 王凤栋; 郝雪儿

doi:10.13272/j.issn.1671-251x.17997

基于三维点云地图和ESKF的无人车融合定位方法

doi: 10.13272/j.issn.1671-251x.17997

1.
国能准能集团有限责任公司机电管理部, 内蒙古鄂尔多斯　010300
2.
中国矿业大学(北京) 机电与信息工程学院, 北京　100083

基金项目: 国家自然科学基金资助项目（61673385）。

详细信息

作者简介:
崔文（1987—），男，内蒙古鄂尔多斯人，工程师，现主要从事煤矿机电、煤矿智能化、露天煤矿设备远程操控及无人驾驶研究及管理工作，E-mail：10572189@ceic.com

通讯作者:
李庆玲（1979—），女，四川眉山人，副教授，博士，研究方向为智能机器人技术，E-mail：doudouhit@163.com

中图分类号: TD67
计量
- 文章访问数: 172
- HTML全文浏览量: 36
- PDF下载量: 30
- 被引次数: 0
出版历程
- 收稿日期: 2022-08-05
- 修回日期: 2022-09-14
- 网络出版日期: 2022-09-23

Unmanned vehicle fusion positioning method based on 3D point cloud map and ESKF

1.
Electromechanical Management Department, CHN Energy Zhunneng Group Co., Ltd., Erdos 010300, China
2.
School of Mechanical Electronic and Information Engineering, China University of Mining and Technology-Beijing, Beijing 100083, China

摘要

摘要: 基于地图匹配的无人车定位方法的精度取决于已创建地图的精度，受外界的影响较小，适用于复杂场景下的无人车定位。然而目前采用的激光雷达点云匹配算法是以单一的特征为核心进行匹配，对于大规模点云匹配准确率较低，导致三维点云地图与实际环境偏差较大，造成基于地图匹配的无人车定位方法精度不高的问题。针对上述问题，提出了一种基于三维点云地图和误差状态卡尔曼滤波（ESKF）的无人车融合定位方法。该方法由三维点云地图构建和ESKF融合定位2个部分组成。在三维点云地图构建部分，通过正态分布变换（NDT）算法进行帧间点云匹配，提高大规模点云匹配准确率，并在根据激光里程计数据建立的位姿图顶点和约束边的基础上添加闭环约束构建图优化问题，采用列文伯格−马夸尔特（LM）算法进行求解，以减少位姿的累计漂移，提高三维点云地图精度。在ESKF融合定位部分，采用ESKF融合惯性测量单元（IMU）数据和三维点云地图数据，实现对无人车先验位姿（位置、姿态和速度）的修正并输出后验位姿。实验结果表明，与基于地图匹配的定位方法相比，该方法定位轨迹相对位姿误差最大值减小了0.176 9 m，平均误差减小了0.027 1 m，均方根误差减小了0.059 4 m，在定位精度和稳定性方面具有更好的表现。
- 无人车定位 /
- 融合定位 /
- 三维点云地图 /
- 误差状态卡尔曼滤波 /
- 激光雷达 /
- 点云匹配 /
- 图优化
Abstract: The precision of the unmanned vehicle positioning method based on map matching depends on the precision of the created map. The precision is less affected by the outside world. The method is suitable for unmanned vehicle positioning in complex scenarios. However, the common laser radar point cloud matching algorithm takes single characteristic as the core for matching. The matching accuracy rate is low for large-scale point clouds. Therefore, the 3D point cloud map deviates greatly from the actual environment, resulting in the low precision of the unmanned vehicle positioning method based on map matching. To solve the above problems, an unmanned vehicle fusion positioning method based on 3D point cloud map and error state Kalman filter (ESKF) is proposed. The method is composed of two parts: 3D point cloud map construction and ESKF fusion positioning. In the 3D point cloud map construction part, inter-frame point cloud matching is performed through the normal distribution transform (NDT) algorithm to improve the accuracy of large-scale point cloud matching. The closed-loop constraint is added to construct graph optimization problem on the basis of the vertex and the constraint edge of the position and posture graph established by the laser odometer data. The Levenberg-Marquardt (LM) algorithm is used for the graph optimization solution in order to reduce the cumulative drift of the position and posture and improve the 3D point cloud map precision. In the ESKF fusion positioning part, ESKF is used to fuse inertial measurement unit (IMU) data and 3D point cloud map data. The correction of the prior position and posture (position, attitude and velocity) of the unmanned vehicle is realized and the posterior position and posture are output. The experimental results show that, compared with the method based on map matching, the maximum relative position and posture error, average error and root-mean-square error of the proposed method are reduced by 0.176 9 m, 0.027 1 m and 0.059 4 m respectively. The proposed method has better performance in positioning precision and stability.
- unmanned vehicle positioning /
- fusion positioning /
- 3D point cloud map /
- error state Kalman filter /
- laser lidar /
- point cloud matching /
- graph optimization

HTML全文

图 1 图优化原理

Figure 1. Graph optimization principle

下载: 全尺寸图片幻灯片

图 2 数据采集环境

Figure 2. Data collection environment

下载: 全尺寸图片幻灯片

图 3 数据采集平台

Figure 3. Data collection platform

下载: 全尺寸图片幻灯片

图 4 基于 KITTI 数据集的三维点云地图

Figure 4. 3D point cloud map based on KITTI dataset

下载: 全尺寸图片幻灯片

图 5 单一特征建图算法构建的三维点云地图

Figure 5. 3D point cloud map constructed by single characteristic mapping algorithm

下载: 全尺寸图片幻灯片

图 6 本文算法构建的三维点云地图

Figure 6. 3D point cloud map constructed by the proposed algorithm

下载: 全尺寸图片幻灯片

图 7 基于 KITTI 数据集的融合定位轨迹

Figure 7. Fusion positioning trajectory based on KITTI dataset

下载: 全尺寸图片幻灯片

图 8 不同定位方法的相对位姿误差对比

Figure 8. Comparison of relative pose error of different positioning methods

下载: 全尺寸图片幻灯片

图 9 基于校园实测数据的融合定位轨迹

Figure 9. Fusion positioning trajectory based on campus measured data

下载: 全尺寸图片幻灯片

图 10 不同定位方法的定位轨迹对比

Figure 10. Comparison of positioning trajectories of different positioning methods

下载: 全尺寸图片幻灯片

表 1 定位轨迹误差统计结果

Table 1. Positioning trajectory error statistics m

方法相对位姿误差
最大值平均
误差均方根
误差

基于地图匹配的定位 1.532 6 0.680 3 0.780 9
ESKF融合定位 1.355 7 0.653 2 0.721 5

下载: 导出CSV

参考文献(15)

[1]	LIU Ran,YUEN C,DO T N,et al. Fusing similarity-based sequence and dead reckoning for indoor positioning without training[J]. IEEE Sensors Journal,2017,17(13):4197-4207. doi: 10.1109/JSEN.2017.2706303
[2]	杨磊,庞硕,杨耀民,等. 基于9DOF IMU的AUV惯性导航技术研究[J]. 计算机测量与控制,2016,24(3):133-135,153. YANG Lei,PANG Shuo,YANG Yaomin,et al. Study of navigation for AUV based on 9DOF IMU[J]. Computer Measurement & Control,2016,24(3):133-135,153.
[3]	方辉. GPS信号遮挡区的智能车定位方法研究[D]. 上海: 上海交通大学, 2010. FANG Hui. Research on intelligent vehicle localization in GPS signal blocked areas[D]. Shanghai: Shanghai Jiao Tong University, 2010.
[4]	顾文华,周波,戴先中. 基于ICP匹配算法的室内移动机器人定位[J]. 华中科技大学学报(自然科学版),2013,41(增刊1):262-266. GU Wenhua,ZHOU Bo,DAI Xianzhong. ICP matching algorithm-based localization of indoor mobile robots[J]. Journal of Huazhong University of Science and Technology(Natural Science Edition),2013,41(S1):262-266.
[5]	杨林,马宏伟,王岩,等. 煤矿巡检机器人同步定位与地图构建方法研究[J]. 工矿自动化,2019,45(9):18-24. doi: 10.13272/j.issn.1671-251x.17444 YANG Lin,MA Hongwei,WANG Yan,et al. Research on method of simultaneous localization and mapping of coal mine inspection robot[J]. Industry and Mine Automation,2019,45(9):18-24. doi: 10.13272/j.issn.1671-251x.17444
[6]	周风余,庄文密,万方,等. 基于路标与云架构的多机器人建图及融合方法[J]. 华中科技大学学报(自然科学版),2020,48(11):30-36. ZHOU Fengyu,ZHUANG Wenmi,WAN Fang,et al. Multi-robot mapping and merging method based on landmarks and cloud architecture[J]. Journal of Huazhong University of Science and Technology(Natural Science Edition),2020,48(11):30-36.
[7]	HENTSCHEL M, WAGNER B. Autonomous robot navigation based on OpenStreetMap geodata[C]. The 13th International IEEE Conference on Intelligent Transportation Systems, Funchal, 2010: 1645-1650.
[8]	MOOSMANN F, STILLER C. Velodyne SLAM[C]. IEEE Intelligent Vehicles Symposium, Baden-Baden, 2011: 393-398.
[9]	YANG Jiaolong, LI Hongdong, JIA Yunde. Go-ICP: solving 3D registration efficiently and globally optimally[C]. IEEE International Conference on Computer Vision, Sydney, 2013: 1457-1464.
[10]	DROESCHEL D,SCHWARZ M,BEHNKE S. Continuous mapping and localization for autonomous navigation in rough terrain using a 3D laser scanner[J]. Robotics and Autonomous Systems,2017,88:104-115. doi: 10.1016/j.robot.2016.10.017
[11]	BLOESCH M,BURRI M,OMARI S,et al. Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback[J]. International Journal of Robotics Research,2017,36(10):1053-1072. doi: 10.1177/0278364917728574
[12]	冯敬伟,车明明,马巍,等. 基于激光测距仪与微惯性测量单元的室内个人导航方法[J]. 计算机工程,2016,42(8):311-315. doi: 10.3969/j.issn.1000-3428.2016.08.055 FENG Jingwei,CHE Mingming,MA Wei,et al. Indoor personal navigation method based on laser range finder and micro-inertial measurement unit[J]. Computer Engineering,2016,42(8):311-315. doi: 10.3969/j.issn.1000-3428.2016.08.055
[13]	余洪山,付强,孙健,等. 面向室内移动机器人的改进3D−NDT点云配准算法[J]. 仪器仪表学报,2019,40(9):151-161. doi: 10.19650/j.cnki.cjsi.J1905346 YU Hongshan,FU Qiang,SUN Jian,et al. Improved 3D-NDT point cloud registration algorithm for indoor mobile robot[J]. Chinese Journal of Scientific Instrument,2019,40(9):151-161. doi: 10.19650/j.cnki.cjsi.J1905346
[14]	吴环宇. 基于激光雷达的智能车辆三维地图构建和定位算法研究[D]. 长春: 吉林大学, 2020. WU Huanyu. Research on 3D mapping and localization algorithm of intelligent vehicle based on lidar[D]. Changchun: Jilin University, 2020.
[15]	WAN Guowei, YANG Xiaolong, CAI Renlan, et al. Robust and precise vehicle localization based on multi-sensor fusion in diverse city scenes[C]. IEEE International Conference on Robotics and Automation, Brisbane, 2018: 4670-4677.