Novel indoor positioning algorithm based on Lidar/inertial measurement unit integrated system

Abstract

As an important research field of mobile robot, simultaneous localization and mapping technology is the core technology to realize intelligent autonomous mobile robot. Aiming at the problems of low positioning accuracy of Lidar (light detection and ranging) simultaneous localization and mapping with nonlinear and non-Gaussian noise characteristics, this article presents a mobile robot simultaneous localization and mapping method that combines Lidar and inertial measurement unit to set up a multi-sensor integrated system and uses a rank Kalman filtering to estimate the robot motion trajectory through inertial measurement unit and Lidar observations. Rank Kalman filtering is similar to the Gaussian deterministic point sampling filtering algorithm in structure, but it does not need to meet the assumptions of Gaussian distribution. It completely calculates the sampling points and the sampling points weights based on the correlation principle of rank statistics. It is suitable for nonlinear and non-Gaussian systems. With multiple experimental tests of small-scale arc trajectories, we can see that compared with the alone Lidar simultaneous localization and mapping algorithm, the new algorithm reduces the mean error of the indoor mobile robot in the X direction from 0.0928 m to 0.0451 m, with an improved accuracy rate of 46.39%, and the mean error in the Y direction from 0.0772 m to 0.0405 m, which improves the accuracy rate of 48.40%. Compared with the extended Kalman filter fusion algorithm, the new algorithm reduces the mean error of the indoor mobile robot in the X direction from 0.0597 m to 0.0451 m, with an improved accuracy rate of 24.46%, and the mean error in the Y direction from 0.0537 m to 0.0405 m, which improves the accuracy rate of 24.58%. Finally, we also tested on a large-scale rectangular trajectory, compared with the extended Kalman filter algorithm, rank Kalman filtering improves the accuracy of 23.84% and 25.26% in the X and Y directions, respectively, it is verified that the accuracy of the algorithm proposed in this article has been improved.

Introduction

Nowadays, navigation and positioning have become an indispensable part of people’s lives and also have the various applications in the field of unmanned system navigation and positioning, such as indoor autonomous sweeping robot, driverless cars, and so on. These products are constantly developing and expanding and have brought a profound impact on the technological progress of the entire society. Simultaneous localization and mapping (SLAM) is a research focus in the field of unmanned system navigation and positioning. In recent years, many investigators have conducted in-depth research on SLAM technology. ^1,2 The most important significance of SLAM technology is that it not only solves the problem of autonomous positioning of mobile robot in unmanned systems but also enables mobile robot to build the environmental maps based on their own positioning in an unknown environments and then to further modify the positioning information according to the environment maps built, so that the positioning effect and the mapping effect can be improved. SLAM technology is an advantageous and substitutional module in the case of indoor mapping and outdoor Global Navigation Satellite System failure. Compared with indoor pedestrian dead reckoning positioning, Wi-Fi positioning, fingerprint library recognition positioning, and other positioning methods, ^{3 –5} SLAM technology has high automation and real time, which can perform mapping and positioning of indoor and outdoor environment in real time.

Smith and Cheesman ⁶ first proposed SLAM problem, combining both positioning and mapping around with a robot. In 1987, they presented a SLAM algorithm based on Kalman filtering (KF), but this state estimation method is only suitable for linear systems obeying Gaussian distribution of the noise. Generally speaking, the state estimation problem of nonlinear and non-Gaussian phenomena are universal, and the extended Kalman filter (EKF) is the earliest proposed state processing algorithm for nonlinear systems and Gaussian noise assumptions. Subsequently, Liu et al. proposed a central difference Kalman filter, which solved the problem of low accuracy of multi-source data fusion in SLAM applications. ⁷ Martinez-Cantin and Castellanos proposed an unscented Kalman filter ⁸ to effectively solve the problem of filter divergence caused by large nonlinear characteristics of the system.

In addition, there are quadrature Kalman filter ⁹ and cubature Kalman filter ¹⁰ that are used to solve the state estimation problem of nonlinear systems. In the algorithm mentioned above, the sampling points are essentially extracted from the prior distribution of the state, and then the sampling points are iterated linearly or nonlinearly. Finally, fusion is performed to obtain an accurate state estimation.

Rank Kalman filter (RKF) does not need to make Gaussian condition assumptions on the system noise in the calculation process and calculates the sampling points and the sampling points weights based on the correlation principle of rank statistics. ^11,12 Because of this characteristic, it is widely used in nonlinear and non-Gaussian noise systems. Therefore, compared with other filtering algorithms, RKF algorithm has a wider application range. ¹³

