A Novel LiDAR–IMU–Odometer Coupling Framework for Two-Wheeled Inverted Pendulum (TWIP) Robot Localization and Mapping with Nonholonomic Constraint Factors

Abstract

This paper proposes a method to solve the problem of localization and mapping of a two-wheeled inverted pendulum (TWIP) robot on approximately flat ground using a Lidar–IMU–Odometer system. When TWIP is in motion, it is constrained by the ground and suffers from motion disturbances caused by rough terrain or motion shaking. Combining the motion characteristics of TWIP, this paper proposes a framework for localization consisting of a Lidar-IMU-Odometer system. This system formulates a factor graph with five types of factors, thereby coupling relative and absolute measurements from different sensors (including ground constraints) into the system. Moreover, we analyze the constraint dimension of each factor according to the motion characteristics of TWIP and propose a new nonholonomic constraint factor for the odometry pre-integration constraint and ground constraint factor in order to add them naturally to the factor graph with the robot state node on SE(3). Meanwhile, we calculate the uncertainty of each constraint. Utilizing such a nonholonomic constraint factor, a complete lidar–IMU–odometry-based motion estimation system for TWIP is developed via smoothing and mapping. Indoor and outdoor experiments show that our method has better accuracy for two-wheeled inverted pendulum robots.

1. Introduction

In recent years, Simultaneous localization and mapping (SLAM) for intelligent mobile robots has become a research hotspot [1,2]. Under GPS-denied and unknown environment conditions, mobile robots must be able to utilize onboard sensors to construct an environment map that can be used for navigation [3] and then rely on this environment map for navigation, collision avoidance, and path planning. Many solutions have been proposed based on different sensing methods, mainly divided into vision-based [4,5,6], lidar-based [7,8,9], and a combination of the two [10,11]. Meanwhile, in order to cope with complex environments, sensors such as IMUs and encoders that are less affected by the environment can be used to assist with visual [12,13] or lidar [14,15,16,17,18] localization. Although vision-based methods are particularly suitable for position recognition, their sensitivity to initialization, illumination, and range as well as their high depth computation cost makes them unreliable when supporting autonomous navigation systems.

On the other hand, lidar can directly obtain depth information about the surrounding environment, and lidar are immune to illumination changes. Many impressive LiDAR-based localization and mapping algorithms have been proposed. In these algorithms, the IMU, encoder and other high-frequency output sensors are usually used to assist with lidar localization and the deskew point cloud. In [7], the authors present lidar odometry and mapping (LOAM) for low-drift real time state estimation and mapping. In this algorithm, the IMU is only used to de-skew the point cloud and does not participate in lidar localization. Here, for the localization algorithm of the lidar–IMU system, we divided it into filter-based and optimization-based segments according to the coupling method. Filter-based methods typically utilize extended Kalman filtering to couple measurements from lidar and IMU to estimate the state of the robot frame-by-frame. In [14], Lynen et al. proposed a modular method that uses EKF to fuse IMU measurements with relative pose measurements from cameras, lidars, etc. In [15], the authors present a lidar-inertial state estimator called R-LINS, which uses a recursive error-state Kalman filter to estimate the robot’s state. In [16], the authors propose a fast, robust and general LiDAR–IMU odometry framework based on an efficient and tightly-coupled iterative Kalman filter for fast, robust, and accurate LiDAR navigation. Optimization-based methods usually use nonlinear optimization to combine a fixed number of lidar keyframes and IMU pre-integration in order to estimate the optimal pose. In the literature [17], a tightly coupled framework lio-mapping of lidar and IMU has been introduced which uses a framework similar to VINS and achieves good accuracy; however, it cannot run in real time. In [18], the authors proposed a framework LIO-SAM for tightly coupling lidar and IMU via smoothing and mapping, achieving highly accurate real-time localization and mapping. Filtering-based methods usually only consider the influence of the previous frame on the current frame, which incurs lower computational costs. The optimization-based method usually adopts the sliding window method to evaluate the constraint relationship between the robot states of the previous n frames (including the current frame). The optimization-based method is more computationally expensive than the filtering-based method, however, it has accuracy. With improved hardware performance, optimization-based methods can run in real time, and should gradually become the mainstream of research. However, in scenarios with sparse structural features, such as long tunnels, wide roads, etc., lidar lacks constraints in certain directions, resulting in decreased localization accuracy. In [19], the authors found that when their mobile robot had constant acceleration or no rotation, the VINS system was unable to distinguish the magnitude of the true body acceleration and the direction of the local gravitational acceleration from that of the accelerometer bias. Similar situations can appear in scenarios where Lidar is degraded.

The localization of mobile ground platforms with unique system architectures has not previously been thoroughly investigated and discussed. Due to ground constraints, many vision-based [20,21] and lidar-based [22,23] SLAM algorithms use the SE(2) pose. However, the ground is rough in the natural environment, and a ground-mobile robot will be disturbed by motion disturbance. In order to better adapt to the natural environment, Ref. [24] proposes a new constraint model, stochastic SE(2) constraints, which usually parameterizes the robot pose on SE(3), allowing for slight disturbance in dimensions other than SE(2). Fan Zheng [25] extended the work of the above article and proposed a complete motion estimation system that utilizes SE(2) constraints and SE(3) pose parameterization.

In this paper, a factor graph is formulated to couple the lidar, IMU, and encoder measurements for the localization and mapping of TWIP robots on an approximately flat road surface. Inspired by the SE(2) constraint–SE(3) pose method used in [24], we propose a new factor with constraints to introduce the encoder’s pre-integration and ground constraints, which we achieve by analyzing the motion profile of the robot and its deviation from planar motion (e.g., due to terrain unevenness or motion vibration) and formulating stochastic (i.e., “soft”) instead of deterministic (i.e., “hard”) constraints. This allows us to properly model the TWIP’s almost-planar motion, as shown in Figure 1. Moreover, we use Lie algebra to represent the rotation in the pose imitation process. Lie algebra has no redundant parameters. Compared with Euler angles, Lie algebra does not have gimbal locking problems, and rotation integration based on Lie algebra can be integrated into a closed form, whereas Euler angles are only exact up to the first order [26]. The main novel contributions of this work are as follows:

• A complete LIDAR–IMU–encoder-coupled state estimation and mapping framework is proposed for a two-wheeled inverted pendulum robot. This method extends the LIDAR–IMU to process odometric measurements and improves the system’s localization accuracy.
• A new nonholonomic factor is proposed to introduce encoder pre-integration constraints and ground constraints, which are naturally coupled to the factor graph optimization of the system.
• Indoor and outdoor experiments demonstrate the improved localization accuracy and robustness of the proposed lidar–IMU–odometry coupling framework when the controller is mounted on a two-wheeled inverted pendulum robot navigating on an approximately flat surface.

The rest of the paper is organized as follows: Section 2 provides an overview of the entire methodology; Section 3 reviews the basic knowledge which is be used later in the paper; Section 4 details the five types of factors introduced in our algorithm; and Section 5 describes several experiments we conducted to demonstrate the effectiveness of the proposed method.

2. Methodology Overview

This paper aims to estimate the motion state of a two-wheeled inverted pendulum robot (TWIP) via lidar–IMU–Odometry coupling. Compared with a six-degrees-of-freedom robot, TWIP cannot move sideways, and is constrained by the ground. Compared with autonomous vehicles on the ground, TWIP swings back and forth during movement to maintain the balance of movement. If we directly parameterize the robot pose on SE(3) without considering the ground constraints, the state estimation accuracy and robustness is depressed due to fewer constraints in the vertical direction of the lidar. If only ground constraints are considered, that is, the deterministic constraint, the performance of system motion estimation is degraded because the six dimensions of the pose are highly coupled with sensor observations. Therefore, neither accurate pose parameterization considering ground constraints nor 3D pose parameterization can satisfactorily represent the pose of the ground robot, because the former suffers from out-of-constraint motion while the latter does not use plane motion constraints. This prompted us to study the state estimation of the two-wheeled inverted pendulum robot through a lidar–IMU–Odometry system.

This paper proposes a framework for localization using a Lidar–IMU–Odometer system. The flow chart of this framework is shown in Figure 2. When the system receives a lidar scan, it first uses the measurement of the IMU to de-skew the point cloud, then extracts the line and surface feature points. If the pose exceeds a pre-set threshold, the current frame is selected as a keyframe; otherwise, it is discarded. Measurements from the IMU provide the initial pose. Then, the keyframes are matched to the global map to obtain the rough global pose and construct the lidar pose factor. The IMU and odometer measurements between two adjacent lidar keyframes are used to construct constraint factors via pre-integration techniques. Meanwhile, we use the random constraint model to construct the ground constraint factor. Finally, all factors are included in the factor graph and jointly optimized to obtain the optimal robot pose. The factor graph formulated for TWIP, which is shown in Figure 3, incorporates the measurement data of each sensor as a factor in our optimization system. In addition to the above four factors, this factor graph contains closed-loop factors grouped into two categories. The IMU pre-integration factor, the lidar pose factor, and the loop closure factor constrain each dimension of the robot pose, and are named holonomic constraint factors. However, the odometry pre-integration factor and the ground constraint factor only constrain part of the dimensions of the robot’s pose, and thus are named non-holonomic constraint factors. For these factors we propose a nonholonomic constraint factor to add them to the system, inspired by the SE(2) constraint SE(3) parameterization method used in [24]. Utilizing such a constraining factor, a complete lidar–IMU–odometry-based motion estimation system for TWIP is developed via smoothing and mapping.

