Real-Time 3D Mapping in Complex Environments Using a Spinning Actuated LiDAR System

Abstract

LiDAR is a crucial sensor for 3D environment perception. However, limited by the field of view of the LiDAR, it is sometimes difficult to achieve complete coverage of the environment with a single LiDAR. In this paper, we designed a spinning actuated LiDAR mapping system that is compatible with both UAV and backpack platforms and propose a tightly coupled laser–inertial SLAM algorithm for it. In our algorithm, edge and plane features in the point cloud are first extracted. Then, for the significant changes in the distribution of point cloud features between two adjacent scans caused by the continuous rotation of the LiDAR, we employed an adaptive scan accumulation method to improve the stability and accuracy of point cloud registration. After feature matching, the LiDAR feature factors and IMU pre-integration factor are added to the factor graph and jointly optimized to output the trajectory. In addition, an improved loop closure detection algorithm based on the Cartographer algorithm is used to reduce the drift. We conducted exhaustive experiments to evaluate the performance of the proposed algorithm in complex indoor and outdoor scenarios. The results showed that our algorithm is more accurate than the state-of-the-art algorithms LIO-SAM and FAST-LIO2 for the spinning actuated LiDAR system, and it can achieve real-time performance.

1. Introduction

Light Detection and Ranging (LiDAR) can achieve highly accurate and efficient distance measurement. I is insensitive to illumination changes because it measures by actively emitting a pulsed laser. By combining LiDAR with Simultaneous Localization and Mapping (SLAM) technology, the laser SLAM system can obtain a three-dimensional map of the surrounding environment both indoors and outdoors. These advantages make the laser SLAM system play an important role in many fields, such as autonomous driving [1], building inspection [2], forestry investigation [3], etc. Currently, the LiDAR used for laser SLAM can be basically divided into two categories. Mechanical LiDAR is the most commonly used LiDAR type. Its horizontal Field Of View (FOV) can reach close to 360°, but its vertical FOV is very limited. Besides, with the development of microelectronics technology, solid-state LiDAR, e.g., the DJI Livox series, has become more and more commonly used [4,5]. Generally, solid-state LiDAR can provide a larger vertical FOV than mechanical LiDAR, but its horizontal FOV is much smaller. Therefore, in many scenarios, both types of LiDAR cannot completely cover the whole environment. This shortcoming greatly limits the mapping efficiency of laser SLAM systems, especially when using a platform with limited endurance such as an Unmanned Aerial Vehicle (UAV) to carry the mapping system [6]. In narrow environments, this limited FOV will degrade the accuracy and stability of localization as only a small number of objects can be scanned.

Much research has been conducted to expand the FOV of LiDAR, and the solutions can be roughly divided into two categories. The first category is to combine multiple LiDARs [7,8,9,10,11]. Such systems typically use a horizontally mounted LiDAR to provide primary positioning information and a vertically mounted LiDAR to improve the scanning coverage. Its advantage is that the mechanical structure is simple. However, due to the high price of LiDARs, the cost of these solutions is greatly increased. Another category of solutions is to actuate the LiDAR dynamically, and LOAM [12] is one of the most-famous methods among them. LOAM first accumulates the point cloud with the assistance of the IMU and then matches the accumulated point cloud with the global map to correct the accumulated errors. Considering that the prediction error of the IMU grows exponentially with time and the feature extraction and matching algorithms need to wait for sufficient data to be accumulated, this time interval will become a bottleneck limiting the accuracy of the SLAM system.

To address this problem, we designed a mapping system based on a spinning actuated multi-beam LiDAR. By using a multi-beam LiDAR, more information can be obtained in each scan, which helps to increase the frequency of point cloud matching and generate a denser point cloud map. We first extracted feature points from each frame point cloud through an improved feature extraction method based on LOAM. Then, we judged whether the current point cloud contains enough information by analyzing the spatial distribution of feature points and performed scan-to-map matching once the requirement is met. Compared with accumulating point clouds with a fixed number of scans, this method allows a better balance between the matching frequency of point clouds and the accuracy and reliability of the matching results. Finally, in order to eliminate the accumulated error, we added a loop closure detection module to the algorithm. The main contributions of this paper can be summarized as follows:

• We propose a tightly coupled laser–inertial SLAM algorithm named Spin-LOAM for a spinning actuated LiDAR system.
• An adaptive scan accumulation method that can improve the accuracy and reliability of matching by analyzing the spatial distribution of feature points.
• Extensive experiments were conducted in indoor and outdoor environments to verify the effectiveness of our algorithm.

2. Related Work

