Intensity enhanced for solid-state-LiDAR in simultaneous localization and mapping

1. INTRODUCTION

Autonomous robotics is a rapidly developing field and has a good application prospect in various fields. Simultaneous localization and mapping (SLAM) is a fundamental aspect of robotics. Visual SLAM ^[1] solutions rely heavily on external lighting conditions to capture images and estimate the robot's pose by extracting or matching visual features. However, vision-based SLAM systems often fail in low-light or no-light conditions. Lidar, on the other hand, which uses light detection and ranging (LiDAR) technology, is more robust to environmental changes, including weather and lighting conditions. LiDAR-based SLAM ^[2] can generate accurate maps that can be used directly for robot navigation and obstacle avoidance, making it increasingly popular in the field.

In recent years, solid-state lidar has garnered increasing attention and development. Compared to mechanical lidar, solid-state lidar offers significant advantages in terms of cost, resolution, and performance that cannot be ignored. However, it also faces several challenges, particularly in SLAM applications. Firstly, its scanning method is often irregular, which means that higher demands are placed on data processing. Unlike the regular scanning of mechanical lidar, irregular scanning requires more complex algorithms for point cloud data processing to achieve accurate localization and map construction^[3]. Secondly, the field of view of most solid-state lidars is relatively small, which can lead to feature matching degradation. In situations with a limited field of view, there may be insufficient matching, resulting in reduced performance of the SLAM system. Furthermore, during the continuous movement of the lidar, intra-frame motion can cause distortion in the point cloud data. This distortion can potentially affect the accuracy of the SLAM system and, therefore, requires compensatory measures to address the issue, such as motion distortion correction.

To solve all these problems, in this paper, we propose an inertial SLAM method for solid-state LiDAR that includes intensity information. The method enables accurate, robust, real-time positioning and mapping, and can also operate effectively in challenging degraded environments. In summary, our main contributions are as follows: (a) We propose a point cloud preprocessing framework specifically designed for the irregular scanning of solid-state LiDAR. The framework solves the problems of excessive noise caused by irregular scanning and difficulty in extracting geometric feature points; (b) An intensity normalization method suitable for solid-state LiDAR is proposed to facilitate the subsequent use of intensity information; and (c) An odometer and mapping algorithm that includes intensity information as an aid. With more abundant information, the localization accuracy and robustness in the scenario of scarce geometric feature points are improved.

The rest of this article is organized as follows. In Section 2, we discuss the relevant literature. In Section 3, algorithms for solid-state LiDAR - inertial range measurement and mapping are introduced. Section 3.1 describes the point cloud data preprocessing framework. See Section 3.2 for the feature extraction method. State estimates with intensity information are presented in Section 3.3. All experimental studies are discussed in Section 4. Finally, Section 5 summarizes the thesis.

2. RELATED WORKS