Although Lidar can provide high-precision position and heading results, but it is easily affected by the external environment and then the positioning accuracy is greatly reduced in nonlinear and non-Gaussian noise. Compared with Lidar positioning technology, inertial navigation technology is not affected by the external environment and has the ability of independent navigation. The 100 Hz high-frequency outputs of IMU device can provide the short-term high accurate navigation parameters (position, velocity, and attitude). However, the navigation and positioning errors of IMU will gradually accumulate as time goes, so the inertial navigation needs other sensors aid to obtain high-precision navigation results. Visual information may be used in a navigation and positioning, but it has the limitation of being easily affected by the environment. In the case of violent motion and dark area, blurred images, insufficient image feature extraction, and obscure textures problems will occur.

Since the performance limitations of a single sensor, multi-sensor information fusion has become the current mainstream method of navigation and positioning. Combining several different navigation systems together can make use of multiple information sources to complement each other for the purpose of forming a multidimensional, multifunctional, and high-accuracy navigation system. At present, the mainstream integration methods are Lidar/IMU, ^14,15 Lidar/Visual, ¹⁶ Visual/IMU, ¹⁷ Lidar/Visual/IMU, ¹⁸ and so on.

In this article, an RKF algorithm in the framework of loosely integrated Lidar/IMU is used to estimate the position and attitude information of the mobile robot, and it improved indoor mobile robot positioning accuracy and increased the system robustness.

SLAM problem and mathematical description

The problem of SLAM of a mobile robot can be summarized as follows: In an unknown environment, the robot calculates the current position and attitude based on the sensor data according to the needs of the position estimation during the movement process and simultaneously builds and updates the environment maps. The essence of this problem is to use the data obtained by the sensors and the known environmental information to estimate the posture of the mobile robot and describe the environmental information. Therefore, the SLAM problem can be described as shown in Figure 1.

Figure 1.

SLAM problem description. SLAM: simultaneous localization and mapping.

Generally speaking, the sensors collect data within a fixed interval, when calculating the posture of the mobile robot and updating the map, the motion of the robot in a continuous period of time can be discreated into the posture and the map at time t = 1,…K. During the two-dimensional motion, the position and orientation of the mobile robot can be represented by a three-dimensional vector $X_k={\left(x_k,y_k,{\theta }_k\right)}^T$ with $x_k,y_k$ as the plane coordinates and θ_k as the orientations of the segments of the trajectory line. The map is composed of multiple features. If there are N feature points, the map can be described as $y=\left({\mathit{y}}_0,{\mathit{y}}_1,\dots ,{\mathit{y}}_N\right)$ . The observed feature at the posture vector j is y _i . In general, the mobile robot is equipped with sensors to measure its own motion information, and the motion process can be expressed by the following equation

$X_t=f\left(X_{t-1},u_t,W_t\right)$

1

Among them, u _t is the data (the position, velocity, and angular rate of the robot itself) of the sensors (coded wheels or inertial sensors) at time t and W _t is the system noise. This equation is also called the motion equation (state-transition), which describes the movement of the robot from t − 1 to t. The observation equation $z_{t,j}$ describes the measurement collected when the sensors on the robot measure the feature y _j at the position x _t . The measurement process is expressed by equations

$z_{t,j}=h\left(y_j,x_t,v_{t,j}\right)$

2

Among them, $v_{t,j}$ is the noise observed at time t.

RKF-based multi-sensor fusion algorithm

Multi-sensor fusion algorithm

The state equation is established through the error model of strapdown inertial navigation system, and the observation equation is established through posture information calculated by the separate Lidar and IMU algorithms. Then the position, velocity, attitude, and inertial device errors of the integrated navigation system are estimated. The method is shown in Figure 2:

Figure 2.

Structure diagram of multi-sensor fusion algorithm.

RKF design based on Lidar/IMU

RKF algorithm is used as a fusion model to process data of both IMU and Lidar to reduce the nonlinear and non-Gaussian effects of the system, and IMU in the method is used to correct the posture error caused by the alone Lidar scan matching. As a result, the final posture information is more accurate. First, a nonlinear system model is introduced as follows.

Nonlinear system model

The navigation coordinate system selected here is the geographic coordinate system of East, North, and Up. The robot error angle model of strapdown inertial navigation system ^{19 –21} is

$\dot{\Phi }=C_{\omega }^{-1}\left(I-C_t^{t^{\prime }}\right){\widehat{\omega }}_{it}^t+C_t^{t^{\prime }}\delta {\omega }_{it}^t-C_b^{t^{\prime }}\delta {\omega }_{ib}^b$