As a fundamental problem in robotics, numerous SLAM algorithms based on LiDAR have been proposed. LeGO-LOAM [13] extracts the ground point cloud from the real-time scan results to improve the accuracy in the elevation direction. T-LOAM [14] simultaneously extracts edge features, sphere features, planar features, and ground features to improve the matching accuracy. The works [15,16,17] refined the localization result by introducing plane constraints. Suma++ [18] not only uses geometric features in point clouds, but also introduces semantic information to assist point cloud matching. Besides these LiDAR-only odometry algorithms, many multi-sensor-fusion-based algorithms have been proposed to improve the accuracy and robustness. LIO-SAM [19] combines the IMU pre-integration factor with the LiDAR odometry factor through a factor graph. LIO-Mapping [20] integrates the LiDAR and IMU in a tightly coupled fashion. FAST-LIO [21] adopts a tightly coupled iterated extended Kalman filter on a manifold to fuse the data and is accelerated by introducing an incremental KD-Tree in FAST-LIO2 [22]. CLINS [23] fuses LiDAR and IMU data by representing trajectories as a continuous-time function, and this framework is well compatible with arbitrary-frequency data from other asynchronous sensors.

For the purposes of acquiring complete 3D information of the environment, researchers have proposed many actuated LiDAR systems. Bosse et al. [24] designed a mapping system named Zebedee, which connects the sensors to the platform via springs. By treating the trajectory as a function of time, a surfel-based matching algorithm was adopted to estimate the 6-DOF pose. Kaul et al. [25] proposed a passively actuated rotating LiDAR system for UAV mapping, and they used a continuous-time SLAM algorithm to produce the trajectory. However, it cannot process the data in real-time. Park et al. [26] addressed this issue by introducing map deformation to replace the original global trajectory optimization in continuous-time SLAM. Fang et al. [27] proposed a two-stage matching algorithm to estimate the trajectory of a rotating LiDAR. In the algorithm, the distortion of the current point cloud was first removed using the estimated motion generated by matching it with the local map, and then, the undistorted point cloud was matched with the global map. Unlike the aforementioned works, Zhan et al. [28] used a rotating multi-beam LiDAR for 3D mapping and combined it with stereo cameras for dense 3D reconstruction. The precision of this system is very high, but it needs to remain stationary while collecting data. R-LOAM [29] improves the localization accuracy of rotating LiDAR by leveraging prior knowledge about a reference object. Karimi et al. [30] proposed an actuated LiDAR system using the Lissajous pattern [31]. By using the scan slice instead of the full-sweep point cloud to match with the global map, they achieved low-latency localization for a UAV without an IMU in an indoor environment.

3. System Overview

We first introduce the hardware systems used in the study. As shown in Figure 1, our device mainly consists of a laser scanner, a step motor, and an IMU, in which the scanner is driven to rotate by the motor. The rotation angle is recorded by an encoder. The IMU is rigidly attached to the platform, so we regarded the IMU frame $\left\{I\right\}$ as the body frame $\left\{B\right\}$ for simplicity. The LiDAR frame is denoted as $\left\{L\right\}$, and the fixed LiDAR frame $\left\{FL\right\}$ coincides with the initial LiDAR frame when the motor is reset. The Y-axis of the motor frame $\left\{M\right\}$ is aligned with the spin axis. The two extrinsic parameters ${\mathit{T}}_I^{FL}\in SE\left(3\right)$ and ${\mathit{T}}_M^{FL}\in SE\left(3\right)$ are both calibrated manually, where ${\mathit{T}}_I^{FL}$ represents the transformation from the frame $\left\{FL\right\}$ to the frame $\left\{I\right\}$ and ${\mathit{T}}_M^{FL}$ represents the transformation from the frame $\left\{FL\right\}$ to the frame $\left\{M\right\}$. The timestamp of each sensor is synchronized at the hardware level to ensure accuracy.

Figure 2 provides an overview of our SLAM algorithm. In the front-end, first, the IMU measurements are used to construct the pre-integration factor and produce pose predictions. Next, the raw LiDAR point cloud is transformed to the fixed LiDAR frame and de-skewed using the pose predictions and motor encoder data. Then, edge and plane feature points are extracted from the de-skewed point cloud. In the scan-to-map registration module, the correspondences of these feature points and the global map are established. The spatial distribution of these matching point pairs is examined to decide whether they are to be added to the factor graph or accumulated to the next scan. In the back-end, the IMU pre-integration factor and LiDAR factors are jointly optimized to estimate the system states, as shown in Figure 3. In order to bound the amount of computation, only the latest state is optimized when no loop closure constraints are added. After optimization, the feature points are added to the global map using the optimized state. Loop closure detection is performed periodically in the background to reduce drift.

4. Methodology

The system state $\mathit{x}$ to be estimated at time t is defined as

$${\mathit{x}}_t=\left({\mathit{R}}_t,{\mathit{p}}_t,{\mathit{v}}_t,{\mathit{b}}_t^{\omega },{\mathit{b}}_t^a\right)$$

(1)