Limited computing resources on mobile platforms result in time-consuming processing of large amounts of raw LiDAR data in SLAM systems. As a solution, several LiDAR-based SLAM frameworks have been proposed that utilize feature extraction. A pioneering work in this field is lidar odometry and mapping (LOAM) ^[4], which extracts geometric edges and surfaces based on the smoothness of raw LiDAR data, and enables real-time odometry and mapping ^[5] through plane-to-plane and edge-to-line matching. In order to speed up the calculation, lightweight and ground-optimized LOAM (LEGO-LOAM) ^[6] extracts the ground features by segmental clustering of geometric plane points, and adopts two-stage optimization for attitude estimation. Using lidar alone can lead to motion distortion and low system output frequency issues. Combining IMU (Inertial Measurement Unit) with lidar can effectively address these problems and significantly improve the system's robustness and accuracy. Unlike the approach of selecting features by sampling uniformity in space, Implicit Moving Least Squares (IMLS)-SLAM ^[7] employs a unique sampling strategy that considers the contribution of points to the observability of the robot's state. This strategy is designed to ensure both time efficiency and accuracy. On the other hand, Pan et al. focuses on extracting various classified geometric features from the scan data and proposes an efficient registration algorithm^[8]. This algorithm can perform pose optimization using point-to-point, plane, and line error metrics. IMU typically acquires data at a much higher rate than lidar. Scholars have proposed various methods to handle high-rate IMU measurements. Zhang^[4] and Shan^[6] employed the IMU to correct the skew in point clouds and initialize motion estimates for LiDAR odometry based on scan matching. The most straightforward approach is to use IMU for state propagation in methods based on Extended Kalman^[9,10]. In the field of graph optimization theory, an effective IMU preintegration technique has been introduced^[11]. This technique, which uses Euler angles to parameterize rotation errors, aims to prevent redundant integration of IMU measurements. Shen et al. derived covariance propagation using continuous-time error state dynamics^[12]. Forster et al. further enhanced the preintegration theory by incorporating posterior IMU bias corrections^[13,14]. Qin et al. represents the pioneering tightly-coupled LiDAR-inertial odometry (LIO) approach that calculates the 6 degrees of freedom (6 DOF) ego-motion through iterated Kalman filtering^[15]. LIO-Smoothing and Mapping (SAM) ^[16] introduces a tightly-coupled LiDAR-Inertial SLAM framework that utilizes redundant information from multiple sensors to achieve more accurate and robust positioning and mapping results. The above methods are specially designed for rotating LiDAR. To address the challenges posed by solid-state lidar, such as limited field of view and irregular scanning patterns, Loam-Livox ^[17] has developed a software package that can be efficiently integrated with the Livox Mid-40 LiDAR. Another approach, FAST-LIO ^[18], uses a tightly coupled iterative extended Kalman filter to combine LiDAR functionality with IMU data to achieve an efficient and accurate LiDAR inertial odometer. The new version of FAST-LIO 2.0 ^[19] eliminates the need for feature extraction and directly matches the original point cloud to the entire map, enabling powerful and efficient LiDAR localization and mapping using the ikd-Tree toolkit. CamVox ^[20] uses a dense point cloud to automatically calibrate the Livox Lidar and camera, taking advantage of the features of the non-repeat scan mode. In order to improve the accuracy and computational efficiency, LiLi-OM ^[21] proposes a keyframe-based LiDAR - Inertial SLAM sliding window optimization method. In solving optimization-based problems, Zhang et al. proposes an online method to mitigate degradation by analyzing the geometry of the problem constraints and explaining the impact of environmental degradation on state estimation^[22]. In a separate study, Zhao et al. analyzed the characteristics of the Livox MID-40 LiDAR, evaluating the intensity distribution of the laser points in the field of view, the accuracy measurement, and the effect of the color of the target surface on the reflection intensity^[23]. This analysis inspired the combination of geometry and intensity information, providing additional constraints for state estimation. Using intensity readings from imaging LiDAR, Shan et al. obtains intensity images that aid in real-time robust positioning^[24]. Finally, Li et al. introduces two multiple weighting functions in the optimization process to deal with geometry and intensity factors, which shows strong robustness in degraded environments^[25].

In recent years, in addition to extracting geometric features, there has been exploration into semantic-based SLAM^[26,27]. These approaches go beyond utilizing just geometric plane and edge features; they also extract semantic information to gain a more comprehensive understanding of the surrounding environment.

3. METHODS

The proposed SLAM framework for solid-state LiDAR is shown in Figure 1. In the figure, red boxes and text mark the main process position of the innovation point in this paper. In order to cope with the challenge of irregular scanning and the need for intensity points in subsequent processing, a feature point extraction algorithm based on intensity and geometric information is proposed. The frame solves the problem of motion distortion by clutter filtering and motion compensation. Firstly, geometric feature points are extracted based on spatial distribution to deal with irregular scanning. In order to ensure the accuracy of extraction, the quadratic plane feature confirmation method is adopted. To solve the problem that the small field of view of solid-state LiDAR leads to the loss of feature points and the degradation of positioning performance, the intensity information is used to extract the intensity feature points. Then, feature point matching is carried out, and a matching algorithm containing intensity information is used to mitigate the wrong plane point matching caused by a limited field of view. Finally, the residual equation is formed by combining the intensity point residual with the original geometric point residual, and the accurate and robust solid-state LiDAR positioning output is obtained.

Figure 1. System pipeline of solid-state-LiDAR-inertial SLAM with intensity information assistance.

3.2. Robust feature extraction

Calculate the curvature after mapping the point cloud to the $$ M \times N $$ rectangular region. The distance difference between the points in the square search area around the current point is calculated to obtain the local smoothness. Taking the current point $$ P_k^{(m, n)} $$ as an example, the calculation formula is as follows:

(5) $$ \begin{equation} c_k^{(m, n)}=\frac{1}{\lambda} . \sum\limits_{P_k^{(i, j)} \in S_k^{(m, n)}}\left(\left\|P_k^{(i, j)}\right\|-\left\|P_k^{(m, n)}\right\|\right) \end{equation} $$

