Scan Matching Algorithm Research Articles

A berthing state estimation pipeline based on 3D point cloud scan-matching and berth line fitting

The accurate estimation of the berthing state is fundamental for ships’ autonomous navigation during berthing processes. However, most existing methods only provide partial perception information, falling short of automatic berthing requirements. To address this issue, this paper presents a berthing state estimation pipeline that only relies on a 3D LiDAR, which can provide multiple types of berthing status information. The method employs the scan-matching algorithm to estimate the ship’s position and attitude. Additionally, the point cloud line-fitting algorithm is used to establish the berth line equation. By integrating the fitted berth line equation with ship motion data, the ship’s berthing state information is calculated. Experimental results demonstrate that, compared to existing methods, the method boasts significantly smaller errors and can provide more accurate and abundant berthing state information. The proposed method is effective and practical for automatic berthing state estimation and further provides key foundations for achieving automatic berthing operations.

Research on applying scan matching algorithm to improve velocity measurement accuracy of single-line ranging lidar

There has been increased demand for safe and efficient speed measurement of moving objects in engineering and practical applications, particularly in scenarios involving the measurement of the movement speed of objects at close range. Thus, this study designed a single-line scanning pulsed lidar system. In addition, algorithms were employed to enhance the measurement accuracy of the single-line ranging pulsed lidar. In this study, the lidar was installed on a guideway-integrated testing platform to obtain point cloud data, which was processed using the normal distributions transform (NDT), iterative closest points (ICP), and correlative scan matching (CSM) algorithms. The experimental results demonstrated that the NDT algorithm outperformed the ICP and CSM algorithms in terms of velocity measurement accuracy and algorithm robustness. Moreover, the velocity measurement results obtained from the NDT algorithm were closer to the actual track speed, with a maximum velocity deviation of lesser than 0.021 m/s compared to the actual guideway speed.

CNN-Based Fault Detection of Scan Matching for Accurate SLAM in Dynamic Environments.

This paper proposes a method for CNN-based fault detection of the scan-matching algorithm for accurate SLAM in dynamic environments. When there are dynamic objects in an environment, the environment that is detected by a LiDAR sensor changes. Thus, the scan matching of laser scans is likely to fail. Therefore, a more robust scan-matching algorithm to overcome the faults of scan matching is needed for 2D SLAM. The proposed method first receives raw scan data in an unknown environment and executes ICP (Iterative Closest Points) scan matching of laser scans from a 2D LiDAR. Then, the matched scans are converted into images, which are fed into a CNN model for its training to detect the faults of scan matching. Finally, the trained model detects the faults when new scan data are provided. The training and evaluation are performed in various dynamic environments, taking real-world scenarios into account. Experimental results showed that the proposed method accurately detects the faults of scan matching in every experimental environment.

Performance of LiDAR-SLAM-based PNT with initial poses based on NDT scan matching algorithm

To achieve higher automation level of vehicles defined by the Society of Automotive Engineers, safety is a key requirement affecting navigation accuracy. We apply Light Detection and Ranging (LiDAR) as a main auxiliary sensor and propose LiDAR-based Simultaneously Localization and Mapping (SLAM) approach for Positioning, Navigation, and Timing. Furthermore, point cloud registration is handled with 3D Normal Distribution Transform (NDT) method. The initial guess of the LiDAR pose for LiDAR-based SLAM comes from two sources: one is the differential Global Navigation Satellite System (GNSS) solution; the other is Inertial Navigation System (INS) and GNSS integrated solution, generated with Extended Kalman Filter and motion constraints added, including Zero Velocity Update and Non-Holonomic Constraint. The experiment compares two initial guesses for scan matching in terms navigation accuracy. To emphasize the importance of a multi-sensor scheme in contrast to the conventional navigation method using the stand-alone system, the tests are conducted in both open sky area and GNSS signal block area, the latter might cause Multipath and Non-Line-Of-Sight effects. To enhance the navigation accuracy, the Fault Detection and Exclusion (FDE) mechanism is applied to correct the navigation outcome. The results show that the application of NDT and FDE for INS/GNSS integrated system can not only reach where-in-lane level navigation accuracy (0.5 m), but also enable constructing the dynamic map.

Dense Normal Based Degeneration-Aware 2-D Lidar Odometry for Correlative Scan Matching