where ${\mathit{b}}_t^{\omega }\in {\mathbb{R}}^3$ and ${\mathit{b}}_t^a\in {\mathbb{R}}^3$ are the gyroscope and accelerometer biases, respectively. ${\mathit{R}}_t\in SO\left(3\right)$, ${\mathit{p}}_t\in {\mathbb{R}}^3$, and ${\mathit{v}}_t\in {\mathbb{R}}^3$ are the orientation, position, and velocity of the sensor platform in the global coordinate frame $\left\{G\right\}$, respectively.

4.1. IMU Processing

The platform’s angular velocity ${\omega }_t$ and acceleration ${\mathit{a}}_t$ in the IMU frame can be obtained by the IMU, but the raw measurements of the IMU are corrupted by noise and bias. Commonly, the slow variations in the bias are modeled with Brownian motion; hence, the IMU measurement model is given by

$$\begin{array}{cc}\hfill & {\tilde{\omega }}_t={\omega }_t+{\mathit{b}}_t^{\omega }+{\eta }_t^{\omega }\hfill \\ \hfill & {\tilde{\mathit{a}}}_t={\mathit{a}}_t-{{\mathit{R}}_t}^T\mathit{g}+{\mathit{b}}_t^a+{\eta }_t^a\hfill \\ \hfill & \dot{{\mathit{b}}^{\omega }}={\eta }^{b\omega },\dot{{\mathit{b}}^a}={\eta }^{ba}\hfill \end{array}$$

(2)

where $\mathit{g}$ is the gravity vector, ${\eta }^{\omega }$, ${\eta }^a$, ${\eta }^{b\omega }$, and ${\eta }^{ba}$ are white Gaussian noise, and their standard deviations are ${\sigma }_{{\eta }^a}$, ${\sigma }_{{\eta }^g}$, ${\sigma }_{{\eta }^{ba}}$, and ${\sigma }_{{\eta }^{b\omega }}$, respectively.

4.1.1. Pose Prediction

Based on the posterior state ${\widehat{\mathit{x}}}_{k-1}$ in the previous moment and the IMU measurements between $\left(t_{k-1},t_k\right)$, we can use the Euler integration to calculate the predicted poses. The recursive formula is

$$\begin{array}{cc}\hfill & {\mathit{R}}_{i+1}={\mathit{R}}_{i}Exp\left(\left({\tilde{\omega }}_i-{\mathit{b}}_i^{\omega }-{\eta }_i^{\omega }\right)\Delta t\right)\hfill \\ \hfill & {\mathit{v}}_{i+1}={\mathit{v}}_i+\mathit{g}\Delta t+{\mathit{R}}_i\left({\tilde{\mathit{a}}}_i-{\mathit{b}}_i^a-{\eta }_i^a\right)\Delta t\hfill \\ \hfill & {\mathit{p}}_{i+1}={\mathit{p}}_i+{\mathit{v}}_i\Delta t+\frac{1}{2}\mathit{g}\Delta t^2+\frac{1}{2}{\mathit{R}}_i\left({\tilde{\mathit{a}}}_i-{\mathit{b}}_i^a-{\eta }_i^a\right)\Delta t^2\hfill \end{array}$$

(3)

where $Exp(·)$ is the exponential map of manifold $SO\left(3\right)$.

These poses are sufficient to de-skew the point cloud and provide an initial value for registration. However, to help reject the outliers in Section 4.4, the covariance of these predicted poses also need to be estimated. The recursive formula of error state covariance propagation is

$${\mathit{P}}_{i+1}={\mathit{F}}_i{\mathit{P}}_i{{\mathit{F}}_i}^T+{\mathit{G}}_i\mathit{Q}{{\mathit{G}}_i}^T$$

(4)

$${\mathit{F}}_i=\left[\begin{array}{ccccc}Exp\left(-{\omega }^{\prime }\Delta t\right)& 0& 0& -{\mathit{J}}_r\left({\omega }^{\prime }\Delta t\right)\Delta t& 0\\ 0& \mathit{I}& \mathit{I}\Delta t& 0& 0\\ -{\mathit{R}}_i{\left[{\mathit{a}}^{\prime }\right]}_{\times }\Delta t& 0& \mathit{I}& 0& -{\mathit{R}}_i\Delta t\\ 0& 0& 0& \mathit{I}& 0\\ 0& 0& 0& 0& \mathit{I}\end{array}\right]$$

$${\mathit{G}}_i=\left[\begin{array}{cccc}-{\mathit{J}}_r\left({\omega }^{\prime }\Delta t\right)\Delta t& 0& 0& 0\\ 0& 0& 0& 0\\ 0& -{\mathit{R}}_i\Delta t& 0& 0\\ 0& 0& \mathit{I}\Delta t& 0\\ 0& 0& 0& \mathit{I}\Delta t\end{array}\right]$$

