Keywords
Cascaded tightly coupled
Hierarchical graph optimization
Urban service robot
Submitted : 2026-04-21
Accepted : 2026-05-06
Published : 2026-05-21
Abstract
Aiming at the trajectory drift and long-term computing power bottleneck of urban service robots in large-scale scenarios, this paper proposes a practical LIO-RTK-PGO multi-source fusion odometry. The front-end adopts a cascaded tightly coupled architecture, introducing RTK observation to correct the state in the filtering prediction stage to fundamentally suppress elevation and heading divergence; the back-end proposes hierarchical pose graph optimization (PGO), combining local high-frequency sliding window and global keyframe sparsification to control the computational complexity at O(1). Verified by real-vehicle tests and standard computing power platforms, the system eliminates long-range cumulative errors, providing a low-computing-power and highly reliable state estimation scheme for large-scale engineering implementation.
References
Lynen S, Achtelik M, Weiss S, et al., 2013, A Robust and Modular Multi-sensor Fusion Approach Applied to MAV Navigation, 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, 3923–3929.
Chen C, Zhu H, Li M, et al., 2018, A Review of Visual-Inertial Simultaneous Localization and Mapping from Filtering-Based and Optimization-Based Perspectives. Robotics, 7(3): 45.
Xu W, Zhang F, 2021, FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter. IEEE Robotics and Automation Letters, 6(2): 3317–3324.
Xu W, Cai Y, He D, et al., 2022, FAST-LIO2: Fast Direct LiDAR-inertial Odometry. IEEE Transactions on Robotics, 38(4): 2053–2073.
Shan T, Englot B, Meyers D, et al., 2020, LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping, 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 5135–5142.
Bai C, Xiao T, Chen Y, et al., 2022, Faster-LIO: Lightweight Tightly Coupled Lidar-Inertial Odometry using Parallel Sparse Incremental Voxels. IEEE Robotics and Automation Letters, 7(2): 4861–4868.
Zhang J, Singh S, 2014, LOAM: Lidar Odometry and Mapping in Real-Time. Robotics: Science and Systems, 2(9): 2014.
Shan T, Englot B, 2018, LeGO-LOAM: Lightweight and Ground-optimized Lidar Odometry and Mapping on Variable Terrain, 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 4758–4765.
Forster C, Carlone L, Dellaert F, et al., 2016, On-Manifold Preintegration for Real-Time Visual-Inertial Odometry. IEEE Transactions on Robotics, 33(1): 1–21.
Ye H, Chen Y, Liu M, 2019, Tightly Coupled 3D Lidar Inertial Odometry and Mapping, 2019 IEEE International Conference on Robotics and Automation (ICRA), 3144–3150.
Le Gentil C, Vidal-Calleja T, Huang S, 2019, IN2LAMA: Inertial Lidar Localisation and Mapping, 2019 IEEE International Conference on Robotics and Automation (ICRA), 6388–6394.
Dellaert F, Kaess M, 2017, Factor Graphs for Robot Perception. Foundations and Trends in Robotics, 6(1-2): 1–139.
Hening S, Ippolito C, Krishnakumar K, et al., 2017, 3D LiDAR SLAM Integration with GPS/INS for UAVs in Urban GPS-Degraded Environments, 2017 AIAA Infotech@Aerospace Conference, 448–457.
Gao Y, Liu S, Atia M, et al., 2015, INS/GPS/LiDAR Integrated Navigation System for Urban and Indoor Environments using Hybrid Scan Matching Algorithm. Sensors, 15(9): 23286–23302.
Demir M, Fujimura K, 2019, Robust Localization with Low-Mounted Multiple LiDARs in Urban Environments, 2019 IEEE Intelligent Transportation Systems Conference (ITSC), 3288–3293.