R-LVIO: Resilient LiDAR-Visual-Inertial Odometry for UAVs in GNSS-denied Environment
<p>The schematic diagram of coordinate transformation. <math display="inline"><semantics> <msub> <mi>T</mi> <mrow> <mi>C</mi> <mi>B</mi> </mrow> </msub> </semantics></math> and <math display="inline"><semantics> <msub> <mi>T</mi> <mrow> <mi>L</mi> <mi>B</mi> </mrow> </msub> </semantics></math> represent the external parameter from camera and LiDAR to IMU. <math display="inline"><semantics> <msub> <mi>T</mi> <mrow> <mi>B</mi> <mi>W</mi> </mrow> </msub> </semantics></math> represents the transformation from the body frame to the world frame.</p> "> Figure 2
<p>Pipeline of the proposed system. The proposed system is divided into the IMU module, the visual-inertial module, and the LiDAR-inertial module. Modules with red borders are highlighted in this paper. In detail, the LiDAR-inertial module provides depth measurements for visual features by aggregating recent multi-frame sweeps. Moreover, the motion estimation of the visual-inertial module is constrained by the back-propagated pose from the LiDAR-inertial module at the previous moment. The visual-inertial module provides the initial guess for the LiDAR-inertial module’s point cloud matching. The camera pose and LiDAR pose are fed into the IMU module and form the measurement residuals with IMU preintegration, followed by minimizing the measurement residuals in the factor graph optimization to estimate the final state.</p> "> Figure 3
<p>Factor graph of the system. The IMU module is constrained by LiDAR and visual modules, and ultimately outputs a refined system state.</p> "> Figure 4
<p>The illustration of edge points aggregation. Different colored edge dots indicate different ranges. Vertical observations are projected onto the segmented horizontal plane for clustering.</p> "> Figure 5
<p>The model of the aligned point. (<b>a</b>) is the point-to-plane model that is employed in structured scenes. (<b>b</b>) is the point-to-surface model with uncertainty for aligning irregular ground points.</p> "> Figure 6
<p>The platform is used for research and collection of the private dataset. The UAV in (<b>a</b>) is equipped with a GPS mobile station, LIDAR, on-board computer and pinhole camera. The world frame is defined as the first IMU frame. Satellite photographs (<b>b</b>–<b>e</b>) show four scenes. The orange curves represent the ground truth of these sequences as determined by the GNSS/IMU positioning system.</p> "> Figure 7
<p>The qualitative structures of the private dataset are shown. (<b>a</b>–<b>c</b>) are grove sequence, beach sequence, and desert sequence, respectively. Cubes are salient edge points. The QS stands for quantitative structure.</p> "> Figure 8
<p>Point cloud maps of NTU dataset are shown. (<b>a</b>–<b>c</b>) are rtp sequences, sbs sequences, and tnp sequences, respectively.</p> "> Figure 9
<p>3D bird’s-eye view map of the campus sequence. The quantitative structure of the three locations is emphasized as (<b>a</b>–<b>c</b>). Figure (<b>d</b>,<b>e</b>) show point cloud maps from a bird’s-eye perspective, presenting the consistent map without point cloud divergence.</p> "> Figure 10
<p>Localization accuracy experiments are performed on the NTU-VIRAL dataset. The trajectories’ errors are compared with the ground truths, which are provided by the public dataset. Subfigures (<b>a</b>–<b>i</b>) represent the trajectory results for the rtp sequence, the sbs sequence, and the tnp sequence, respectively. The “reference” in the subfigure is the ground truth of the UAV trajectory. The heat map color of the estimated trajectories indicate the error level.</p> "> Figure 11
<p>Trajectory comparisons on the private dataset. Subfigures (<b>a</b>–<b>c</b>) are indicate the comparisons on the grove, beach, and desert sequences. As the degree of non-structuring increases, the robustness of LVI-SAM and FAST-LIVO decreases. The proposed system can still maintain high localization accuracy.</p> ">
Abstract
:1. Introduction
- Improve the accuracy of short-term IMU predictions by increasing the frequency of corrections from the LiDAR and visual modules. LiDAR pose frequency is boosted by sweep segmentation to synchronize the LiDAR input time with the camera sampling time.
- Devise an outliers rejection strategy of depth association between the camera image and LiDAR points to select accurate depth points by evaluating the reprojection error of visual feature points in a sliding window.
- Design a structure monitor to distinguish structured scenes and unstructured scenes by analyzing the vertical landmarks. The environmental structuring is quantified to switch the operating modes of the LiDAR module.
- Propose a novel point-to-surface model to register irregular surfaces in unstructured scenes, achieving three horizontal DoF state estimation. The vertical 3-DoF state is predicted by IMU relative measurement.
2. Related Works
2.1. Sensor Fusion Localization System
2.2. Point Cloud Registration
3. Problem Statement
4. Proposed Method
4.1. System Overview
4.2. Imu Module with High Frequency Correction
4.2.1. Time Synchronization Based on Sweep Segmentation
4.2.2. IMU Kinetic Model
4.3. Visual Module with Position Consistency Constraint
4.3.1. Depth Association by Outliers Rejection
4.3.2. Motion Estimation Assisted by LiDAR Odometry Back-propagation
4.4. Adaptive LiDAR Module with Hybrid Registration Mode
4.4.1. Structure Monitor Based on Vertical Landmarks
4.4.2. Hybrid Point Cloud Alignment
Algorithm 1 Hybrid Point Cloud Registration |
|
5. Experimental Results
5.1. Structure Monitor Evaluation
5.2. System Localization Accuracy Evaluation
5.3. System Robustness Evaluation
5.4. Runtime Evaluation
6. Conclusions
Author Contributions
Funding
Data Availability Statement
Conflicts of Interest
References
- Liu, Y.; Bai, J.; Wang, G.; Wu, X.; Sun, F.; Guo, Z.; Geng, H. UAV Localization in Low-Altitude GNSS-Denied Environments Based on POI and Store Signage Text Matching in UAV Images. Drones 2023, 7, 451. [Google Scholar] [CrossRef]
- Wang, R.; Deng, Z. Rapid Initialization Method of Unmanned Aerial Vehicle Swarm Based on VIO-UWB in Satellite Denial Environment. Drones 2024, 8, 451. [Google Scholar] [CrossRef]
- Fu, J.; Yao, W.; Sun, G.; Ma, Z.; Dong, B.; Ding, J.; Wu, L. Multi-robot Cooperative Path Optimization Approach for Multi-objective Coverage in a Congestion Risk Environment. IEEE Trans. Syst. Man Cybern. Syst. 2024, 54, 1816–1827. [Google Scholar] [CrossRef]
- Shao, X.; Sun, G.; Yao, W.; Liu, J.; Wu, L. Adaptive Sliding Mode Control for Quadrotor UAVs with Input Saturation. IEEE-ASME T MECH. 2022, 27, 1498–1509. [Google Scholar] [CrossRef]
- Qin, T.; Li, P.; Shen, S. VINS-mono: A Robust and Versatile Monocular Visual-inertial State Estimator. IEEE Trans. Robot. 2018, 34, 1004–1020. [Google Scholar] [CrossRef]
- Qin, C.; Ye, H.; Pranata, C.E.; Han, J.; Zhang, S.; Liu, M. LINS: A LiDAR-inertial State Estimator for Robust and Efficient Navigation. In Proceedings of the 2020 IEEE International Conference on Robotics and Automation, Paris, France, 31 May–31 August 2020; pp. 8899–8906. [Google Scholar]
- Shu, C.; Luo, Y. Multi-Modal Feature Constraint Based Tightly Coupled Monocular Visual-LiDAR Odometry and Mapping. IEEE Trans. Intell. Veh. 2023, 8, 3384–3393. [Google Scholar] [CrossRef]
- Xie, J.; He, X.; Mao, J.; Zhang, L.; Hu, X. C2VIR-SLAM: Centralized Collaborative Visual-Inertial-Range Simultaneous Localization and Mapping. Drones 2022, 6, 312. [Google Scholar] [CrossRef]
- Xu, W.; Zhang, F. FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-coupled Iterated Kalman Filter. IEEE Robot. Autom. Lett. 2021, 6, 3317–3324. [Google Scholar] [CrossRef]
- Shan, T.; Englot, B.; Meyers, D.; Wang, W.; Ratti, C.; Rus, D. LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping. In Proceedings of the 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems, Las Vegas, NV, USA, 25–29 October 2020; pp. 5135–5142. [Google Scholar]
- Nguyen, T.M.; Cao, M.; Yuan, S.; Lyu, Y.; Nguyen, T.H.; Xie, L. VIRAL-Fusion: A Visual-Inertial-Ranging-Lidar Sensor Fusion Approach. IEEE Trans. Robot. 2022, 38, 958–977. [Google Scholar] [CrossRef]
- Zuo, X.; Yang, Y.; Geneva, P.; Lv, J.; Liu, Y.; Huang, G.; Pollefeys, M. LIC-Fusion 2.0: LiDAR-Inertial-Camera Odometry with Sliding-Window Plane-Feature Tracking. In Proceedings of the 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems, Las Vegas, NV, USA, 25–29 October 2020; pp. 5112–5119. [Google Scholar]
- Shan, T.; Englot, B.; Ratti, C.; Rus, D. LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping. In Proceedings of the 2021 IEEE International Conference on Robotics and Automation, Xi’an, China, 30 May–5 June 2021; pp. 5692–5698. [Google Scholar]
- Lin, J.; Zhang, F. R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package. In Proceedings of the 2022 International Conference on Robotics and Automation, Philadelphia, PA, USA, 23–27 May 2022; pp. 10672–10678. [Google Scholar]
- Zheng, C.; Zhu, Q.; Xu, W.; Liu, X.; Guo, Q.; Zhang, F. FAST-LIVO: Fast and Tightly-coupled Sparse-Direct LiDAR-Inertial-Visual Odometry. In Proceedings of the 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems, Kyoto, Japan, 23–27 October 2022; pp. 4003–4009. [Google Scholar]
- Zhang, J.; Singh, S. Laser-visual-inertial Odometry and Mapping with High Robustness and Low Drift. J. Field Robot. 2018, 35, 1242–1264. [Google Scholar] [CrossRef]
- Wisth, D.; Camurri, M.; Fallon, M. VILENS: Visual, Inertial, Lidar, and Leg Odometry for All-Terrain Legged Robots. IEEE Trans. Robot. 2023, 39, 309–326. [Google Scholar] [CrossRef]
- Yuan, Z.; Wang, Q.; Cheng, K.; Hao, T.; Yang, X. SDV-LOAM: Semi-Direct Visual–LiDAR Odometry and Mapping. IEEE Trans. Pattern Anal. Mach. Intell. 2023, 45, 11203–11220. [Google Scholar] [CrossRef] [PubMed]
- Shan, T.; Englot, B. LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain. In Proceedings of the 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems, Madrid, Spain, 1–5 October 2018; pp. 4758–4765. [Google Scholar]
- Chen, S.; Ma, H.; Jiang, C.; Zhou, B.; Xue, W.; Xiao, Z.; Li, Q. NDT-LOAM: A Real-Time Lidar Odometry and Mapping With Weighted NDT and LFA. IEEE Sensors J. 2022, 22, 3660–3671. [Google Scholar] [CrossRef]
- Cui, Y.; Zhang, Y.; Dong, J.; Sun, H.; Chen, X.; Zhu, F. LinK3D: Linear Keypoints Representation for 3D LiDAR Point Cloud. IEEE Robot. Autom. Lett. 2024, 9, 2128–2135. [Google Scholar] [CrossRef]
- Zhang, J.; Singh, S. LOAM: Lidar Odometry and Mapping in Real-time. In Proceedings of the 2014 Robotics: Science and Systems, Berkeley, CA, USA, 12–16 July 2014. [Google Scholar]
- Guo, S.; Rong, Z.; Wang, S.; Wu, Y. A LiDAR SLAM with PCA-based feature extraction and two-stage matching. IEEE Trans. Instrum. Meas. 2022, 71, 1–11. [Google Scholar] [CrossRef]
- Choi, S.; Chae, W.H.; Jeung, Y.; Kim, S.; Cho, K.; Kim, T.W. Fast and Versatile Feature-Based LiDAR Odometry via Efficient Local Quadratic Surface Approximation. IEEE Robot. Autom. Lett. 2023, 8, 640–647. [Google Scholar] [CrossRef]
- Chen, K.; Lopez, B.T.; Agha, M.; Ali, A.; Mehta, A. Direct LiDAR Odometry: Fast Localization with Dense Point Clouds. IEEE Robot. Autom. Lett. 2022, 7, 2000–2007. [Google Scholar] [CrossRef]
- Chen, G.; Hong, L. Research on Environment Perception System of Quadruped Robots Based on LiDAR and Vision. Drones 2023, 7, 329. [Google Scholar] [CrossRef]
- Forster, C.; Carlone, L.; Dellaert, F.; Scaramuzza, D. On-Manifold Preintegration for Real-Time Visual-inertial Odometry. IEEE Trans. Robot. 2017, 33, 1–21. [Google Scholar] [CrossRef]
- Zuo, X.; Geneva, P.; Lee, W.; Liu, Y.; Huang, G. LIC-Fusion: LiDAR-Inertial-Camera Odometry. In Proceedings of the 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems, Macau, China, 4–8 November 2018; pp. 5848–5854. [Google Scholar]
- Campos, C.; Elvira, R.; Rodriguez, G.J.J.; Montiel, M.M.J.; Tardos, J.D. ORB-SLAM3: An Accurate Open-source Library for Visual, Visual¨Cinertial, and Multimap SLAM. IEEE Trans. Robot. 2021, 37, 1874–1890. [Google Scholar] [CrossRef]
- Xu, M.; Lin, S.; Wang, J.; Chen, Z. A LiDAR SLAM System With Geometry Feature Group-Based Stable Feature Selection and Three-Stage Loop Closure Optimization. IEEE Trans. Instrum. Meas. 2023, 72, 1–10, Erratum in IEEE Trans. Instrum. Meas. 2023, 7, 524. [Google Scholar] [CrossRef]
- Zhou, R.; Sun, H.; Ma, K.; Tang, J.; Chen, S.; Fu, L.; Liu, Q. Improving Estimation of Tree Parameters by Fusing ALS and TLS Point Cloud Data Based on Canopy Gap Shape Feature Points. Drones 2023, 7, 524. [Google Scholar] [CrossRef]
- Baah, G.A.; Savin, I.Y.; Vernyuk, Y.I. Pollution from Highways Detection Using Winter UAV Data. Drones 2023, 7, 178. [Google Scholar] [CrossRef]
- Nguyen, T.M.; Yuan, S.; Cao, M.; Lyu, Y.; Nguyen, T.H.; Xie, L. NTU VIRAL: A visual-inertial-ranging-lidar dataset, from an aerial vehicle viewpoint. Int. J. Robot. Res. 2022, 41, 270–280. [Google Scholar] [CrossRef]
- Wang, Y.; Ma, H. mVIL-Fusion: Monocular Visual-Inertial-LiDAR Simultaneous Localization and Mapping in Challenging Environments. IEEE Robot. Autom. Lett. 2023, 8, 504–511. [Google Scholar] [CrossRef]
Notations | Descriptions |
---|---|
, | Input time of camera image and LiDAR sweep |
Set of all states up to moment | |
State at time | |
, and | Rotation matrix, position vector, and linear velocity at time |
, | Observations of camera and LiDAR at time |
Set of IMU measurements between moments and | |
, , , | Residuals of prior, IMU preintegration, and visual and LiDAR feature |
, | Measurements value of accelerometer and gyroscope at time |
, | Acceleration and angular velocity of platform motion |
gravity | |
, , , | Biases and noise of accelerometer and gyroscope |
, , | Preintegrated measurements for orientation, translation, and velocity |
, , | Preintegration error |
, , | Prediction states from IMU |
, | IMU measurement errors with VIO and LIO |
, | Uncertain matrix of LiDAR and camera poses |
Error of marginalized prior | |
Reprojection residual error of the visual feature | |
Constraint from the back-propagated LiDAR pose | |
Set of camera poses | |
Smoothness of the LiDAR point | |
The range of the point . | |
, | Edge and plane points of n-th sweep |
All feature points of n-th sweep | |
Clustering results of n-th sweep | |
The weighted distance vector of the i-th sector | |
Environmental structuring of n-th sweep | |
Initial LiDAR pose from prediction state | |
Weighted point-to-feature distance | |
Feature registration error | |
Point-to-Gaussian surface error | |
, , | Covariance matrix, eigenvector matrix, and diagonal matrix |
LiDAR pose at time | |
Rotation matrix on angle | |
, | Jacobian matrix and Hessian matrix by differentiating the projected point to the pose |
Data | rtp1 | rtp2 | rtp3 | sbs1 | sbs2 | sbs3 | tnp1 | tnp2 | tnp3 | Camp | Grove | Beach | Desert |
---|---|---|---|---|---|---|---|---|---|---|---|---|---|
Result | 5.21 | 5.53 | 5.39 | 6.03 | 5.86 | 5.15 | 6.84 | 6.67 | 5.51 | 6.30 | 4.13 | 2.26 | 0.04 |
Data | LIO-SAM | LVI-SAM | FAST-LIO2 | FAST-LIVO | mVIL-Fusion | Ours |
---|---|---|---|---|---|---|
rtp1 | 0.674 | 0.954 | 0.265 | |||
rtp2 | 0.177 | X | 0.195 | 0.861 | 0.673 | 0.201 |
rtp3 | 0.385 | 0.204 | 0.195 | 0.283 | 0.426 | 0.141 |
sbs1 | 0.214 | 0.215 | 0.223 | 0.351 | 0.213 | 0.114 |
sbs2 | 0.208 | 0.208 | 0.213 | 0.232 | 0.225 | 0.200 |
sbs3 | 0.179 | X | 0.210 | 0.210 | 0.193 | 0.175 |
tnp1 | 0.193 | 0.134 | 0.146 | 0.202 | 0.269 | 0.214 |
tnp2 | 0.192 | 0.180 | 0.169 | 0.124 | 0.229 | 0.168 |
tnp3 | 0.176 | 0.479 | 0.181 | 0.165 | 0.223 | 0.109 |
campus | 0.466 | 1.641 | 0.457 | 0.395 | 0.312 | |
grove | 5.237 | 6.318 | 5.047 | 5.215 | - | 4.880 |
beach | 0.453 | 6.449 | 1.171 | 2.159 | - | 0.241 |
desert | X | X | X | X | - | 10.87 |
Component | Median | Mean | Std | |
---|---|---|---|---|
Visual module | 17.11 | 17.26 | 5.16 | |
Feature management | 17.22 | 18.31 | 4.39 | |
Pose estimation | 13.19 | 12.53 | 5.33 | |
LiDAR module | Preprocessing | 9.49 | 11.61 | 3.92 |
Feature extraction | 1.15 | 1.29 | 1.72 | |
Structure monitor | 0.15 | 1.56 | 3.67 | |
Hybrid point registration | 22.55 | 24.71 | 17.43 | |
IMU module | Preintegration | 0.143 | 0.151 | 0.360 |
Factor graph optimization | 0.280 | 0.274 | 0.311 |
Disclaimer/Publisher’s Note: The statements, opinions and data contained in all publications are solely those of the individual author(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to people or property resulting from any ideas, methods, instructions or products referred to in the content. |
© 2024 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https://creativecommons.org/licenses/by/4.0/).
Share and Cite
Zhang, B.; Shao, X.; Wang, Y.; Sun, G.; Yao, W. R-LVIO: Resilient LiDAR-Visual-Inertial Odometry for UAVs in GNSS-denied Environment. Drones 2024, 8, 487. https://doi.org/10.3390/drones8090487
Zhang B, Shao X, Wang Y, Sun G, Yao W. R-LVIO: Resilient LiDAR-Visual-Inertial Odometry for UAVs in GNSS-denied Environment. Drones. 2024; 8(9):487. https://doi.org/10.3390/drones8090487
Chicago/Turabian StyleZhang, Bing, Xiangyu Shao, Yankun Wang, Guanghui Sun, and Weiran Yao. 2024. "R-LVIO: Resilient LiDAR-Visual-Inertial Odometry for UAVs in GNSS-denied Environment" Drones 8, no. 9: 487. https://doi.org/10.3390/drones8090487