Fast Lidar Inertial Odometry and Mapping for Mobile Robot SE(2) Navigation

Abstract

This paper presents a fast Lidar inertial odometry and mapping (F-LIOM) method for mobile robot navigation on flat terrain with high real-time pose estimation, map building, and place recognition. Existing works on Lidar inertial odometry have mostly parameterized the keyframe pose as SE(3) even when the robots moved on flat ground, which complicated the motion model and was not conducive to real-time non-linear optimization. In this paper, F-LIOM is shown to be cost-effective in terms of model complexity and computation efficiency for robot SE(2) navigation, as the motions in other degrees of freedom in 3D, including roll, pitch, and z, are considered to be noise terms that corrupt the pose estimation. For front-end place recognition, the smoothness information of the feature point cloud is introduced to construct a novel global descriptor that integrates geometry and environmental texture characteristics. Experiments under challenging scenarios, including self-collected datasets and public datasets, were conducted to validate the proposed method. The experimental results demonstrated that F-LIOM could achieve competitive real-time performance in terms of accuracy compared with state-of-the-art counterparts. Our solution has significant superiority and the potential to be deployed in limited-resource mobile robot systems.

1. Introduction

Simultaneous localization and mapping (SLAM) plays a crucial role in diverse navigation missions and is indispensable for autonomous mobile robots [1,2]. It provides reliable state estimation and high-precision maps required for collision-free path planning and real-time obstacle avoidance. Over the past thirty years, numerous excellent visual SLAM techniques, Lidar SLAM techniques, and back-end optimization methods have been proposed to support 6-DOF state estimation for mobile robots. Vision SLAM methods, including monocular-vision-based [3,4], RGB-D-based [5,6], and vision-inertial-based [7,8,9,10] SLAM frameworks, usually employ a camera to obtain a semantic interpretation of the scene and have advantages in place recognition; at the same time, they cannot work in poor lighting situations. Lidar SLAM [11,12,13,14,15] is invariant to illumination changes, which is essential for obstacle detection and tracking to achieve autonomous navigation. Therefore, this paper mainly focuses on mobile robot pose estimation and mapping based on 3D Lidar.

In 3D Lidar-based SLAM, one of the goals is to estimate Lidar odometry (LO), or Lidar inertial odometry (LIO) if the inertial measurement unit (IMU) is employed. LIO is usually represented by SE(3) [16], involving translation terms for the x, y, and z axes and the rotation of the three axes (roll, pitch, and yaw). A robot’s changes in the z direction and pitch and roll dimensions are insignificant for navigation on the ground, especially in indoor or outdoor flat terrain. In these cases, the traditional SLAM system can be almost equivalently converted to estimate only the x, y, and yaw angle, and mobile robots move in SE(2). Although these perturbations are trivial, they can still represent the state change of the robot and may corrupt the measurements, and it is not wise to ignore them directly during the state estimation process. For mobile robots, the state space in terms of extended and minimal representations is SE(2) and can be conveniently optimized by incremental smoothing and mapping with a Bayes tree (iSAM2) [17]. These perturbations are considered as a noise term and integrated into the model to construct a new constraint in the factor graph. In addition, loop closure, as one of the indispensable components of SLAM, is used to correct the accumulated error. The smoothness information calculated in the feature extraction process can not only be used to distinguish edge or plane features [18] but can also be used for the front-end place recognition algorithm to detect loop closures. More importantly, it integrates the geometry and environmental texture information of the point cloud over a local region and requires less memory.

This study proposes a fast Lidar inertial odometry and mapping (F-LIOM) method for real-time local accuracy and the global drift-free state estimation of mobile robots in flat terrain. F-LIOM contains all the essential elements required for a SLAM system, including point-cloud de-skewing, pose estimation, and loop closure. We first redefine the data structure of the point cloud in the feature extraction stage for later place recognition and jointly apply the Gaussian process (GP) and IMU measurements to correct the point-cloud distortion in a non-linear form, thereby replacing the traditional assumption of uniform motion. Next, this paper assumes that the robots perform SE(2) motion and proposes a novel constraint model integrating the perturbations as noises to optimize the state estimation by associating the extended pose and the 3D points via the Lidar measurements. Last, for the front-end place recognition algorithm, we construct a new global descriptor, the smoothness scanning context, to detect loop closure and construct a globally consistent trajectory and map. The main contributions of our work can be summarized as follows:

• An efficient, fast F-LIOM method is developed that integrates the perturbations as weights in noise terms. It enables the system to be computationally tractable.
• We propose a novel global descriptor consisting of only the smoothness of the feature point cloud to aid the Lidar inertia odometry in detecting loop closures.
• The results show that F-LIOM achieved competitive performance on self-collected real-world and public datasets, making it more suitable for deployment in limited-resource service robots.

The rest of the paper is organized as follows: Section 2 provides a discussion of the relevant works. Section 3 presents a systematic overview of the proposed approach. The open dataset and self-collected dataset are described in Section 4. Lastly, the conclusions are presented in Section 5.

2. Related Work