Degeneracy detection and pose estimation of mobile robots have long been difficult task for laser scan matching techniques. Since the information perceived by lidar is underconstrained or deficient in degeneration scenes such as long corridors and lobbies, it is challenging for common hardware-limited lidar odometry to operate safely and stably. In this article, we propose a novel degeneration-aware correlative scan matching (CSM) algorithm for such situations. First, we carry out a low-dependency degeneration scene detection and analysis approach to determine the current degeneration situation accurately and instantly. Then, an environmentally sensitive CSM approach is designed based on real-time results, which dynamically adjusts the motion weight to achieve the autonomous adaptability to a variety of environments. Extensive experiments on both datasets and real-world scenarios demonstrate the superiority of the proposed algorithm in both degeneration scene detection and lidar odometry. In comparison to the CSM algorithm, the proposed algorithm not only minimizes the need of manually weights setup under dynamic environments but also ameliorates the accuracy of lidar odometry.

Sharp Performance Bounds for PASTA

LiDAR is a standard sensor choice for self-localization and SLAM on indoor autonomous robots. While there are many methods to estimate a robot’s location using LiDAR measurements, most rely on algorithms that solve a generic LiDAR scan matching problem. When safety is a concern, these algorithms must provide a bound on the localization error to enable safety enforcing controllers, such as those based on Control Barrier Functions. Unfortunately, most existing scan matching algorithms offer no formal guarantees and are tailored to structured, high-resolution 3D point clouds. In this paper, we present an improved theoretical analysis for a low-cost alternative to these methods named Pasta (Provably Accurate Simple Transformation Alignment), originally introduced in marchi2022lidar. We provide a formal worst-case guarantee on the localization error and show, experimentally, that it is tight. This characterization of the localization error simplifies the interface between high-dimensional perception data and safety-critical control.

Efficient Hardware Accelerator Design of Non-Linear Optimization Correlative Scan Matching Algorithm in 2D LiDAR SLAM for Mobile Robots.

Simultaneous localization and mapping (SLAM) is the major solution for constructing or updating a map of an unknown environment while simultaneously keeping track of a mobile robot's location. Correlative Scan Matching (CSM) is a scan matching algorithm for obtaining the posterior distribution probability for the robot's pose in SLAM. This paper combines the non-linear optimization algorithm and CSM algorithm into an NLO-CSM (Non-linear Optimization CSM) algorithm for reducing the computation resources and the amount of computation while ensuring high calculation accuracy, and it presents an efficient hardware accelerator design of the NLO-CSM algorithm for the scan matching in 2D LiDAR SLAM. The proposed NLO-CSM hardware accelerator utilizes pipeline processing and module reusing techniques to achieve low hardware overhead, fast matching, and high energy efficiency. FPGA implementation results show that, at 100 MHz clock, the power consumption of the proposed hardware accelerator is as low as 0.79 W, while it performs a scan match at 8.98 ms and 7.15 mJ per frame. The proposed design outperforms the ARM-A9 dual-core CPU implementation with a 92.74% increase and 90.71% saving in computing speed and energy consumption, respectively. It has also achieved 80.3% LUTs, 84.13% FFs, and 20.83% DSPs saving, as well as an 8.17× increase in frame rate and 96.22% improvement in energy efficiency over a state-of-the-art hardware accelerator design in the literature. ASIC implementation in 65 nm can further reduce the computing time and energy consumption per scan to 5.94 ms and 0.06 mJ, respectively, which shows that the proposed NLO-CSM hardware accelerator design is suitable for resource-limited and energy-constrained mobile and micro robot applications.

3D LiDAR Based SLAM System Evaluation with Low-Cost Real-Time Kinematics GPS Solution

Positioning mobile systems with high accuracy is a prerequisite for intelligent autonomous behavior, both in industrial environments and in field robotics. This paper describes the setup of a robotic platform and its use for the evaluation of simultaneous localization and mapping (SLAM) algorithms. A configuration using a mobile robot Husky A200, and a LiDAR (light detection and ranging) sensor was used to implement the setup. For verification of the proposed setup, different scan matching methods for odometry determination in indoor and outdoor environments are tested. An assessment of the accuracy of the baseline 3D-SLAM system and the selected evaluation system is presented by comparing different scenarios and test situations. It was shown that the hdl_graph_slam in combination with the LiDAR OS1 and the scan matching algorithms FAST_GICP and FAST_VGICP achieves good mapping results with accuracies up to 2 cm.