where ${\omega }^{\prime }={\tilde{\omega }}_i-{\mathit{b}}_i^{\omega }$, ${\mathit{a}}^{\prime }={\tilde{\mathit{a}}}_i-{\mathit{b}}_i^a$, and ${\mathit{P}}_i$ is the covariance matrix of system state at time $t_i$. The diagonal elements of the noise covariance matrix $\mathit{Q}$ are ${{\sigma }_{{\eta }^{\omega }}}^2$, ${{\sigma }_{{\eta }^a}}^2$, ${{\sigma }_{{\eta }^{ba}}}^2$, and ${{\sigma }_{{\eta }^{b\omega }}}^2$. ${\mathit{J}}_r$ is the right Jacobian of $SO\left(3\right)$. Note that the covariance of $\mathit{R}$ is defined in the tangent space.

4.1.2. IMU Pre-Integration

The IMU pre-integration technique was first proposed in [32], and it has been widely applied in SLAM research. It uses IMU measurements between $(t_{k-1},t_k)$ to establish the constraint between two states $x_{k-1}$ and $x_k$. The IMU pre-integration factor is calculated as follows:

$$\begin{array}{cc}\hfill & \Delta \mathit{R}={{\mathit{R}}_{k-1}}^T{\mathit{R}}_k=\prod _{t_i\in (t_{k-1},t_k)}^{}Exp\left(\left({\tilde{\omega }}_i-{{\mathit{b}}^{\omega }}_i-{{\eta }^{\omega }}_i\right)\Delta t\right)\hfill \\ \hfill & \Delta \mathit{v}={\mathit{R}}_{k-1}^T\left({\mathit{v}}_k-{\mathit{v}}_{k-1}-\mathit{g}\Delta t\right)\hfill \\ \hfill & =\sum _{t_i\in (t_{k-1},t_k)}^{}\Delta {\mathit{R}}_{k-1,i}\left({\tilde{\mathit{a}}}_i-{\mathit{b}}_i^a-{\eta }_i^a\right)\Delta t\hfill \\ \hfill & \Delta \mathit{p}={{\mathit{R}}_{k-1}}^T\left({\mathit{p}}_k-{\mathit{p}}_{k-1}-{\mathit{v}}_{k-1}\Delta t-\frac{1}{2}\mathit{g}{\Delta t}^2\right)\hfill \\ \hfill & =\sum _{t_i\in (t_{k-1},t_k)}^{}\left[\Delta {\mathit{v}}_{k-1,i}\Delta t+\frac{1}{2}\Delta {\mathit{R}}_{k-1,i}\left({\tilde{\mathit{a}}}_i-{\mathit{b}}_i^a-{\eta }_i^a\right)\Delta t^2\right]\hfill \end{array}$$

(5)

where $\Delta R$, $\Delta v$, and $\Delta p$ are the relative motion between two timestamps $t_{k-1},t_k$. More details about the on-manifold IMU pre-integration can be found in [33].

4.2. Feature Extraction

The raw point cloud is measured in the LiDAR frame and distorted by the sensor’s motion. Therefore, before feature extraction, it needs to be transformed to frame $\left\{FL\right\}$ and de-skewed. Suppose ${{\mathit{c}}_i}^L$ is a point in raw point cloud scan $\mathcal{C}=\left\{{\mathit{c}}_0,{\mathit{c}}_1,\dots ,{\mathit{c}}_n\right\}$:

$${{\mathit{c}}_i}^{FL}={{\mathit{T}}_I^{FL}}^{-1}{\mathit{T}}_{B_k}^{B_i}{\mathit{T}}_I^{FL}{{\mathit{T}}_M^{FL}}^{-1}{\mathit{R}}_i^M{\mathit{T}}_M^{FL}{{\mathit{c}}_i}^L$$

(6)

where ${\mathit{R}}_i^M$ is the rotation matrix generated by encoder data at time $t_i$, which represents the rotation of the LiDAR frame relative to the fixed LiDAR frame in the motor frame. ${\mathit{T}}_{B_k}^{B_i}={{\mathit{T}}^{B_k}}^{-1}{\mathit{T}}^{B_i}$, ${\mathit{T}}^{B_i}$, and ${\mathit{T}}^{B_k}$ are the poses of the body frame obtained by the linear interpolation of the predicted pose, and $t_k$ is the timestamp of the latest point in the point cloud scan $\mathcal{C}$.

Our feature extraction method extracts planar features ${\mathcal{F}}_p$ and edge features ${\mathcal{F}}_e$ from the input point cloud as shown in Figure 4. The workflow of the method is as follows:

(1)

For a point ${\mathit{c}}_i\in \mathcal{C}$, find its previous neighbors ${\mathcal{N}}_i^{pre}=\left\{{\mathit{c}}_{i-k},\dots ,{\mathit{c}}_{i-1}\right\}$ and succeeding neighbors ${\mathcal{N}}_i^{succ}=\left\{{\mathit{c}}_{i+1},\dots ,{\mathit{c}}_{i+k}\right\}$ in the same scan line.