3. Preliminaries

This paper formulates the state estimation problem based on the Lidar–IMU–Odometry system in terms of a factor graph, that is, a nonlinear optimization problem involving the quantities existing on smooth manifolds. Before delving into details, we review several of the related practical geometric concepts.

3.1. On-Manifold Pose Parameterization

We usually use the Matrix Lie group to describe three-dimensional rotation, which is defined as follows:

$$\mathrm{SO}\left(3\right)=\left\{\mathbf{R}\in {\mathbb{R}}^{3x3},{\mathbf{R}}^{\mathrm{T}}\mathbf{R}=\mathbf{I}\right\}$$

(1)

However, the rotation combination operation in the SO(3) involves matrix multiplication in the optimisation process, which cannot be directly operated as Euclidean vectors can. Due to Lie groups having the properties of differential manifolds, the tangent space to the manifold (at the identity), called the Lie algebra, can be introduced as a representation of the most diminutive vector form, and is related to Lie groups through matrix exponents and logarithms. Lie algebra is defined as the follows:

$$s\mathrm{o}\left(3\right)=\left\{\varphi \in {\mathbb{R}}^3,\Phi ={\varphi }^{\wedge }\in {\mathbb{R}}^{3\times 3}\right\}$$

(2)

where $\varphi$ is the rotation vector of ${\mathbb{R}}^3$, corresponding to a 3 × 3 skew-symmetric matrix. The hat operator ${\left(•\right)}^{\wedge }$ is used to find the antisymmetric matrix of a vector, and its form is

$$\Phi ={\varphi }^{\wedge }=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]\varphi =\left[\begin{array}{c}{\varphi }_1\\ {\varphi }_2\\ {\varphi }_3\end{array}\right]$$

(3)

At the same time, we define the inverse mapping of the hat operator ${\left(•\right)}^{\vee }$ as follows:

$$\varphi =\left[\begin{array}{c}{\varphi }_1\\ {\varphi }_2\\ {\varphi }_3\end{array}\right]={\Phi }^{\vee }$$

(4)

For any two 3D vectors $\mathbf{r}$ and $\mathbf{s}$ and the rotation matrix $\mathbf{R}$, the skew-symmetric matrix has the following properties:

$${\mathbf{r}}^{\wedge }\mathbf{s}=-{\mathbf{s}}^{\wedge }\mathbf{r}$$

(5)

$${\left(\mathbf{Rs}\right)}^{\wedge }=\mathbf{R}{\mathbf{s}}^{\wedge }{\mathbf{R}}^T$$

(6)

$${\left({\mathbf{r}}^{\wedge }\right)}^{\mathrm{T}}=-{\mathbf{r}}^{\wedge }$$

(7)

$${\mathbf{r}}^{\wedge }{\mathbf{r}}^{\wedge }=\mathbf{r}{\mathbf{r}}^{\mathrm{T}}-{\mathbf{I}}_{3\times 3}$$

(8)

Lie algebra can be transformed into a Lie group by exponential mapping (Rodriguez formula):

$$exp\left({\varphi }^{\wedge }\right)=\mathbf{I}+\frac{sin\left(∥\varphi ∥\right)}{∥\varphi ∥}{\varphi }^{\wedge }+\frac{1-cos∥\varphi ∥}{{∥\varphi ∥}^2}{\left({\varphi }^{\wedge }\right)}^2$$

(9)

A first-order approximation of the exponential map used later on is

$$exp\left({\varphi }^{\wedge }\right)\approx \mathbf{I}+{\varphi }^{\wedge }$$

(10)

In the optimization process, we often use a left-multiplicative perturbation on the manifold through the left Jacobian matrix, approximating a local on-manifold transformation with Lie algebra:

$$exp\left(\delta {\varphi }^{\wedge }\right)exp\left({\varphi }^{\wedge }\right)=exp\left({\left({\mathbf{J}}_l^{-1}\left(\varphi \right)\delta \varphi +\varphi \right)}^{\wedge }\right)$$

(11)

where the left Jacobian matrix is

$${\mathbf{J}}_l\left(\varphi \right)=\frac{sin\theta }{\theta }\mathbf{I}+\left(1-\frac{sin\theta }{\theta }\right)\mathbf{a}{\mathbf{a}}^{\mathrm{T}}+\frac{1-cos\theta }{\theta }{\mathbf{a}}^{\wedge }$$

(12)

where $\theta$ and $\mathbf{a}$ are the norm and direction of the rotation vector $\mathbf{\varphi }$, $\mathbf{\varphi }=\theta \mathbf{a}$. The inverse of the left Jacobian matrix is

$${\mathbf{J}}_l^{-1}\left(\varphi \right)=\frac{\theta }{2}cot\frac{\theta }{2}\mathbf{I}+\left(1-\frac{\theta }{2}cot\frac{\theta }{2}\right)\mathbf{a}{\mathbf{a}}^{\mathrm{T}}-\frac{\theta }{2}{\mathbf{a}}^{\wedge }$$

(13)

Another valuable property of the exponential map is

$$\mathbf{R}\mathrm{Exp}\left(\varphi \right){\mathbf{R}}^T=\exp \left(\mathbf{R}{\varphi }^{\wedge }{\mathbf{R}}^T\right)=\mathrm{Exp}\left(\mathbf{R}\varphi \right)$$

(14)

3.2. Factor Graph Optimization

As TWIP moves, lidar measurements are continuously collected to estimate the robot’s motion state and model the surrounding environment. This process can be described by a factor graph, where the robot states to be estimated are state nodes. The continuously collected measurements are introduced as factor nodes connected by edges to the involved state nodes (see Figure 3). In general, we assume that these measurements are independent of each other and are affected by Gaussian noise. The robot state estimation problem represented by the factor graph above can be formulated for maximum a posteriori estimation. Therefore, we formulate the above problem as the following nonlinear least-squares problem:

$$\widehat{\mathbf{x}}=arg\underset{\mathbf{x}}{min}\sum _i{∥{\mathbf{r}}_i\left(\mathbf{x}\right)∥}_{{\sum }_i}^2$$

(15)

where ${\mathbf{r}}_i$ is the zero-mean residual associated with measurement i, $\mathbf{x}$ is the parameter vector to be optimized, ${\Sigma }_i$ is the measurement covariance, and ${∥{\mathbf{r}}_i∥}_{{\Sigma }_i}^2={\mathbf{r}}_i^{\mathrm{T}}{\Sigma }_i^{-1}{\mathbf{r}}_i$ is the energy norm.

The above least squares problem can be solved iteratively using the Gauss–Newton method. At each GN iteration, we approximate the optimized objective function (the residual function) by first-order Taylor expansion at the current estimated state ${\widehat{\mathbf{x}}}^{-}$:

$$\begin{array}{c}\Delta \widehat{\mathbf{x}}=arg\underset{\Delta \mathbf{x}}{min}{\displaystyle \sum _i}{∥{\mathbf{r}}_i\left(\mathbf{x}\oplus \Delta \mathbf{x}\right)∥}_{{\sum }_i}^2\hfill \\ =arg\underset{\Delta \mathbf{x}}{min}{\displaystyle \sum _i}{∥{\mathbf{r}}_i\left(\mathbf{x}\right)+{\mathbf{H}}_i\Delta \mathbf{x}∥}_{{\sum }_i}^2\hfill \end{array}$$

(16)

where ${\mathbf{H}}_i=\frac{\partial {\mathbf{r}}_i}{\partial \mathbf{x}}$ is the Jacobian matrix of the $ith$ residual for the robot state and $\Delta \mathbf{x}$ is the increment of the state obtained at each iteration. We define the generalized update operation, ⊕, which maps a change in the error state to one in the entire state. After solving the linearized equation, the current state is updated as ${\widehat{\mathbf{x}}}^{+}={\widehat{\mathbf{x}}}^{-}\oplus \Delta \mathbf{x}$. This linearization process is then repeated until convergence

4. State Estimation with Accurate Parameterization of TWIP

4.1. Pose Parameterization of TWIP

Compared with a six-degrees-of-freedom robot, TWIP cannot move sideways and is constrained by the ground. Compared with autonomous vehicles on the ground, TWIP swings back and forth during movement to maintain the balance of movement. Therefore, we cannot characterize its motion state completely in SE(2) or SE(3) space. A simplified diagram of this movement is shown in Figure 4.

In order to analyze the movement of TWIP more conveniently, its coordinate system is defined as follows:

Lidar coordinate system (L): The lidar frame originates at the lidar center. The y-axis points to the left, the z-axis points upward, and the x-axis points forward, coinciding with the positive direction of TWIP.

IMU coordinate system (I): The IMU is located below the lidar; its frame originates at the IMU measurement center. The x-, y-, and z-axes are parallel to (L), pointing in the exact directions, and the z-axis points forward, coinciding with the z-axis of the lidar frame.

Robot body coordinate system (B): As shown in Figure 4, the robot body frame is connected to the robot body during the movement process. Its origin is at the midpoint of the line connecting the centers of the two wheels. The x-, y-, and z-axes are parallel to (L), pointing in the exact directions, and the z-axis points forward, coinciding with the z-axis of the lidar frame.

World coordinate system (W): The world frame is the gravity-aligned coordinate system, which is initially coincident with the robot body coordinate system (B).

