移动机器人SLAM位姿估计的改进四元数无迹卡尔曼滤波

doi:10.11947/j.AGCS.2022.20210082

摘要/Abstract

摘要： 在全自主运动控制的移动机器人系统中，自身位姿的估计和校正对于移动机器人的运动至关重要。卡尔曼滤波是解决移动机器人同步定位与地图构建（SLAM）常用方法。相较于卡尔曼滤波，无迹卡尔曼滤波（UKF）无须对复杂的非线性函数进行雅可比矩阵运算。本文基于无迹卡尔曼滤波，根据先验协方差的平方根选择sigma点，计算协方差以及加权均值。用四元数表示姿态，将四元数矢量转换为旋转空间进行矩阵运算，在此基础上设计了一种位姿估计算法——基于四元数平方根的无迹卡尔曼滤波（QSR-UKF）算法。试验将EKF、QSR-UKF、SR-UKFEKF 3种算法的位姿估计结果进行仿真分析，并通过相关定量指标进行了描述，验证了本文算法的有效性。

关键词: 移动机器人, 同时定位与地图构建, 无迹卡尔曼滤波, 四元数

Abstract: In automatic motion controlled mobile robot system, the estimation and correction of its own pose is very crucial for the motion of robot. Kalman filter is a classical method to solve the problem of simultaneous localization and mapping (SLAM) in robot system. Compared with Kalman filter, unscented Kalman filter (UKF) uses nonlinear model directly, avoids operation of Jacobian matrix of complex nonlinear function. In this paper, based on the unscented Kalman filter, sigma points are selected by square root decomposition of prior covariance, and then weighted mean and covariance are calculated. In addition, Quaternion is used to represent attitude of robot, and quaternion vector is converted to rotation space for matrix operation. According to the characteristic of square root decomposition and quaternion vector, a quaternion square root unscented Kalman filter is proposed. By comparing the robot poses estimation results on quaternion square root unscented Kalman filter (QSR-UKF), square root unscented Kalman filter (SR-UKF) and extended Kalman filter (EKF), the simulation results show that the proposed QSR-UKF method is effective.

Key words: mobile robot, simultaneous localization and mapping, unscented Kalman filter, quaternion

中图分类号:

P237
TP2

赵玏洋, 闫利. 移动机器人SLAM位姿估计的改进四元数无迹卡尔曼滤波[J]. 测绘学报, 2022, 51(2): 212-223.

ZHAO Leyang, YAN Li. Advanced quaternion unscented Kalman filter based on SLAM of mobile robot pose estimation[J]. Acta Geodaetica et Cartographica Sinica, 2022, 51(2): 212-223.

参考文献

