-
摘要:
为实现城市道路环境中车辆的高精度位姿估计，提出了一种基于非线性因子恢复的位姿图优化算法，在因子图框架下实现对全球导航卫星系统(GNSS)、视觉和惯性信息的有效融合。针对以往位姿图优化算法中各因子协方差估计不明的问题，采取非线性因子恢复算法从边缘化产生的稠密先验因子中提取所需信息，将先验因子替换成相对位姿因子，并对各因子的协方差矩阵做最优估计；设计一种利用视觉惯性里程计对GNSS信号异常情况做主动检测的方法，该方法能自适应地调整GNSS信号的协方差矩阵。上述机制保证了位姿图优化过程中各因子的信息一致性。在数据集和真实道路环境下开展相关测试，实验结果表明：所提方案有效提高了多源异步信号的融合效率，并能够提供稳健且全局一致的高精度位姿估计结果。
Abstract:A pose graph optimization method employing nonlinear factor recovery was created in order to accomplish high-precision vehicle pose estimation in urban road situations. This method successfully incorporates vision, inertial information, and global navigation satellite system (GNSS) data inside the factor graph framework. To address the issue of unclear covariance estimation in previous pose graph optimization algorithms, the nonlinear factor recovery algorithm extracts the required information from the dense prior factors generated by marginalization, replacing the prior factors with relative pose factors and conducting optimal estimates for the covariance matrices of these factors. A procedure utilizing visual-inertial odometry for the active detection of GNSS signal anomalies has been designed, capable of adaptively adjusting the covariance matrix of GNSS signals. Throughout the pose graph optimization process, these strategies guaranteed consistency of factor information. Tests conducted on datasets and in real road environments indicate that this approach significantly improves the fusion efficiency of multisource asynchronous signals, providing robust and globally consistent high-precision pose estimation results.
-
图 7 不同算法的关键状态量估计误差
Figure 7. Estimation errors of key state quantities for different algorithms
图 8 异常系数矩阵和实际位置误差关系
Figure 8. Relationship of anomaly coefficient matrix and actual position error
表 1 绝对轨迹误差
Table 1. Absolute trajectory error
算法 位置ATE/m 方位角ATE/(°) 最大 最小 RMSE 最大 最小 RMSE VINS-Mono 39.03 3.77 29.38 12.79 0.476 6.613 本文算法 10.94 2.06 7.287 16.36 1.188 6.842 
下载: 导出CSV
-
[1] Davison A J, Reid I D, Molton N D, et al. MonoSLAM: real-time single camera SLAM[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2007, 29(6): 1052-1067. [2] Mourikis A I, Roumeliotis S I. A multi-state constraint Kalman filter for vision-aided inertial navigation[C]//Proceedings of the IEEE International Conference on Robotics and Automation. Piscataway: IEEE Press, 2007: 3565-3572. [3] Huang G P, Mourikis A I, Roumeliotis S I. Observability-based rules for designing consistent EKF SLAM estimators[J]. International Journal of Robotics Research, 2010, 29(5): 502-528. [4] Klein G, Murray D. Parallel tracking and mapping for small AR workspaces[C]//Proceedings of the 6th IEEE and ACM International Symposium on Mixed and Augmented Reality. Piscataway: IEEE Press, 2008: 225-234. [5] Qin T, Li P L, Shen S J. VINS-Mono: a robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4): 1004-1020. [6] Strasdat H, Montiel J M M, Davison A. Scale drift-aware large scale monocular SLAM[C]//Proceedings of the Robotics: Science and Systems Foundation. [S.l.:s.n.], 2010: 1-8. [7] Mur-artal R, Tardós J D. Visual-inertial monocular SLAM with map reuse[J]. IEEE Robotics and Automation Letters, 2017, 2(2): 796-803. [8] Campos C, Elvira R, Rodríguez J J G, et al. ORB-SLAM3: an accurate open-source library for visual, visual-inertial, and multimap SLAM[J]. IEEE Transactions on Robotics, 2021, 37(6): 1874-1890. [9] Shi P C, Zhu Z K, Sun S Y, et al. Covariance estimation for pose graph optimization in visual-inertial navigation systems[J]. IEEE Transactions on Intelligent Vehicles, 2023, 8(6): 3657-3667. [10] Gong Z, Liu P L, Wen F, et al. Graph-based adaptive fusion of GNSS and VIO under intermittent GNSS-degraded environment[J]. IEEE Transactions on Instrumentation and Measurement, 2021, 70: 8501116. [11] Mazuran M, Burgard W, Tipaldi G D. Nonlinear factor recovery for long-term SLAM[J]. International Journal of Robotics Research, 2016, 35(1-3): 50-72. [12] Usenko V, Demmel N, Schubert D, et al. Visual-inertial mapping with non-linear factor recovery[J]. IEEE Robotics and Automation Letters, 2020, 5(2): 422-429. [13] Qin T, Cao S, Pan J, et al. A general optimization-based framework for global pose estimation with multiple sensors[EB/OL]. (2019-01-11)[2024-05-01]. http://arxiv.org/abs/1901.03642. [14] Han B, Xiao Z Y, Huang S, et al. Multi-layer VI-GNSS global positioning framework with numerical solution aided MAP initialization[C]//Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway: IEEE Press, 2021: 5448-5455. [15] Dellaert F, Kaess M. 机器人感知: 因子图在SLAM中的应用[M]. 刘富强, 董靖, 译. 北京: 电子工业出版社, 2018: 4-5.Dellaert F, Kaess M. Factor graphs for robot perception[M]. LIU F Q, DONG J, translated. Beijing: Publishing House of Electronics Industry, 2018: 4-5(in Chinese). [16] Qin T, Shen S J. Robust initialization of monocular visual-inertial estimation on aerial robots[C]//Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway: IEEE Press, 2017: 4225-4232. [17] Indelman V, Williams S, Kaess M, et al. Factor graph based incremental smoothing in inertial navigation systems[C]//Proceedings of the 15th International Conference on Information Fusion. Piscataway: IEEE Press, 2012: 2154-2161. [18] Sibley G, Matthies L, Sukhatme G. Sliding window filter with application to planetary landing[J]. Journal of Field Robotics, 2010, 27(5): 587-608. [19] Wen W S, Zhou Y Y, Zhang G H, et al. UrbanLoco: a full sensor suite dataset for mapping and localization in urban scenes[C]//Proceedings of the IEEE International Conference on Robotics and Automation. Piscataway: IEEE Press, 2020: 2310-2316. -
下载:
点击查看大图
计量
- 文章访问数: 464
- HTML全文浏览量: 230
- PDF下载量: 32
- 被引次数: 0
