A laser inertial SLAM approach based on planar expansion and constrained optimization

Hu Wenxue; Wang Zehua; Yu Cheng; Yang Kui; Liang Dongtai

doi:10.12086/oee.2024.230279

Article navigation > Opto-Electronic Engineering > 2024 Vol. 51 > No. 4 > 230279

Next Article Previous Article

Hu W X, Wang Z H, Yu C, et al. A laser inertial SLAM approach based on planar expansion and constrained optimization[J]. Opto-Electron Eng, 2024, 51(4): 230279. doi: 10.12086/oee.2024.230279

Citation:

Hu W X, Wang Z H, Yu C, et al. A laser inertial SLAM approach based on planar expansion and constrained optimization[J]. Opto-Electron Eng, 2024, 51(4): 230279. doi: 10.12086/oee.2024.230279

A laser inertial SLAM approach based on planar expansion and constrained optimization

1.
School of Mechanical Engineering and Mechanics, Ningbo University, Ningbo, Zhejiang 315211, China
2.
Quanhang Technology Co., Ltd., Ningbo, Zhejiang 315100, China

Fund Project: Project supported by Zhejiang Provincial Public Welfare Technology Application Research Program Project (LGG21E050008), Ningbo Science and Technology Innovation 2025 Major Special Project (2022Z075), and Ningbo Public Welfare Science and Technology Program Project (2022S004)

More Information

^*Corresponding author: liangdongtai@nbu.edu.cn

Received Date 16 November 2023

Revised Date 07 February 2024

Accepted Date 08 February 2024

Published Date 25 April 2024

Abstract

Abstract

Aiming at the problem of low positioning accuracy of laser SLAM algorithm in indoor scenes with feature scarcity and narrow corners, a laser inertial SLAM method based on planar extension and constraint optimization is proposed. The IMU is fused in laser SLAM, and the laser point cloud is position compensated and key frames are judged according to the IMU state estimation results. The global planar map is constructed, the planar extraction of key frames is performed based on the RANSAC algorithm and combined with the pre-extraction method to track the planar features in order to reduce the time cost, and the fitting results are optimized by iPCA to remove the effect of noise on the RANSAC. Using the distance from the point to the surface to construct the plane constraint optimization equation, and integrate it with the edge point constraints and pre-integration constraints in a unified way to establish a nonlinear optimization model, and solve to get the optimized plane information and key frame bit position. Finally, to verify the effectiveness of the algorithm, experiments are carried out on the M2DGR public dataset and private dataset respectively, and the experimental results show that the present algorithm performs well on most of the public datasets, especially in the private dataset compared with the widely used fast-lio algorithm, the localization accuracy is improved by 61.9%, which demonstrates good robustness and real-time performance.
- SLAM /
- indoor scene /
- global planar maps /
- pre-extraction /
- iPCA optimization /
- planar constraints

FullText(HTML)

References