where $$ \mathcal{S}_k^{(m, n)}=\left\{\mathbf{p}_k^{(i, j)} \mid i \in[m-L, m+L], j \in[n-L, n+L]\right\} $$, $$ L $$ is predefined square search area side length, and $$ \lambda $$ is the number of points in that region. $$ c_k^{(m, n)} $$ represents the local smoothness of the point; the larger the value, the steeper the local surface, and the corresponding point is the edge point. Conversely, a lower value indicates that the corresponding point is a plane point.

In order to ensure the accuracy of the selected plane features and edge features, the two-stage geometric distribution verification method is used to determine whether the feature points meet the criteria by relying on the geometric distribution of the point cloud. For the current point, select a certain number of adjacent points, calculate their covariance matrix, and get the eigenvalue. The eigenvalues are arranged in ascending order $$ \lambda_1 $$, $$ \lambda_2 $$, and $$ \lambda_3 $$. If the set of these points satisfies the geometry of the plane, then $$ \lambda_1 / \lambda_2<\alpha $$. If the set of these points satisfies the geometric characteristics of the edge, then $$ \lambda_2 / \lambda_3<\beta $$. In this study, $$ \alpha $$is set to 0.22 and $$ \beta $$is set to 0.2 through experimental testing. Intensity feature extraction needs to use the intensity information of point cloud. Intensity measurements are determined by:

(6) $$ \begin{equation} I=\frac{P_r}{P_e}=\frac{D_r^2 \rho}{4 R^2} \eta_{\text{sys}} \eta_{\text{atm}} \cos \theta=\eta_{\text{all}} \frac{\rho \cos \theta}{R^2} \end{equation} $$

where $$ \eta_{\text {all }} $$ is a constant. Therefore, the surface reflectivity $$ \rho $$ is only related to the incident angle $$ \theta $$ and the measurement distance $$ R $$.

(7) $$ \begin{equation} \rho \propto \frac{I R^2}{\cos \theta} \end{equation} $$

Before extracting the intensity features, it is necessary to normalize the intensity values of the point cloud to obtain a unified intensity feature, and its distance can be directly calculated to correspond with the intensity information. When a point is identified as a plane point by two-stage spatial feature determination, the incidence Angle of the point is calculated according to the normal vector of the point. Select the point $$ P_a $$ farthest from the solid-state laser radar source point from the plane point set, and then find the point $$ P_b $$ farthest from the point $$ P_a $$. According to the following geometric formula, we can determine the point $$ P_c $$.

(8) $$ \begin{equation} \mathbf{p}_c=\underset{\mathbf{p}_i \in \mathbb{P}}{\operatorname{argmax}}\left\|\left(\mathbf{p}_a-\mathbf{p}_b\right) \times\left(\mathbf{p}_a-\mathbf{p}_i\right)\right\|_2 \end{equation} $$

Planes can be represented by plane equations:

(9) $$ \begin{equation} \Pi: n_x \cdot x+n_y \cdot y+n_z \cdot z+\bar{d}_i=0 \end{equation} $$

where $$ n_x $$, $$ n_y $$, $$ n_z $$, and $$ \bar{d}_i $$ are the parameters of the plane, and the calculation formula is as follows:

(10) $$ \begin{equation} \overline{\mathbf{n}}_i=\left[n_x, n_y, n_z\right]^T=\frac{\left(\mathbf{p}_a-\mathbf{p}_b\right) \times\left(\mathbf{p}_a-\mathbf{p}_c\right)}{\left\|\left(\mathbf{p}_a-\mathbf{p}_b\right) \times\left(\mathbf{p}_a-\mathbf{p}_c\right)\right\|_2} \end{equation} $$

(11) $$ \begin{equation} \bar{d}_i=-\left(n_x \cdot p_x+n_y \cdot p_y+n_z \cdot p_z\right) \end{equation} $$

(12) $$ \begin{equation} \overline{\mathbf{p}}=\left[p_x, p_y, p_z\right]^T=\frac{1}{m} \sum\limits_{\mathbf{p}_i \in \mathbb{P}} \mathbf{p}_i \end{equation} $$

Where $$ \overline{\mathbf{n}}_i $$ is the unit normal vector of the plane, and $$ \overline{\mathbf{p}} $$ is the center of the plane. The formula for calculating the incident angle of the point is as follows:

(13) $$ \begin{equation} \cos \theta=\frac{\mathbf{p}^T \cdot \mathbf{n}}{|\mathbf{p}|} \end{equation} $$