The iterative closest point (ICP) [19,20] methods were introduced to achieve rigid transformation between two Lidar frames in back-end optimization by finding their correspondences at a point-wise or feature-wise level. However, due to the sensor motion and the rotation mechanism of 3D Lidar, the first step of Lidar SLAM is to eliminate the point-cloud distortion. Lidar odometry and mapping (LOAM) [18], as one of the representative methods of 3D Lidar SLAM, was proposed by Zhang Ji in 2014 for low-drift and real-time state estimation and mapping. The linear interpolation method used IMU measurements to correct point-cloud distortion. LOAM extracted features (edge/planar) by calculating the smoothness of a point in its local Lidar scan and subjected edge and planar features to scan-matching to find correspondences between scans. In order to achieve real-time requirements, the two threads were designed to return low-precision odometry with a high frequency and high-precision mapping with a low frequency, respectively. A lightweight and ground-optimized Lidar odometry and mapping (LeGO-LOAM) method was proposed in [12] for real-time six-degree-of-freedom mobile robot state estimation. On the one hand, LeGO-LOAM adapted the primary pipeline of [18] and designed a more advanced feature segmentation method at the front end and two steps for state optimization at the back end. On the other hand, the loop closure module was integrated into LeGO-LOAM to make it more robust in complex environments. A general approach that aimed to achieve an accurate framework was provided in [13] for Lidar SLAM, where the local smoothness was taken into account for iterative pose optimization. Compared with the loosely coupled scheme, the Lidar inertial odometry via smoothing and mapping (LIO-SAM) adapted tightly coupled system framework was proposed in [14], which applied a factor graph to optimize the robot state using incremental smoothing and mapping. A versatile Lidar SLAM system based on the efficient multi-metric linear least-squares ICP was presented in [15]. LINS was proposed in [21] as a lightweight Lidar inertial state estimator for the real-time navigation of ground vehicles. It was also based on a tightly coupled framework and introduced an error-state Kalman filter to compensate for the estimated motion state recursivity. IMU measurements have been used to characterize system motion at any time to correct point-cloud distortion [22,23,24]. Traditional Lidar SLAM methods are corrupted by noise, and the estimated trajectories have drift issues in the z axis. Ground parameters have been extracted to build ground factors in order to eliminate the drift in the altitude direction [25].

The concept of continuous time was proposed in [26] to present mobile robot trajectory estimation and state prediction, a non-parametric robotic state estimation method based on the GP. Chi Hay [27] derived a mathematical model of the GP based on the weight-space and function-space methods and proposed an efficient approach that could provide robot state predictions at any time of interest. In addition, the GP [28] can estimate the probability distribution (mean and variance) of the multi-dimensional navigation parameters of ground vehicles and has been widely involved in the domains of trajectory optimization [29], target tracking [30], fusion [31], and terrain modeling [32].

Although the lack of texture information in the point cloud makes it difficult to construct global descriptors, Lidar scanning is more robust due to its illumination- and view-point-invariant properties. For place recognition, the global descriptors were represented in the form of histograms in [33,34]. The scan context (SC) was proposed in [35] as a non-histogram-based global descriptor from a 3D Lidar scan that encoded a point cloud of a 3D scan into a matrix by selecting the maximum height of the point in each bin. The similarity score was calculated by matching the candidate SC and the current SC, and the frame corresponding to the SC with the highest score was considered a loop closure. In [36], the intensity information over the local Lidar scan was introduced to build the intensity scan context (ISC). SC-LiDAR-SLAM was proposed in [37] as a complete SLAM system that integrated front-end place recognition with Scan Context++ to eliminate accumulated errors. These methods verify that both geometric and intensity information can construct descriptors to achieve loop detection effectively. A semantic-based place recognition algorithm was proposed in [37], named Semantic Scan Context (SSC), which consisted of a two-step global iterative closest point and semantic-based descriptor.

3. Methodology

The system structure of the proposed F-LIOM is shown in Figure 1. The sensor data required by the system are mainly obtained from an IMU and a 3D Lidar. We first redefine the data structure of the point cloud in the feature extraction stage for subsequent loop closure. GP and IMU measurements are combined jointly to correct point-cloud distortion using the non-linear method. An efficient, fast constraint model is presented to estimate the robot state for scan matching, and we introduce the concept of the smoothness scan context to detect loop closure. For back-end optimization, a factor graph using incremental smoothing and mapping with a Bayes tree (iSAM2) [17] is used to model the state estimation problems, as it is more appropriate to conduct inference.

3.1. Point-Cloud De-Skewing

Due to the mechanical rotation mechanism of the 3D Lidar, the movement of the sensor can occur while scanning, which directly impacts the point in a frame scan, making it differ from the true position. At present, the scanning frequency of Lidar is generally 10 Hz, and existing works [12,18] have mostly assumed that the robot moves at a constant angular velocity and linear velocity within one frame scanning period, which does not always hold.

In this paper, we jointly combine IMU preintegration and the GP to de-skew the point cloud by taking the measurements from IMU propagation as the training points in the GP. IMU preintegration can avoid repeated integrations once the linearization point at time $t_i$ changes, and it is more convenient to present the relative motion increments between two time steps. The formulation of IMU preintegration can be simplified as follows:

$$\begin{array}{c}\Delta {\mathbf{R}}_{ij}={\mathbf{R}}_i^T{\mathbf{R}}_j\hfill \\ \Delta {\mathbf{v}}_{ij}={\mathbf{R}}_i^T\left({\mathbf{v}}_j-{\mathbf{v}}_i-{\mathbf{v}}_i\Delta t_{ij}\right)\hfill \\ \Delta {\mathbf{p}}_{ij}={\mathbf{R}}_i^T\left({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}\right)\hfill \end{array}$$

(1)