(2)

Calculate the features $\alpha$, $\beta$ of the point using

$$\alpha =arccos\left(\frac{{\mathit{c}}_i-{\mathit{c}}_{i-1}}{{\mathit{c}}_{i+1}-{\mathit{c}}_i}\right)$$

(7)

$$\beta =\frac{max\left(∥{\mathit{c}}_{i+1}-{\mathit{c}}_i∥,∥{\mathit{c}}_i-{\mathit{c}}_{i-1}∥\right)}{min\left(∥{\mathit{c}}_{i+1}-{\mathit{c}}_i∥,∥{\mathit{c}}_i-{\mathit{c}}_{i-1}∥\right)}$$

(8)

where $\alpha$ indicates the changing angle of the scan line at the point ${\mathit{c}}_i$, which is used to characterize the smoothness. The points with $\alpha <{\alpha }^{thr}$ will be labeled as smooth points. $\beta$ is the ratio of the distance from point ${\mathit{c}}_i$ to ${\mathit{c}}_{i-1}$ and ${\mathit{c}}_{i+1}$, which is used to determine whether the point is an edge point.

(3)

For point ${\mathit{c}}_i$ with $\beta >{\beta }^{thr}$, if all points in its closer neighbors (depending on the closest point belonging to the neighbor ${\mathcal{N}}_i^{pre}$ or ${\mathcal{N}}_i^{succ}$) are smooth points, then add ${\mathit{c}}_i$ to ${\mathcal{F}}_e$.

(4)

For point ${\mathit{c}}_i$ with $\beta \le {\beta }^{thr}$, if all points in its previous and succeeding neighbors are smooth points, then add ${\mathit{c}}_i$ to the candidate set of edge points.

(5)

Use the standard LOAM-based method to extract planar features ${\mathcal{F}}_p$ and edge points ${\mathcal{F}}_e$, except that the edge points must belong to the candidate set.

This modification was based on the consideration that there are many unstructured objects (e.g., vegetation) in outdoor environments, which will degrade the performance of edge extraction. A comparison result is shown in Figure 5. In our experiment, ${\alpha }^{thr}$ was set to 15° and k and ${\beta }^{thr}$ were set to 2 and 4, respectively, according to [34].

4.3. Scan-to-Map Registration

Similar to [19], for an input point cloud scan C, we used the pose predicted by the IMU to find the adjacent point cloud scans in the global map and merged them into the feature map ${\mathcal{M}}_p$ and ${\mathcal{M}}_e$ for data association. The map is downsampled by a voxel filter to accelerate the computation. Then, we find the k nearest neighboring points $N_f$ of each feature point $f_i\in {\mathcal{F}}_p$ or ${\mathcal{F}}_e$ in the corresponding feature map and denote its nearest point as $m_i$. For $f_i\in {\mathcal{F}}_p$; the plane normal vector ${\mathit{n}}_p$ of its ${\mathcal{N}}_f$ is computed, and for $f_i\in {\mathcal{F}}_e$, the line direction ${\mathit{n}}_e$ of its $N_f$ is computed. In addition, as the LiDAR is continuously rotating, to ensure the reliability of the matching in the initial stage, the initial global map is constructed by keeping the sensor platform stationary for about 3 s.

According to the results of feature matching, the LiDAR residual $r_{LiDAR}$ can be computed using the point-to-plane $d_p$ and point-to-line distance $d_e$.

$$d_p={{\mathit{n}}_p}^T\left(\mathit{R}{\mathit{f}}_i+\mathit{p}-{\mathit{m}}_i\right)$$

(9)

$$d_e=∥{\mathit{n}}_e\times \left(\mathit{R}{\mathit{f}}_i+\mathit{p}-{\mathit{m}}_i\right)∥$$

(10)

where $\mathit{R}$ and $\mathit{p}$ are the predicted orientation and position, ${\mathit{n}}_p$ and ${\mathit{n}}_e$ are normalized, and ${\mathit{m}}_i$ is the corresponding point of feature point ${\mathit{f}}_i$ in the global map.

4.4. Adaptive Scan Accumulation

The spatial distribution of the point cloud has an important influence on the accuracy of the registration result. Taking point-to-plane registration as an example, the matched points should involve at least three non-parallel planes. Unlike the point cloud obtained by the fixed mounted LiDAR, the LiDAR in our device is continuously rotating, resulting in the continuous change of the spatial distribution of the obtained point cloud. As shown in Figure 6, using only a single frame of the point cloud sometimes fails to provide reliable registration results. A simple solution is to accumulate multi-scan point clouds before each registration, but this will reduce the computational efficiency, so we propose a method to adaptively decide whether to perform point cloud accumulation and how many scans need to be accumulated according to the spatial distribution of the point cloud.

