Papers
Topics
Authors
Recent
Gemini 2.5 Flash
Gemini 2.5 Flash
110 tokens/sec
GPT-4o
56 tokens/sec
Gemini 2.5 Pro Pro
44 tokens/sec
o3 Pro
6 tokens/sec
GPT-4.1 Pro
47 tokens/sec
DeepSeek R1 via Azure Pro
28 tokens/sec
2000 character limit reached

GNSS/Multi-Sensor Fusion Using Continuous-Time Factor Graph Optimization for Robust Localization (2309.11134v3)

Published 20 Sep 2023 in cs.RO

Abstract: Accurate and robust vehicle localization in highly urbanized areas is challenging. Sensors are often corrupted in those complicated and large-scale environments. This paper introduces GNSS-FGO, an online and global trajectory estimator that fuses GNSS observations alongside multiple sensor measurements for robust vehicle localization. In GNSS-FGO, we fuse asynchronous sensor measurements into the graph with a continuous-time trajectory representation using Gaussian process regression. This enables querying states at arbitrary timestamps so that sensor observations are fused without requiring strict state and measurement synchronization. Thus, the proposed method presents a generalized factor graph for multi-sensor fusion. To evaluate and study different GNSS fusion strategies, we fuse GNSS measurements in loose and tight coupling with a speed sensor, IMU, and lidar-odometry. We employed datasets from measurement campaigns in Aachen, Duesseldorf, and Cologne in experimental studies and presented comprehensive discussions on sensor observations, smoother types, and hyperparameter tuning. Our results show that the proposed approach enables robust trajectory estimation in dense urban areas, where the classic multi-sensor fusion method fails due to sensor degradation. In a test sequence containing a 17km route through Aachen, the proposed method results in a mean 2D positioning error of 0.48m while fusing raw GNSS observations with lidar odometry in a tight coupling.