In order to avoid redundancy of point selection based on intensity Angle and geometric Angle, only the normalized intensity information of plane points is considered in calculating the incidence Angle. After intensity calibration, the intensity value reflects the reflectivity of the point, indicating its material properties. The point cloud image after intensity processing is shown in Figure 2.

Figure 2. Intensity normalization and feature extraction. The upper left corner is the distribution map before the intensity normalization, the upper right corner is the distribution map after the intensity normalization, the lower left corner is the feature extraction, and the lower right corner is the real scene with a camera.

The previous section explains in detail the intensity preprocessing steps and mapping point cloud frames onto a two-dimensional matrix plane. Based on the surface smoothness of the point, the intensity smoothness is introduced as a measure to describe the local intensity distribution of the surface. Taking the current point $$ P_k^{(m, n)} $$ as an example, the intensity smoothness is calculated by:

(14) $$ \begin{equation} c_{ik}^{(m, n)}=\frac{1}{\lambda} \cdot \sum\limits_{I_k^{(i, j)} \in S_k^{(m, n)}}\left(\left\|I_k^{(i, j)}\right\|-\left\|I_k^{(m, n)}\right\|\right) \mid \end{equation} $$

The diagram in Figure 2 illustrates the process of feature extraction. Blue denotes geometric plane feature points, yellow represents geometric edge feature points, and red indicates intensity edge feature points.

4. RESULTS

In order to validate the accuracy and robustness of the proposed solid-state LiDAR location algorithm, this section conducts an experimental evaluation using both publicly available and customized datasets. The computational cost and evaluation metrics of different algorithms were measured on laptops equipped with AMD 6-core 5800H processors and Nvidia 3050 graphics cards.

To quantitatively evaluate the accuracy of the algorithm, two widely used SLAM evaluation indexes, absolute trajectory error (ATE) and relative attitude error (RPE), are introduced. ATE provides an intuitive representation of overall positioning accuracy, while RPE compares relative attitude increments rather than absolute attitude, providing insight into local accuracy. These two indexes effectively measure the accuracy and robustness of the algorithm.

The selection of parameters is based on a comprehensive understanding of the algorithm and results from experiments across various scenarios. The robustness of the algorithm is verified through systematic experiments in different environmental settings. Additionally, comparative experiments with existing algorithms are conducted to demonstrate the superiority of our method. Same as Zhao ^[28], we test the proposed method in three scenarios.

We selected three scenarios to evaluate our approach. Scenario 1 involved capturing the indoor environment of a warehouse using an automated guided vehicle (AGV) equipped with an Intel-L515 solid-state Lidar. Scenario 2 entailed recording an outdoor environment on a university campus library using an AGV with a Livox-Xsens suite. Scenario 3 depicted a de-escalation scene recorded in an indoor hallway. To showcase the effectiveness of our method, we compared it with several traditional algorithms in LiDAR systems, including Solid-State LiDAR SLAM (SSL-SLAM), LIvox Lidar-inertial Odometry and Mapping (LLI-OM), and Fast LiDAR-inertial Odometry.

In Scene 1, the ground truth is provided by the VICON system. The data set used in the indoor scene is the public data set supporting the SSL-SLAM project. The acquisition device is Intel RealSense l, 515 solid-state Lidar, and SSL-SLAM is the only open source algorithm suitable for l, 515 solid-state LiDAR. Other algorithms are not applicable, so we only use this algorithm to compare with our method. The algorithm is evaluated against ground truth and SSL-SLAM algorithms to assess the localization results. The experimental findings are presented in Figure 3. It is evident that the SSL-SLAM algorithm exhibits a significant deviation from the actual ground truth at 35 s, whereas the algorithm proposed in this study only deviates during the final turn. Towards the end of the experiment, the robot platform moves backward and then forward. For such short trajectory segments, SSL-SLAM fails to provide accurate localization, whereas our proposed algorithm closely aligns with real-world conditions on the ground. The error comparison of experimental results is detailed in Table 1. It can be observed that our proposed algorithm demonstrates superior accuracy and robustness compared to state-of-the-art algorithms in indoor scenarios. The error level is one order of magnitude lower due to the introduction of the intensity information of the algorithm in this paper. Especially for indoor scenes, due to less interference and more one-dimensional data, the accuracy of regular features will be greatly improved, reflecting the advantages of the algorithm in this paper. The ATE and RPE are decreased by 54.5% and 5.3%.

3. Experimental results. The first column is the localization and mapping performance in Scenes 1, 2, 3, respectively, from top to bottom. The second column is the trajectory plot in every Scene. The third column is the comparison of three-axis positions in every Scene.