4.4.1. Features’ Distribution Inspection

To ensure the registration results are reliable, we used the covariance of the registration result as the indicator. In the least-squares registration problem, the normal equation formed by matched feature points is

$${\mathit{J}}^T\mathit{J}\mathit{x}={\mathit{J}}^T\mathit{b}$$

(11)

where $\mathit{J}\in {\mathbb{R}}^{n\times 6}$ is constructed by stacking Jacobian matrices ${\mathit{J}}_p^{d_p}$ and ${\mathit{J}}_p^{d_e}$, n is the number of matched points, and $b\in {\mathbb{R}}^{n\times 1}$ is constructed by stacking distance residuals $d_p$ and $d_e$. The solution to Equation (11) is given by

$$\widehat{\mathit{x}}={\left({\mathit{J}}^T\mathit{J}\right)}^{-1}{\mathit{J}}^T\mathit{b}$$

(12)

The covariance of the estimated pose is

$$E\left\{{\widehat{\mathit{x}}}^T\widehat{\mathit{x}}\right\}={\left({\mathit{J}}^T\mathit{J}\right)}^{-1}{\mathit{J}}^{T}E\left\{{\mathit{b}}^T\mathit{b}\right\}\mathit{J}{\left({\mathit{J}}^T\mathit{J}\right)}^{-1}$$

(13)

where $E\left\{·\right\}$ is the expected value operator. The covariance of $\mathit{b}$ is determined by the laser measurements, and each laser observation can be treated as an independent observation. Therefore, its covariance is:

$$E\left\{{\mathit{b}}^T\mathit{b}\right\}={\sigma }_l^2\mathit{I}$$

(14)

where ${\sigma }_l$ is the accuracy of the laser measurement, which can be set depending on the model of the LiDAR. Substituting Equation (14) into Equation (13), then

$$E\left\{{\widehat{\mathit{x}}}^T\widehat{\mathit{x}}\right\}={\left({\mathit{J}}^T\mathit{J}\right)}^{-1}{\sigma }_l^2=\mathit{H}{\sigma }_l^2$$

(15)

Equation (15) reveals how the spatial distribution of matched points in matrix $\mathit{J}$ affects the covariance of the registration result. If the spatial distribution is suitable for registration, the covariance of the estimated pose should not vary greatly in all directions. Suppose ${\mathit{H}}_{tran}\in {\mathbb{R}}^{3\times 3}$ is the submatrix of $\mathit{H}$ corresponding to the translation part in the pose; the ratio $\gamma$ can be computed by

$$\gamma =\frac{\sqrt{max\left(diag\left({\mathit{H}}_{tran}\right)\right)}}{\sqrt{min\left(diag\left({\mathit{H}}_{tran}\right)\right)}}$$

(16)

where operator $diag(·)$ extracts the diagonal elements of the matrix as a vector. The smaller the value of $\gamma$, the more uniform the distribution of the point cloud is, and vice versa. Here, we set the threshold ${\gamma }_{th}=3$. If $\gamma <{\gamma }_{th}$, the matched pairs will be accepted, and these plane and edge features will be added to the factor graph as constraints; otherwise, the extracted features will be accumulated to the next scan.

4.4.2. Outlier Removal in Matched Features

Since the point cloud in a single scan is sparse, there will inevitably be errors in the feature extraction results. These incorrectly matched point pairs will interfere with the features’ distribution inspection and reduce the accuracy of pose estimation. To remove them while performing distribution inspection, an outlier removal algorithm based on the propagation of covariance is applied. We assumed that the error in the global map can be ignored, which means that the distance residual is mainly caused by the error of the predicted pose and the error of the laser measurement. Then, the standard deviation of the distance residuals can be computed by

$${\sigma }_{d_p}=\sqrt{{\mathit{J}}_p^{d_p}{\mathit{D}}_p{{\mathit{J}}_p^{d_p}}^T+{\mathit{J}}_l^{d_p}{\mathit{D}}_l{{\mathit{J}}_l^{d_p}}^T}$$

(17)

$${\sigma }_{d_e}=\sqrt{{\mathit{J}}_p^{d_e}{\mathit{D}}_p{{\mathit{J}}_p^{d_e}}^T+{\mathit{J}}_l^{d_e}{\mathit{D}}_l{{\mathit{J}}_l^{d_e}}^T}$$

(18)

$${\mathit{J}}_p^{d_p}=\left[\begin{array}{cc}\frac{\partial d_p}{\partial \mathit{R}}& \frac{\partial d_p}{\partial \mathit{p}}\end{array}\right]=\left[\begin{array}{cc}{{\mathit{n}}_p}^T\mathit{R}{⌊-{\mathit{f}}_i⌋}_{\times }& {{\mathit{n}}_p}^T\end{array}\right],$$