where ${\mathbf{v}}_{ij}$, ${\mathbf{p}}_{ij}$, and ${\mathbf{R}}_{ij}$ are used to represent the velocity, position, and acceleration of mobile robots at time i; $\mathbf{g}$ is the constant gravity vector in world coordinates; and $\Delta {\mathbf{v}}_{ij}$, $\Delta {\mathbf{p}}_{ij}$, and $\Delta {\mathbf{R}}_{ij}$ denote the preintegrated measurements between times i and j. Detailed information on IMU preintegration can be found in [23], and we do not repeat it here. On the one hand, thanks to the development of SLAM back-end optimization, IMU preintegration factors and Lidar odometry factors can be directly added to the factor graph as different constraint types for joint optimization. On the other hand, we use IMU preintegration as the training set in the GP to correct the distortion Lidar scan, given the point-wise timestamp of each Lidar frame.

The GP can be regarded as a generalization of a Gaussian random variable to the continuous-time domain, described by the mean function $m\left(K\right)$ and covariance function $k\left(k,k^{\prime }\right)$ [28]. The process $f\left(t\right)$ over time dimension t can be defined by the mean function $m\left(t\right)$ and the covariance function $k\left(t,t^{\prime }\right)$ as

$$m\left(t\right)=\mathbb{E}\left[f\right(t\left)\right]$$

(2)

$$k\left(t,t^{\prime }\right)=\mathbb{E}\left[(f\left(t\right)-m\left(t\right))\left(f\left(t^{\prime }\right)-m\left(t^{\prime }\right)\right)\right],$$

(3)

with the GP written as

$$f\left(t\right)\sim \mathcal{G}\mathcal{P}\left(m\left(t\right),k\left(t,t^{\prime }\right)\right)$$

(4)

without a loss of generality, where the index set of the random variables is the time dimension t. The covariance function uses the squared exponential as the kernel function, involving two time variables to account for cross-temporal relations.

Assuming that $t_s$ and $t_e$ are the measurement timestamps of the beginning and end of a frame scan, respectively, the relative transform $\Delta {\mathbf{T}}_t\in \mathrm{SE}\left(3\right)\subset {\mathbb{R}}^{4\times 4},t_i\in \left(t_s,t_e\right]$ between two sequential frames from $k-1$ to k can be accurately estimated using (2). Let ${\mathbf{T}}_k$ be the keyframe pose at time k; the transform within the period $\left(t_s,t_e\right]$ can be calculated as follows:

$${\mathbf{T}}_{k+t_i}={\mathbf{T}}_k·\Delta {\mathbf{T}}_{t_i}.$$

(5)

Since the timestamp of the IMU preintegration within the period $\left(t_s,t_e\right]$ and the corresponding pose information are known, the GP can correct the distortion efficiently by referring to the timestamp information of the point cloud.

The detailed process of point-cloud de-skewing is shown in Algorithm 1. Let $y=\left\{{\mathbf{T}}_{k+t_i}\mid t_i\in \left(t_s,t_e\right],i=1,\cdots ,N_n\right\}$ be the set of IMU preintegration within this period, and let the timestamp $t_{r,i}\in \left(t_s,t_e\right]$ be the ith point on the rth ring of the Lidar frame.

Algorithm 1 The algorithm for de-skewing the point cloud

Input: $X:=t^{\prime }\left(t_1^{\prime },t_{N_n}^{\prime }\right]$ (training set),    $\mathit{y}$ (targets),    ${\mathbf{x}}_{\ast }:=t\in \left(t_s,t_e\right]$ (test inputs).

1: Determining the hyperparameters from training data;

2: for $t_{N_n}\leftarrow t_1$ to $t_i$ do

for $t_{N_n}\leftarrow t_1$ to $t_j$ do

$\mathrm{K}=k\left(t_i,t_j\right);$

end for

end for

3: $L:=cholesky\left(\mathrm{K}\right);$

$\left.\begin{array}{c}\mathit{\alpha }:=L^{\mathrm{T}}\setminus (L\setminus \mathit{y});\hfill \\ {\mathbf{k}}_{\ast }=k\left(t^{\prime },{\mathbf{x}}_{\ast }\right);\hfill \end{array}\right\}$ predictive mean (point cloud)

4: $\overline{f_{\ast }}:={\mathbf{k}}_{\ast }^{\mathrm{T}}\mathit{\alpha };$

$\left.\begin{array}{c}\mathbf{v}:=L\setminus {\mathbf{k}}_{\ast };\hfill \\ \mathbb{V}\left[f_{\ast }\right]:=k\left({\mathbf{x}}_{\ast },{\mathbf{x}}_{\ast }\right)-{\mathbf{v}}^{\mathrm{T}}\mathbf{v};\hfill \end{array}\right\}$ predictive variance

5: Output: ${\overline{f}}_{\ast }$ (mean), $\mathbb{V}\left[f_{\ast }\right]$ (variance).

The mean $\overline{f}$ returned from Algorithm 1 is the undistorted point cloud ${\tilde{\mathcal{P}}}_k.$

$${\tilde{\mathcal{P}}}_k=\left\{p_k^{m,n}={\overline{f}}_{\ast }\mid p_k^{m,n}\in {\mathcal{P}}_k\right\}$$

(6)

More importantly, the variance $\mathbb{V}\left[f_{\ast }\right]$ returned from Algorithm 1 can be applied to evaluate the uncertainty of the predicted pose of each point in the point cloud. Correspondingly, the undistorted edge feature set and planar feature set are represented as ${\tilde{\mathrm{F}}}_k^e$ and ${\tilde{\mathrm{F}}}_k^p$, respectively.

3.2. Redefining the Data Structure of the Point Cloud

