Real-Time 3D Mapping in Complex Environments Using a Spinning Actuated LiDAR System
使用旋转驱动的 LiDAR 系统在复杂环境中进行实时 3D 映射

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.
LiDAR 是用于 3D 环境感知的关键传感器。然而，受限于激光雷达的视场，有时很难用单个激光雷达实现对环境的完全覆盖。
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.
本文设计了一种同时兼容无人机和背包平台的旋转驱动激光雷达测绘系统，并提出了一种紧密耦合的激光-惯性SLAM算法。在我们的算法中，首先提取点云中的边和平面特征。
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.
然后，针对LiDAR连续旋转导致的两次相邻扫描之间点云特征分布的显著变化，采用自适应扫描累积方法提高点云配准的稳定性和准确性。
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.
特征匹配后，将LiDAR特征因子和IMU预积分因子添加到因子图中，并共同优化以输出轨迹。此外，还利用一种基于Cartographer算法的改进闭环检测算法来减少漂移。
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.
结果表明，该算法比最先进的旋转驱动激光雷达系统算法LIO-SAM和FAST-LIO2更准确，并且能够实现实时性能。

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.
为了获取完整的环境3D信息，研究人员提出了许多驱动LiDAR系统。Bosse等[24]设计了一个名为Zebedee的测绘系统，该系统通过弹簧将传感器连接到平台。通过将轨迹视为时间的函数，采用基于表面的匹配算法来估计6自由度姿态。Kaul等[25]提出了一种用于无人机测绘的被动驱动旋转LiDAR系统，他们使用连续时间SLAM算法来生成轨迹。但是，它无法实时处理数据。Park等[26]通过引入地图变形来取代连续时间SLAM中原来的全局轨迹优化，解决了这个问题。Fang等[27]提出了一种两阶段匹配算法来估计旋转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.
在该算法中，首先利用与局部贴图匹配产生的估计运动去除当前点云的畸变，然后将未变形的点云与全局贴图进行匹配。与上述工作不同的是，Zhan等[28]使用旋转的多波束LiDAR进行3D测绘，并将其与立体相机相结合进行密集的三维重建。该系统的精度非常高，但在收集数据时需要保持静止。R-LOAM [ 29] 利用参考对象的先验知识，提高了旋转激光雷达的定位精度。Karimi等[30]提出了一种使用Lissajous模式的驱动LiDAR系统[31]。通过使用扫描切片而不是全扫描点云来匹配全球地图，他们实现了室内环境中没有 IMU 的无人机的低延迟定位。

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 𝑻𝐹𝐿𝐼∈𝑆𝐸(3)${\mathit{T}}_I^{FL}\in SE\left(3\right)$ and 𝑻𝐹𝐿𝑀∈𝑆𝐸(3)${\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 1. The mechanical structure of the hardware device.

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.
图 2 概述了我们的 SLAM 算法。在前端，首先，IMU测量用于构建预积分因子并生成姿态预测。
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.
接下来，将原始LiDAR点云转换为固定的LiDAR帧，并使用姿态预测和电机编码器数据进行去偏斜。然后，从去倾斜的点云中提取边和平面特征点。
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.
在后端，对IMU预积分因子和LiDAR因子进行联合优化，以估计系统状态，如图3所示。为了限制计算量，在不添加循环闭合约束的情况下，仅优化最新状态。优化后，要素点将使用优化状态添加到全局地图中。
Loop closure detection is performed periodically in the background to reduce drift.
在后台定期执行闭环检测，以减少漂移。

Figure 2. System overview of the proposed laser–inertial SLAM algorithm for the spinning actuated LiDAR system.

Figure 3. Structure of the factor graph. Only the LiDAR feature factor of the latest scan is directly used in the factor graph, and the others are replaced by the odometry factors, which are relative transformed between two adjacent scans.

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 𝒃𝜔𝑡∈ℝ3${\mathit{b}}_t^{\omega }\in {\mathbb{R}}^3$ and 𝒃𝑎𝑡∈ℝ3${\mathit{b}}_t^a\in {\mathbb{R}}^3$ are the gyroscope and accelerometer biases, respectively. 𝑹𝑡∈𝑆𝑂(3)${\mathit{R}}_t\in SO\left(3\right)$, 𝒑𝑡∈ℝ3${\mathit{p}}_t\in {\mathbb{R}}^3$, and 𝒗𝑡∈ℝ3${\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 𝒙̂𝑘−1${\widehat{\mathit{x}}}_{k-1}$ in the previous moment and the IMU measurements between (𝑡𝑘−1,𝑡𝑘)$\left(t_{k-1},t_k\right)$, we can use the Euler integration to calculate the predicted poses. The recursive formula is

𝑹𝑖+1=𝑹𝑖𝐸𝑥𝑝((𝜔̃𝑖−𝒃𝜔𝑖−𝜂𝜔𝑖)Δ𝑡)𝒗𝑖+1=𝒗𝑖+𝒈Δ𝑡+𝑹𝑖(𝒂̃𝑖−𝒃𝑎𝑖−𝜂𝑎𝑖)Δ𝑡𝒑𝑖+1=𝒑𝑖+𝒗𝑖Δ𝑡+12𝒈Δ𝑡2+12𝑹𝑖(𝒂̃𝑖−𝒃𝑎𝑖−𝜂𝑎𝑖)Δ𝑡2$$\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 𝑆𝑂(3)$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

𝑷𝑖+1=𝑭𝑖𝑷𝑖𝑭𝑖𝑇+𝑮𝑖𝑸𝑮𝑖𝑇$${\mathit{P}}_{i+1}={\mathit{F}}_i{\mathit{P}}_i{{\mathit{F}}_i}^T+{\mathit{G}}_i\mathit{Q}{{\mathit{G}}_i}^T$$

(4)

𝑭𝑖=⎡⎣⎢⎢⎢⎢⎢⎢𝐸𝑥𝑝(−𝜔′Δ𝑡)0−𝑹𝑖[𝒂′]×Δ𝑡000𝑰0000𝑰Δ𝑡𝑰00−𝑱𝑟(𝜔′Δ𝑡)Δ𝑡00𝑰000−𝑹𝑖Δ𝑡0𝑰⎤⎦⎥⎥⎥⎥⎥⎥$${\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]$$

𝑮𝑖=⎡⎣⎢⎢⎢⎢⎢⎢−𝑱𝑟(𝜔′Δ𝑡)Δ𝑡000000−𝑹𝑖Δ𝑡00000𝑰Δ𝑡00000𝑰Δ𝑡⎤⎦⎥⎥⎥⎥⎥⎥$${\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 𝜎𝜂𝜔2${{\sigma }_{{\eta }^{\omega }}}^2$, 𝜎𝜂𝑎2${{\sigma }_{{\eta }^a}}^2$, 𝜎𝜂𝑏𝑎2${{\sigma }_{{\eta }^{ba}}}^2$, and 𝜎𝜂𝑏𝜔2${{\sigma }_{{\eta }^{b\omega }}}^2$. 𝑱𝑟${\mathit{J}}_r$ is the right Jacobian of 𝑆𝑂(3)$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 (𝑡𝑘−1,𝑡𝑘)$(t_{k-1},t_k)$ to establish the constraint between two states 𝑥𝑘−1$x_{k-1}$ and 𝑥𝑘$x_k$. The IMU pre-integration factor is calculated as follows:
IMU预积分技术最早于[32]提出，并在SLAM研究中得到了广泛的应用。它使用 之间的 IMU 测量 (𝑡𝑘−1,𝑡𝑘)$(t_{k-1},t_k)$ 来建立两个状态 𝑥𝑘−1$x_{k-1}$ 和 𝑥𝑘$x_k$ 之间的约束。IMU预积分系数计算如下：

Δ𝑹=𝑹𝑘−1𝑇𝑹𝑘=∏𝑡𝑖∈(𝑡𝑘−1,𝑡𝑘)𝐸𝑥𝑝((𝜔̃𝑖−𝒃𝜔𝑖−𝜂𝜔𝑖)Δ𝑡)Δ𝒗=𝑹𝑇𝑘−1(𝒗𝑘−𝒗𝑘−1−𝒈Δ𝑡)=∑𝑡𝑖∈(𝑡𝑘−1,𝑡𝑘)Δ𝑹𝑘−1,𝑖(𝒂̃𝑖−𝒃𝑎𝑖−𝜂𝑎𝑖)Δ𝑡Δ𝒑=𝑹𝑘−1𝑇(𝒑𝑘−𝒑𝑘−1−𝒗𝑘−1Δ𝑡−12𝒈Δ𝑡2)=∑𝑡𝑖∈(𝑡𝑘−1,𝑡𝑘)[Δ𝒗𝑘−1,𝑖Δ𝑡+12Δ𝑹𝑘−1,𝑖(𝒂̃𝑖−𝒃𝑎𝑖−𝜂𝑎𝑖)Δ𝑡2]$$\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 𝑡𝑘−1,𝑡𝑘$t_{k-1},t_k$. More details about the on-manifold IMU pre-integration can be found in [33].
其中 Δ𝑅$\Delta R$ ， Δ𝑣$\Delta v$ 和 Δ𝑝$\Delta p$ 是两个时间戳之间的相对运动 𝑡𝑘−1,𝑡𝑘$t_{k-1},t_k$ 。有关歧管上 IMU 预集成的更多详细信息，请参见 [ 33]。

4.2. Feature Extraction 4.2. 特征提取

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 𝒞={𝒄0,𝒄1,…,𝒄𝑛}$\mathcal{C}=\left\{{\mathit{c}}_0,{\mathit{c}}_1,\dots ,{\mathit{c}}_n\right\}$:
原始点云在 LiDAR 帧中测量，并因传感器的运动而扭曲。因此，在特征提取之前，需要将其转换为帧 {𝐹𝐿}$\left\{FL\right\}$ 并去歪斜。假设 𝒄𝑖𝐿${{\mathit{c}}_i}^L$ 是原始点云扫描 𝒞={𝒄0,𝒄1,…,𝒄𝑛}$\mathcal{C}=\left\{{\mathit{c}}_0,{\mathit{c}}_1,\dots ,{\mathit{c}}_n\right\}$ 中的一个点：

𝒄𝑖𝐹𝐿=𝑻𝐹𝐿𝐼−1𝑻𝐵𝑖𝐵𝑘𝑻𝐹𝐿𝐼𝑻𝐹𝐿𝑀−1𝑹𝑀𝑖𝑻𝐹𝐿𝑀𝒄𝑖𝐿$${{\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. 𝑻𝐵𝑖𝐵𝑘=𝑻𝐵𝑘−1𝑻𝐵𝑖${\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}$.
式中 𝑹𝑀𝑖${\mathit{R}}_i^M$ ，是编码器数据在时间 𝑡𝑖$t_i$ 生成的旋转矩阵，它表示电机帧中激光雷达帧相对于固定激光雷达帧的旋转。 𝑻𝐵𝑖𝐵𝑘=𝑻𝐵𝑘−1𝑻𝐵𝑖${\mathit{T}}_{B_k}^{B_i}={{\mathit{T}}^{B_k}}^{-1}{\mathit{T}}^{B_i}$ 、 𝑻𝐵𝑖${\mathit{T}}^{B_i}$ 和 𝑻𝐵𝑘${\mathit{T}}^{B_k}$ 是预测姿态的线性插值得到的体架位， 𝑡𝑘$t_k$ 是点云扫描 𝒞$\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:
我们的特征提取方法从输入点云中提取平面特征 ℱ𝑝${\mathcal{F}}_p$ 和边缘特征 ℱ𝑒${\mathcal{F}}_e$ ，如图 4 所示。该方法的工作流程如下：

Figure 4. The illustration of the edge point extraction. As described in Steps 3 and 4, there are two types of edge points. The first type of edge point is at the end of the wall, and the second type of edge point is at the corners.
图4.边缘点提取的图示。如步骤 3 和 4 中所述，有两种类型的边缘点。第一种边缘点位于墙的末端，第二种类型的边缘点位于拐角处。
These edge points can be identified from unstructured points by analyzing the properties of the points in their neighborhoods.
通过分析这些点邻域中点的属性，可以从非结构化点中识别这些边缘点。

(1)

For a point 𝒄𝑖∈𝒞${\mathit{c}}_i\in \mathcal{C}$, find its previous neighbors 𝒩𝑝𝑟𝑒𝑖={𝒄𝑖−𝑘,…,𝒄𝑖−1}${\mathcal{N}}_i^{pre}=\left\{{\mathit{c}}_{i-k},\dots ,{\mathit{c}}_{i-1}\right\}$ and succeeding neighbors 𝒩𝑠𝑢𝑐𝑐𝑖={𝒄𝑖+1,…,𝒄𝑖+𝑘}${\mathcal{N}}_i^{succ}=\left\{{\mathit{c}}_{i+1},\dots ,{\mathit{c}}_{i+k}\right\}$ in the same scan line.
对于某一点 𝒄𝑖∈𝒞${\mathit{c}}_i\in \mathcal{C}$ ，在同一扫描线 𝒩𝑠𝑢𝑐𝑐𝑖={𝒄𝑖+1,…,𝒄𝑖+𝑘}${\mathcal{N}}_i^{succ}=\left\{{\mathit{c}}_{i+1},\dots ,{\mathit{c}}_{i+k}\right\}$ 中查找其先前的 𝒩𝑝𝑟𝑒𝑖={𝒄𝑖−𝑘,…,𝒄𝑖−1}${\mathcal{N}}_i^{pre}=\left\{{\mathit{c}}_{i-k},\dots ,{\mathit{c}}_{i-1}\right\}$ 邻居和后续的邻居。

(2)

Calculate the features 𝛼$\alpha$, 𝛽$\beta$ of the point using
计算点 𝛼$\alpha$ 的特征 ， 𝛽$\beta$ 使用

𝛼=𝑎𝑟𝑐𝑐𝑜𝑠(𝒄𝑖−𝒄𝑖−1𝒄𝑖+1−𝒄𝑖)$$\alpha =arccos\left(\frac{{\mathit{c}}_i-{\mathit{c}}_{i-1}}{{\mathit{c}}_{i+1}-{\mathit{c}}_i}\right)$$

(7)

𝛽=𝑚𝑎𝑥(∥𝒄𝑖+1−𝒄𝑖∥,∥𝒄𝑖−𝒄𝑖−1∥)𝑚𝑖𝑛(∥𝒄𝑖+1−𝒄𝑖∥,∥𝒄𝑖−𝒄𝑖−1∥)$$\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 𝒄𝑖−1${\mathit{c}}_{i-1}$ and 𝒄𝑖+1${\mathit{c}}_{i+1}$, which is used to determine whether the point is an edge point.
其中 𝛼$\alpha$ 表示扫描线在点 𝒄𝑖${\mathit{c}}_i$ 处的变化角度，用于表征平滑度。带有 𝛼<𝛼𝑡ℎ𝑟$\alpha <{\alpha }^{thr}$ 的点将被标记为平滑点。 𝛽$\beta$ 是从点到 𝒄𝑖−1${\mathit{c}}_{i-1}$ 和 𝒄𝑖+1${\mathit{c}}_{i+1}$ 𝒄𝑖${\mathit{c}}_i$ 的距离之比，用于确定该点是否为边缘点。

(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].

Figure 5. Feature point extraction result. The green points are the raw point cloud. The edge points extracted by the standard LOAM-based extraction method are shown in blue, and the edge points extracted by our method are shown in red.
It can be seen that many blue points are located in the vegetation, and these noises will affect the accuracy of the alignment.

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.

Figure 6. Illustration of multi-scan point clouds’ accumulation. The first scan point cloud is rendered in green and only contains a few points in the horizontal plane, which will lead to an inaccurate registration result, especially in the Z direction.
This problem can be solved by merging it with the next scan 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 𝑱∈ℝ𝑛×6$\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 𝑏∈ℝ𝑛×1$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

𝒙̂=(𝑱𝑇𝑱)−1𝑱𝑇𝒃$$\widehat{\mathit{x}}={\left({\mathit{J}}^T\mathit{J}\right)}^{-1}{\mathit{J}}^T\mathit{b}$$

(12)

The covariance of the estimated pose is

𝐸{𝒙̂𝑇𝒙̂}=(𝑱𝑇𝑱)−1𝑱𝑇𝐸{𝒃𝑇𝒃}𝑱(𝑱𝑇𝑱)−1$$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:

𝐸{𝒃𝑇𝒃}=𝜎2𝑙𝑰$$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

𝐸{𝒙̂𝑇𝒙̂}=(𝑱𝑇𝑱)−1𝜎2𝑙=𝑯𝜎2𝑙$$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 𝑯𝑡𝑟𝑎𝑛∈ℝ3×3${\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 𝛾𝑡ℎ=3${\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 𝑑𝑝≥3𝜎𝑑𝑝$d_p\ge 3{\sigma }_{d_p}$ or 𝑑𝑒≥3𝜎𝑑𝑒$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 𝑻𝐶𝑘−1𝑆𝑙${\mathit{T}}_{S_l}^{C_k-1}$ should be consistent with the odometry transformation 𝑻𝐶𝑘−1𝐶𝑘${\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.

Figure 7. Overview of the experimental sensor platform. (a) The sensor platform is carried by a backpack. (b) The sensor platform is carried by a UAV.

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, where “Spin-LOAM (odom)” represents our algorithm without loop closure and “Spin-LOAM (full)” represents the full SLAM algorithm.

Figure 8. Mapping results of our algorithm in indoor environments. The trajectories are represented by the red lines in the figure.

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.

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.

Figure 9. Comparison of the mapping results of the stairs in Data 1.

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.

Figure 10. Mapping results of our algorithm in outdoor environments. The ground truth trajectories generated by GNSS are represented by the black lines.

Table 2 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.

Figure 11. Point cloud maps generated by different algorithms in Data 6.

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.

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.

Figure 12. The evaluation results of the time cost per scan for each algorithm. The selected indoor data and outdoor data are Data 2 and Data 7, respectively, as they have the longest trajectories.

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.