A Mobile Robot Localization Method Based on Polar Scan Matching and Adaptive Niching Chaos Optimization Algorithm

A Mobile Robot Localization Method Based on Polar Scan Matching and Adaptive Niching Chaos Optimization Algorithm

Performance Analysis of Localization Algorithms for Inspections in 2D and 3D Unstructured Environments Using 3D Laser Sensors and UAVs

One of the most relevant problems related to Unmanned Aerial Vehicle’s (UAV) autonomous navigation for industrial inspection is localization or pose estimation relative to significant elements of the environment. This paper analyzes two different approaches in this regard, focusing on its application to unstructured scenarios where objects of considerable size are present, such as a truck, a wind tower, an airplane, a building, etc. The presented methods require a previously developed Computer-Aided Design (CAD) model of the main object to be inspected. The first approach is based on an occupancy map built from a horizontal projection of this CAD model and the Adaptive Monte Carlo Localization (AMCL) algorithm to reach convergence by considering the likelihood field observation model between the 2D projection of 3D sensor data and the created map. The second approach uses a point cloud prior map of the 3D CAD model and a scan-matching algorithm based on the Iterative Closest Point Algorithm (ICP) and the Unscented Kalman Filter (UKF). The presented approaches have been extensively evaluated using simulated as well as previously recorded real flight data. We focus on aircraft inspection as a test example, but our results and conclusions can be directly extended to other applications. To support this assertion, a truck inspection has been performed. Our tests reflected that creating a 2D or 3D map from a standard CAD model and using a 3D laser scan on the created maps can optimize the processing time, resources and improve robustness. The techniques used to segment unexpected objects in 2D maps improved the performance of AMCL. In addition, we showed that moving around locations with relevant geometry after take-off when running AMCL enabled faster convergence and high accuracy. Hence, it could be used as an initial position estimation method for other localization algorithms. The ICP-NL method works well in environments with elements other than the object to inspect, but it can provide better results if some techniques to segment the new objects are applied. Furthermore, the proposed ICP-NL scan-matching method together with UKF performed faster, in a more robust manner, than NDT. Moreover, it is not affected by flight height. However, ICP-NL error may still be too high for applications requiring increased accuracy.

Multi-LiDAR Mapping for Scene Segmentation in Indoor Environments for Mobile Robots.

Nowadays, most mobile robot applications use two-dimensional LiDAR for indoor mapping, navigation, and low-level scene segmentation. However, single data type maps are not enough in a six degree of freedom world. Multi-LiDAR sensor fusion increments the capability of robots to map on different levels the surrounding environment. It exploits the benefits of several data types, counteracting the cons of each of the sensors. This research introduces several techniques to achieve mapping and navigation through indoor environments. First, a scan matching algorithm based on ICP with distance threshold association counter is used as a multi-objective-like fitness function. Then, with Harmony Search, results are optimized without any previous initial guess or odometry. A global map is then built during SLAM, reducing the accumulated error and demonstrating better results than solo odometry LiDAR matching. As a novelty, both algorithms are implemented in 2D and 3D mapping, overlapping the resulting maps to fuse geometrical information at different heights. Finally, a room segmentation procedure is proposed by analyzing this information, avoiding occlusions that appear in 2D maps, and proving the benefits by implementing a door recognition system. Experiments are conducted in both simulated and real scenarios, proving the performance of the proposed algorithms.

A Frame-to-Frame Scan Matching Algorithm for 2D Lidar Based on Attention

The frame-to-frame scan matching algorithm is the most basic robot localization and mapping module and has a huge impact on the accuracy of localization and mapping tasks. To achieve high-precision localization and mapping, we propose a 2D lidar frame-to-frame scanning matching algorithm based on an attention mechanism called ASM (Attention-based Scan Matching). Inspired by human navigation, we use a heuristic attention selection mechanism that only considers the areas covered by the robot’s attention while ignoring other areas when performing frame-to-frame scan matching tasks to achieve a similar performance as landmark-based localization. The selected landmark is not switched to another one before it becomes invisible; thus, the ASM cannot accumulate errors during the life cycle of a landmark, and the errors will only increase when the landmark switches. Ideally, the errors accumulate every time the robot moves the distance of the lidar sensing range, so the ASM algorithm can achieve high matching accuracy. On the other hand, the number of involved data during scan matching applications is small compared to the total number of data due to the attention mechanism; as a result, the ASM algorithm has high computational efficiency. In order to prove the effectiveness of the ASM algorithm, we conducted experiments on four datasets. The experimental results show that compared to current methods, ASM can achieve higher matching accuracy and speed.