Feature point pair matching has been broadly used in Lidar SLAM and has proved very effective. In this paper, the feature extraction process follows the popular method used in [18] to extract edge and planar features by calculating the smoothness $s_k^{i,j}$ of points over a local region. For place recognition, the smoothness can also be used as a new dimension combined with geometry information ${[x,y,z]}^T$ smoothness. Let ${\mathcal{P}}_k=\left\{p_k^{m,n}\right\}$ be the raw measurements at time k, where $p_k^{i,j}={[x,y,z]}^T$ is the jth point on the ith ring of the frame at time k. The raw point-cloud measurement is first converted into a range image, and then the smoothness $s_k^{i,j}$ of points is calculated as follows:

$$s_k^{i,j}=\frac{1}{\left|\mathbb{C}\right|·∥p_k^{i,j}∥}∥\sum _{m\in \mathbb{C},m\ne j}\left(p_k^{i,m}-p_k^{i,j}\right)∥$$

(7)

where $\mathbb{C}$ denotes the set of the nearest points adjacent to the left and right of the point $p_k^{i,j}$ in the same row of the range image, $\mid \mathbb{C}\mid$ is the size of set, and $∥p_k^{i,j}∥$ is the Euclidean distance from point $p_k^{i,j}$ to the scan center. According to the given threshold, the points with a smoothness exceeding the threshold are classified as edge/corner features, and those with a smoothness lower than the threshold are ranked as planar points. For convenience, we denote the edge feature set and the planar feature set as ${\mathrm{F}}_k^e$ and ${\mathrm{F}}_k^p$, respectively.

We redefine the data structure of the features as $p_k^{i,j}={[x,y,z,s]}^T$, including the raw geometry ${[x,y,z]}^T$ and the smoothness s calculated above. The newly added dimension is significant for the front-end place recognition algorithm and will be explained in a later section.

3.3. Pose Estimation

For mobile robot SE(2) navigation, existing works have mostly assumed that the robot’s changes in the z direction and the pitch and roll dimensions are insignificant for navigation on the ground. Most of them only paid attention to the changes in the x, y, and yaw dimensions, and the keyframe pose was set as ${\mathbf{T}}_k^w\in \mathrm{SE}\left(2\right)$, where the vector tangents and Lie algebra can be presented by elements of the type

$$\tau =\left[\begin{array}{c}\rho \hfill \\ \theta \hfill \end{array}\right]\in {\mathbb{R}}^3,{\tau }^{\wedge }=\left[\begin{array}{cc}{\left[\theta \right]}_{\mathrm{x}}& \rho \\ \mathbf{0}& 0\end{array}\right]\in se\left(2\right)$$

(8)

where the operator ${\left[\right]}_x$ is a skew-symmetric matrix, and the superscript ${}^{\wedge }$ indicates that the variable is an element of Lie algebra. Meanwhile, the pose is also extended to ${\mathbf{T}}_k^w$ for specific conditions to maintain the dimension consistently:

$${\mathbf{T}}_k^w=\left[\begin{array}{cc}{\mathbf{R}}_k& {\mathbf{t}}_k\\ \mathbf{0}& 1\end{array}\right]\in \mathrm{SE}\left(3\right)\subset {\mathbb{R}}^{4\times 4},$$

(9)

where rotation and translation are redefined as

$$\begin{array}{cc}\hfill & {\mathbf{R}}_k=\left\{Exp\left(\xi \right)\in \mathrm{SO}\left(3\right)\mid \xi ={\left[00{\theta }_k\right]}^T\right\}\hfill \\ \hfill & {\mathbf{t}}_k={\left[xy0\right]}^T=\left\{\mathrm{V}\left(\xi \right)\epsilon \in {\mathbb{R}}^3\mid \epsilon ={\left[\rho 0\right]}^T\right\}\hfill \end{array}$$

(10)

with

$$\mathrm{V}\left(\xi \right)=\mathbf{I}+\frac{1-cos\xi }{{\xi }^2}{\left[\xi \right]}_{\times }+\frac{\xi -sin\xi }{{\xi }^3}{\left[\xi \right]}_{\times }^2$$

(11)

When a Lidar scan arrives at time k, the current frame is first transformed from the b frame to the w frame by the pose ${\mathbf{T}}_k^w$:

$${\tilde{\mathrm{F}}}_k^w=\mu \left({\tilde{\mathrm{F}}}_k^b\right)={\mathbf{T}}_k^w{\tilde{\mathrm{F}}}_k^b+{\mathit{n}}_l={\mathbf{R}}_k·{\tilde{\mathrm{F}}}_k^b+{\mathbf{t}}_k+{\mathit{n}}_l.$$

(12)

Note that the transformation from homogeneous coordinates to non-homogeneous coordinates is implicit. For convenience, we still apply the equal sign to represent with ${\tilde{\mathrm{F}}}_k^b={\tilde{\mathrm{F}}}_k^e\cup {\tilde{\mathrm{F}}}_k^p$ and ${\tilde{\mathrm{F}}}_k^b$ the set of all features in the b frame and w frame at time k, respectively. The transform process is corrupted by a zero mean additive Gaussian noise ${\mathit{n}}_l$.

The pose estimation strategy is to match the edge features ${\tilde{\mathrm{F}}}_k^e$ and planar features ${\tilde{\mathrm{F}}}_k^p$ from the current frame with the global map $\mathit{m}$, which consists of an edge map and planar map updated increasingly. Once the transformed feature points are associated with the map using the KD-tree method, the distance from point to edge and point to planar can be calculated and then used as the residual for optimization. Thus, the error function is formulated as