$${\mathit{J}}_l^{d_p}={{\mathit{n}}_p}^T\mathit{R}$$

$${\mathit{J}}_p^{d_e}=\left[\begin{array}{cc}\frac{\partial d_e}{\partial \mathit{R}}& \frac{\partial d_e}{\partial \mathit{p}}\end{array}\right]=\left[\begin{array}{cc}\frac{{{\mathit{v}}_{d_e}}^T}{∥{\mathit{v}}_{d_e}∥}{⌊{\mathit{n}}_e⌋}_{\times }\mathit{R}{⌊-{\mathit{f}}_i⌋}_{\times }& \frac{{{\mathit{v}}_{d_e}}^T}{∥{\mathit{v}}_{d_e}∥}{⌊{\mathit{n}}_e⌋}_{\times }\end{array}\right],$$

$${\mathit{J}}_l^{d_e}=\frac{{{\mathit{v}}_{d_e}}^T}{∥{\mathit{v}}_{d_e}∥}{⌊{\mathit{n}}_e⌋}_{\times }\mathit{R},{\mathit{v}}_{d_e}={\mathit{n}}_e\times \left(\mathit{R}{\mathit{f}}_i+\mathit{p}-{\mathit{m}}_i\right)$$

where $D_p$ is the covariance of the predicted pose and can be obtained by Equation (4) and $D_l$ is the accuracy of the LiDAR point.

The point pair with $d_p\ge 3{\sigma }_{d_p}$ or $d_e\ge 3{\sigma }_{d_e}$ is considered an outlier and removed. In theory, if the distance residuals follow a Gaussian distribution, we can remove most of the mismatched point pairs while retaining 99.5% of the correct matching results.

4.5. Loop Closure Detection

Our loop closure detection method was developed based on Cartographer [35]. As shown in Algorithm 1, there are two main improvements compared to the original algorithm: (1) point-to-plane ICP is used to replace the original probability occupancy-grid-based registration method in the fine registration stage; (2) the loop closure detection result is checked using the previous scan. Due to the rotation of the LiDAR, there is a noticeable difference even between two adjacent scans. This can provide auxiliary information to help reject false detection results. If the loop closure detection result of the current scan is correct, then the registration results ${\mathit{T}}_{S_l}^{C_k}$ and ${\mathit{T}}_{S_l}^{C_k-1}$ should be consistent with the odometry transformation ${\mathit{T}}_{C_k}^{C_{k-1}}$. The threshold $d_{th}$ was set to 5 cm in the experiment.

Algorithm 1: Loop closure detection.

5. Experiments

Since there is no publicly available dataset containing spinning actuated LiDAR and IMU data, we evaluated the performance of the Spin-LOAM algorithm using the data collected in indoor and outdoor environments with the device shown in Figure 7. In the experiments, the sampling frequencies of the Velodyne VLP-16 LiDAR and Sensonor STIM300 IMU were 10 Hz and 200 Hz, respectively. The angular velocity of the step motor was set to 4.5 rad/s, and the angular resolution of the encoder was 10 bit. All experiments were conducted on an Intel NUC computer with an Intel Core i5-1135G7 CPU and 16 GB memory.

5.1. Evaluation in Indoor Environments

To verify the accuracy and effectiveness of Spin-LOAM, we first conducted tests in indoor environments. We compared our algorithm with two state-of-the-art LIO algorithms, FAST-LIO2 and LIO-SAM (modified for compatibility with a 6-axis IMU). In the indoor tests, the map voxel size in all algorithms was set to 0.2 m. The standard deviations of the noise related to the IMU and LiDAR were uniformly set according to the device model.

As shown in Figure 8, we collected three datasets in different environments using a backpack to evaluate the performance of our algorithm. Data 1 is two narrow corridors on different floors connected by stairs; Data 2 is an underground garage; Data 3 is a badminton hall. Since it was difficult to obtain the ground truth trajectory in the indoor environment, we returned to the starting position at the end of the trajectory and used the end-to-end translation error as the metric. The qualitative analysis results are given in

Table 1. End-to-end translation error (m).

	Length (m)	FAST-LIO2	LIO-SAM	Spin-LOAM (Odom)	Spin-LOAM (Full)
Data 1	189.0993	2.2631	/	0.6976	0.0090
Data 2	382.3672	0.0210	0.0128	0.0126	0.0114
Data 3	315.1776	0.0166	0.0145	0.0175	0.0164

Bold font represents better results.

, where “Spin-LOAM (odom)” represents our algorithm without loop closure and “Spin-LOAM (full)” represents the full SLAM algorithm.