Real-Time 2-D Lidar Odometry Based on ICP

This study presents a 2-D lidar odometry based on an ICP (iterative closest point) variant used in a simple and straightforward platform that achieves real-time and low-drift performance. With a designated multi-scale feature extraction procedure, the lidar cloud information can be utilized at multiple levels and the speed of data association can be accelerated according to the multi-scale data structure, thereby achieving robust feature extraction and fast scan-matching algorithms. First, on a large scale, the lidar point cloud data are classified according to the curvature into two parts: smooth collection and rough collection. Then, on a small scale, noise and unstable points in the smooth or rough collection are filtered, and edge points and corner points are extracted. Then, the proposed tangent-vector-pairs based on edge and corner points are applied to evaluate the rotation term, which is significant for producing a stable solution in motion estimation. We compare our performance with two excellent open-source SLAM algorithms, Cartographer and Hector SLAM, using collected and open-access datasets in structured indoor environments. The results indicate that our method can achieve better accuracy.

LiDAR‐based robust localization for field autonomous vehicles in off‐road environments

AbstractRobust localization is an essential capability for autonomous land vehicles. While a lot of work focused on structured environments, this article focuses on navigation in off‐road environments. In the off‐road environment, due to the lack of salient features, scan matching algorithms tend to degenerate. Therefore, the first contribution of this paper is to propose a reliable degeneracy indicator which can evaluate the scan matching performance. The evaluated degeneracy indicator is then integrated into the factor graph optimization framework which is used in both the offline mapping system and the online localization system. Moreover, a complete navigation system that can handle the incomplete and partly outdated LiDAR maps is developed. Extensive tests on real‐world data sets show that the proposed system outperforms state‐of‐the‐art approaches, especially in degenerate scenarios.

Correlation scan matching algorithm based on multi‐resolution auxiliary historical point cloud and lidar simultaneous localisation and mapping positioning application

The matching algorithm is an important part of simultaneous location and mapping. Aiming at the problem of large computation and poor real-time performance of two-dimensional lidar traditional correlation scan matching (CSM) algorithm, a multi-resolution auxiliary historical point cloud matching algorithm is proposed, which combines high and low resolution and adopts a single-frame to multi-frame step-by-step matching scheme. The algorithm was carried out on the sweeping robot. Compared with the traditional CSM algorithm and iterative closest points algorithm, the single position accuracy of the method in this study is improved. In the indoor space of ∼10 m × 10 m, the cumulative error is reduced by 16.24 and 33.96%, respectively. Consequently, our algorithm can still manage to process in real-time.

Relative multiplicative extended Kalman filter for observable GPS-denied navigation

This work presents a multiplicative extended Kalman filter (MEKF) for estimating the relative state of a multirotor vehicle operating in a GPS-denied environment. The filter fuses data from an inertial measurement unit and altimeter with relative-pose updates from a keyframe-based visual odometry or laser scan-matching algorithm. Because the global position and heading states of the vehicle are unobservable in the absence of global measurements such as GPS, the filter in this article estimates the state with respect to a local frame that is colocated with the odometry keyframe. As a result, the odometry update provides nearly direct measurements of the relative vehicle pose, making those states observable. Recent publications have rigorously documented the theoretical advantages of such an observable parameterization, including improved consistency, accuracy, and system robustness, and have demonstrated the effectiveness of such an approach during prolonged multirotor flight tests. This article complements this prior work by providing a complete, self-contained, tutorial derivation of the relative MEKF, which has been thoroughly motivated but only briefly described to date. This article presents several improvements and extensions to the filter while clearly defining all quaternion conventions and properties used, including several new useful properties relating to error quaternions and their Euler-angle decomposition. Finally, this article derives the filter both for traditional dynamics defined with respect to an inertial frame, and for robocentric dynamics defined with respect to the vehicle’s body frame, and provides insights into the subtle differences that arise between the two formulations.

Effect of Inertial Measurement Unit on Creating High-Definition 3D Map for Autonomous Vehicle

