结合扩展卡尔曼滤波与基于点线的最近点迭代扫描匹配算法的机器人位姿自适应估计

doi:10.13474/j.cnki.11-2246.2022.0202

测绘通报 ›› 2022, Vol. 0 ›› Issue (7): 49-53.doi: 10.13474/j.cnki.11-2246.2022.0202

结合扩展卡尔曼滤波与基于点线的最近点迭代扫描匹配算法的机器人位姿自适应估计

岳胜杰¹, 王红旗^1,2, 刘群坡^1,2, 赵荣亮¹

1. 河南理工大学电气工程与自动化学院, 河南焦作 454000;
2. 河南省智能装备直驱技术与控制国际联合实验室, 河南焦作 454000

收稿日期:2021-09-29 修回日期:2022-05-25 出版日期:2022-07-25 发布日期:2022-07-28
通讯作者: 刘群坡。E-mail:lqpny@hpu.edu.cn
作者简介:岳胜杰(1995—),男,硕士生,研究方向为运动驱动与控制。E-mail:2673189645@qq.com
基金资助:
国家重点研发计划(2016YFC0600906);河南省高校科技创新团队(20IRTSTHN019);河南省创新型科技人才队伍建设工程(CXTD2016054);河南科技攻关项目(172102210270)

Adaptive pose estimation for robot based on extended Kalman filter and point-line iterative closest point

YUE Shengjie¹, WANG Hongqi^1,2, LIU Qunpo^1,2, ZHAO Rongliang¹

1. School of Mechanical and Power Engineering, Henan Polytechnic University, Jiaozuo 454000, China;
2. Henan International Joint Laboratory of Direct Drive and Control of Intelligent Equipment, Jiaozuo 454000, China

Received:2021-09-29 Revised:2022-05-25 Online:2022-07-25 Published:2022-07-28

摘要/Abstract

摘要： 针对机器人利用单一位姿估计方法累积误差大、精度低的问题,本文提出了一种基于扩展卡尔曼滤波算法(EKF)和基于点线的最近点迭代扫描匹配算法(PL-ICP)的机器人位姿自适应估计方法。为了减少轮式里程计造成的累积误差,利用Mahony算法对陀螺仪和加速度计进行姿态解算,进而基于扩展卡尔曼滤波融合轮式里程计初步估计机器人位姿;为了减少轮子变形、打滑等对机器人位姿的影响,利用PL-ICP点云匹配算法构建单线激光里程计,对机器人位姿再次进行估计;为了提高位姿估计的准确度,提出了一种基于两种位姿总均方差和前后时刻位姿误差构建累积误差的自适应修正算法,通过分析两种位姿总均方差及前后时刻位姿误差,得到全局最优权重因子和局部动态权重因子,实现累积误差修正因子的自适应调整,得到机器人更精确的位姿估计。试验结果表明,该方法可对机器人的位姿累积误差进行修正,显著提高机器人的位姿估计精度。

关键词: 激光里程计, EKF, 机器人位姿估计, PL-ICP, 多源信息融合

Abstract: In this paper, a method of robot pose correction based on multi-source information fusion using extended Kalman filter (EKF)and point-line iterative closest point(PL-ICP)point cloud matching algorithm is proposed to solve the problems of large cumulative error and low accuracy of single pose estimation method for robots. In order to reduce the cumulative error caused by the wheel odometer, Mahony algorithm is used to calculate the attitude of the gyroscope and accelerometer. The preliminary estimation of robot pose is obtained by fusing the wheel odometer based on the extended Kalman filter. In order to reduce the influence of wheel deformation and slip on the pose of the robot, using the PL-ICP point cloud matching algorithm to construct a single-line laser odometer to estimate the robot pose again; In order to improve the accuracy of pose estimation, based on the total mean square error of the two poses and the pose error at the time before and after, an adaptive correction algorithm for the cumulative error is constructed. The method in this paper obtains global optimal weight factor and local dynamic weight factor to realize the adaptive adjustment of the cumulative error correction factor by analyzing the total mean square error of the two poses and the pose error at the time before and after, which can obtain more accurate pose estimation of the robot. Experimental results show that this method can correct the cumulative error of the robot's pose, and significantly improve the accuracy of robot pose estimation.

Key words: laser odometer, EKF, robot pose estimation, PL-ICP, multi-source information fusion

中图分类号:

岳胜杰, 王红旗, 刘群坡, 赵荣亮. 结合扩展卡尔曼滤波与基于点线的最近点迭代扫描匹配算法的机器人位姿自适应估计[J]. 测绘通报, 2022, 0(7): 49-53.