$$\mathit{e}=\mathbf{r}\left({\tilde{\mathrm{F}}}_k^w,\mathit{m}\right),$$

(13)

where $\mathbf{r}(·)$ is the cost function, and $\mathit{e}$ is the residual. The current optimal pose ${\mathbf{T}}_k^w$ can be solved by iteratively minimizing the distance until it converges.

Although the environments are generally in 2D, the robot model is sensitive to perturbations from slipping and complex terrain and can then be corrupted, making it challenging to be consistent with the dimensions of the SE(2) model. It is observed that these perturbations are part of the robot’s state, which is more critical for the accuracy of the pose. Therefore, we propose an efficient constraint model to integrate these perturbations. To balance the complexity and accuracy of the pose, the perturbations, including rotation perturbations ${\mathit{n}}_{{\beta }_{xy}}\sim \mathcal{N}\left({\mathbf{0}}_{2\times 1},{\Sigma }_{{\beta }_{xy}}\right)$ and vertical translation perturbation $n_z\sim \mathcal{N}\left(0,{\sigma }_z^2\right)$, are incorporated into the pose model as the noise terms. The rotation matrix and the translation exist in non-Euclidean space and Euclidean space, respectively. The parameterization of the perturbation must be consistent with the space to which it belongs.

$$\begin{array}{cc}\hfill & {\mathbf{R}}_k\leftarrow Exp\left({\mathit{n}}_{\beta }\right){\mathbf{R}}_k,{\mathit{n}}_{\beta }={\left[{\mathit{n}}_{{\beta }_w}0\right]}^T\in {\mathbb{R}}^3\hfill \\ \hfill & {\mathbf{t}}_k\leftarrow {\mathbf{t}}_k+{\mathit{n}}_z,{\mathit{n}}_z={\left[00n_z\right]}^T\in {\mathbb{R}}^3,\hfill \end{array}$$

(14)

since the disturbance in the z direction is a Euclidean translation that can be directly used for robot poses according to the principle of standard vector addition. The perturbations are integrated into the transformation process of feature points via dimension extension. In this case, we rewrite (12) as

$$\begin{array}{cc}\hfill {\tilde{\mathrm{F}}}_k^w& =\mu \left({\tilde{\mathrm{F}}}_k^b\right)=Exp\left({\mathit{n}}_{\beta }\right){\mathbf{R}}_k{\tilde{\mathrm{F}}}_k^b+{\mathbf{t}}_k+{\mathit{n}}_z+{\mathit{n}}_l\hfill \\ \hfill & \approx \mu \left({\tilde{\mathrm{F}}}_k^b\right)+{\mathbf{J}}_{n_{\beta }}^{\mu }{\mathit{n}}_{\beta }+{\mathbf{J}}_n^{\mu }{\mathit{n}}_z+{\mathit{n}}_l.\hfill \end{array}$$

(15)

The perturbations are considered noise terms in the model and can be reached by a general mathematical method (first-order Taylor expansion). Exploiting the left perturbation model, the Jacobian matrix of $\mu$ with respect to ${\mathit{n}}_z$ and ${\mathit{n}}_{\beta }$ can be derived as follows:

$$\begin{array}{cc}\hfill {\mathbf{J}}_{\mathit{n}}& =\left[{\mathbf{J}}_{{\mathit{n}}_{\beta }}^{\mu }{\mathbf{J}}_{n_z}^{\mu }\right]\hfill \\ \hfill & =\left[-{\left(Exp\left({\mathit{n}}_{\beta }\right){\mathbf{R}}_k{\tilde{\mathrm{F}}}_k^b\right)}^{\wedge }{\mathbf{I}}_3\right],\hfill \end{array}$$

(16)

where the meaning of hat operator ${(·)}^{\wedge }$ is redefined to map a vector into a skew-symmetric matrix and no longer represents the element of Lie algebra.

The noise consists of process noise, rotation perturbations, and vertical translation perturbation. We artificially add different noises together to highlight the contribution of the noise and reformulate the zero-mean noise term $\delta {\mathit{n}}_l$. It can be seen that in the specific form of the noise term, the third term of ${\mathit{n}}_{\beta }$ is zero, and the first two terms of ${\mathit{n}}_z$ are zero. Due to the influence of these zero elements, only the part involved with out-SE(2) perturbations in the Jacobian matrix is applied to the noise term. Accordingly, the covariance matrix is approximately represented as

$$\begin{array}{cc}\hfill \sum _{\delta {\mathit{n}}_l}={\mathbf{J}}_{n_{\beta }}^{\mu }& {\varphi }_{\beta }\sum _{{\beta }_{\lambda y}}{\left({\mathbf{J}}_{{\mathit{n}}_{\beta }}^{\mu }{\varphi }_{\beta }\right)}^T\hfill \\ \hfill & +{\sigma }_z^2{\mathbf{J}}_{{\mathit{n}}_z}^{\mu }{\varphi }_z{\left({\mathbf{J}}_{{\mathit{n}}_z}^{\mu }{\varphi }_z\right)}^T+{\sigma }_l^2\mathbf{I},\hfill \end{array}$$

(17)

where the coefficients ${\varphi }_{\beta }$ and ${\varphi }_z$ are appropriate for eliminating useless matrix components and have no other practical meaning.