[1] 程传奇, 郝向阳, 李建胜, 等. 移动机器人视觉动态定位的稳健高斯混合模型[J]. 测绘学报, 2018, 47(11):1446-1456. DOI:10.11947/j.AGCS.2018.20170649. CHENG Chuanqi, HAO Xiangyang, LI Jiansheng, et al. Robust Gaussian mixture model for mobile robots' vision-based kinematical localization[J]. Acta Geodaetica et Cartographica Sinica, 2018, 47(11):1446-1456. DOI:10.11947/j.AGCS.2018.20170649.
[2] 王晨捷, 罗斌, 李成源, 等. 无人机视觉SLAM协同建图与导航[J]. 测绘学报, 2020, 49(6):767-776. DOI:10.11947/j.AGCS.2020.20190145. WANG Chenjie, LUO Bin, LI Chengyuan, et al. The collaborative mapping and navigation based on visual SLAM in UAV platform[J]. Acta Geodaetica et Cartographica Sinica, 2020, 49(6):767-776. DOI:10.11947/j.AGCS.2020.20190145.
[3] 王志文,郭戈.移动机器人导航技术现状与展望[J].机器人,2003(5):470-474. WANG Zhiwen, GUO Yi. Status and prospect of mobile robot navigation technology[J]. Machine, 2003(5):470-474.
[4] NILSSON N J. A mobile automation:an application of artificial intelligence techniques[C]//Proceedings of the 1st International Joint Conference on Artificial Intelligence. Washington, DC:ACM, 1969:509-520.
[5] GIRALT G, SOBEK R, CHATILA R. A multi-level planning and navigation system for a mobile robot:a first approach to HILARE[C]//Proceedings of the 6th International Joint Conference on Artificial Intelligence. Tokyo, Japan:ACM, 1979:335-337.
[6] DURRANT-WHYTE H F. An autonomous guided vehicle for cargo handling applications[J]. The International Journal of Robotics Research, 1996, 15(5):407-440.
[7] LEONARD J J, DURRANT-WHYTE H F. Mobile robot localization by tracking geometric beacons[J]. IEEE Transactions on Robotics and Automation, 1991, 7(3):376-382.
[8] DURRANT-WHYTE H F. Uncertain geometry in robotics[J]. IEEE Journal on Robotics and Automation, 1988, 4(1):23-31.
[9] SMITH R C, CHEESEMAN P. On the representation and estimation of spatial uncertainty[J]. The International Journal of Robotics Research, 1986, 5(4):56-68.
[10] DURRANT-WHYTE H, RYE D, NEBOT E. Localization of autonomous guided vehicles[C]//Proceedings of the 7th International Symposium on Robotics Research. London:Springer, 1996:613-625.
[11] 乌萌, 郝金明, 付浩, 等. 利用解耦光流运动场模型的双目视觉里程计位姿优化方法[J]. 测绘学报, 2019, 48(4):460-472. DOI:10.11947/j.AGCS.2019.20180429. WU Meng, HAO Jinming, FU Hao, et al. A stereo visual odometry pose optimization method via flow-decoupled motion field model[J]. Acta Geodaetica et Cartographica Sinica, 2019, 48(4):460-472. DOI:10.11947/j.AGCS.2019.20180429.
[12] 邸凯昌, 万文辉, 赵红颖, 等. 视觉SLAM技术的进展与应用[J]. 测绘学报, 2018, 47(6):770-779. DOI:10.11947/j.AGCS.2018.20170652. DI Kaichang, WAN Wenhui, ZHAO Hongying, et al. Progress and applications of visual SLAM[J]. Acta Geodaetica et Cartographica Sinica, 2018, 47(6):770-779. DOI:10.11947/j.AGCS.2018.20170652.
[13] CHEN Yu, YAN Li, XU Bo, et al. Multi-stage matching approach for mobile platform visual imagery[J]. IEEE Access, 2019, 7:160523-160535.
[14] 闫利, 戴集成, 谭骏祥, 等. SLAM激光点云整体精配准位姿图技术[J]. 测绘学报, 2019, 48(3):313-321. DOI:10.11947/j.AGCS.2019.20170716. YAN Li, DAI Jicheng, TAN Junxiang, et al. Global fine registration of point cloud in LiDAR SLAM based on pose graph[J]. Acta Geodaetica et Cartographica Sinica, 2019, 48(3):313-321. DOI:10.11947/j.AGCS.2019.20170716.
[15] KAESS M, DELLAERT F. Covariance recovery from a square root information matrix for data association[J]. Robotics and Autonomous Systems, 2009, 57(12):1198-1210.
[16] MOVAFAGHPOUR M A, MASEHIAN E. Poly line map extraction in sensor-based mobile robot navigation using a consecutive clustering algorithm[J]. Robotics and Autonomous Systems, 2012, 60(8):1078-1092.
[17] BAKAR M N A, SAAD A R M. A monocular vision-based specific person detection system for mobile robot applications[J]. Procedia Engineering, 2012, 41:22-31.
[18] WILLIAMS B, CUMMINS M, NEIRA J, et al. A comparison of loop closing techniques in monocular SLAM[J]. Robotics and Autonomous Systems, 2009, 57(12):1188-1197.
[19] 张一, 姜挺, 江刚武, 等. 特征法视觉SLAM逆深度滤波的三维重建[J]. 测绘学报, 2019, 48(6):708-717. DOI:10.11947/j.AGCS.2019.20180421. ZHANG Yi, JIANG Ting, JIANG Gangwu, et al. 3D reconstruction with inverse depth filter of feature-based visual SLAM[J]. Acta Geodaetica et Cartographica Sinica, 2019, 48(6):708-717. DOI:10.11947/j.AGCS.2019.20180421.
[20] 尚洋, 孙晓亮, 张跃强, 等. 三维目标位姿跟踪与模型修正[J]. 测绘学报, 2018, 47(6):799-808. DOI:10.11947/j.AGCS.2018.20170626. SHANG Yang, SUN Xiaoliang, ZHANG Yueqiang, et al. Research on 3D target pose tracking and modeling[J]. Acta Geodaetica et Cartographica Sinica, 2018, 47(6):799-808. DOI:10.11947/j.AGCS.2018.20170626.
[21] GUIVANT J E, NEBOT E M. Optimization of the simultaneous localization and map-building algorithm for real-time implementation[J]. IEEE Transactions on Robotics and Automation, 2001, 17(3):242-257.
[22] 弋英民,刘丁.有色过程噪声下的轮式机器人同步定位与地图构建[J].电子学报,2010,38(06):1339-1343. GE Yingming, LIU Ding. Synchronous positioning and map construction of wheeled robot under colored process noise[J]. Journal of Electronic, 2010, 38(6):1339-1343.
[23] JULIER S J, UHLMANN J K. New extension of the Kalman filter to nonlinear systems[C]//Signal processing, sensor fusion, and target recognition VI. International Society for Optics and Photonics, Harbin, China:IEEE,[S.l.]:SPIE, 1997, 3068:182-193.
[24] LI Shurong, NI Pengfei. Square-root unscented Kalman filter based simultaneous localization and mapping[C]//Proceedings of 2010 IEEE International Conference on Information and Automation. Harbin, China:IEEE, 2010:2384-2388.
[25] QU Liping, HE Shuiqing, QU Yongyin. An SLAM algorithm based on improved UKF[C]//Proceedings of the 24th Chinese Control and Decision Conference (CCDC). Taiyuan, China:IEEE, 2012:4154-4157.
[26] 赵琳, 闫鑫, 郝勇. 基于迭代平方根UKF的SLAM算[J]. 弹箭与制导学报, 2011, 31(1):157-160, 166. ZHAO Lin, YAN Xin, HAO Yong. SLAM algorithm based on iterated square unscented Kalman filter[J]. Journal of Projectiles, Rockets, Missiles and Guidance, 2011, 31(1):157-160, 166.
[27] MAEYAMA S, TAKAHASHI Y, WATANABE K. A solution to SLAM problems by simultaneous estimation of kinematic parameters including sensor mounting offset with an augmented UKF[J]. Advanced Robotics, 2015, 29(17):1137-1149.
[28] 李开龙, 胡柏青, 常路宾. 改进四元数无味卡尔曼滤波算法[J]. 系统工程与电子技术, 2016, 38(6):1399-1404. LI Kailong, HU Baiqing, CHANG Lubin. Modified quaternion unscented Kalman filter[J]. Systems Engineering and Electronics, 2016, 38(6):1399-1404.
[29] 王丹丹, 谭开拓, 高素玲, 等. 基于模糊逻辑四元数的平方根UKF算法[J]. 华南理工大学学报(自然科学版), 2019, 47(4):53-60. WANG Dandan, TAN Kaituo, GAO Suling, et al. Square root UKF algorithm based on fuzzy logical quaternion[J]. Journal of South China University of Technology (Natural Science Edition), 2019, 47(4):53-60.
[30] VALENTI R G, DRYANOVSKI I, XIAO Jizhong. A linear Kalman filter for MARG orientation estimation using the algebraic quaternion algorithm[J]. IEEE Transactions on Instrumentation and Measurement, 2016, 65(2):467-481.
[31] 蔡安江, 刘凯峰, 郭师虹, 等. 基于四元数衍生无迹卡尔曼滤波的二段式多旋翼无人机姿态估计算法[J]. 控制理论与应用, 2020, 37(2):365-373. CAI Anjiang, LIU Kaifeng, GUO Shihong, et al. Quaternion derivative unscented Kalman filter-based two-step attitude estimation algorithm for multi-rotor unmanned aerial vehicle[J]. Control Theory & Applications, 2020, 37(2):365-373.
[32] 乔相伟, 周卫东, 吉宇人. 用四元数状态切换无迹卡尔曼滤波器估计的飞行器姿态[J]. 控制理论与应用, 2012, 29(1):97-103. QIAO Xiangwei, ZHOU Weidong, JI Yuren. Aircraft attitude estimation based on quaternion state-switching unscented Kalman filter[J]. Control Theory & Applications, 2012, 29(1):97-103.
[33] ARMESTO L, TORNERO J. SLAM based on Kalman filter for multi-rate fusion of laser and encoder measurements[C]//Proceedings of 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE Cat. No. 04CH37566). Sendai, Japan:IEEE, 2004:1960-1865.
[34] VAN DER MERWE R, WAN E A. The square-root unscented Kalman filter for state and parameter-estimation[C]//Proceedings of 2001 IEEE International Conference on Acoustics, Speech, and Signal Processing. Proceedings (Cat. No. 01CH37221). Salt Lake City, UT:IEEE, 2001:3461-3464.
[35] PITTELKAU M E. An analysis of the quaternion attitude determination filter[J]. The Journal of the Astronautical Sciences, 2003, 51(1):103-120.
[36] VATHSAL S. Spacecraft attitude determination using a second-order nonlinear filter[J]. Journal of Guidance, Control, and Dynamics, 1987, 10(6):559-566.
[37] CRASSIDIS J L, MARKLEY F L. Unscented filtering for spacecraft attitude estimation[J]. Journal of Guidance, Control, and Dynamics, 2003, 26(4):536-542.