Scene 2 is an outdoor. Because the data set used in the outdoor scene is Livox solid-state LiDAR, and the SSL-SLAM algorithm is inapplicable, so it is not compared with this algorithm. The Livox-Avia LiDAR is used in sync with the Xsens MTi-G-700 IMU (six-axis, 150 Hz). The ground truth is provided by Real-Time Kinematic (RTK). It can be seen from the result figure that compared with the advanced LiLi-OM and FAST-LIO algorithms, the proposed algorithm has smaller error and higher positioning accuracy and robustness. The error comparison of experimental results is shown in Table 2. It can be seen that in outdoor scenes, compared with LiLi-OM and FAST-LIO algorithms, the proposed algorithm has higher accuracy and robustness. Outdoor scenes are much more complex than indoor ones. In the back-end optimization stage, the geometric and intensity feature residuals are combined to enhance the robustness of the system. The experimental results show the importance of this step for outdoor scenes. The ATE and RPE are decreased by 29.6% and 58.8%.

Scene 3 is a challenging scenario, the actual length is 274 m, and the duration is 417 s. The LiDAR is directed toward the ceiling, where there are hardly any geometric edge points. In this scene, the other two algorithms have data at the beginning. In the operation process, due to the challenging scene, the positioning failed successively, so there is no data. The results show that the LiLi-OM and FAST-LIO algorithms fail at 65 and 240 s, respectively, due to the lack of geometric feature points on the ceiling. However, the algorithm successfully achieved positioning in 417 s, with an acceptable error range. The error comparison of experimental results is shown in Table 3. In challenging environments, the algorithm manages to keep the error within an acceptable range when the other two algorithms fail. Because the intensity information is added to the algorithm, and the residuals of geometric features and intensity features are fused in the residual part, the proposed method can still obtain a good positioning effect in the degenerate environment with fewer features.

Regarding the complexity of the algorithm, we conducted a comprehensive analysis. All algorithms were tested on a laptop equipped with an AMD 6-core 5800H processor and Nvidia 3050 graphics card. We recorded the percentage of CPU usage of each method at execution time and calculated the average usage to evaluate the computational complexity of each method. The conclusions are shown in Table 4. In Scene 1, only the SSL-SLAM method is available, and it is found that it occupies the same proportion of resources as the proposed method. In Scene 2, the LiLi-OM and FAST-LIO methods and the proposed method can successfully complete all the positioning functions, while the resource proportion of the proposed method is slightly higher because the algorithm complexity of the proposed method is slightly higher and there are too many feature points in the scene which need more computing resource. In Scene 3, both LiLi-OM and FAST-LIO methods failed to complete their tasks and failed to stop after only running for a period of time at the beginning, so the resource proportion is very small. The proposed method also takes up a smaller proportion of resources than that in Scene 2 due to fewer feature points.

Despite the introduction of new computational steps, the overall complexity of our algorithms did not increase significantly due to our skillful optimization of the data processing workflow. In a complex environment, the algorithm has a fast convergence speed. This is due to the introduction of an adaptive feature extraction mechanism in the algorithm, which enables it to better adapt to environmental changes and integrate new observational data faster.

5. CONCLUSIONS

A localization and mapping algorithm for solid-state LiDAR is proposed. To address the problem of irregular scanning modes of solid-state lidar, a geometric feature extraction strategy suitable for solid-state lidar is proposed. To solve the problem of feature point loss and degradation caused by a limited field of view in solid-state lidar location, an intensity normalization method is designed, and an intensity-based feature point extraction and matching algorithm is proposed. In the process of position update, the intensity point residual is introduced and combined with the geometric point residual to form a complete residual equation, which realizes the accurate and robust positioning output of solid-state Lidar. Tests using open source datasets and self-collected datasets validate the effectiveness and reliability of the algorithm, which is more accurate and robust than the most advanced solid-state LiDAR localization algorithms. Compared with the corresponding method, in indoor scenes, the ATE and RPE are decreased by 54.5% and 5.3%. In outdoor scenes, the ATE and RPE are decreased by 29.6% and 58.8%. The proposed method can be well adapted to indoor and outdoor scenes, but only for relatively stable scenes. In addition, due to the small field of view of solid-state lidar, although the proposed algorithm can have relatively good positioning results, there will be drags during map construction, which will lead to the accumulation of continuous positioning accuracy errors. These are the areas that should be improved in the future.

DECLARATIONS