Now, we return the focus to the previously defined error function (13), which finds the link between features from ${\tilde{\mathcal{P}}}_k$ and map $\mathit{m}$ and calculates the distance sum consisting of point-to-edge and point-to-planar displacement as the new error term. The optimal robot state can be calculated by using the Gauss–Newton method to minimize the distance iteratively:

$$\underset{{\mathbf{T}}_k^{\ast }}{min}\left\{\sum _{{\tilde{F}}_k^e\in {\tilde{\mathcal{P}}}_k}{d}_k^e+\sum _{{\tilde{F}}_k^p\in {\tilde{\mathcal{P}}}_k}{d}_k^p\right\},$$

(18)

where $d_k^e$ and $d_k^p$ are the -to-edge distance and point-to-planar distance, respectively. The general description of the iteration formula is as follows:

$${\mathbf{J}}^T\mathbf{J}·\Delta T=-{\mathbf{J}}^T\mathbf{e},$$

(19)

where $\mathbf{J}$ is the Jacobian matrix of the residual with respect to the optimization variables ${\mathbf{T}}_k^{\ast }$, and $\Delta T$ is the increment. According to the formulation of the error function, the chain rule is used to infer the Jacobian $\mathbf{J}$:

$$\mathbf{J}=\frac{\partial {\mathbf{c}}_L}{\partial {\tilde{\mathrm{F}}}_k^w}\frac{\partial {\tilde{\mathrm{F}}}_k^w}{\partial {\mathbf{T}}_k^w},$$

(20)

where $\frac{\partial {\mathbf{c}}_L}{\partial {\tilde{\mathrm{F}}}_k^w}$ is the Jacobian matrix of the edge and planar residual. As the optimization variable is composed of rotation and translation, the Jacobian matrix can be divided into the following forms:

$$\begin{array}{cc}\hfill \mathbf{J}& =\left[\frac{\partial {\mathbf{c}}_L}{\partial {\mathbf{t}}_k}\frac{\partial {\mathbf{c}}_L}{\partial {\theta }_k}\right]\hfill \\ \hfill & =\left[\frac{\partial {\mathbf{c}}_L}{\partial {\tilde{\mathrm{F}}}_k^w}{\mathbf{I}}_3{\varphi }_{\beta }-\frac{\partial {\mathbf{c}}_L}{\partial {\tilde{\mathrm{F}}}_k^w}{\left(Exp\left({\mathit{n}}_{\beta }\right){\mathbf{R}}_k{\tilde{\mathrm{F}}}_k^b\right)}^{\wedge }{\varphi }_z\right]\hfill \end{array}$$

(21)

Finally, the coefficients ${\varphi }_{\beta }$ and ${\varphi }_z$ are used to hold the dimension consistent, since the robot is assumed to perform SE(2) movement in this paper. The optimal pose ${\mathbf{T}}_k^w$ can be derived by non-linear optimization based on the above constraint. Following the tightly coupled Lidar inertial odometry structure, the estimated Lidar odometry factors can be utilized to correct the bias of the IMU.

3.4. Smoothness Scan Context

Lidar SLAM may degenerate into simple odometry if there is no loop closure mechanism to eliminate the accumulated error. We recall the previously redefined data structure $p_k^{i,j}={[x,y,z,s]}^T$ of the feature, which adds the smoothness s elements based on the raw geometry ${[x,y,z]}^T$. The smoothness can reveal the texture of the environmental surface over a local region to a certain extent because it corresponds to all features ${\tilde{\mathrm{F}}}_k^b$ from the frame. It is believed that smoothness is an important attribute that can be further exploited for place recognition, similar to the intensity-based and geometric-based loop closure detection solutions [35,36]. However, a Lidar scan is composed of tens of thousands of points, making the scan context computationally weak.

Therefore, smoothness s, consisting of only thousands of features, is introduced to construct the smoothness scan context as a global descriptor for place recognition. Some basic properties of the smoothness can be derived from (1), where the value is always non-negative, and the variance corresponding to the edge features is large. The planar features with a lower smoothness are less distinctive in the local frames, while the edge features are the opposite. To improve the place recognition accuracy, we first set the threshold ${\rho }_s$ to filter features with a lower smoothness than ${\rho }_s$, just like certain meaningless planar features. For the remaining available features, the logarithmic function is used to smooth the selected features:

$$s\left(p_k^{i,j}\right)=log\left(s\left(p_k^{i,j}\right)\right),$$

(22)

where the function $s(·)$ provides the smoothness value of a feature point $p_k^{i,j}$.

We rewrite the bin encoding function to construct the smoothness scan context as follows:

$$\varphi \left({\mathcal{P}}_{ij}\right)=\underset{p_k^{ij}\in {\mathcal{P}}_{ij}}{max}\left(s\left(p_k^{i,j}\right)\right),$$

(23)

where $p_k^{ij}$ is the set of points belonging to the bin with the ith ring and jth sector. Using the smoothness elements, the number of point clouds for the scan context is reduced by two orders of magnitude. The proposed algorithm only needs thousands of features to achieve the bin encoding function; in contrast, taking a 16-line Lidar as an example, the number of raw point clouds exceeds 30,000 in a frame scan. In the following scan context process, the search module is implemented by the two-phase search algorithm proposed in [36]. Building on this logic, the candidate loop closure frame ${\mathcal{P}}_c$ can be calculated. Once ${\mathcal{P}}_c$ is determined, the relative motion between the current frame ${\mathcal{P}}_k$ and the candidate frame ${\mathcal{P}}_c$ can be calculated using the ICP function $h(·)$, as follows:

$$\Delta {\mathbf{T}}_{k,c}=h\left({\mathbf{T}}_k,{\mathbf{T}}_c\right),(k\ne c+1)$$

(24)

where ${\mathbf{T}}_k$ and ${\mathbf{T}}_c$ are the poses corresponding to keyframes ${\mathcal{P}}_k$ and ${\mathcal{P}}_c$, respectively. The relative transformation $\Delta {\mathbf{T}}_{k,c}$ between ${\mathcal{P}}_k$ and ${\mathcal{P}}_k$ is the loop closure factor linking these two keyframes.

4. Experimental Evaluation

To qualitatively and quantitatively analyze the proposed method, we conducted a series of experiments to evaluate F-LIOM and compare it with the state-of-the-art FLOAM [13], LIO-SAM [14], and LeGO-LOAM [12] methods. The physical experimental platform of this paper is shown in Figure 2a. A Leishen C-16 Lidar and a MicroStrain 3DM-GX3-25 IMU were installed on the ground mobile robot TIABBOT TOM08Q2. TOM08Q2 is a four-wheel independently driven mobile robot with a maximum speed of 2 m/s and a drive wheel diameter of 152 mm. All the experiments were implemented in C++ and executed on an Intel NUC equipped with a Core i3 CPU and 4Gib memory using the robot operating system (ROS) in the Ubuntu operation system. Different datasets were collected to validate the performance of F-LIOM in terms of rotating, moving, and loop closure.

4.1. Outdoor Environment

This test aimed to evaluate the performance of F-LIOM in outdoor scenes when the system encountered aggressive perturbations in translation or rotation. We first drove the robot at a non-uniform speed in different outdoor scenes to validate the performance of the mapping. The reason for emphasizing non-uniform speed was to prove that GP contributed to eliminating point-cloud distortion under natural working conditions. Note that this paper aimed not to eliminate these perturbations but to reduce their influence on pose estimation effectively. The corresponding maps generated by our method are shown in Figure 2. For convenience, we label the self-collected datasets corresponding to Figure 2b,c as $Garden$ and $Bumpy$, respectively. The environment of $Garden$ was relatively open, mainly containing many trees and bushes, but the ground was relatively flat. The $Bumpy$ dataset represented a bumpy and complex environment with multiple large and small pits in the ground. F-LIOM was capable of accomplishing mapping in all tested conditions. As supplementary material, the mapping and robot movement videos recorded by the camera can be found using the links in Appendix A.

The robot moved a long distance and then returned to the vicinity of the initial position or revisited the scenes that were passed by to verify the place recognition. Figure 3 provides the complete trajectory generated by FLOAM, LIO-SAM, LeGO-LOAM, and our method using the $Garden$, $Bumpy$, and public $Park$ datasets [14]. Because there was no ground truth, we first aimed to show that our method could achieve the same localization accuracy as the state-of-the-art methods FLOAM, LIO-SAM, and LeGO-LOAM, thus proving the effectiveness and versatility of the proposed approach.

It should be emphasized that although the perturbations are considered in this paper, the robot model is still parameterized as SE(2), so the z axis is always meaningless. FLOAM, LIO-SAM, and LeGO-LOAM assume that the robot pose exists in SE(3), where the z axis will drift during robot movement, even if the robot moves on flat ground in practice. In order to fairly evaluate the results of the different methods, the trajectory results were preprocessed before comparison. We replaced all the z-axis trajectories with 0 for the comparison of the translation part. The SLAM accuracy assessment tool evaluation of odometry (EVO) [38] was used to calculate the absolute trajectory error (ATE) of FLOAM, LIO-SAM, and LeGO-LOAM. Taking the trajectory from our method as a reference, the detailed data for comparing the different ATE results are shown in

Table 1. Results of absolute trajectory error.

Dataset	Method	Max	Mean	Rmse	Std
Garden	FLOAM	4.00	1.96	2.20	0.99
	LeGO-LOAM	0.60	0.24	0.26	0.10
	LIO-SAM	0.66	0.29	0.31	0.10
	LIO-SAM_FixZ	0.61	0.27	0.29	0.10
Bumpy	FLOAM	12.3	1.85	2.73	2.01
	LeGO-LOAM	0.62	0.25	0.28	0.13
	LIO-SAM	0.45	0.13	0.16	0.09
	LIO-SAM_FixZ	0.27	0.13	0.15	0.08
Park	FLOAM	47.95	25.25	28.51	13.26
	LeGO-LOAM	1.25	0.25	0.30	0.17
	LIO-SAM	0.51	0.21	0.23	0.10
	LIO-SAM_FixZ	0.59	0.26	0.29	0.12

.

As is shown, the error between the trajectory generated by our method and the trajectories generated by LIO-SAM and LeGO-LOAM was close to zero, indicating that our method achieved the same accuracy as LIO-SAM and LeGO-LOAM. LIO-SAM_FixZ denotes a particular set of experiments, where we set the z axis of LIO-SAM throughout to 0 during the optimization process. The accuracy of LIO-SAM_FixZ was the same as that of LIO-SAM, but its running time was longer, which will be explained later. The trajectory of FLOAM had a significant deviation from the other methods.

We ran the different datasets to verify the performance of F-LIOM in loop closure. A list of parameters pertaining to the smoothness scan context used in the experiment is shown in

Table 2. Parameters related to the smoothness scan context.