All of the above frames follow the right-hand rule. We calibrate the transformation between the lidar frame and the robot body frame and between the IMU frame and the robot body frame. The transformation between the robot body coordinate system and the world coordinate system is the motion state of the robot, that is, the target parameters for optimization. Figure 5 shows the posture of TWIP as obtained by the Attitude and Heading Reference System (ARHS) during movement. As seen from the figure, when TWIP moves, there are movements on the two-dimensional plane (yaw) and front and back swings (pitch), as well as movement disturbances in the roll direction. Therefore, we parameterize the robot’s pose on SE(3). The motion state of the robot corresponding to the lidar keyframe ${\mathrm{F}}_i$ at time i is

$${\mathit{K}}_i=\left[\begin{array}{cc}{\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}& {\mathbf{P}}_{{\mathrm{B}}_i}^{\mathrm{W}}\end{array}\right]$$

(17)

where ${\mathbf{P}}_{{\mathrm{B}}_i}^{\mathrm{W}}={\left[\begin{array}{ccc}{x}_i& y_i& z_i\end{array}\right]}^{\mathrm{T}}$ is the translation and ${\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}\in {\mathbb{R}}^{3x3}$ is the robot’s rotation. Because the rotation matrix is not additive, it is not convenient for iterative optimization; thus, we use Euler angles and Lie algebra to represent the robot’s rotation in the following derivation. Euler angles easily distinguish the constraint dimension of each factor, and the pitch does not reach $\pm 90$ degrees during the movement of TWIP, avoiding any problems with gimbal lock. In this paper, $\mathbf{\theta }={\left[\begin{array}{ccc}{\theta }_r& {\theta }_p& {\theta }_y\end{array}\right]}^{\mathrm{T}}$ represents the Euler angle and ${\theta }_r,{\theta }_p,{\theta }_y$ represents the values of the roll, pitch, and yaw angles, respectively. The rotation integral based on Euler angles is only exact up to the first order in robot state estimation [26]. Therefore, we use Lie algebra, $\varphi ={\left[\begin{array}{ccc}{\varphi }_1& {\varphi }_2& {\varphi }_3\end{array}\right]}^{\mathrm{T}}$, to represent the rotation in the optimization process. Lie algebra can be converted to rotation matrices using Equation (7). According to the motion characteristics of TWIP, $\left[\begin{array}{cccc}x& y& {\theta }_p& {\theta }_y\end{array}\right]$ is the result of the robot’s motion, which is named the primary parameter, while $\left[\begin{array}{cc}z& {\theta }_r\end{array}\right]$ is the motion disturbance caused by uneven ground or the vibration of the robot, and is named the second parameter. The partial parameters in the figure are shown in

Table 1. State parameters and frame of TWIP.

Symbols	Descriptions
$\mathrm{X},\mathrm{Z},\mathrm{O}$	The x-axis, z-axis, and origin of the world frame
${\mathrm{X}}_{\mathrm{B}},{\mathrm{Z}}_{\mathrm{B}},{\mathrm{O}}_{\mathrm{B}}$	The x-axis, z-axis, and origin of the robot body frame
${\mathrm{X}}_{\mathrm{L}},{\mathrm{Z}}_{\mathrm{L}},{\mathrm{O}}_{\mathrm{L}}$	The x-axis, z-axis, and origin of the lidar frame
$\left(x_k,y_k,z_k\right),k=i,j$	The position of the robot in the world frame at time k (in meters)
${\theta }_{pk},k=i,j$	The pitch angle of the robot relative to the world frame at time k (in radians)
${\theta }_{yk},k=i,j$	The yaw angle of the robot relative to the world frame at time k (in radians)

.

4.2. Coupling of Lidar–IMU–Odometer via Factor Graph

In this section, we introduce five types of factors to construct factor graphs in order to estimate the motion state of TWIP. The robot’s state at a specific time is attributed to the graph’s nodes. According to the constraints of factors on nodes, these factors are divided into holonomic and nonholonomic constraints. The holonomic constraint factors include the lidar pose factor, the IMU pre-integration factor, and the loop closure factor. The nonholonomic constraint factor consists of the odometer pre-integration and ground constraints. A new robot state node is added to the graph when the change in robot pose exceeds a user-defined threshold. The factor graph is optimized upon the insertion of a new node using incremental smoothing and mapping with the Bayes tree (iSAM2) [17]. The process for generating these factors is described in the following sections.

4.2.1. Lidar Pose Factor

Mechanical 3D LiDAR senses its surroundings by rotating a vertically aligned array of laser beams. When a lidar keyframe ${\mathrm{F}}_i$ is selected, we first de-skew the point cloud. The movement of the carrier causes point cloud distortion during the lidar data collection process, and the points in a lidar scan are not collected at the same time; that is, the frames of different lidar points are inconsistent. The bias of the IMU obtained by the factor graph optimization is used to correct the IMU measurement, which can estimate the pose of the carrier. Here, we find the pose corresponding to each lidar point, transform them into the same coordinate system, and obtain the resulting point cloud without distortion. The feature point matching method is used in this paper because it is more robust and efficient than noise-sensitive matching methods, such as Iterative Closest Point (ICP). Similar to [7], this paper uses plane feature points and edge feature points as matching points, which are extracted by smoothness. For any point ${\mathbf{p}}_k$ in ${\mathrm{F}}_i$, We find ten straight points of ${\mathbf{p}}_k$ from the same sweep, of which half are on either side of ${\mathbf{p}}_k$. The smoothness of ${\mathbf{p}}_k$ can then be calculated by the following formula:

$$c=\frac{1}{\left|D\right|·∥r_k∥}∥\sum _{j\in D,j\ne k}\left(r_j-r_k\right)∥$$

(18)

where D is the point set adjacent to this point we selected and $\left|D\right|$ is the number of points in D.

We use ${\mathrm{F}}_i=\left\{{\mathrm{F}}_i^{\epsilon },{\mathrm{F}}_i^{\pi }\right\}$ to represent the feature points extracted from the lidar scan at time i, where ${\mathrm{F}}_i^{\pi }$ represents the planar points and ${\mathrm{F}}_i^{\epsilon }$ represents the edge points. Points with slight smoothness are selected as plane points and points with significant smoothness are selected as edge points. A detailed description of the feature extraction process is provided in [7].

After the above processing is complete, we have the feature points of the current lidar frame without distortion. We then need to find the plane and edge line corresponding to the current frame feature point in the target point cloud for the feature-based matching method. Unlike traditional algorithms such as lio_sam and lego_loam, which take lidar scans close to the current frame in the time series as matching point clouds, this paper searches for the matching point cloud of the current frame in Euclidean space, which is more accurate because our optimization process is to minimize the point-to-line and point-to-plane spatial distances. Therefore, in this algorithm the position of the current robot is taken as a priori, and a part of the point cloud is extracted from the global map to align with the feature points without distortion in the current frame in order to estimate the pose of the robot. The IMU provides the initial position. The global map consists of edge feature maps and plane feature maps, which are updated and maintained, respectively. The global map is stored in an incremental 3D tree [18]. Compared with the traditional dynamic data structure, incremental 3D tree has an efficient nearest neighbor search and supports the incremental update of the map. Similar to [18], we convert the current frame to the world frame to obtain ${}^{\mathrm{W}}{\mathrm{F}}_i=\left\{{}^{\mathrm{W}}{\mathrm{F}}_i^{\epsilon },{}^{\mathrm{W}}{\mathrm{F}}_i^{\pi }\right\}$, while the IMU provides the initial pose. We find the N nearest edge points in the global edge map for any edge feature point ${\mathbf{p}}_i^{\epsilon }$, then calculate the variance of these nearby points. If one of the eigenvalues of the variance matrix is significantly larger than the other two, the points are distributed in a line. The eigenvector ${\mathbf{n}}_i^{\epsilon }$, which corresponds to the largest eigenvalue, is the direction vector of the line. The edge point ${\mathbf{p}}_j^{\epsilon }$ closest to the feature point is taken as a point on the line. The distance from the feature point to the global line is

$$d_i^{\epsilon }={∥{\left({\mathbf{n}}_i^{\epsilon }\right)}^{\wedge }\left({\tilde{\mathbf{T}}}_{{\mathrm{L}}_i}^{\mathrm{W}}{\mathbf{p}}_i^{\epsilon }-{\mathbf{p}}_j^{\epsilon }\right)∥}_2$$

(19)

For any plane feature point ${\mathbf{p}}_i^{\pi }$, we find the M nearest plane points in the global plane map and calculate the variance of these nearby points. If one of the eigenvalues of the variance matrix is significantly smaller than the other two, the points are distributed in a plane. The eigenvector ${\mathbf{n}}_i^{\pi }$, which corresponds to the smallest eigenvalue, is the normal vector of the plane. We select the closest plane feature point ${\mathbf{p}}_j^{\pi }$ to the feature point as a point on the plane. Then, the distance from the feature point to the global plane is

$$d_i^{\pi }=\left|\left({\mathbf{n}}_i^{\pi }\right)·\left({\tilde{\mathbf{T}}}_{{\mathrm{L}}_i}^{\mathrm{W}}{\mathbf{p}}_i^{\pi }-{\mathbf{p}}_j^{\pi }\right)\right|$$

(20)

We can obtain the optimal pose of the current lidar scan relative to the world frame by minimizing the point-to-line and point-to-face distances:

$$\underset{{\tilde{\mathbf{T}}}_{{\mathrm{L}}_i}^{\mathrm{W}}}{min}\sum _{i=0}^N{\left(d_i^{\epsilon }\right)}^2+\sum _{i=0}^M{\left(d_i^{\pi }\right)}^2$$

(21)