One of the most famous technologies for the autonomous vehicle was using a scan matching algorithm, in which a high-definition 3D map created by the LIDAR sensor plays a significantly important role in localizing and path planning. Within this manuscript, a novel way of finding the effect from the Inertial Measurement Unit (IMU) on creating a high-definition 3D map from the LIDAR sensor was investigated. The collection data system was first ever created and collected in Vietnam. The results show that the normal distributions transform shows very good performance for creating the HD 3D map with have IMU sensor. On the other hand, without IMU the accuracy and the robustness of the creating map were reduced especially in the non-flat area. This manuscript will start the evolution of preparation for autonomous vehicles in Vietnam as well as contribute to the autonomous vehicle research society in the world.

Improving Robot Localization Using Doppler-Based Variable Sensor Covariance Calculation.

This paper describes a localization module for an autonomous wheelchair. This module includes a combination of various sensors such as odometers, laser scanners, IMU and Doppler speed sensors. Every sensor used in the module features variable covariance estimation in order to yield a final accurate localization. The main problem of a localization module composed of different sensors is the accuracy estimation of each sensor. Average static values are normally used, but these can lead to failure in some situations. In this paper, all the sensors have a variable covariance estimation that depends on the data quality. A Doppler speed sensor is used to estimate the covariance of the encoder odometric localization. Lidar is also used as a scan matching localization algorithm, comparing the difference between two consecutive scans to obtain the change in position. Matching quality gives the accuracy of the scan matcher localization. This structure yields a better position than a traditional odometric static covariance method. This is tested in a real prototype and compared to a standard fusion technique.

Robust 3-Dimension Point Cloud Mapping in Dynamic Environment Using Point-Wise Static Probability-Based NDT Scan-Matching

Graph-based simultaneous localization and mapping (SLAM) is one of the methods to generate point cloud maps which are used for various applications in autonomous vehicles. Graph-based SLAM represents the pose of the vehicle as a node and the odometry between two different nodes as an edge. Among the edge generating methods, scan matching, light detection and ranging (LiDAR) based method, can provide an accurate pose between two nodes based on the high distance accuracy of the LiDAR. However, the point cloud in real driving situations contains numerous moving objects, which degrade the scan-matching performance. Therefore, this article defines the static probability which means the likelihood that an acquired point is from a static object, and proposes the weighted normal distribution transformation (NDT), which is achieved by modifying NDT. Weighted NDT is a scan-matching algorithm which can reflect the static probability of each point as a weight. The odometry from the weighted NDT is utilized for graph construction to generate a robust point cloud map even in a dynamic environment. Finally, the proposed algorithm was compared with the existing object removal algorithms in two areas: dynamic object classification and scan-matching performance. Based on the scan-matching results, the accuracy of the point cloud map generated by the proposed algorithm was evaluated with a reference map using high-performance global navigation satellite system (GNSS). It was confirmed that the proposed algorithm has higher classification accuracy and lower scan-matching error compared with other dynamic object removal methods. The proposed algorithm was able to generate a point cloud map, despite the presence of many dynamic objects, that was similar to a map generated in the absence of dynamic objects in the same environment.

3D mapping of outdoor environments by scan matching and motion averaging

Building 3D maps of surroundings is a fundamental and important task for autonomous robots operating in large previously unknown semi-structured or unstructured outdoor environments, where external methods for localization like GPS are not always available. Most of existing 3D mapping approaches are based on the local scan matching algorithm, which are difficult to achieve accurate mapping with good efficiency. In this paper, we propose an effective graph-based SLAM approach for 3D mapping of outdoor environments without odometry measurement. It first utilizes scan matching algorithm to sequentially estimate initial robot poses and progressively construct a pose graph, where each vertex denotes the robot pose at a point in time and each edge indicates the constraint between two robot poses. As scan matching algorithm cannot be error-free, the estimation of robot poses suffers from the error accumulation problem. To address this issue, the hypothesis of loop closure can be generated by an adaptive search circle and then verified by the global scan matching algorithm. Once the loop closure is verified, more edges should be added into the pose graph, which is subsequently optimized by the weighted motion averaging algorithm so as to obtain accurate robot poses for 3D mapping. Experimental results carried out on multiple outdoor environment data sets show that the proposed approach can build 3D maps with good performance.