Parameter	Brief	Threshold
$L_{max}$	Maximum sensing range (m)	70
$N_s$	Number of sectors	60
$N_r$	Number of rings	20
${\rho }_{\tau }$	Smoothness matching threshold	0.3
${\rho }_c$	Smoothness threshold	0.1

. The test datasets consisted of the self-collected $Garden$ and $Bumpy$ and the public $Park$ datasets. The scenarios recorded by these datasets were considered very challenging for loop closure detection because they contained many factors that are not conducive to place recognition, such as reverse and forward visits and similar geometric structures and objects. Figure 4 shows the loop closure based on the smoothness scan context proposed in this paper, where each of the loop closure points identified is marked with a red dot, and the detected loop pairs are connected by the black line.

Recall rate and precision are two fundamental indicators for evaluating loop detection. For low-power embedded systems, we quantitatively analyzed the conditions required to trigger loop closure. The results were compared with existing place recognition algorithms generally applied in SLAM, such as SC [35] and ISC [36].

Table 3. The results of the smoothness scan context.

Dataset	Method	Avg No. of Point Clouds	No. of Loops	Precision
$Garden$	SC	29,721	22	81.81%
	ISC	29,721	18	94.44%
	our	338	23	95.65%
$Bumpy$	SC	26,100	13	64.54%
	ISC	26,100	12	75.00%
	our	940	12	75.00%
$Park$	SC	26,000	21	84.12%
	ISC	26,000	21	84.12%
	our	359	23	90.10%

lists the results. Compared to SC and ISC, our method only required hundreds of point clouds instead of tens of thousands to achieve highly competitive precision across all three datasets. The number of loops implied the high performance of the method for place recognition, even for challenging scenes, such as forward or reverse visits. More importantly, the smaller the number of point clouds required, the fewer memory resources are required; this can significantly relieve the load on the computation and memory of embedded devices in practice.

4.2. Real-Time Performance

In the above experiments, we demonstrated that our method could achieve an accuracy in terms of pose estimation and loop closure that was competitive in relation to advanced methods. Next, we prove that the method proposed in this paper has more powerful advantages from a real-time perspective. As one of the most time-consuming core components in the system, the scan-matching module constructs local map constraints and solves keyframe poses via optimization methods. We ran different datasets to compare the run-time of the scan-matching modules in FLOAM, LeGO-LOAM, LIO-SAM, LIO-SAM_FixZ, and our method. The results for the time consumed by the four different scenarios are shown in Figure 5, where the data represent the run-times of the scan-matching modules and thus serve as a quantitative metric to compare and analyze the processes. It can be seen that in the more complex scenario Bumpy, the single optimization time of some methods reached the second level.

Table 4. Average run-times of scan-matching module processing (ms).

Method	Garden	Bumpy	Park	Synchronized with Dataset?
FLOAM	146.98	192.03	154.95	No
LeGO-LOAM	245.05	421.51	97.45	Yes
LIO-SAM	521.32	578.49	238.84	Yes
LIO-SAM_FixZ	730.68	970.23	242.86	Yes
Our method	265.10	258.05	69.15	Yes

quantitatively shows the average run-times of the scan-matching modules. If we intuitively analyze the data in Table 4, then the optimization speed of FLOAM is undoubtedly the fastest. However, it should be pointed out that the optimization process of FLOAM was not synchronized with the dataset when it was played at normal speed; the play speed of the dataset was set to 0.3 to achieve synchronization. The whole running time of FLOAM was more than three times the duration of the dataset, and it did not achieve real-time performance. Throughout all tests, the scan-matching run-time of F-LIOM in the three scenarios was significantly shorter than that of LIO-SAM and LeGO-LOAM. In some highly challenging cases, such as the bumpy ground scene shown in Figure 2c, the average run-times of LIO-SAM and LeGO-LOAM reached 578.49 milliseconds and 421.54 milliseconds, respectively. This meant that some Lidar frames were rejected in the process when the Lidar rotation rate was 10 Hz. Lastly, let us analyze LIO-SAM_FixZ; its average run-time was the longest under the different datasets. The initial idea of setting the z axis to a fixed value was intended to eliminate the possible drift of the z axis, so the z axis was reset during each optimization process. Due to the lack of z-axis information, the model did not match the actual state, making it difficult to converge during optimization. Finally, the experiment showed quantitatively that the optimization process took longer, and the average time reached 970 milliseconds in extreme cases. As shown, F-LOIM still achieved a quite notable performance in real time, even in complex environments, making it more promising for deployment in limited-resource embedded systems.

5. Conclusions

This paper developed F-LIOM, a robust smoothness-aided Lidar inertial odometry method for ground mobile robots equipped with 3D Lidar in complex environments with rough terrain. The F-LIOM is cost-effective in terms of model complexity and calculation units, as it employs the proposed out-SE(2) constraint method that assumes the mobile robot is still moving in SE(2) and integrates the out-SE(2) perturbations as weights in the noise term and covariance matrix to optimize the estimated state. The system follows a tightly coupled Lidar inertial odometry structure, including tracking, real-time state estimation, mapping, and loop closure functions. Inspired by the recent work on Lidar intensity and geometry, a new global descriptor, the smoothness scan context, was presented that exploits geometry and smoothness properties and only requires a few thousand features, instead of tens of thousands of point clouds like existing methods. Multiple sets of experiments were carried out to evaluate the performance of the proposed algorithm in various scenes, including indoor, road, building, garden, and public datasets. The results showed that the proposed method achieved a competitive performance in real-time pose estimation, making it more suitable for deployment in low-power embedded systems.