Data 1 is the most-challenging scene among the three indoor datasets. In this scene, LIO-SAM failed to finish the localization, and FAST-LIO2 also suffered from severe drift. Our algorithm achieved the lowest odometry drift and successfully corrected the drift through loop closure detection, as shown in Figure 9. In Data 2, our algorithm and LIO-SAM achieved similar localization accuracy, and FAST-LIO2 was slightly worse because it has no loop closure detection. However, the accuracy of our pure odometry method was comparable to LIO-SAM, which validates the effectiveness of our algorithm. In Data 3, the accuracy of all algorithms was at the same level, because the scene was free of occlusions and full of plane features. It is worth noting that the height of the roof in this scene was about 15 m, but our device completely acquired the point cloud of the entire scene with only one LiDAR. This reveals the advantage of the spinning actuated LiDAR system.

5.2. Evaluation in Outdoor Environments

In the outdoor test, the map voxel size in Spin-LOAM was changed to 0.4 m, and the voxel size in FAST-LIO2 and LIO-SAM was set according to the default value. Figure 10 gives an overview of the datasets collected in the outdoor environments. Data 5 was collected around a building; Data 6 was collected on a large ring road. Data 7 was collected in a residential area. Data 8 was collected using a drone on a construction site. To quantitatively compare the performance of the algorithms, we used the GNSS RTK trajectories as the ground truth and computed the absolute trajectory error (ATE) of the trajectories.

Table 2. Absolute trajectory error (ATE) of trajectories (m).

	Length (m)	FAST-LIO2	LIO-SAM	Spin-LOAM (wo-asa)	Spin-LOAM (Odom)	Spin-LOAM (Full)
Data 5	415.3925	0.0799	0.0854	0.0760	0.0753	0.0725
Data 6	782.0214	0.8844	(0.4908)	0.2623	0.2323	0.2175
Data 7	1335.2931	0.2271	0.2771	0.2064	0.1871	0.1786
Data 8	623.8667	0.1670	/	0.1431	0.1353	0.1308

Bold font represents better results.

shows that our algorithm achieved the best accuracy in the outdoor environments. FAST-LIO2 also performed very well in the experiments with only significant drift in Data 6 and completed all tests as our algorithm. However, LIO-SAM failed to finish the localization in Data 6 and Data 8. The reason for its failure in Data 6 is that it directly uses the ICP algorithm for loop closure. This strategy is not suitable when large drift occurs, and the accumulated errors in the large ring road make the LIO-SAM algorithm fail. After turning off the loop closure, the accuracy of LIO-SAM in Data 6 was 0.4908 m. Figure 11 gives a detailed comparison of the point cloud at the road junction. It can be seen that, even without the loop closure, our algorithm still maintained high accuracy after walking through a long loop. In Data 8, the drone performed an aggressive motion to test the robustness of the algorithm, in which the maximum angular velocity was over 780°/s and the maximum linear velocity was over 6.5 m/s. This test proved that our tightly coupled algorithm can work when aggressive motion occurs.

An ablation study was conducted to further analyze the contribution of our proposed adaptive scan accumulation method. “Spin-LOAM (wo-asa)” in Table 2 represents our algorithm without adaptive scan accumulation and loop closure. Comparing it with “Spin-LOAM (odom)”, the results showed that our method can improve the accuracy, especially in complex environments such as Data 6 and 7. This is because the FOV of the LiDAR is limited; it is often occluded by trees or can only scan the ground in these environments, which will lead to an uneven distribution of features in a single scan. Our method can alleviate this problem during scan-to-map registration, which can help to improve the accuracy and robustness of registration.

5.3. Runtime Analysis

We selected indoor data and an outdoor data, respectively, for the algorithm performance evaluation. The processing time per scan of each algorithm is shown in Figure 12. FAST-LIO2 was much faster than LIO-SAM and our algorithm because it is a filter-based algorithm and does not require feature extraction. The average time cost per scan of our algorithm was 63.2 ms indoors and 43.8 ms outdoors. The computation time was more for the indoor data because the map voxel size had a significant impact on the performance. The average time cost for the loop closure was 808.4 ms indoors and 328.7 ms outdoors. Since the loop closure was performed by a separate thread in the background, it did not affect the real-time performance of our algorithm.

6. Conclusions

In this paper, we presented a tightly coupled laser–inertial SLAM algorithm specifically designed for a spinning actuated LiDAR system. In the front-end, to mitigate the influence of the unstable spatial distribution of the point cloud caused by the continuously rotating LiDAR, an adaptive scan accumulation method based on point cloud distribution inspection was adopted. In the back-end, a voxel-grid-based loop closure detection method was used to reduce the drift. We use the previous scan point cloud to assist in eliminating errors in the loop closure detection results. The experimental results demonstrated that our algorithm achieves high-precision localization results in various complex indoor and outdoor environments. We are committed to further refining and improving our algorithm, with a focus on improving its robustness in more extreme environmental conditions. One potential avenue for improvement is the integration of semantic information from the point cloud, which will aid in loop closure detection and removal of dynamic objects.