After optimization, we have the optimal absolute transformation ${\tilde{\mathbf{T}}}_{{\mathrm{L}}_i}^{\mathrm{W}}$ from the current keyframe to the world frame, which is used to measure the lidar pose factor.

We obtain the residual of the lidar pose factor using the following formula:

$${\mathbf{r}}_{{\mathrm{L}}_i}=log\left({\left({\mathbf{T}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{T}}_{\mathrm{L}}^{\mathrm{B}}\right)}^{\mathrm{T}}{\tilde{\mathbf{T}}}_{{\mathrm{L}}_i}^{\mathrm{W}}\right)$$

(22)

where ${\mathbf{T}}_{\mathrm{L}}^{\mathrm{B}}$ is the transformation between the lidar coordinate system and the robot body coordinate system, calibrated in advance.

The variance of the factor is calculated by the method used in [26]:

$${\Sigma }_{{\mathrm{L}}_i}=\sum _{k=1}^N{\mathbf{H}}_k^T{\Lambda }^{-1}{\mathbf{H}}_k$$

(23)

$${\mathbf{H}}_k=\left[\begin{array}{cc}{\left({\tilde{\mathbf{R}}}_{{\mathrm{L}}_i}^{\mathrm{W}}{\mathbf{p}}_k+{\tilde{\mathbf{P}}}_{{\mathrm{L}}_i}^{\mathrm{W}}\right)}^{\wedge }& -{\tilde{\mathbf{R}}}_{{\mathrm{L}}_i}^{\mathrm{W}}\end{array}\right]$$

(24)

$$\Lambda =\left[\begin{array}{ccc}2{\sigma }_x^2& 0& 0\\ 0& 2{\sigma }_y^2& 0\\ 0& 0& 2{\sigma }_z^2\end{array}\right]$$

(25)

where ${\mathbf{p}}_k$ is the feature point in the current lidar keyframe, and is transformed into the matching point cloud through the optimal transformation, ${\tilde{\mathbf{T}}}_{{\mathrm{L}}_i}^{\mathrm{W}}=\left[\begin{array}{cc}{\tilde{\mathbf{R}}}_{{\mathrm{L}}_i}^{\mathrm{W}}& {\tilde{\mathbf{P}}}_{{\mathrm{L}}_i}^{\mathrm{W}}\\ \mathbf{0}& 1\end{array}\right]$, obtained through the above matching. Here, N is the number of features in the current lidar keyframe, $\Lambda$ is the zero-mean white Gaussian noise of the measurements, and ${\sigma }_x,{\sigma }_y$ and ${\sigma }_z$ are the noise sigma (in meters) on the x, y, and z axes, respectively. The sigmas are multiplied by two, as both the target point and the matching are affected by this noise.

4.2.2. IMU Pre-Integration Factor

The main components of an IMU are a three-axis accelerometer and a three-axis gyroscope that measure the linear acceleration and angular velocity of the carrier, respectively. The pose transformation of the carrier to an inertial frame can be obtained by integral operation. The raw measurements, ${\tilde{\mathbf{a}}}_t$ and ${\tilde{\omega }}_t$, at moment t are provided by

$$\begin{array}{cc}\hfill & {\tilde{\mathbf{a}}}_t={\mathbf{a}}_t+{\mathbf{b}}_{a_t}+{\mathbf{R}}_{\mathrm{W}}^{{\mathrm{I}}_t}{\mathbf{g}}^w+{\mathbf{n}}_a\hfill \\ \hfill & {\tilde{\mathbf{\omega }}}_t={\omega }_t+{\mathbf{b}}_{{\omega }_t}+{\mathbf{n}}_{\omega }\hfill \end{array}$$

(26)

These IMU measurements are measured in the IMU frame, and are affected by the additive noise, ${\mathbf{n}}_a$,${\mathbf{n}}_{\omega }$, the acceleration bias, ${\mathbf{b}}_{a_t}$, and the gyroscope bias, ${\mathbf{b}}_{{\omega }_t}$. Generally, in this paper we model the bias as a random walk with Gaussian derivatives, while the additive noise of the accelerometer and the gyroscope is assumed to be subject to a Gaussian distribution:

$${\mathbf{n}}_a\sim \mathcal{N}\left(\mathbf{0},{\sigma }_a^2\right),{\mathbf{n}}_{\omega }\sim \mathcal{N}\left(\mathbf{0},{\sigma }_{\omega }^2\right)$$

(27)

Over time, high-frequency IMU measurement leads to rapid growth in the number of variables in the optimization, increasing the computational cost. We therefore convert the IMU measurements between keyframes into motion constraints via the pre-integration technique, reducing the computational cost and providing a factor, namely, the IMU pre-integration factor. The parameters to be optimized include the robot’s pose and the bias of the IMU.

According to the kinematic model in [26], the pose and velocity of the current keyframe in the world frame can be obtained from the measurement of the previous frame. With the pose and velocity of a keyframe i, the pose and velocity of the current keyframe j are:

$$\begin{array}{cc}\hfill {\mathbf{P}}_{{\mathrm{I}}_j}^{\mathrm{W}}=& {\mathbf{P}}_{{\mathrm{I}}_i}^{\mathrm{W}}+{\mathbf{v}}_{{\mathrm{I}}_i}^{\mathrm{W}}\Delta t+{\int \int }_{t\in [i,j]}\left({\mathbf{R}}_{{\mathrm{I}}_t}^{\mathrm{W}}\left({\tilde{\mathbf{a}}}_t-{\mathbf{b}}_{a_t}-{\mathbf{n}}_a\right)-{\mathbf{g}}^{\mathrm{W}}\right)d{t}^2\hfill \\ \hfill {\mathbf{v}}_{{\mathrm{I}}_j}^{\mathrm{W}}=& {\mathbf{v}}_{{\mathrm{I}}_i}^{\mathrm{W}}+{\int }_{t\in [i,j]}\left({\mathbf{R}}_{{\mathrm{I}}_t}^{\mathrm{W}}\left({\tilde{\mathbf{a}}}_t-{\mathbf{b}}_{a_t}-{\mathbf{n}}_a\right)-{\mathbf{g}}^{\mathrm{W}}\right)dt\hfill \\ \hfill {\mathbf{R}}_{{\mathrm{I}}_j}^{\mathrm{W}}=& {\mathbf{R}}_{{\mathrm{I}}_i}^{\mathrm{W}}Exp\left({\int }_{t\in [i,j]}\left({\tilde{\omega }}_t-{\mathbf{b}}_{{\omega }_t}-{\mathbf{n}}_{\omega }\right)dt\right)\hfill \end{array}$$

(28)

Using Formula (28), we iterate the IMU integral between times i and j, and obtain:

$$\begin{array}{cc}\hfill & {\mathbf{I}}_{{\mathrm{I}}_j}^{\mathrm{W}}={\mathbf{P}}_{{\mathrm{I}}_i}^{\mathrm{W}}+{\mathbf{v}}_{{\mathrm{I}}_i}^{\mathrm{W}}\Delta t+\sum _{k=i}^{j-1}\left({\mathbf{v}}_{{\mathrm{I}}_k}^{\mathrm{W}}\Delta t-\frac{1}{2}{\mathbf{g}}^{\mathrm{W}}\Delta t^2+\frac{1}{2}{\mathbf{R}}_{{\mathrm{I}}_k}^{\mathrm{W}}\left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a_k}-{\mathbf{n}}_a\right)\Delta t^2\right)\hfill \\ \hfill & {\mathbf{v}}_{{\mathrm{I}}_j}^{\mathrm{W}}={\mathbf{v}}_{{\mathrm{I}}_i}^{\mathrm{W}}+{\mathbf{g}}^{\mathrm{W}}\Delta t_{ij}+\sum _{k=i}^{j-1}{\mathbf{R}}_{{\mathrm{I}}_k}^{\mathrm{W}}\left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a_k}-{\mathbf{n}}_a\right)\Delta t\hfill \\ \hfill & {\mathbf{R}}_{{\mathrm{I}}_j}^{\mathrm{W}}={\mathbf{R}}_{{\mathrm{I}}_i}^{\mathrm{W}}\prod _{k=i}^{j-1}Exp\left({\tilde{\omega }}_k-{\mathbf{b}}_{{\omega }_k}-{\mathbf{n}}_{\omega }\right)\Delta t\hfill \end{array}$$

(29)

where $\Delta t$ is the interval between two adjacent measurements, $\Delta t_{ij}={\displaystyle \sum _{k=i}^{j-1}}\Delta t$. Next, we separate the parts related to IMU measurement from the pose and velocity. The parts related to IMU measurement are as follows:

$$\begin{array}{cc}\hfill & \Delta {\tilde{\mathbf{P}}}_{I_j}^{I_i}=\sum _{k=i}^{j-1}\left[\Delta {\mathbf{V}}_k^i\Delta t+\frac{1}{2}\mathbf{R}\left({\gamma }_k^i\right)\left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a_k}\right)\Delta t^2\right]-\delta {\mathbf{P}}_{ij}\hfill \\ \hfill & \Delta {\tilde{\mathbf{V}}}_{I_j}^{I_i}=\sum _{k=i}^{j-1}{\mathbf{R}}_k^i\left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a_k}\right)\Delta t-\delta {\mathbf{V}}_{ij}\hfill \\ \hfill & \Delta {\tilde{\mathbf{R}}}_{I_j}^{I_i}=\left(\prod _{k=i}^{j-1}Exp\left(\left({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_{{\omega }_k}\right)\Delta t\right)\right)Exp\left(-\delta {\mathbf{\phi }}_{ij}\right)\hfill \end{array}$$

(30)

where $\delta {\mathbf{P}}_{ij}$, $\delta {\mathbf{V}}_{ij}$, and $\delta {\mathbf{\phi }}_{ij}$ are the measurement error of IMU, and are obtained by iterating the initial value at time i. In order to avoid repeated integration due to changes in the bias during the optimization process, we use first-order approximation to linearize the bias terms, as follows:

$$\begin{array}{c}{\Delta \tilde{\mathit{P}}}_{{\mathrm{I}}_j}^{{\mathrm{I}}_i}\approx {\Delta \tilde{\mathit{P}}}_{{\mathrm{I}}_j}^{{\mathrm{I}}_i}+{\mathbf{J}}_{b_a}^{\Delta \tilde{\mathit{P}}}\delta {\mathbf{b}}_a+{\mathbf{J}}_{b_w}^{\Delta \mathit{P}}\delta {\mathbf{b}}_w\hfill \\ {\Delta \tilde{\mathit{V}}}_{{\mathrm{I}}_j}^{{\mathrm{I}}_i}\approx {\Delta \tilde{\mathit{V}}}_{{\mathrm{I}}_j}^{{\mathrm{I}}_i}+{\mathbf{J}}_{b_a}^{\Delta \tilde{\mathit{V}}}\delta {\mathbf{b}}_a+{\mathbf{J}}_{b_w}^{\Delta V}\delta {\mathbf{b}}_w\hfill \\ {\Delta \tilde{\mathit{R}}}_{{\mathrm{I}}_j}^{{\mathrm{I}}_i}\approx {\Delta \tilde{\mathit{R}}}_{{\mathrm{I}}_j}^{{\mathrm{I}}_i}\mathrm{Exp}\left({\mathbf{J}}_{b_{\omega }}^{\Delta \tilde{\mathit{R}}}\delta {\mathbf{b}}_w\right)\hfill \end{array}$$

(31)

The items related to the state at times i and j are as follows:

$$\begin{array}{c}\Delta {\mathbf{P}}_{{\mathrm{I}}_j}^{{\mathrm{I}}_i}={\left({\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{R}}_{\mathrm{I}}^{\mathrm{B}}\right)}^{\mathrm{T}}\left({\mathbf{P}}_{{\mathrm{I}}_j}^{\mathrm{W}}-{\mathbf{P}}_{{\mathrm{I}}_i}^{\mathrm{W}}-{\mathbf{v}}_{{\mathrm{I}}_i}^{\mathrm{W}}\Delta t+\frac{1}{2}{\mathbf{g}}^{\mathrm{W}}\Delta t^2\right)\hfill \\ \Delta {\mathbf{V}}_{{\mathrm{I}}_j}^{{\mathrm{I}}_i}={\left({\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{R}}_{\mathrm{I}}^{\mathrm{B}}\right)}^{\mathrm{T}}\left({\mathbf{v}}_{{\mathrm{I}}_j}^{\mathrm{W}}-{\mathbf{v}}_{{\mathrm{I}}_i}^{\mathrm{W}}+{\mathbf{g}}^{\mathrm{W}}\Delta t\right)\hfill \\ \Delta {\mathbf{R}}_{{\mathrm{I}}_j}^{{\mathrm{I}}_i}={\left({\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{R}}_{\mathrm{I}}^{\mathrm{B}}\right)}^{\mathrm{T}}{\mathbf{R}}_{{\mathrm{B}}_j}^{\mathrm{W}}{\mathbf{R}}_{\mathrm{I}}^{\mathrm{B}}\hfill \end{array}$$

(32)

We construct the motion constraints of the IMU between two keyframes, which naturally provide the IMU pre-integration factor. This factor takes the bias of the IMU as an optimization parameter, and its residual is defined as follows:

$$\begin{array}{cc}\hfill {\mathbf{r}}_{\mathbf{P}}& =\Delta {\mathbf{P}}_{{\mathrm{I}}_j}^{{\mathrm{I}}_i}-\Delta {\tilde{\mathbf{P}}}_{{\mathrm{I}}_j}^{{\mathrm{I}}_i}\hfill \\ \hfill & ={\left({\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{R}}_{\mathrm{I}}^{\mathrm{B}}\right)}^{\mathrm{T}}\left({\mathbf{P}}_{{\mathrm{B}}_j}^{\mathrm{W}}-{\mathbf{P}}_{{\mathrm{B}}_i}^{\mathrm{W}}-{\mathbf{v}}_{{\mathrm{I}}_i}^{\mathrm{W}}\Delta t+\frac{1}{2}{\mathbf{g}}^{\mathrm{W}}\Delta t^2\right)-\sum _{k=i}^{j-1}\left[\Delta {\mathbf{V}}_k^i\Delta t+\frac{1}{2}\mathbf{R}\left({\gamma }_k^i\right)\left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a_k}\right)\Delta t^2\right]\hfill \\ \hfill {\mathbf{r}}_{\mathbf{v}}& =\Delta {\mathbf{V}}_{{\mathrm{I}}_j}^{{\mathrm{I}}_i}-\Delta {\tilde{\mathbf{V}}}_{{\mathrm{I}}_j}^{{\mathrm{I}}_i}\hfill \\ \hfill & ={\left({\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{R}}_{\mathrm{I}}^{\mathrm{B}}\right)}^{\mathrm{T}}\left({\mathbf{v}}_{{\mathrm{I}}_j}^{\mathrm{W}}-{\mathbf{v}}_{{\mathrm{I}}_i}^{\mathrm{W}}+{\mathbf{g}}^{\mathrm{W}}\Delta t\right)-\sum _{k=i}^{j-1}{\mathbf{R}}_k^i\left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a_k}\right)\Delta t\hfill \\ \hfill {\mathbf{r}}_{\mathbf{R}}& =log\left({\left(\Delta {\mathbf{R}}_{{\mathrm{I}}_j}^{{\mathrm{I}}_i}\right)}^{\mathrm{T}}\Delta {\tilde{\mathbf{R}}}_{{\mathrm{I}}_j}^{{\mathrm{I}}_i}\right)\hfill \\ \hfill & =log\left({\left({\mathbf{R}}_{{\mathrm{B}}_j}^{\mathrm{W}}{\mathbf{R}}_{\mathrm{I}}^{\mathrm{B}}\right)}^{\mathrm{T}}{\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{R}}_{\mathrm{I}}^{\mathrm{B}}\prod _{k=i}^{j-1}Exp\left(\left({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_{w_{t_i}}\right)\Delta t\right)\right)\hfill \\ \hfill {\mathbf{r}}_{{\mathbf{b}}_a}& ={\mathbf{b}}_{a_j}-{\mathbf{b}}_{a_i}\hfill \\ \hfill {\mathbf{r}}_{{\mathbf{b}}_{\omega }}& ={\mathbf{b}}_{{\omega }_j}-{\mathbf{b}}_{{\omega }_i}\hfill \end{array}$$

(33)

where ${\mathbf{R}}_{\mathrm{I}}^{\mathrm{B}}$ is the rotation between the lidar coordinate system and the robot body coordinate system, calibrated in advance.

The variance of the IMU pre-integration factor can be obtained iteratively from an initial value 0 using the method in [26].

4.2.3. Odometry Pre-Integration Factor

Generally, wheeled robots are equipped with wheel encoders that often provide noisy and only intermittently reliable measurements of the motion of each wheel. In order to reduce the amount of computation and accumulated error, by analogy to IMU we use the pre-integration technique on SE(2) to derive the constraints of the odometer. Unlike general ground mobile robots, TWIP swings back and forth in addition to plane motion, while the encoder only provides two-dimensional plane motion. In order to analyze the constraints of the encoder more conveniently, we add an auxiliary coordinate system, that is, a plane coordinate system. The coordinate system is fixed to the robot; its origin coincides with the origin of the robot body frame, the xy plane coincides with the physical plane, the z-axis is vertically upward, and the y axis coincides with the y axis of the robot body frame, as shown in Figure 6. The encoder measurements between two adjacent keyframes indicate the pose transformation between the plane coordinate systems of the corresponding moments. Therefore, we analyze the constraints provided by the encoder in the planar coordinate system.

The measurement data of the encoder can be converted into linear velocity and angular velocity on a two-dimensional plane:

$$\begin{array}{c}v=\frac{r_l{\omega }_l+r_r{\omega }_r}{2}\hfill \\ \omega =\frac{r_l{\omega }_l-r_r{\omega }_r}{l}\hfill \end{array}$$

(34)

where ${\omega }_l$ and ${\omega }_r$ are the rotational velocities of the left and right wheels, respectively, $r_l$ and $r_r$ are their corresponding radii, and l denotes the robot’s baseline.

We model the encoder’s measurement error with a Gaussian model to process the noisy odometer data. We begin by deriving the measurement model, assuming that between successive odometer readings the motion is planar; the encoder provides the following measurements in the planar coordinate system of the current keyframe:

$$\begin{array}{c}{\tilde{\mathbf{v}}}_i={\mathbf{v}}_i+{\mathbf{\epsilon }}_{vi}\hfill \\ {\tilde{w}}_i={\omega }_i+{\epsilon }_{\omega i}\hfill \end{array}$$

(35)

where ${\mathbf{v}}_i=\left[\begin{array}{c}{v}_i\\ 0\end{array}\right],{\mathbf{\epsilon }}_{vi}=\left[\begin{array}{c}{\epsilon }_{vi}\\ 0\end{array}\right]$; ${\epsilon }_{vi}$ and ${\epsilon }_{wi}$ represent zero-mean Gaussian noise.

With two consecutive lidar keyframes, i and j, we obtain the pose of the plane coordinate system corresponding to the current keyframe in the world coordinate system from the pose of the previous keyframe and the measurements of the encoder:

$$\begin{array}{c}{\varphi }_{{\pi }_j}^{\mathrm{W}}={\varphi }_{{\pi }_i}^{\mathrm{W}}+{\displaystyle \sum _{k=i}^{j-1}}\left({\tilde{w}}_k-{\epsilon }_{\omega k}\right)\Delta t\hfill \\ {\mathbf{P}}_{{\pi }_j}^{\mathrm{W}}={\mathbf{P}}_{{\pi }_i}^{\mathrm{W}}+{\displaystyle \sum _{k=i}^{j-1}}\mathrm{R}\left({\varphi }_{{\pi }_k}^{\mathrm{W}}\right)\left(\left({\tilde{\mathbf{v}}}_k-{\mathbf{\epsilon }}_{vk}\right)\Delta t\right)\hfill \end{array}$$

(36)

where ${\varphi }_{{\pi }_k}^{\mathrm{W}},k=i,j$ is the yaw angle of the plane coordinate system corresponding to keyframe k relative to the world coordinate system and ${\mathbf{P}}_{{\pi }_k}^{\mathrm{W}}=\left[\begin{array}{c}{x}_k\\ y_k\end{array}\right],k=i,j$ is the 2D position of the plane coordinate system corresponding to keyframe k relative to the world coordinate system:

$$\mathrm{R}\left(\varphi \right)=exp\left({\varphi }^{\wedge }\right)=exp\left(\left[\begin{array}{cc}0& -\varphi \\ \varphi & 0\end{array}\right]\right)=\left[\begin{array}{cc}cos\varphi & -\sin \varphi \\ sin\varphi & cos\varphi \end{array}\right]$$

(37)

Note that the position propagated here depends on the rotational state. In order to generate a measurement between two keyframes i and j without depending on the robot’s state in the world coordinate system, we place the items related to the measurement together and eliminate the items related to the state of i and j. Equation (36) can be written as:

$$\begin{array}{c}\Delta {\varphi }_{ij}={\varphi }_{{\pi }_j}^{\mathrm{W}}-{\varphi }_{{\pi }_i}^{\mathrm{W}}\hfill \\ ={\displaystyle \sum _{k=i}^{j-1}}\left({\tilde{\omega }}_i-{\epsilon }_{\omega }\right)\Delta t\hfill \\ ={\displaystyle \sum _{k=i}^{j-1}}{\tilde{\omega }}_i\Delta t-{\displaystyle \sum _{k=i}^{j-1}}{\epsilon }_{\omega k}\Delta t\hfill \\ =\Delta {\tilde{\varphi }}_{ij}-\delta {\varphi }_{ij}\hfill \end{array}$$

(38)

$$\begin{array}{c}\Delta {\mathbf{P}}_{ij}=\mathrm{R}\left(-{\varphi }_{{\pi }_i}^{\mathrm{W}}\right)\left({\mathbf{P}}_{{\pi }_j}^{\mathrm{W}}-{\mathbf{P}}_{{\pi }_i}^{\mathrm{W}}\right)\hfill \\ ={\displaystyle \sum _{k=i}^{j-1}}\mathrm{R}\left({\varphi }_{ik}\right)\left(\left({\tilde{\mathbf{v}}}_k-{\mathbf{\epsilon }}_{vk}\right)\Delta t\right)\hfill \\ \approx {\displaystyle \sum _{k=i}^{j-1}}\mathrm{R}\left({\tilde{\varphi }}_{ik}\right)\exp \left(-\delta {\varphi }_{ik}\right)\left(\left({\tilde{\mathbf{v}}}_k-{\mathbf{\epsilon }}_{vk}\right)\Delta t\right)\hfill \\ \approx {\displaystyle \sum _{k=i}^{j-1}}\mathrm{R}\left({\tilde{\varphi }}_{ik}\right)\left({\mathbf{I}}_{2\times 2}-\delta {\varphi }_{ik}^{\wedge }\right)\left(\left({\tilde{\mathbf{v}}}_k-{\mathbf{\epsilon }}_{vk}\right)\Delta t\right)\hfill \\ \approx {\displaystyle \sum _{k=i}^{j-1}}\mathrm{R}\left({\tilde{\varphi }}_{ik}\right){\tilde{\mathbf{v}}}_k\Delta t-{\displaystyle \sum _{k=i}^{j-1}}\mathrm{R}\left({\tilde{\varphi }}_{ik}\right){\mathbf{\epsilon }}_{vk}\Delta t-{\displaystyle \sum _{k=i}^{j-1}}\mathrm{R}\left({\tilde{\varphi }}_{ik}\right)1^{\wedge }{\tilde{\mathbf{v}}}_k\Delta t\delta {\varphi }_{ik}\hfill \\ =\Delta {\tilde{\mathbf{P}}}_{ij}-\delta {\mathbf{P}}_{ij}\hfill \end{array}$$

(39)

In order to obtain the error transfer model, we derive the integrated noise terms in the form of iterative transfer:

$$\begin{array}{c}\delta {\varphi }_{i,k+1}=\delta {\varphi }_{ik}+{\epsilon }_{\omega k}\Delta t\hfill \\ \delta {\mathbf{p}}_{i,k+1}=\delta {\mathbf{p}}_{ik}+\mathrm{R}\left({\tilde{\varphi }}_{ik}\right)1^{\wedge }{\tilde{\mathbf{v}}}_k\Delta t\delta {\varphi }_{ik}+\mathrm{R}\left({\tilde{\varphi }}_{ik}\right)\Delta t{\mathbf{\epsilon }}_{vk}\hfill \end{array}$$

(40)

Writing Equation (40) in a compact form, we then have:

$${\left[\begin{array}{c}\delta {\mathbf{p}}_{i,k+1}\\ \delta {\varphi }_{i,k+1}\end{array}\right]}_{3\times 1}={\mathbf{A}}_k{\left[\begin{array}{c}\delta {\mathbf{p}}_{ik}\\ \delta {\varphi }_{ik}\end{array}\right]}_{3\times 1}+{\mathbf{B}}_k{\left[\begin{array}{c}{\mathbf{\epsilon }}_{vk}\\ {\epsilon }_{\omega k}\end{array}\right]}_{3\times 1}$$

(41)

where

$$\begin{array}{c}{\mathbf{A}}_k=\left[\begin{array}{cc}{\mathbf{I}}_{2\times 2}& \mathrm{R}\left({\varphi }_{ik}\right)1^{\wedge }{\tilde{\mathbf{v}}}_k\Delta t\\ 0& 1\end{array}\right]\hfill \end{array}$$

(42)

$$\begin{array}{c}{\mathbf{B}}_k=\left[\begin{array}{cc}\mathrm{R}\left({\varphi }_{ik}\right)\Delta t& {\mathbf{0}}_{2\times 1}\\ {\mathbf{0}}_{1\times 2}& \Delta t\end{array}\right]\hfill \end{array}$$

(43)

Iterating from the initial value, ${\Sigma }_{\delta i}={\mathbf{0}}_{3\times 3}$, at the time i, we can incrementally calculate the pre-integrated terms $\Delta {\varphi }_{ij}$ and $\Delta {\mathbf{P}}_{ij}$ and their covariance matrix, ${\Sigma }_{\delta }=\left[\begin{array}{cc}{\Sigma }_{\delta {\mathbf{P}}_{ij}}& {\mathbf{0}}_{2\times 1}\\ {\mathbf{0}}_{1\times 2}& {\Sigma }_{\delta {\varphi }_{ij}}\end{array}\right]$.

Thus far, we have the encoder’s pre-integration terms, $\Delta {\tilde{\varphi }}_{ij}$ and $\Delta {\tilde{\mathbf{P}}}_{ij}$, which provide the measurements of the odometer constraints. Next, we formulate the residual of the odometer pre-integration constraint. Through the definition of the plane coordinate system, we can obtain the rotation of the plane coordinate system corresponding to the keyframes i and j relative to the world coordinate system, respectively:

$$\begin{array}{c}{\mathbf{R}}_{{\pi }_i}^{\mathrm{W}}=\left[\begin{array}{ccc}-{\mathbf{e}}_3\times {\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{e}}_2& {\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{e}}_2& {\mathbf{e}}_3\end{array}\right]\hfill \end{array}$$

(44)

$$\begin{array}{c}{\mathbf{R}}_{{\pi }_j}^{\mathrm{W}}=\left[\begin{array}{ccc}-{\mathbf{e}}_3\times {\mathbf{R}}_{{\mathrm{B}}_j}^{\mathrm{W}}{\mathbf{e}}_2& {\mathbf{R}}_{{\mathrm{B}}_j}^{\mathrm{W}}{\mathbf{e}}_2& {\mathbf{e}}_3\end{array}\right]\hfill \end{array}$$

(45)

where ${\mathbf{e}}_i$ is the $ith$ column vector of the identity matrix, ${\mathbf{I}}_{3\times 3}$.

Then, the relative transformation of the plane coordinate system corresponding to keyframes i and j is as follows:

$$\begin{array}{c}{\mathbf{R}}_{{\pi }_i}^{{\pi }_j}={\left({\mathbf{R}}_{{\pi }_j}^{\mathrm{W}}\right)}^{\mathrm{T}}{\mathbf{R}}_{{\pi }_i}^{\mathrm{W}}\hfill \\ =\left[\begin{array}{c}{\left(-{\mathbf{e}}_3\times {\mathbf{R}}_{{\mathrm{B}}_j}^{\mathrm{W}}{\mathbf{e}}_2\right)}^{\mathrm{T}}\\ {\left({\mathbf{R}}_{{\mathrm{B}}_j}^{\mathrm{W}}{\mathbf{e}}_2\right)}^{\mathrm{T}}\\ {\mathbf{e}}_3^{\mathrm{T}}\end{array}\right]\left[\begin{array}{ccc}-{\mathbf{e}}_3\times {\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{e}}_2& {\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{e}}_2& {\mathbf{e}}_3\end{array}\right]\hfill \\ =\left[\begin{array}{ccc}-{\mathbf{e}}_2^{\mathrm{T}}{\left({\mathbf{R}}_{{\mathrm{B}}_j}^{\mathrm{W}}\right)}^{\mathrm{T}}{\mathbf{e}}_3^{\wedge }{\mathbf{e}}_3^{\wedge }{\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{e}}_2& {\mathbf{e}}_2^{\mathrm{T}}{\left({\mathbf{R}}_{{\mathrm{B}}_j}^{\mathrm{W}}\right)}^{\mathrm{T}}{\mathbf{e}}_3^{\wedge }{\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{e}}_2& 0\\ -{\mathbf{e}}_2^{\mathrm{T}}{\left({\mathbf{R}}_{{\mathrm{B}}_j}^{\mathrm{W}}\right)}^{\mathrm{T}}{\mathbf{e}}_3^{\wedge }{\mathbf{R}}_i^{\mathrm{W}}{\mathbf{e}}_2& {\mathbf{e}}_2^{\mathrm{T}}{\left({\mathbf{R}}_{{\mathrm{B}}_j}^{\mathrm{W}}\right)}^{\mathrm{T}}{\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{e}}_2& 0\\ 0& 0& 1\end{array}\right]\hfill \end{array}$$

(46)

As can be seen from Appendix A and Appendix B, the measurement of the rotating part of the encoder $\Delta {\tilde{\varphi }}_{ij}$ provides only one constraint, and the residual of the rotation part is:

$${\mathbf{r}}_{\mathbf{R}}={\mathbf{e}}_2^{\mathrm{T}}{\left({\mathbf{R}}_{{\mathrm{B}}_j}^{\mathrm{W}}\right)}^{\mathrm{T}}{\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{e}}_2-\cos \left(\Delta {\tilde{\varphi }}_{ij}\right)$$

(47)

We obtain the variance of the rotation residuals using a first-order linear approximation of the trigonometric functions:

$$\begin{array}{c}cos\varphi \approx cos\left(\Delta {\varphi }_{ij}\right)-sin\left(\Delta {\varphi }_{ij}\right)\left(\varphi -\Delta {\varphi }_{ij}\right)\hfill \end{array}$$

(48)

$$\begin{array}{c}{\Sigma }_{{\mathbf{r}}_{\mathbf{R}}}={sin}^2\left(\Delta {\varphi }_{ij}\right){\Sigma }_{\delta {\varphi }_{ij}}\hfill \end{array}$$

(49)

Then, its Jacobian matrix relative to the TWIP motion state (the pose of the robot body frame relative to the world frame) is

$$\begin{array}{c}{\mathbf{J}}_{{\varphi }_i}^{{\mathbf{r}}_{\mathbf{R}}}=-{\mathbf{e}}_2^{\mathrm{T}}{\left({\mathbf{R}}_{{\mathrm{B}}_j}^{\mathrm{W}}\right)}^{\mathrm{T}}{\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{e}}_2^{\wedge }\hfill \end{array}$$

(50)

$$\begin{array}{c}{\mathbf{J}}_{{\varphi }_j}^{{\mathbf{r}}_{\mathbf{R}}}={\mathbf{e}}_2^{\mathrm{T}}{\left({\left({\mathbf{R}}_{{\mathrm{B}}_j}^{\mathrm{W}}\right)}^{\mathrm{T}}{\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{e}}_2\right)}^{\wedge }\hfill \end{array}$$

(51)

and the residual of the translation part is

$$\begin{array}{c}{\mathbf{r}}_{\mathbf{P}}={\left[{\left({\mathbf{R}}_{{\pi }_i}^{\mathrm{W}}\right)}^{\mathrm{T}}\left({\mathbf{P}}_{B_j}^{\mathrm{W}}-{\mathbf{P}}_{B_i}^{\mathrm{W}}\right)\right]}_{xy}-\Delta {\tilde{\mathbf{P}}}_{ij}\hfill \\ ={\left[\begin{array}{c}{\left(-{\mathbf{e}}_3\times {\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{e}}_2\right)}^{\mathrm{T}}\left({\mathbf{P}}_{B_j}^{\mathrm{W}}-{\mathbf{P}}_{B_i}^{\mathrm{W}}\right)\\ {\left({\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{e}}_2\right)}^{\mathrm{T}}\left({\mathbf{P}}_{B_j}^{\mathrm{W}}-{\mathbf{P}}_{B_i}^{\mathrm{W}}\right)\\ 0\end{array}\right]}_{xy}-\Delta {\tilde{\mathbf{P}}}_{ij}\hfill \end{array}$$

(52)

where ${\left[\right]}_{xy}$ represents the first two rows of the vector, and its covariance is

$${\Sigma }_{{\mathbf{r}}_{\mathbf{P}}}={\Sigma }_{\delta {\mathbf{P}}_{ij}}$$

(53)

Then, its Jacobian matrix relative to the motion state of TWIP is

$$\begin{array}{c}{\mathbf{J}}_{{\varphi }_i}^{{\mathbf{r}}_{\mathbf{P}}}=\left[\begin{array}{c}{\mathbf{e}}_2^{\mathrm{T}}{\left({\left({\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}\right)}^{\mathrm{T}}{\mathbf{e}}_3^{\wedge }\left({\mathbf{P}}_{B_j}^w-{\mathbf{P}}_{B_i}^w\right)\right)}^{\wedge }\\ {\mathbf{e}}_2^{\mathrm{T}}{\left({\left({\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}\right)}^{\mathrm{T}}\left({\mathbf{P}}_{B_j}^w-{\mathbf{P}}_{B_i}^w\right)\right)}^{\wedge }\end{array}\right]\hfill \end{array}$$

(54)

$$\begin{array}{c}{\mathbf{J}}_{{\mathbf{P}}_{{\mathrm{B}}_i}^{\mathrm{W}}}^{{\mathbf{r}}_{\mathbf{P}}}=\left[\begin{array}{c}-{\mathbf{e}}_2^{\mathrm{T}}{\left({\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}\right)}^{\mathrm{T}}{\mathbf{e}}_3^{\wedge }\\ -{\mathbf{e}}_2^{\mathrm{T}}{\left({\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}\right)}^{\mathrm{T}}\end{array}\right]\hfill \end{array}$$

(55)

$$\begin{array}{c}{\mathbf{J}}_{{\mathbf{P}}_{{\mathrm{B}}_j}^{\mathrm{W}}}^{{\mathbf{r}}_{\mathbf{P}}}=\left[\begin{array}{c}{\mathbf{e}}_2^{\mathrm{T}}{\left({\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}\right)}^{\mathrm{T}}{\mathbf{e}}_3^{\wedge }\\ {\mathbf{e}}_2^{\mathrm{T}}{\left({\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}\right)}^{\mathrm{T}}\end{array}\right]\hfill \end{array}$$

(56)

4.2.4. Ground Constraint Factor

This paper proposes a new nonholonomic constraint factor in order to add ground constraints. We assume that TWIP moves on an approximately flat surface and that the robot is always constrained by the ground plane during movement. Ideally, the z and pitch angle would remain constant during the planar motion for a two-wheeled inverted pendulum robot. In the natural environment, however, there are motion disturbances in the other dimensions of the robot’s pose due to rough terrain and shaking in the robot’s motion. In order to reduce the error caused by this motion disturbance, we use a random model to introduce ground constraints and assume that the motion disturbance is a zero-mean Gaussian model. This is more in line with the natural environment, allowing a least squares to be used to model the ground constraints, which naturally adds the ground constraints as a new factor to the factor graph. As shown in Figure 7, when TWIP moves on an ideal plane the roll angle and z of its body frame relative to the world frame should both be zero. Therefore, the residual of the ground constraint is defined as follows:

$$\begin{array}{c}{r}_{\mathbf{R}}={\mathbf{e}}_3^{\mathrm{T}}{\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{e}}_2\hfill \end{array}$$

(57)

$$\begin{array}{c}{r}_{\mathbf{P}}={\mathbf{e}}_3^{\mathrm{T}}·{\mathbf{P}}_{{\mathrm{B}}_i}^{\mathrm{W}}\hfill \end{array}$$

(58)

The corresponding covariance matrix is

$${\Sigma }_{{\mathbf{r}}_p}=diag\left(\begin{array}{cc}{\sigma }_r^2& {\sigma }_z^2\end{array}\right)$$

(59)

In this paper, we assume that the motion disturbance of the robot due to rough terrains or motion shaking obeys the Gaussian distribution; ${\sigma }_z$ and ${\sigma }_r$ are the noise sigma of the z and roll, respectively, which are determined by the conditions of the terrain and the robot’s structure.

The Jacobian matrix relative to the state of TWIP is

$$\begin{array}{c}{\mathbf{J}}_{{\varphi }_i}^{{\mathbf{r}}_{\mathbf{R}}}=-{\mathbf{e}}_3^{\mathrm{T}}{\mathbf{R}}_{{\mathrm{B}}_i}^{\mathrm{W}}{\mathbf{e}}_2^{\wedge }\hfill \end{array}$$

(60)

$$\begin{array}{c}{\mathbf{J}}_{{\mathbf{P}}_{{\mathrm{B}}_i}^{\mathrm{W}}}^{{\mathbf{r}}_{\mathbf{P}}}={\mathbf{e}}_3^{\mathrm{T}}\hfill \end{array}$$

(61)

4.2.5. Loop Closure Factor

When a new lidar keyframe, ${\mathrm{F}}_{i+1}$, arrives, its corresponding robot state, ${\mathit{K}}_{i+1}$, is added to the factor graph. We obtain the absolute pose of the current lidar keyframe as in Section 4.2.1. We obtain the absolute pose of the current lidar keyframe as in Section 4.2.1, and use this pose as a prior to find the previous keyframe closest to ${\mathrm{F}}_{i+1}$ in Euclidean space as a candidate frame. For example, the lidar keyframe ${\mathrm{F}}_3$ in Figure 3 is one of the returned loop closure candidates. Then, taking the position of the lidar keyframe ${\mathrm{F}}_3$ as the center, we extract the point cloud in the cuboid with a length of 10 m and a height of 5 m in the global map and transform the point cloud into the robot body coordinate system corresponding to the lidar keyframe ${\mathrm{F}}_3$ to construct a local map ${\mathrm{M}}_{i+1}$. We try to match ${\mathrm{F}}_{i+1}$ to the local map ${\mathrm{M}}_{i+1}$ using the scan-matching method discussed in Section 4.2.1. In the local map, we select the N nearest feature points for each feature point of the current frame to fit a line or plane. After optimization, we obtain the relative transformation, $\Delta {\tilde{T}}_3^{i+1}$, and add it to the graph as a loop closure factor. Throughout this paper, we choose to set the loop closure’s search distance to 15 m from the new lidar keyframe.

5. Experiment

We conducted experiments on our collected datasets, as we could not find an open dataset to provide calibrated Lidar–IMU–Odometry for a two-wheeled inverted pendulum robot. All experiments were carried out with an Intel CPU i7-8559U (8 [email protected] GHz) laptop computer with 8 GB RAM. As shown in Figure 8, the platform was equipped with a Velodyne VLP-16 lidar oprating at 10 HZ, an XSENS MTI-G-710 IMU to provide acceleration and angular velocity at 200 HZ, and an encoder with 1khz, all of which were synchronized and calibrated in advance.

The dataset comes from two scenes; one is a long straight corridor scene, named Dataset Corridor, and the other is a flat outdoor road, named Dataset Outdoor. The environment of the two datasets is shown in Figure 9. As with the process of data collection, the actual ground truth of the robot is unavailable. Inspired by [25], we equipped TWIP with downward-viewing cameras on the front and rear (their position is shown in Figure 7) and an attitude sensor with a dynamic accuracy of $0.2^{\circ }$. Tens of Aruco code markers were laid out on the floor, and their locations in a predefined world reference were measured with a location error of less than 1 cm. Transformations between industrial cameras and attitude sensors and the robot were all pre-calibrated. The two industrial cameras detected and recorded these marks during the robot’s movement. In order not to affect the algorithm in this paper as well as to improve the true accuracy, the trajectory truth was obtained offline. We extracted the marked corner points in each recorded image and approximated the trajectory truth by optimizing the reprojection error of the image and the pose constraints of the pose sensor in an offline batch. In order to verify the algorithm used in this article, we tested the constructed SLAM algorithm and the state-of-the-art lidar slam method (Fast_lio2 [16], Lio_sam [18], Loam [7]) on the same dataset.

5.1. Experimental Results and Analysis of Indoor Corridors

Figure 10 shows the results of our algorithm for localization and mapping with Dataset Corridor. Figure 11 compares trajectories in the $xy$ direction recovered by our algorithm, Fast_lio2, Lio_sam, Loam, Odometry, and ground truth on the Long Straight Corridor dataset. The odometer is a trajectory obtained using the less accurate odometer raw data calculated directly from the encoder to emphasize the correction effect of the lidar method on the odometer’s accumulated error. Meanwhile, to compare the long-term positioning accuracy of the algorithms, all algorithms did not add a loop detection module, and the pose estimation of all algorithms was performed in the robot body frame. Figure 12 and Figure 13 show the corresponding trajectory errors relative to the ground truth, respectively. It can be seen from Figure 12 that the x-direction is the running direction of TWIP, which has fewer constraints, and these algorithms have more significant errors in the x-direction. Among them, the accuracy of loam in the x-direction is significantly lower than the other algorithms due to the lack of measurements from other sensors. There is a constraint from the wall in the y-direction; all the lidar-based SLAM algorithms have little difference in accuracy, as they all use the same matching method. While the results by our method exhibit much better accuracy, there is a smaller upper limit of errors in the x and y directions. Attention should be paid to the z-axis values in Figure 14, where the z-coordinates estimated by our method are well-bounded around zero (which is consistent with natural indoor environments) while the other algorithms have significantly larger upper bounds. Using a nonholonomic constraint factor and adding planar constraints and encoder pre-integration constraints improves indoor pose estimation accuracy, especially in the z-direction. The numerical results of the estimation errors are presented in

Table 2. The estimation errors (RMSE) of different methods on Dataset Corridor.

Dimension	Fast_lio2	Lio_sam	Loam	Odom	Ours
$x\left(m\right)$	0.4680	0.4111	0.9218	1.5468	0.1747
$y\left(m\right)$	0.2636	0.4695	0.5550	1.1035	0.2921
$pitch\left(rad\right)$	0.0287	0.0420	0.0590	−	0.0176
$yaw\left(rad\right)$	0.1740	0.1810	0.3170	0.6010	0.0970

. The root mean square of the errors (RMSE) is used here, including those of the errors in principal parameter directions, namely, the x, y, pitch, and yaw coordinates. In terms of RMSE, the error of our algorithm is lower than the other algorithms in all directions for a two-wheeled inverted pendulum robot.

5.2. Experimental Results and Analysis of Outdoor Structured Environment

In our outdoor experiments, we used algorithms such as lio_sam for comparison with our algorithm. Figure 15 shows our algorithm’s trajectory and environment map on Dataset Outdoor. Figure 16 compares estimated trajectories in the x and y directions on Dataset Outdoor, including our algorithm, fast_lio2, lio_sam, loam, odometry, and ground truth. Figure 17 and Figure 18 are the corresponding errors relative to the ground truth, respectively. Figure 19 shows the trajectory in the z-direction, which should be constrained around zero. As can be seen from the results, our algorithm continues to outperform the other algorithms in outdoor scenes.

Moreover, the accuracy improvement of our algorithm is more significant in the z-axis direction than in the x- and y-axis directions. There is no degradation scene in the outdoor experiment. The lidar feature points mainly come from the reflection of surrounding objects, which constrains the robot’s pose in the horizontal direction. In contrast, lidar provides relatively few feature points in the vertical direction due to the limitation of the vertical field of view. Our algorithm introduces ground constraints, which increase the pose constraints of the robot in the vertical direction and improve the localization accuracy of the system, especially in the z-direction. Meanwhile, we quantitatively analyzed the performance of our algorithm and other algorithms in the direction of the principal parameters on the outdoor dataset in terms of RMSE. The results are shown in

Table 3. The estimation errors statistics (RMSE) of different methods on Dataset Outdoor.

Dimension	Fast_lio2	Lio_sam	Loam	Odom	Ours
$x\left(m\right)$	0.3113	0.3486	0.4770	1.6786	0.0635
$y\left(m\right)$	0.4549	0.4213	0.4999	1.2751	0.1625
$pitch\left(rad\right)$	0.0220	0.0281	0.0282	−	0.0172
$yaw\left(rad\right)$	0.2990	0.4900	0.6170	0.9368	0.1803

, indicating that the error of our algorithm in the direction of principal parameters is lower than that of the other algorithms.

Our indoor and outdoor experiments show that the non-full constraint factor proposed by our algorithm naturally introduces the pre-integration constraint of the encoder while adding a ground constraint in the form of the random constraint model, which is more in line with the actual motion of the two-wheeled inverted pendulum robot, resulting in the improved positioning and mapping accuracy of the system.

6. Conclusions

This paper proposes a novel Lidar–IMU–Odometer coupled framework for localization and mapping for a two-wheeled inverted pendulum robot on approximately flat ground. We formulate a factor graph incorporating relative and absolute measurements from different sensors (including ground constraints) into the system as factors. In this coupled framework using a lidar–IMU–encoder system based on a factor graph, we introduce five types of factors and analyze the constraint dimension of each type of factor on the robot state node according to the motion characteristics of TWIP. For the non-fully constrained factors (the encoder pre-integration factor and ground constraint factor), we propose a new factor with constraints in order to add them naturally to the factor graph with the robot state node on SE(3). Experiments in indoor and outdoor environments validate the superior performance of our system compared to other state-of-the-art lidar-based SLAM algorithms.

This proposal paves the way for using constrained models to consider motion perturbations for specially-structured ground robot state estimation with different sensors. In the future, based on the work inof this paper we intend to realize the positioning and mapping of TWIP on undulating roads via the proposed LiDAR–IMU–Encoder coupling framework