Definition Search Book Streamline Icon: https://streamlinehq.com
References (68)
  1. N. Zhu, J. Marais, D. Bétaille, and M. Berbineau, “GNSS position integrity in urban environments: A review of literature,” IEEE Trans. Intell. Transp. Syst., vol. 19, no. 9, pp. 2762–2778, 2018.
  2. T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, “LIO-SAM: Tightly-coupled LiDAR inertial odometry via smoothing and mapping,” 2020 IEEE/RSJ Int. Conf. on Intell. Robots and Syst. (IROS), pp. 5135–5142, 2020.
  3. G. Bresson, Z. Alsayed, L. Yu, and S. Glaser, “Simultaneous localization and mapping: A survey of current trends in autonomous driving,” IEEE Trans. on Intelli. Vehicles, vol. 2, no. 3, pp. 194–220, 2017.
  4. C. Sung, S. Jeon, H. Lim, and H. Myung, “What if there was no revisit? Large-scale graph-based SLAM with traffic sign detection in an HD map using LiDAR inertial odometry,” Intell. Service Robotics, vol. 15, pp. 161–170, 2022.
  5. W. Wen, T. Pfeifer, X. Bai, and L.-T. Hsu, “Factor graph optimization for GNSS/INS integration: A comparison with the extended Kalman filter,” NAVIGATION, vol. 68, no. 2, pp. 315–331, 2021.
  6. H. Zhang, X. Xia, M. Nitsch, and D. Abel, “Continuous-Time factor graph optimization for trajectory smoothness of GNSS/INS navigation in temporarily GNSS-denied environments,” IEEE Robot. Autom. Lett., vol. 7, no. 4, pp. 9115–9122, 2022.
  7. F. Dellaert, “Factor Graphs: Exploiting structure in robotics,” Annual Review of Control, Robotics, and Autonomous Systems, vol. 4, no. 1, pp. 141–166, 2021.
  8. K. Koide, J. Miura, and E. Menegatti, “A portable three-dimensional lidar-based system for long-term and wide-area people behavior measurement,” Int. Journal of Advanced Robotic Systems, vol. 16, no. 2, 2019.
  9. S. Cao, X. Lu, and S. Shen, “GVINS: tightly coupled GNSS–Visual–Inertial fusion for smooth and consistent state estimation,” IEEE Trans. on Robot., vol. 38, no. 4, pp. 2004–2021, 2022.
  10. J. Zhang, W. Wen, H. Feng, X. Chen, and L.-T. Hsu, “Continuous GNSS-RTK aided by LiDAR/Inertial odometry with intelligent GNSS selection in urban canyons,” in Proc. of 34th Int. Tech. Meet. Satell. Div. Inst. Navig. (ION GNSS+), 09 2021, pp. 4198–4207.
  11. W. Wen, X. Bai, and L.-T. Hsu, “3D vision aided GNSS real-time kinematic positioning for autonomous systems in urban canyons,” NAVIGATION: Journal of the Institute of Navigation, vol. 70, no. 3, 2023.
  12. L.-T. Hsu, “Analysis and modeling GPS NLOS effect in highly urbanized area,” GPS Solutions, vol. 22, 11 2017.
  13. K. Burnett, Y. Wu, D. J. Yoon, A. P. Schoellig, and T. D. Barfoot, “Are we ready for radar to replace lidar in all-weather mapping and localization?” IEEE Robot. Autom. Lett., vol. 7, no. 4, pp. 10 328–10 335, 2022.
  14. J. Jeong, Y. Cho, Y.-S. Shin, H. Roh, and A. Kim, “Complex urban dataset with multi-level sensors from highly diverse urban environments,” The Int. Journal of Robotics Research, vol. 38, no. 6, pp. 642–657, 2019.
  15. T. Y. Tang, D. J. Yoon, and T. D. Barfoot, “A White-Noise-on-Jerk motion prior for continuous-time trajectory estimation on SE(3),” IEEE Robot. Autom. Lett., vol. 4, no. 2, pp. 594–601, 2019.
  16. J. Zhang and S. Singh, “Low-drift and real-time LiDAR odometry and mapping,” Auton. Robots, vol. 41, pp. 401–416, 02 2017.
  17. A. Geiger, P. Lenz, and R. Urtasun, “Are we ready for autonomous driving? the kitti vision benchmark suite,” in Conf. on Computer Vision and Pattern Recognition (CVPR), 2012.
  18. N. Sünderhauf and P. Protzel, “Towards robust graphical models for GNSS-based localization in urban environments,” in Int. Multi-Conf. on Systems, Signals & Devices, 2012, pp. 1–6.
  19. R. Watson and J. Gross, “Robust navigation in GNSS degraded environment using graph optimization,” in Proc. of 30th Int. Tech. Meet. Satell. Div. Inst. Navig. (ION GNSS+), 09 2017.
  20. W. Wen, Y. C. Kan, and L.-T. Hsu, “Performance comparison of GNSS/INS integrations based on EKF and factor graph optimization,” in Proc. of 32th Int. Tech. Meet. Satell. Div. Inst. Navig. (ION GNSS+), 09 2019, pp. 3019–3032.
  21. W. Wen, X. Bai, Y. C. Kan, and L.-T. Hsu, “Tightly coupled GNSS/INS integration via factor graph and aided by fish-eye camera,” IEEE Trans. on Vehicular Technology, vol. 68, no. 11, pp. 10 651–10 662, 2019.
  22. W. Wen and L.-T. Hsu, “Towards robust GNSS positioning and real-time kinematic using factor graph optimization,” in 2021 IEEE Int. Conf. on Robot. and Automat. (ICRA), 2021, pp. 5884–5890.
  23. X. Bai, W. Wen, and L.-T. Hsu, “Time-correlated window-carrier-phase-aided GNSS positioning using factor graph optimization for urban positioning,” IEEE Trans. Aerosp. Electron. Syst., vol. 58, no. 4, pp. 3370–3384, 2022.
  24. T. Suzuki, “GNSS Odometry: Precise trajectory estimation based on carrier phase cycle slip estimation,” IEEE Robot. Autom. Lett., vol. 7, no. 3, pp. 7319–7326, 2022.
  25. B. Congram and T. D. Barfoot, “Field testing and evaluation of single-receiver gps odometry for use in robotic navigation,” Field Robotics, vol. 2, pp. 1849–1873, 2022.
  26. T. Pfeifer, P. Weissig, S. Lange, and P. Protzel, “Robust factor graph optimization - a comparison for sensor fusion applications,” in 2016 IEEE 21st Int. Conf. on Emerging Technologies and Factory Automation (ETFA), 2016, pp. 1–4.
  27. T. Suzuki, “1st place winner of the smartphone decimeter challenge: Two-step optimization of velocity and position using smartphone’s carrier phase observations,” in Proc. of the 35th Int. Tech. Meet. Satell. Div. Inst. Navig. (ION GNSS+), 09 2022, pp. 2276–2286.
  28. T. Pfeifer and P. Protzel, “Incrementally learned mixture models for GNSS localization,” in 2019 IEEE Intelli. Vehicles Symposium (IV), 2019, pp. 1131–1138.
  29. W. Wen, G. Zhang, and L.-T. Hsu, “GNSS outlier mitigation via graduated non-convexity factor graph optimization,” IEEE Trans. on Vehicular Technology, vol. 71, no. 1, pp. 297–310, 2022.
  30. Z. Gong, P. Liu, F. Wen, R. Ying, X. Ji, R. Miao, and W. Xue, “Graph-based adaptive fusion of GNSS and VIO under intermittent GNSS-degraded environment,” IEEE Trans. Instrum. Meas., vol. 70, pp. 1–16, 2021.
  31. Y. Wang, W. Song, Y. Lou, Y. Zhang, F. Huang, Z. Tu, and Q. Liang, “Rail vehicle localization and mapping with LiDAR-vision-inertial-GNSS fusion,” IEEE Robot. Autom. Lett., vol. 7, no. 4, pp. 9818–9825, 2022.
  32. D. Wisth, M. Camurri, S. Das, and M. Fallon, “Unified multi-modal landmark tracking for tightly coupled LiDAR-Visual-Inertial odometry,” IEEE Robot. Autom. Lett., vol. 6, no. 2, pp. 1004–1011, 2021.
  33. W. Shao, S. Vijayarangan, C. Li, and G. Kantor, “Stereo Visual Inertial LiDAR simultaneous localization and mapping,” in 2019 IEEE/RSJ Int. Conf. on Intell. Robots and Syst. (IROS), 2019, pp. 370–377.
  34. J. Nubert, S. Khattak, and M. Hutter, “Graph-based multi-sensor fusion for consistent localization of autonomous construction robots,” in 2022 Int. Conf. on Robotics and Automation (ICRA), 2022, pp. 10 048–10 054.
  35. S. Zhao, H. Zhang, P. Wang, L. Nogueira, and S. Scherer, “Super Odometry: IMU-centric LiDAR-Visual-Inertial estimator for challenging environments,” in 2021 IEEE/RSJ Int. Conf. on Intell. Robots and Syst. (IROS), 2021, pp. 8729–8736.
  36. G. Cioffi and D. Scaramuzza, “Tightly-coupled fusion of global positional measurements in optimization-based visual-inertial odometry,” in 2020 IEEE/RSJ Int. Conf. on Intell. Robots and Syst. (IROS), 2020, pp. 5089–5095.
  37. S. Boche, X. Zuo, S. Schaefer, and S. Leutenegger, “Visual-inertial SLAM with tightly-coupled dropout-tolerant GPS fusion,” in 2022 IEEE/RSJ Int. Conf. on Intell. Robots and Syst. (IROS), 2022, pp. 7020–7027.
  38. C. Hu, S. Zhu, Y. Liang, and W. Song, “Tightly-coupled visual-inertial-pressure fusion using forward and backward imu preintegration,” IEEE Robot. Autom. Lett., vol. 7, no. 3, pp. 6790–6797, 2022.
  39. P. Furgale, T. D. Barfoot, and G. Sibley, “Continuous-time batch estimation using temporal basis functions,” in 2012 IEEE Int. Conf. on Robot. and Automat., 2012, pp. 2088–2095.
  40. D. Hug, P. Bänninger, I. Alzugaray, and M. Chli, “Continuous-time stereo-inertial odometry,” IEEE Robot. Autom. Lett., vol. 7, no. 3, pp. 6455–6462, 2022.
  41. T. D. Barfoot, C. H. Tong, and S. Särkkä, “Batch continuous-time trajectory estimation as exactly sparse Gaussian process regression,” in Robotics: Science and Systems, 2014.
  42. M. Mukadam, J. Dong, F. Dellaert, and B. Boots, “STEAP: simultaneous trajectory estimation and planning,” Auton. Robots, vol. 43, 02 2019.
  43. X. Yan, V. Indelman, and B. Boots, “Incremental sparse GP regression for continuous-time trajectory estimation and mapping,” Robotics and Autonomous Systems, vol. 87, pp. 120–132, 2017.
  44. J. Dong, J. G. Burnham, B. Boots, G. C. Rains, and F. Dellaert, “4D Crop Monitoring: Spatio-temporal reconstruction for agriculture,” 2017 IEEE Int. Conf. on Robot. and Automat. (ICRA), pp. 3878–3885, 2017.
  45. J. Johnson, J. Mangelson, T. D. Barfoot, and B. Randal, “Continuous-time trajectory estimation: A comparative study between Gaussian process and spline-based approaches,” 2024. [Online]. Available: https://arxiv.org/abs/2402.00399
  46. M. Labbé and F. Michaud, “Memory management for real-time appearance-based loop closure detection,” in 2011 IEEE/RSJ Int. Conf. on Intell. Robots and Syst. (IROS), 2011, pp. 1271–1276.
  47. ——, “RTAB-Map as an open-source lidar and visual simultaneous localization and mapping library for large-scale and long-term online operation,” J. Field Robot., vol. 36, no. 2, pp. 416–446, 2019.
  48. J. Vincent, M. Labbé, J.-S. Lauzon, F. Grondin, P.-M. Comtois-Rivet, and F. Michaud, “Dynamic object tracking and masking for visual slam,” in 2020 IEEE/RSJ Int. Conf. on Intell. Robots and Syst. (IROS), 2020, pp. 4974–4979.
  49. M. Labbé and F. Michaud, “Multi-session visual slam for illumination-invariant re-localization in indoor environments,” Frontiers in Robotics and AI, vol. 9, 2022.
  50. M. Colosi, I. Aloise, T. Guadagnino, D. Schlegel, B. D. Corte, K. O. Arras, and G. Grisetti, “Plug-and-Play SLAM: A unified SLAM architecture for modularity and ease of use,” in 2020 IEEE/RSJ Int. Conf. on Intell. Robots and Syst. (IROS), 2020, pp. 5051–5057.
  51. J. Solà, J. Vallvé, J. Casals, J. Deray, M. Fourmy, D. Atchuthan, A. Corominas-Murtra, and J. Andrade-Cetto, “WOLF: A modular estimation framework for robotics based on factor graphs,” IEEE Robot. Autom. Lett., vol. 7, no. 2, pp. 4710–4717, 2022.
  52. M. Warren, M. Greeff, B. Patel, J. Collier, A. P. Schoellig, and T. D. Barfoot, “There’s no place like home: Visual teach and repeat for emergency return of multirotor UAVs during GPS failure,” IEEE Robot. Autom. Lett., vol. 4, no. 1, pp. 161–168, 2019.
  53. F. Dellaert and M. Kaess, “Factor graphs for robot perception,” Foundations and Trends® in Robotics, vol. 6, no. 1-2, pp. 1–139, 2017.
  54. M. Heikkinen, “Geschlossene Formeln zur Berechnung rauemlicher geodaetischer Koordinaten aus rechtwinklingen Koordinaten,” Zeitschrift fuer Vermessungswessen, vol. 107, pp. 207–211, 1982.
  55. J. Zhu, “Conversion of Earth-centered Earth-fixed coordinates to geodetic coordinates,” IEEE Trans. Aerosp. Electron. Syst., vol. 30, no. 3, pp. 957–961, 1994.
  56. S. Anderson and T. D. Barfoot, “Full STEAM ahead: Exactly sparse Gaussian process regression for batch continuous-time trajectory estimation on SE(3),” in 2015 IEEE/RSJ Int. Conf. on Intell. Robots and Syst. (IROS), 2015, pp. 157–164.
  57. J. N. Wong, D. J. Yoon, A. P. Schoellig, and T. D. Barfoot, “A data-driven motion prior for continuous-time trajectory estimation on SE(3),” IEEE Robot. Autom. Lett., vol. 5, no. 2, pp. 1429–1436, 2020.
  58. T. D. Barfoot and P. T. Furgale, “Associating uncertainty with three-dimensional poses for use in estimation problems,” IEEE Trans. on Robot., vol. 30, no. 3, pp. 679–693, 2014.
  59. J. Dong, M. Mukadam, B. Boots, and F. Dellaert, “Sparse Gaussian processes on matrix Lie groups: A unified framework for optimizing continuous-time trajectories,” in 2018 IEEE Int. Conf. on Robot. and Automat. (ICRA), 2018, pp. 6497–6504.
  60. C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual-inertial odometry,” IEEE Trans. on Robot., vol. PP, 08 2016.
  61. T. Shan and B. Englot, “LeGO-LOAM: Lightweight and ground-optimized LiDAR odometry and mapping on variable terrain,” in 2018 IEEE/RSJ Int. Conf. on Intell. Robots and Syst. (IROS), 2018, pp. 4758–4765.
  62. M. Agrawal, “A Lie algebraic approach for consistent pose registration for general Euclidean motion,” in 2006 IEEE/RSJ Int. Conf. on Intell. Robots and Syst. (IROS), 2006, pp. 1891–1897.
  63. S. W. Anderson, “Batch continuous-time trajectory estimation,” PhD thesis, University of Toronto, June 2017, available at https://hdl.handle.net/1807/77974.
  64. A. Ranganathan, M. Kaess, and F. Dellaert, “Fast 3D pose estimation with out-of-sequence measurements,” in 2007 IEEE/RSJ Int. Conf. on Intell. Robots and Syst. (IROS), 2007, pp. 2486–2493.
  65. M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. Leonard, and F. Dellaert, “iSAM2: Incremental smoothing and mapping with fluid relinearization and incremental variable reordering,” in 2011 IEEE Int. Conf. on Robot. and Automat., 2011, pp. 3281–3288.
  66. D. Wisth, M. Camurri, and M. Fallon, “VILENS: Visual, inertial, lidar, and leg odometry for all-terrain legged robots,” IEEE Trans. on Robot., vol. 39, no. 1, pp. 309–326, 2023.
  67. D. J. Yoon, K. Burnett, J. Laconte, Y. Chen, H. Vhavle, S. Kammel, J. Reuther, and T. D. Barfoot, “Need for speed: Fast correspondence-free lidar odometry using Doppler velocity,” in 2023 IEEE/RSJ Int. Conf. on Intell. Robots and Syst. (IROS), 2023.
  68. J. N. Wong, D. J. Yoon, A. P. Schoellig, and T. D. Barfoot, “Variational inference with parameter learning applied to vehicle trajectory estimation,” IEEE Robot. Autom. Lett., vol. 5, no. 4, pp. 5291–5298, 2020.
User Edit Pencil Streamline Icon: https://streamlinehq.com
Authors (4)
  1. Haoming Zhang (15 papers)
  2. Chih-Chun Chen (2 papers)
  3. Heike Vallery (6 papers)
  4. Timothy D. Barfoot (89 papers)
Citations (3)
View on Semantic Scholar

Summary

We haven't generated a summary for this paper yet.

Ai Sparkles Streamline Icon: https://streamlinehq.com Summarize Now