YUE Shengjie, WANG Hongqi, LIU Qunpo, ZHAO Rongliang. Adaptive pose estimation for robot based on extended Kalman filter and point-line iterative closest point[J]. Bulletin of Surveying and Mapping, 2022, 0(7): 49-53.

参考文献

[1] 陈宗海,裴浩渊,王纪凯,等.基于单目相机的视觉重定位方法综述[J].机器人,2021,43(3):373-384.
[2] 徐爱功,宋帅,隋心,等.一种单目视觉/INS组合室内定位抗差方法[J].测绘科学,2019,44(12):1-6.
[3] 张旭,王智,崔粲,等.二维激光雷达定位建图系统设计与实现[J].光学技术,2019,45(5):596-600.
[4] CHOI B S,LEE J J.Mobile robot localization scheme based on RFID and sonar fusion system[C]//Proceedings of 2009 IEEE International Symposium on Industrial Electronics.Seoul:IEEE,2009:1035-1040.
[5] SHEN Jinglin,TICK D,GANS N.Localization through fusion of discrete and continuous epipolar geometry with wheel and IMU odometry[J].Proceedings of 2011 American Control Conference,2011:1292-1298.
[6] MCMANUS C,FURGALE P,BARFOOT T D.Towards lighting-invariant visual navigation:an appearance-based approach using scanning laser-rangefinders[J].Robotics and Autonomous Systems,2013,61(8):836-852.
[7] BOUGOUFFA A,SEIGNEZ E,BOUAZIZ S,et al.SmartTrolley:an experimental mobile platform for indoor localization in warehouses[C]//Proceedings of 2020 International Conference on Robotics,Control and Automation Engineering (RCAE).Chongqing:IEEE,2020:108-115.
[8] 傅军,许达,付阳.一种自适应UKF在UWB室内定位中的应用[J].测绘通报,2019(S1):12-17.
[9] 姜祚鹏,梅天灿.一种基于PL-ICP及NDT点云匹配的单线激光里程计[J].激光杂志,2020,41(3):21-24.
[10] 许智宾,李宏伟,张斌,等.双目视觉与惯导融合的移动机器人定位方法[J].测绘学报,2021,50(11):1512-1521.
[11] 陈国定,周鹏豪,胡朕豪,等.基于MPU6050的四轴硬件姿态解算研究[J].机电工程,2018,35(1):95-100.
[12] 储开斌,赵爽,冯成涛.基于Mahony-EKF的无人机姿态解算算法[J].电子测量与仪器学报,2020,34(12):12-18.
[13] 田红丽,孙永全,刘洪普.基于改进Mahony互补滤波算法的三维运动轨迹恢复[J].传感器与微系统,2018,37(12):118-121.
[14] 张健铭,施元昊,徐正蓺,等.基于误差预测的自适应UWB/PDR融合定位算法[J].计算机应用,2020,40(6):1755-1762.
[15] 王随阳,蒋林,雷斌,等.基于多传感器融合的室内机器人定位研究[J].组合机床与自动化加工技术,2020(10):72-75,89.
[16] CENSI A.An ICP variant using a point-to-line metric[C]//Proceedings of 2008 IEEE International Conference on Robotics and Automation.Pasadena,CA:IEEE,2008.
[17] ARUN K S,HUANG T S,BLOSTEIN S D.Least-squares fitting of two 3-d point sets[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,1987,9(5):698-700.
[18] SERAFIN J,GRISETTI G.NICP:Dense normal based point cloud registration[C]//Proceedings of 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).Hamburg:IEEE,2015.
[19] ALMEIDA J,SANTOS V M.Real time egomotion of a nonholonomic vehicle using LiDAR measurements[J].Journal of Field Robotics,2013,30(1):129-141.
[20] 张书亮,谭向全,吴清文.基于多传感器融合技术的室内移动机器人定位研究[J].传感器与微系统,2021,40(8):53-56.

结合扩展卡尔曼滤波与基于点线的最近点迭代扫描匹配算法的机器人位姿自适应估计

Adaptive pose estimation for robot based on extended Kalman filter and point-line iterative closest point

可视化

摘要/Abstract

引用本文

使用本文

参考文献

相关文章 2

编辑推荐

Metrics

本文评价

[1]	李景文, 韦晶闪, 周俊芬, 毛佳影, 陆妍玲. 融合UWB+PDR的室内定位方法改进[J]. 测绘通报, 2022, 0(3): 36-40.
[2]	宋宝, 柯福阳, 赵兴旺. 面向LiDAR/Radar松组合的迭代加权IEKF-BP组合算法精度分析[J]. 测绘通报, 2021, 0(2): 44-48.