Localization and Offline Mapping of High-Voltage Substations in Rough Terrain Using a Ground Vehicle

Introduction

The paper discusses the localization problem, which refers to estimating the pose (position and orientation) of a mobile robot within a prior map. It highlights the importance of this problem for autonomous navigation in known environments, particularly for outdoor navigation in uneven or rough terrain, where three-dimensional information is crucial.

The paper critiques the current localization algorithms, which are often extensions of Simultaneous Localization and Mapping (SLAM) algorithms. These algorithms focus on creating a 3D map and estimating the robot's pose concurrently. However, they are not optimized for localization within a known map.

In dynamic environments where the map continually changes, these algorithms may fail to localize accurately due to insufficient correspondences or mismatches between 3D points. The paper suggests that for accurate localization in dynamic environments, the algorithm should update the map continuously, allowing the robot to localize with reference to the prior map.

The paper discusses the importance of generating accurate 3D point clouds from Lidar-inertial odometry algorithms, which are crucial for tasks such as inspection and navigation within high-voltage electric substations (HVSS). The noisy nature of point clouds due to measurement errors and misregistration can lead to undesirable surface thickness, hindering processes like raycasting and visibility determination.

The paper highlights the necessity of generating sharp, de-noised 3D models for effective inspection tasks within a known environment. After acquiring an accurate point cloud and determining regions of interest, the next challenge is safe navigation within the HVSS map. This requires precise extraction of the ground from the point cloud, a topic not widely researched. Most algorithms rely on the cloth simulation filter (CSF) or three-dimensional random sample consensus (RANSAC), which may not be effective in all scenarios.

The paper describes a complete pipeline for crafting high-quality maps from collected point clouds and navigating within rough terrain inside an HVSS using ground vehicles. The main contributions include:

1. An extended version of FAST-LIO2 capable of localization within a known environment and updating pre-built maps through a hybrid scheme.
2. Techniques for smoothing and de-noising 3D point clouds generated from real-time SLAM algorithms and extracting the ground map.
3. Application of methods to determine traversable regions of rough ground terrain for safe navigation within an HVSS.

The authors have made the code open-source and available on GitHub (https://github.com/iral-ntua/FAST_LIO_LOCALIZATION).

Related Work

The passage discusses various algorithms and techniques related to 3D localization, point cloud denoising, and ground modeling for autonomous navigation.

For 3D localization, it mentions the 3D Adaptive Monte Carlo Localization (AMCL) algorithm, which extends the 2D AMCL approach and uses a particle filter. However, it requires precise odometry data, which often necessitates computationally expensive SLAM algorithms. Recent 3D SLAM algorithms like LIORF and FAST-LIO2 have improved efficiency while maintaining accuracy.

Regarding point cloud denoising, traditional techniques like bilateral and guided filters are discussed, which leverage geometric attributes and solve optimization problems. However, they have limitations in parameter tuning and handling noise. Recent deep learning-based approaches, such as Neural Projection Denoising (NPD) and Total Denoising Neural Network, have shown promising results in robustness and unsupervised learning.

For ground modeling, a real-time approach using Bayesian generalized kernel inference is presented. It assigns grid cells to the point cloud and performs regression and classification to obtain an elevation map and identify traversable regions. An extension incorporates semantic segmentation using visual information from a camera to improve obstacle detection.

The passage highlights the advancements and challenges in these areas, aiming to improve the performance and efficiency of autonomous navigation systems.

System Overview

The system overview is as follows: The SLAM (Simultaneous Localization and Mapping) algorithm generates an initial point cloud map of the environment. This point cloud undergoes smoothing post-processing. A ground extraction technique separates the terrain from the point cloud, forming the basis for constructing a traversability map. This traversability map is crucial for planning the motion of the robot to explore user-defined regions of interest. Concurrently, a localization module estimates the pose (position and orientation) of the unmanned ground vehicle (UGV) within the prior map, using the SLAM-generated point cloud as input. The localization information is essential for executing the motion commands generated by the planner. The proposed scheme functions as a subsystem within a broader system designed for inspection purposes.

V Method

The paper discusses the localization framework which is based on the FAST-LIO2 architecture. Key points regarding localization:

• It employs scan-to-scan motion undistortion using IMU measurements.
• It performs pose estimation on each scan by minimizing residuals from registering current scan points to the map using an Extended Kalman Filter.
• It utilizes an incremental kd-tree for efficient nearest neighbor searches during registration.

The contribution of the extended FAST-LIO2 version includes:

1. Robust localization on pre-built maps
2. Pose initialization within known maps using ICP
3. Publishing complete odometry messages
4. Improved memory handling for Lidar and IMU data

For pose initialization, ICP is performed between accumulated static scans and the prior map, using a Euclidean fitness score to ensure accurate initialization.

The paper then describes the map crafting process:

1. Uniform sampling to discretize and remove noise
2. Moving Least Squares for denoising and surface smoothing

Finally, it covers traversability mapping using a Grid Map representation of the terrain point cloud and applying surface normals, slope, and roughness filters to determine traversable regions for navigation.

Experimental Results

This section evaluates the performance of the proposed localization method, called FAST-LIO-LOCALIZATION, against two other methods: LIORF and traditional ICP (Iterative Closest Point). The evaluation metric used is the mean cloud-to-cloud distance between the point clouds generated by each localization algorithm and the prior map. This metric is chosen because the localization point clouds are formed from odometry-registered undistorted scans, and the goal of localization is to accurately position the robot within a known map.

The experiments were conducted on a laptop with an Intel Core i9 CPU and 32GB RAM, using data collected from a Robosense RS-LiDAR-16 LiDAR and a Vectornav VN-100 IMU. Two distinct datasets from the HVSS (Hyundai Venue Summer School) were used: one for generating the prior map using FAST-LIO2, and the other for evaluating the localization methods.

The results in Table I show that FAST-LIO-LOCALIZATION outperforms the other methods in terms of accuracy, with a mean error of 0.026 meters and a standard deviation of 0.049 meters. Traditional ICP fails to localize the sensor after a few seconds due to its high computational burden. LIORF performs better than ICP but is still considerably demanding on computational resources, with a mean error of 0.050 meters and a standard deviation of 0.069 meters.

Figure 6 visualizes the alignment of the corresponding point clouds. The point cloud from LIORF appears more dominant due to noise around the surfaces, even though it is sparser since only keyframe scans are projected to the map. In contrast, the point cloud from FAST-LIO-LOCALIZATION is well-balanced and aligned almost exactly to the prior map, as every scan is aligned to the map.

Conclusion

The paper presents a hybrid localization and simultaneous localization and mapping (SLAM) method, along with an effective scheme for filtering raw point clouds to generate noise-free maps. The localization method is based on the FAST-LIO2 framework, while the map crafting incorporates a two-step filtering process. First, a uniform sampling filter is applied, followed by a smoothing filter that utilizes the moving least squares algorithm. The noise-free point cloud enables efficient development of essential tasks for inspecting points of interest within the HVSS (presumably, a system or environment being studied). The localization module is necessary for safe navigation of the UGV (unmanned ground vehicle). The presented material demonstrates the effectiveness of the map crafting method, and the results related to localization confirm the superiority of the proposed method compared to current robust localization algorithms.