3

Velocity error model

$$\begin{gathered} \delta \dot{V}=\left[I-{\left(C_t^{t^{\prime }}\right)}^T\right]C_b^{t^{\prime }}{\widehat{f}}^b+{\left(C_t^{t^{\prime }}\right)}^T{C}_b^t\delta f^b-\left(2{\widehat{\omega }}_{ie}^t+{\widehat{\omega }}_{et}^t\right) \\ \times \delta V-\left(2\delta {\omega }_{ie}^t+\delta {\omega }_{et}^t\right)\times V+\delta g \end{gathered}$$

4

Position error model

$\left\{\begin{array}{c}\delta \dot{B}=\frac{\delta v_y}{R_m+h}-\delta h\frac{v_y}{{\left(R_m+h\right)}^2}\hfill \\ \delta \dot{\lambda }=\frac{\delta v_x\text{ sec }B}{R_n+h}+\delta B\frac{v_x\text{tan }B\text{ sec }B}{R_m+h}-\delta h\frac{v_x\text{ sec }B}{{\left(R_n+h\right)}^2}\\ \delta \dot{h}=\delta v_z\hfill \end{array}\right.$

5

where $C_{\omega }^{-1}$ describes the relationship between the angular rate and the robot error angle (refer to ²¹ ); $C_t^{t^{\prime }}$ is the transformation matrix from the geographic coordinates to the calculation coordinates; $C_b^t$ is the attitude matrix; $C_b^{t^{\prime }}$ is the transformation matrix from the IMU coordinates to the calculation coordinates; ${\omega }_{ie}^t$ the earth rotation rate; ${\widehat{\omega }}_{ie}^t={\omega }_{ie}^t+\delta {\omega }_{ie}^t$ is the value of ${\omega }_{ie}^t$ in the calculation coordinates; ${\omega }_{et}^t$ is the angular rate vector generated by the robot motion; ${\widehat{\omega }}_{et}^t={\omega }_{et}^t+\delta {\omega }_{et}^t$ is the value of ${\omega }_{et}^t$ in the calculating coordinates; ${\widehat{\omega }}_{it}^t$ is the value of the sum ${\omega }_{ie}^t$ and ${\omega }_{et}^t$ in the calculating geographic coordinates; $\delta {\omega }_{it}^t,\delta {\omega }_{ie}^t,\delta {\omega }_{et}^t$ are the calculation error of ${\widehat{\omega }}_{it}^t,{\widehat{\omega }}_{ie}^t,{\widehat{\omega }}_{et}^t$ ; ${\widehat{f}}^b=f^b+\delta f^b$ is the specific force; $\delta f^b$ is the accelerometer error; $\delta g$ is the gravity error; B is the latitude; λ is the longitude; h is the height; R_m is the radius of the meridian, and R_n is the radius of the prime vertical circle.

Nonlinear system model state equation and observation equation

Selecting a 15-dimensional state vector, the state equation is as follows

$X_k=f\left(X_{k-1}\right)+w_k$

6

where $X_k={\left[\begin{array}{ccccc}\delta {\Phi }_k& \delta {\omega }_k& \delta L_k& \delta V_k& \delta a_k\end{array}\right]}^T$ is the state vector; $\delta {\Phi }_k$ = $\left[{\varphi }_x{\varphi }_y{\varphi }_z\right]$ represents the east, north, and azimuth misalignment angle at the time k; $\delta {\omega }_k=\left[{\omega }_x{\omega }_y{\omega }_z\right]$ is three-axis gyroscope constant drift error at the time k; $\delta L_k$ = $\left[\delta x\delta y\delta z\right]$ is the position error at the time k; $\delta V_k=\left[\delta v_x\delta v_y\delta v_z\right]$ is the velocity error at the time k; $\delta a_k$ = $\left[\delta a_x\delta a_y\delta a_z\right]$ is the constant error of the accelerometer at the time k; Taking the velocity difference and position difference calculated by the alone IMU and Lidar algorithms as the measurements, the observation equation is as follows

$Z_k=H{X}_k+{\nu }_k$

7

where the measurement vector is $Z_k={\left[\begin{array}{c}\delta v_x^{DN} \delta v_y^{DN} \delta v_z^{DN}\delta L^{DN} \delta {\lambda }^{DN} \delta h^{DN}\end{array}\right]}^T$ at the time k, the observation equation coefficient matrix is $H=\left[\begin{array}{ccc}{0}_{6\times 3}& I_{6\times 6}& 0_{6\times 6}\end{array}\right]$ , and ${\nu }_k$ is the observation equation noise vector. DN represents the velocity and position difference between IMU and Lidar solutions.

Calculation process of RKF

This section will introduce the RKF the calculation process.

System model analysis

Assume the following nonlinear discrete system

$\{\begin{array}{c}{X}_k=f\left(X_{k-1}\right)+w_k\\ Z_k=h\left(X_k\right)+v_k\end{array},k=0,1,\cdots ,N-1$

8

where $X_k$ represents the state vector of the system at time k; $Z_k$ is the measurement vector of the system at time k; $f\left(·\right)$ is the nonlinear equation with the state vector; $h\left(·\right)$ is the nonlinear observation equation with an measurement vector; $w_k\text{ and }{v}_k$ are the system noise vector at time k and the measurement noise vector at time k, respectively.

Principle of rank sampling

RKF algorithm calculates the sampling points and their weights according to the principle of the rank statistic correlation, therefore, there is no requirement for the system to follow the Gaussian distribution assumption. ^12,13 By incorporating the idea of rank statistics into the filtering algorithm that determines the sampling type, and then the quantile is used as its sampling point to construct the sets of sampling points corresponding to the different distributions, that is,

$\text{{}{\mathit{\text{χ}}}_{k-1}^i\text{}}:\begin{array}{c}{\mathit{\text{χ}}}_{k-1}^i={\widehat{X}}_{k-1}+r_j{\lambda }_{p_j}{\left(\sqrt{P_{k-1}}\right)}_l,l=1,2,\cdots ,\\ \{\begin{array}{c}i=\left(j-1\right)n+l,j=1,2,\cdots ,\rho \\ i=\left(j-2\right)n+l,j=\rho +2,\rho +3,\cdots ,2\rho +1\end{array}\end{array}$

9

In the formula, ${\mathit{\text{χ}}}_{k-1}^i\left(1⩽i⩽2\rho n\right)$ represents the sampling point i of $X_{k-1}$ ; n is the dimension of the state vector; $\rho$ is the total number of layers sampled; 2 $\rho$ n is the total number of sampling points; $P_{k-1}$ is the covariance matrix of the state; ${\lambda }_{p_j}$ represents the quantile point of the one-dimensional Gaussian distribution probability p_j of the state estimation error $\epsilon$ . p_j is the probability corresponding to the sampling point of the j layer, and the general value range is $\left(j-0.3\right)/\left(2n+1.4\right)$ or $j/\left(2n+2\right)$ ; r_j is the correction coefficient corresponding to the sampling point of the j layer, which satisfies to the following formula

${\displaystyle \sum }_{j\hspace{0.17em}=\hspace{0.17em}1,\hspace{0.17em}j\hspace{0.17em}\ne \hspace{0.17em}m+1}^{2\rho +1}{r}_j{\lambda }_{p_j}=0$

10

RKF calculation process

The following are the specific steps of RKF algorithm:

First step: time update process

1. According to the rank sampling mechanism shown in section “Principle of rank sampling,” the sampling point set $\text{{}{\mathbf{\text{χ}}}_{k-1|k-1}^i\text{} }$ is obtained

$\begin{array}{c}{\mathbf{\text{χ}}}_{k-1|k-1}^i={\widehat{X}}_{k-1|k-1}+r_j{\lambda }_{p_j}{\left(\sqrt{P_{k-1|k-1}}\right)}_l,l=1,2,\cdots \\ \{\begin{array}{c}i=\left(j-1\right)n+l,j=1,2,\cdots ,\rho \\ i=\left(j-2\right)n+l,j=\rho +2,\rho +3,\cdots ,2\rho +1\end{array}\end{array}$

11

• (2) Calculate the set of sampling points iterated through the non-linear state equation, that is, $\text{{}{X}^{\text{*}}{}_{k|k-1}^i\text{}}$

$X^{*}{}_{k|k-1}^i=f\left({\mathbf{\text{χ}}}_{k-1|k-1}^i,w_{k-1}\right)i=1,2,\dots ,2\rho n$

12

• (3) Forecast the state

${\widehat{X}}_{k|k-1}=\frac{1}{2\rho n}{\displaystyle \sum }_{i=1}^{2\rho n}{X}^{*}{}_{k|k-1}^i$

13

• (4) Calculate the covariance matrix of the prediction error

$P_{k|k-1}=\frac{r_i^{\text{*}}}{\omega }{\displaystyle \sum }_{i=1}^{2\rho n}\left[ \left(X^{*}{}_{k|k-1}^i-{\widehat{X}}_{k|k-1}\right){\left(X^{*}{}_{k|k-1}^i-{\widehat{X}}_{k|k-1}\right)}^T\right]+Q_{k-1}$

14

In the above formula, $r_i^{\text{*}}$ represents the covariance correction coefficient corresponding to the sampling point $i$ , and for convenience, it is 1 here in this article. $\omega$ represents the covariance weighting coefficient, Q is the system noise covariance matrix, and the relevant calculation formula is

$\omega ={\displaystyle \sum }_{j=1,\hspace{0.17em}j\ne \rho +1}^{2\rho +1}{r}_j^2{\lambda }_{p_j}^2$

15

Second step: measurement update process

• (1) Re-rank sampling to get the sampling point set $\text{{}{\mathbf{\text{χ}}}_{k|k-1}^i\text{}}$

$\begin{array}{c}{\mathbf{\text{χ}}}_{k|k-1}^i={\widehat{X}}_{k|k-1}+r_j{\lambda }_{p_j}{\left(\sqrt{P_{k|k-1}}\right)}_l,l=1,2,\dots ,n\\ \{\begin{array}{c}i=\left(j-1\right)n+l,i=1,2,\dots \rho \\ i=\left(j-2\right)n+l,i=\rho +2,\rho +3,\dots 2\rho +1\end{array}\end{array}$

16

• (2) Calculate the measurement value iterated through the non-linear observation equation, that is, $\left\{Z_{k|k-1}^i\right\}$

$Z_{k|k-1}^i=h\left({\chi }_{k|k-1}^i,v_k\right)i=1,2,\dots ,2\rho n$

17

• (3) Estimated measurement value

${\widehat{Z}}_{k|k-1}=\frac{1}{2\rho n}{\displaystyle \sum }_{i=1}^{2\rho n}{Z}_{k|k-1}^i$

18

• (4) Measurement update covariance matrix

$P_{ZZ,k|k-1}=\frac{r_i^{\text{*}}}{\omega }{\displaystyle \sum }_{i=1}^{2\rho n}\left[\left(Z_{k|k-1}^i-{\widehat{Z}}_{k|k-1}\right){\left(Z_{k|k-1}^i-{\widehat{Z}}_{k|k-1}\right)}^T\right]+R_k$

19

where R is the measurement covariance matrix.

• (5) Measurement cross-covariance matrix

$P_{XZ,k|k-1}=\frac{r_i^{\text{*}}}{\omega }{\displaystyle \sum }_{i=1}^{2\rho n}\left[\left({\chi }_{k|k-1}^i-{\widehat{X}}_{k|k-1}\right){\left(Z_{k|k-1}^i-{\widehat{Z}}_{k|k-1}\right)}^T\right]$

20

• (6) Calculate the gain matrix

$K_k=P_{XZ,k|k-1}{P}_{ZZ,k|k-1}^{-1}$

21

• (7) State estimation

${\widehat{X}}_{k|k}={\widehat{X}}_{k|k-1}+K_k\left(Z_k-{\widehat{Z}}_{k|k-1}\right)$

22

• (8) Covariance matrix of the state estimation

$P_{k|k}=P_{k|k-1}-K_k{P}_{ZZ,k|k-1}{K}_k^T$

23

Experimental analysis

Experiment environment for indoor positioning analysis

Lidar used in this experiment is R-Fans 16-line. This product is based on the time-of-flight ranging principle to obtain the point cloud data with three-dimensional scanning. The positioning principle is based on the relative motion estimation of point cloud matching, which decomposes the motion in the three-dimensional space into a three-dimensional rotation matrix R and a three-dimensional translation vector t. The performance index parameters are listed in Table 1. IMU is the model of Xsens MTI. It consists of a three-axis gyroscope, a three-axis accelerometer, and a three-axis magnetometer. The technical parameters are listed in Table 2. The experimental equipment is shown in Figure 3. The ground truth of the mobile robot in the experiment is provided by the optical space positioning system Realis Tracking System. This system can be applied to navigation trajectory of sensors, robot, or drones. It is currently the only high-precision positioning system that can meet the submillimeter level of indoor positioning. The trajectory information is provided by the corresponding TRACKER software, as shown in Figure 4. We have conducted experiments on two trajectories, one is a small-scale arc trajectory environment, and the other is a large-scale rectangular trajectory environment.

Table 1.

R-Fans-16 parameter table.

Parameter	Numerical specification
Maximum detection distance	200 m
Minimum detection distance	<0.1 m
Horizontal angle of view	360°
Distance resolution	3 mm
Frame measurement rate	5–20 Hz
Measurement accuracy	<3 cm

Table 2.

IMU parameter table.

Parameter	Numerical specification
Zero bias stability of the gyroscope	1°/s
Gyroscopic range	±300°/s
Zero bias stability of accelerometer	±50 m/s2
Accelerometer range	0.02 m/s2
Zero bias stability of the magnetometer	0.1 × 10−7 Tesla
Magnetometer range	±7.5 × 10−5 Tesla

IMU: inertial measurement unit.

Figure 3.

Mobile robot data acquisition system.

Figure 4.

TRACKER software operation interface.

Small-scale arc trajectories environment test analysis

To verify the positioning accuracy of the algorithm, we conducted a navigation experiment of the indoor environment. The experimental site is in China BeiDou unmanned system comprehensive experimental innovation base at Shanghai Jiao Tong University, China, as shown in Figure 5. In a small-scale arc trajectory environment, the robot starts from the selected point and finally returns to the same point. The walking path is a closed loop path. Each experiment includes Lidar alone and Lidar/IMU integrated positioning tests and analyses. A KF algorithm for alone Lidar sensor, an EKF fusion algorithm for Lidar/IMU integrated system, and an RKF fusion algorithm for Lidar/IMU integrated system are used to estimate the location. The results of the positioning errors estimated by the above three algorithms are compared and analyzed. Since the alone IMU test accuracy was at the meter level for this loop path that was done by our group several years ago, ²² and is not what we discuss here at the accuracy level of the centimeter, we did not compare with the test results of IMU alone in this article.

Figure 5.

Experimental environment.

During the positioning experiment, the trajectory is 16.28 m long, the robot starts from the starting point A (7.5057, 2.9011), at a speed not exceeding 20 cm per second walks clockwise around the polygon, finally returns to the point A. There are four turning points in the walking trajectory, namely A, B, C, D. Among them, the three turning points of A, B, and D have relatively large angles, and the turning angle of C is relatively small. The nonlinearity and non-Gaussian characteristics of the system at the turning point are more obvious, which is good for evaluating the performance of the algorithm in solving nonlinear and non-Gaussian problems. The trajectory solved by the alone Lidar algorithm, the trajectory solved by Lidar/IMU integrated system EKF fusion algorithm, and the trajectory solved by Lidar/IMU integrated system RKF fusion algorithm are compared with the ground truth. The comparison result is shown in Figure 6. The black line is the ground truth, the green line is the trajectory solved by alone Lidar algorithm, the blue line is the trajectory solved by Lidar/IMU EKF fusion algorithm, and the red line is the trajectory solved by Lidar/IMU RKF fusion algorithm. (a) is the trajectory solved by the alone Lidar algorithm and the ground truth comparison figure, (b) is the trajectory solved by Lidar/IMU integrated system EKF fusion algorithm and the ground truth comparison figure, (c) is the trajectory solved by Lidar/IMU integrated system RKF fusion algorithm and the ground truth comparison figure, and (d) is the all trajectories comparison figure.

First, the alone Lidar positioning test is analyzed. In section AB, the walking path of the robot is a straight line. During this time, due to the linear characteristics of the system, no large positioning error will occur. When it turns at the point B, C, D, and A, there is a nonlinearity and non-Gaussian characteristics of the system, which is caused by two wheels of differential rotation, and make the mobile robot vibrative (quivery). At this time, the nonlinearity and non-Gaussian characteristics of the system are enhanced, the Lidar is prone to the scan matching errors. As a result, the trajectory fluctuates greatly and trajectory drift occurs. In the whole process, the maximum error in the X direction is 0.1489 m, and the maximum error in the Y direction is 0.1456 m. With the time goes, due to the limited ability of Lidar Loop Closing function, the cumulative error is difficult to be eliminated, and it increases significantly. As can be seen from Figure 6(a), the mobile robot did not eventually return to the end point but returned to the point (7.4533, 2.9492).

Figure 6.

The trajectories of small-scale arc environment. (a) The trajectory solved by the alone Lidar algorithm and the ground truth comparison figure; (b) the trajectory solved by Lidar/IMU integrated system EKF fusion algorithm and the ground truth comparison figure; (c) the trajectory solved by Lidar/IMU integrated system RKF fusion algorithm and the ground truth comparison figure; (d) all trajectories comparison figure.

Second, Lidar/IMU EKF integrated system positioning test is analyzed. When it turns at B, the trajectory wave still fluctuates and there is a little trajectory drift. This is because the robustness of the EKF algorithm in processing nonlinear systems and non-Gaussian noise is not very strong, so there are certain limitations. However, compared with the alone Lidar algorithm, the trajectory solved by Lidar/IMU EKF fusion algorithm in the whole process was closer to the trajectory of ground truth.

At last, Lidar/IMU RKF integrated system positioning test is analyzed. During the whole trajectory, the positioning error of the mobile robot is very small, basically close to the ground truth. In the whole process, the maximum error in the X direction is 0.0753 m, and the maximum error in the Y direction is 0.0479 m. It can be seen RKF algorithm has a stronger ability to restrain the error divergence caused by nonlinear systems and non-Gaussian noise. Although the IMU position drift error is existing, the attitude deviation of Xsens MTi is relatively small due to its better attitude constraints. And the attitude information collected by MTi is used to compensate for the posture error caused by the alone Lidar scan matching through RKF, which can improve the positioning accuracy of the direction angle, and also improve the stability of the integrated system. In addition, by adding IMU sensing information, the cumulative errors ^14,18 in the process of Lidar data processing are compensated and updated in real time, the fusion algorithm can better perform cumulative errors, it can be seen from Figure 6(c) that the last position (7.5032, 2.9110) the robot returns (the end point) to is very close to the starting point (7.4533, 2.9492). Further quantitative analysis is shown in Table 3.

Table 3.

Results of error statistics of three positioning methods.

Axis direction	Technical index	Results of three positioning methods
		Lidar (m)	EKF Lidar/IMU (m)	RKF Lidar/IMU (m)
X direction	STD	0.0281	0.0214	0.0159
	MAX	0.1489	0.1120	0.0753
	Mean	0.0936	0.0667	0.0420
Y direction	STD	0.0222	0.0141	0.0068
	MAX	0.1456	0.0935	0.0479
	Mean	0.0630	0.0467	0.0305

Table 3 shows the error statistics in the X and Y directions with the three algorithms for indoor positioning, where STD is the standard deviation, MAX is the maximum error value, and mean is the average error. Compared with the results of the alone Lidar KF algorithm and Lidar/IMU integrated system with EKF, Lidar/IMU integrated system with RKF obtains better the results. It can be seen from Table 3 that nonlinearity and non-Gaussian properties have a greater impact on the alone Lidar positioning with KF and Lidar/IMU integrated system with EKF, but the Lidar/IMU integrated system with RKF fusion algorithm can deal with this situation very well and has a better accuracy than the alone Lidar positioning algorithm.

Through multiple experiments to further verify the positioning accuracy of the algorithm, we did five tests around the polygon and get five sets of data for analysis. The results of alone Lidar algorithm and RKF algorithm comparison are shown in Table 4, and the results of EKF algorithm and RKF algorithm comparison are shown in Table 5. As we can see, the mean value of X direction error with alone Lidar algorithm is 0.0928 m, the mean value of Y direction error is 0.0772 m, the mean value of X direction error with Lidar/IMU integrated system with EKF is 0.0597 m, and the mean value of Y direction error is 0.0537 m, while the mean value of X direction error with Lidar/IMU integrated system with RKF is 0.0451 m, and the mean value of Y direction error is 0.0405 m. As a result, compare with alone Lidar algorithm, the new algorithm improves the mean accuracy rate of 46.39% in the X direction and 48.40% in the Y direction. Compared with EKF algorithm, the new algorithm improves the mean accuracy rate of 24.46% in the X direction and 24.58% in the Y direction.

Table 4.

Alone Lidar algorithm and RKF algorithm comparison.

Experiments	Lidar X mean/m	Lidar Y mean/m	RKF Lidar/IMU X mean/m	RKF Lidar/IMU Y mean/m	Increased accuracy rate X (%)	Increased accuracy rate Y (%)
1	0.0856	0.0930	0.0490	0.0427	57.24	45.91
2	0.0936	0.0630	0.0420	0.0305	44.87	48.41
3	0.0674	0.1041	0.0372	0.0613	40.36	38.89
4	0.0989	0.0612	0.0456	0.0384	46.11	62.75
5	0.1187	0.0647	0.0515	0.0298	43.39	46.02
Mean	0.0928	0.0772	0.0451	0.0405	46.39	48.40

RKF: rank Kalman filtering; IMU: inertial measurement unit.

Table 5.

EKF algorithm and RKF algorithm comparison.

Experiments	EKF Lidar/IMU X mean/m	EKF Lidar/IMU Y mean/m	RKF Lidar/IMU X mean/m	RKF Lidar/IMU Y mean/m	Increased accuracy rate X (%)	Increased accuracy rate Y (%)
1	0.0664	0.0547	0.0490	0.0427	26.20	21.94
2	0.0519	0.0555	0.0420	0.0305	19.08	45.05
3	0.0478	0.0706	0.0372	0.0613	22.18	13.17
4	0.0625	0.0412	0.0456	0.0384	27.04	6.80
5	0.701	0.0465	0.0515	0.0298	26.53	35.91
Mean	0.0597	0.0537	0.0451	0.0405	24.46	24.58

EKF: extended Kalman filter; RKF: rank Kalman filtering; IMU: inertial measurement unit.

Large-scale rectangular trajectories environment test analysis

To ensure the reliability of the results, we repeated the experiment several times in a large-scale rectangular trajectory environment. The trajectory is 54.7 m long, the trajectory solved by Lidar/IMU integrated system EKF fusion algorithm and the trajectory solved by Lidar/IMU integrated system RKF fusion algorithm are compared with the ground truth. The comparison result is shown in Figure 7. The black line is the ground truth, the blue line is the trajectory solved by Lidar/IMU EKF fusion algorithm, and the red line is the trajectory solved by Lidar/IMU RKF fusion algorithm. The black solid point is the starting point, the black hollow point is the end point of the RKF algorithm, and the green hollow point is the end point of the EKF algorithm. Table 6 shows the error statistics in the X and Y directions with the two algorithms for indoor positioning. In the EKF algorithm, the maximum error in the X direction is 0.3472 m, and the maximum error in the Y direction is 0.4181 m. In the RKF algorithm, the maximum error in the X direction is 0.2321 m, and the maximum error in the Y direction is 0.2516 m.

Figure 7.

The trajectories of large-scale rectangular environment.

Table 6.

Results of error statistics of two positioning methods.

Axis direction	Technical index	Results of three positioning methods
		EKF Lidar/IMU (m)	RKF Lidar/IMU (m)
X direction	MAX	0.3472	0.2321
	Mean	0.1067	0.0718
Y direction	MAX	0.4181	0.2516
	Mean	0.0984	0.0707

EKF: extended Kalman filter; RKF: rank Kalman filtering; IMU: inertial measurement unit.

To ensure the reliability of the results, we repeated the experiment several times in a large-scale rectangular trajectory. The results of EKF algorithm and RKF algorithm comparison are shown in Table 7, the mean value of X direction error with EKF algorithm is 0.1147 m, and the mean value of Y direction error is 0.1052 m, while the mean value of X direction error with RKF algorithm is 0.0875 m, and the mean value of Y direction error is 0.0786 m.

Table 7.

EKF algorithm and RKF algorithm comparison.

Experiments	EKF Lidar/IMU X mean/m	EKF Lidar/IMU Y mean/m	RKF Lidar/IMU X mean/m	RKF Lidar/IMU Y mean/m	Increased accuracy rate X (%)	Increased accuracy rate Y (%)
1	0.1124	0.1136	0.0981	0.0721	12.69	36.54
2	0.1280	0.0899	0.0902	0.0709	29.56	21.17
3	0.1222	0.0950	0.0938	0.0719	23.25	24.36
4	0.0972	0.1257	0.0730	0.1019	26.12	18.91
5	0.1135	0.1019	0.0822	0.0761	27.58	25.32
Mean	0.1147	0.1052	0.0875	0.0786	23.84	25.26

EKF: extended Kalman filter; RKF: rank Kalman filtering; IMU: inertial measurement unit.

Conclusions

Aiming at the problems of low positioning accuracy of Lidar SLAM with nonlinear and non-Gaussian noise characteristic, this article uses Lidar and IMU integrated navigation system and proposes a fusion RKF algorithm to realize positioning estimation of the indoor mobile robot. RKF algorithm is the core of the fusion method. With its unique sampling mechanism, it can improve the positioning accuracy of the indoor mobile robot under nonlinear model and non-Gaussian noise model. Finally, multiple experimental results and analysis verify that the proposed algorithm improves the accuracy and robustness of the positioning system compared with the alone Lidar positioning algorithm. It has higher positioning accuracy, better system stability, and stronger robustness and is very important in trajectory calculation.