[1]	周治国, 曹江微, 邸顺帆. 3D激光雷达SLAM算法综述[J]. 仪器仪表学报, 2021, 41(9): 13−27. doi: 10.19650/j.cnki.cjsi.J2107897 CrossRef Google Scholar Zhou Z G, Cao J W, Di S F. Overview of 3D lidar SLAM algorithms[J]. Chin J Sci Instrum, 2021, 41(9): 13−27. doi: 10.19650/j.cnki.cjsi.J2107897 CrossRef Google Scholar
[2]	Zhang J, Singh S. LOAM: lidar odometry and mapping in real-time[C]//Robotics: Science and Systems, 2014. https://doi.org/10.15607/RSS.2014.X.007. Google Scholar
[3]	Shan T X, Englot B. LeGO-LOAM: lightweight and ground-optimized lidar odometry and mapping on variable terrain[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2018: 4758–4765. https://doi.org/10.1109/IROS.2018.8594299. Google Scholar
[4]	Besl P J, McKay N D. A method for registration of 3-D shapes[J]. IEEE Trans Pattern Anal Mach Intell, 1992, 14(2): 239−256. doi: 10.1109/34.121791 CrossRef Google Scholar
[5]	Qin C, Ye H Y, Pranata C E, et al. LINS: a lidar-inertial state estimator for robust and efficient navigation[C]//Proceedings of 2020 IEEE International Conference on Robotics and Automation (ICRA), 2020: 8899–8906. https://doi.org/10.1109/ICRA40945.2020.9197567. Google Scholar
[6]	Shan T X, Englot B, Meyers D, et al. LIO-SAM: tightly-coupled lidar inertial odometry via smoothing and mapping[C]//2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020: 5135–5142. https://doi.org/10.1109/IROS45743.2020.9341176. Google Scholar
[7]	Xu W, Zhang F. FAST-LIO: a fast, robust lidar-inertial odometry package by tightly-coupled iterated Kalman filter[J]. IEEE Robot Autom Lett, 2021, 6(2): 3317−3324. doi: 10.1109/LRA.2021.3064227 CrossRef Google Scholar
[8]	Lin J R, Zhang F. R₃LIVE: a robust, real-time, RGB-colored, LiDAR-inertial-visual tightly-coupled state estimation and mapping package[C]//2022 International Conference on Robotics and Automation (ICRA), 2022: 10672–10678. https://doi.org/10.1109/ICRA46639.2022.9811935. Google Scholar
[9]	Li Y Y, Brasch N, Wang Y D, et al. Structure-SLAM: low-drift monocular slam in indoor environments[J]. IEEE Robot Autom Lett, 2020, 5(4): 6583−6590. doi: 10.1109/LRA.2020.3015456 CrossRef Google Scholar
[10]	Grant W S, Voorhies R C, Itti L. Efficient Velodyne SLAM with point and plane features[J]. Auton Robots, 2019, 43(5): 1207−1224. doi: 10.1007/s10514-018-9794-6 CrossRef Google Scholar
[11]	Geneva P, Eckenhoff K, Yang Y L, et al. LIPS: LiDAR-inertial 3D plane SLAM[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2018: 123–130. https://doi.org/10.1109/IROS.2018.8594463. Google Scholar
[12]	Kaess M. Simultaneous localization and mapping with infinite planes[C]//2015 IEEE International Conference on Robotics and Automation (ICRA), 2015: 4605–4611. https://doi.org/10.1109/ICRA.2015.7139837. Google Scholar
[13]	Zhou L P, Koppel D, Ju H, et al. An efficient planar bundle adjustment algorithm[C]//2020 IEEE International Symposium on Mixed and Augmented Reality (ISMAR), 2020: 136–145. https://doi.org/10.1109/ISMAR50242.2020.00035. Google Scholar
[14]	Zhou L P, Wang S Z, Kaess M. π-LSAM: LiDAR smoothing and mapping with planes[C]//2021 IEEE International Conference on Robotics and Automation (ICRA), 2021: 5751–5757. https://doi.org/10.1109/ICRA48506.2021.9561933. Google Scholar
[15]	Chen X Y, Wu P X, Li G, et al. LIO-PPF: fast LiDAR-inertial odometry via incremental plane pre-fitting and skeleton tracking[C]//2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2023: 1458–1465. https://doi.org/10.1109/IROS55552.2023.10341524. Google Scholar
[16]	Zhu F C, Ren Y F, Zhang F. Robust real-time LiDAR-inertial initialization[C]//2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2022: 948–3955. https://doi.org/10.1109/IROS47612.2022.9982225. Google Scholar
[17]	Qin T, Li P L, Shen S J. VINS-mono: a robust and versatile monocular visual-inertial state estimator[J]. IEEE Trans Robot, 2018, 34(4): 1004−1020. doi: 10.1109/TRO.2018.2853729 CrossRef Google Scholar
[18]	刘亚坤, 李永强, 刘会云, 等. 基于改进RANSAC算法的复杂建筑物屋顶点云分割[J]. 地球信息科学学报, 2021, 23(8): 1497−1507. doi: 10.12082/dqxxkx.2021.200742 CrossRef Google Scholar Liu Y K, Li Y Q, Liu H Y, et al. An improved RANSAC algorithm for point cloud segmentation of complex building roofs[J]. J Geo-Inf Sci, 2021, 23(8): 1497−1507. doi: 10.12082/dqxxkx.2021.200742 CrossRef Google Scholar
[19]	夏金泽, 孙浩铭, 胡盛辉, 等. 基于图像信息约束的三维激光点云聚类方法[J]. 光电工程, 2023, 50(2): 220148. doi: 10.12086/oee.2023.220148 CrossRef Google Scholar Xia J Z, Sun H M, Hu S H, et al. 3D laser point cloud clustering method based on image information constraints[J]. Opto-Electron Eng, 2023, 50(2): 220148. doi: 10.12086/oee.2023.220148 CrossRef Google Scholar
[20]	Bai C G, Xiao T, Chen Y J, et al. Faster-LIO: lightweight tightly coupled lidar-inertial odometry using parallel sparse incremental voxels[J]. IEEE Robot Autom Lett, 2022, 7(2): 4861−4868. doi: 10.1109/LRA.2022.3152830 CrossRef Google Scholar
[21]	Yuan C J, Lin J R, Zou Z H, et al. STD: stable triangle descriptor for 3D place recognition[C]//2023 IEEE International Conference on Robotics and Automation (ICRA), 2023: 1897–1903. https://doi.org/10.1109/ICRA48891.2023.10160413. Google Scholar
[22]	Cui Y G, Chen X Y L, Zhang Y L, et al. BoW3D: bag of words for real-time loop closing in 3D LiDAR SLAM[J]. IEEE Robot Autom Lett, 2023, 8(5): 2828−2835. doi: 10.1109/LRA.2022.3221336 CrossRef Google Scholar
[23]	Kim G, Kim A. Scan context: egocentric spatial descriptor for place recognition within 3D point cloud map[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2018: 4802–4809. https://doi.org/10.1109/IROS.2018.8593953. Google Scholar

Overview

Overview

The scarcity of features and narrow corners in indoor environments make the laser SLAM algorithm have low localization accuracy and even algorithm failure. To solve the above problems, a laser inertial SLAM method based on plane extension and constraint optimization is proposed. Fusion of IMU in laser SLAM, position compensation of laser point cloud, and judgment of key frames based on IMU state estimation results. Build a global planar map, planar extraction of key frames based on the RANSAC algorithm, and track planar features by combining the pre-extraction method to reduce the time cost, and the fitting results are optimized by iPCA to remove the effect of noise on RANSAC. Using the distance from the point to the plane, construct the plane constraint optimization equation. Integrate it with the edge point constraints and pre-integration constraints in a unified way to establish a nonlinear optimization model. Solve this model to get the optimized plane information and key frame bit position. To verify the effectiveness of the algorithm, experiments are carried out in the M2DGR public dataset and private dataset respectively. The results of plane extraction are shown in Table 1, facing different scenes and distances, the position accuracy error of the plane fitting method, which is based on the combination of RANSAC and iPCA and can be controlled within 10mm. Additionally, the attitude accuracy error is less than 2, meeting the initial value requirements. Figures 9 and 10 visualize the localization effect of this method and other algorithms. The experimental results show that the algorithm not only performs well on the open dataset, but also in the closed-loop dataset "Indoor_01", which has narrow corners and fewer features, the algorithm improves 61.9% compared with the comparison algorithm, which can effectively inhibit the drift caused by the corners and the lack of features (Fig. 10). The planar pre-extraction method effectively reduces the time cost of RANSAC, and the use of planar constraints instead of planar point constraints saves the unnecessary search and fitting process (Fig. 11), which provides the possibility of deploying the algorithm in mobile devices. The experimental results show that the comnbination of planar pre-extraction and the iPCA-based planar optimization scheme effectively eliminates noise and the error caused by the unstrict planes, while also saves the unnecessary RANSAC fitting iterations. The plane constraints also effectively replace the plane point constraints, which are uniformly fused with the edge point constraints and preintegration constraints to participate in the optimization after compression. The proposed method effectively improves the localization accuracy of laser SLAM in indoor environments, demonstrating robustness and real-time capabilities.