Sage Journals: Discover world-class research

Abstract

To overcome the problem of the low accuracy and large accumulated errors of indoor mobile navigation and positioning, a method to integrate the light detection and ranging- and inertial measurement unit-based measurement is proposed. Firstly, the voxel-scale-invariant feature transform feature extraction algorithm for light detection and ranging is studied. Then, the errors of light detection and ranging measurement due to the change of the scan plane compensated based on aiding information from inertial measurement unit. The relative position parameters and the differences of measurements of light detection and ranging at adjacent times are used to estimate the error of inertial measurement unit sensors by a Kalman filter. Several experiments are carried out in the indoor corridor and the results demonstrate that precision of the light detection and ranging/inertial measurement unit-integrated indoor mobile robot localization is higher than that of single light detection and ranging sensor.

Keywords

Positioning LiDAR IMU voxel-SIFT Kalman filter indoor navigation

Introduction

In the recent years, the indoor autonomous service robot has gradually become the research focus of many enterprises, with a very good application field and development prospects.¹ Many start-ups have exhibited their indoor service robots, only a small number of them have the function of autonomous indoor navigation. Based on the simultaneous localization and mapping (SLAM), the robot is able to “see” its own location and accurately move to the target’s position in an unknown environment, which is an important indicator for improving the intelligence of the robot. At present, there are two ways for indoor service robot’s self-localization and navigation system using the SLAM method, one is to use visual camera and the other is to use light detection and ranging (LiDAR). Although the precision of single-point measurement by camera is lower than that of LiDAR, it can build more dense and rich point cloud maps based on repeated observation of the texture information of pictures and improve the positioning accuracy. The camera relies heavily on lighting conditions and cannot work in dark scenes or scenes with insufficient texture information; however, the LiDAR has the characteristics of fast real-time ranging, not limited by the light environment, high ranging accuracy, and relatively constant error, and is more reliable working performance, which has been widely used in the construction of maps, navigation and positioning of robots, environmental modeling, and other aspects.^2,3 Although some problems have been solved, it is also a big challenge in the field of LiDAR SLAM, which has been attracted more attention in mobile robots.^4
–6

It should be emphasized that the positioning method relying on a single sensor has disadvantages, which is greatly affected by the signal strength, and even fails in the case of the signal occlusion. Although LiDAR scanning and matching technology can provide high-precision position and course positioning results, when the environmental characteristics are not obvious, the positioning accuracy of LiDAR will be greatly reduced and easily interfered by the external environment. Moreover, the sampling frequency of LiDAR is low.⁷ Compared with LiDAR positioning technology, the inertial navigation technology has the ability of autonomous navigation, which can provide full navigation parameters of high-frequency speed, position, and attitude. However, the inertial navigation system -derived navigation and positioning errors will gradually accumulate with time, which can only provide high-precision navigation and positioning results in a very short time. The LiDAR- and inertial measurement unit (IMU)-integrated navigation combines the advantages of LiDAR and IMU navigation and positioning technologies to learn from each other and realize a combination system with high sampling rate and high precision that can work continuously indoors.^8,9 Even without global positioning system (GPS) signals, the integrated LiDAR/IMU can achieve high positioning precision in indoor environments. Research shows that LiDAR/IMU-integrated navigation positioning has become one of the hot spots in navigation technology research.^10,11

To improve the localization accuracy, an improving indoor robot positioning algorithm for LiDAR/IMU-integrated navigation using Kalman filter (KF) is investigated in this article. Firstly, collecting experimental data. Then, the kinematics parameters of the mobile robot are calculated by the inertial system, and the local features of the environment are detected by scale-invariant feature transform (SIFT) of voxel lattice (voxel-SIFT) algorithm. Over time, the accumulated environmental feature data are used to update the environmental map. Then, the mobile robot can use its environmental map to locate itself. LiDAR/IMU-integrated positioning is an important and effective method for detecting unknown environments and solving indoor positioning problems of mobile robots. To facilitate the real-time implementation of the algorithm in hardware, the relative pose parameters obtained from the LiDAR data are converted into global coordinate for the measurement of KF.

Coordinate system

Consider that the system has LiDAR and IMU sensors, calibrated the external parameters between them. The relative posture between LiDAR and IMU is calibrated by solving the hand-eye problem,¹² and the calibration method is the same as in the work of Zhang and Singh.¹³ The IMU frame is in the same direction as the LiDAR frame. Figure 1 shows three related reference frames, fixed world frame {W}, the LiDAR frame {L}, and the IMU frame {I}, installed together.¹⁴

Figure 1.

{L} is the LiDAR frame, {I} is the IMU frame, {W} the fixed world frame which are mounted together. The subscripts “WL” represent the rotation from the LiDAR frame {L} to the world frame {W} and vice versa. The acceleration a is measured in the IMU frame and then resolved in the world frame, where we subtract the gravity vector g. “LI” represents the rotation from the acceleration frame {I} to the LiDAR frame {L}.

{L} is the LiDAR frame of axes originating from the LiDAR center. The X-axis and Y-axis point the left and forward, respectively, and the Z-axis points upward coinciding with the LiDAR principal axis.

{I} is the IMU frame of axes originating from the IMU measurement center. The coordinate axis is in the same direction and parallel to {L}.

{W} is world frame of axes, this is the gravity-aligned frame of axes with the same as {L}.

Employed sensors

The LiDAR system mainly includes LiDAR and IMU, which are used to measure the distance from the sensor to the ground point and the spatial attitude parameters of the main optical axis of the measurement unit, respectively.¹⁵ The LiDAR system mainly has the following three characteristics:

LiDAR data have high precision. With a height and horizontal position accuracy of 10 cm and below 0.4 m, respectively. In addition, high-resolution digital images can be collected.

LiDAR data have high density. With a large amount of point cloud, it can really reflect the terrain and topography.

LiDAR adopts automatic scan measurement method; it can work all the time without much control.

LiDAR

LiDAR measures the distance and angle between the measured object and the sensor. The distance from the measured object is calculated according to the time from laser pulse to receiving pulse.^16,17

LiDAR updates IMU data by scanning indoor environmental and extracting information. The Rfans-16 LiDAR sends out infrared laser pulses, which, according to their “time of flight,” obtain the running time of LiDAR, the scan rate is 12.22 ms. The resolution of detecting angle and ranging scope of LiDAR, respectively, is 0.1° and 0.1–200 m, the collection frequency of the LiDAR is 10 Hz. By using the equations x = dcos(Φ) and y = dsin(Φ) to get the LiDAR scanning point coordinates, which d is the scan distance and Φ is the scan angle. Since the scanning point coordinates are x and y in the carrier coordinate system, as so transforms them to the navigation coordinate system to obtain navigation coordinates.¹⁸

IMU

IMU sensor is used to measure the three-axis attitude angle and acceleration of an object. The data of gyroscope and accelerometer are processed in IMU, and the acceleration of the object in the carrier coordinate system is obtained by the three-axis accelerometer, and the angular velocity of the carrier relative to the navigation frame of axes is obtained by the three-axis gyroscope. The bias stability of IMU (MTI-100) gyroscopes is about 10°/h, bias stability of IMU accelerators is about 40 µg, and IMU data acquisition frequency is 100 Hz. Based on the acceleration and angular velocity of the object in the 3-D space, the attitude of it is then solved.¹⁹

Due to various unavoidable interference factors, the error of gyroscope and accelerometer is generated from the initial alignment, and its navigation error increases with time. Therefore, its error and noise should be considered, where b for error and n for noise. Assuming that the noise is white Gaussian noise with zero mean value

w_{t} = w_{m} - b_{w} - n_{w}

a_{t} = C (q_{t}) (a_{m} - b_{a} - n_{a}) + g

where in equations (1) and (2), w_t is true angular velocity, $a_{t}$ is measured acceleration, $q_{t}$ is the quaternion equivalent of the true attitude, $C (q_{t})$ is the rotation matrix of the true attitude, n_w and n_a represent the noise of gyro and accelerometer, b_w and b_a represent the relevant biases, and g is the gravitational acceleration of the {W}. Since these biases, IMU’s measurements drift over time. As a result, IMUs are often integrated with cameras or LiDAR.

LiDAR/IMU integrated navigation positioning

Algorithm framework

The schematic diagram of the fusion algorithm is shown in Figure 2.

Figure 2.

Schematic diagram of the integrated algorithm.

Principle of SINS

The basic working principle of strapdown inertial navigation system (SINS)²⁰ algorithm is by measuring the acceleration of the carrier in the inertial reference frame, integrating the time and then transforms it to the navigation coordinate system to obtain the speed, yaw angle, and position information in the navigation coordinate system. The function and concept of SINS are embodied in the computer. The acceleration and angular acceleration measured by the IMU directly mounted on the carrier are components along the axis of the carrier. These components can be converted to components in the required computer coordinate system through a coordinate conversion direction cosine matrix. If this matrix can describe the carrier and the relationship between the world coordinate system, then the vector coordinates measured relative to the inertial acceleration and angular velocity of the space, transformed the world coordinate system can be got after the acceleration and angular velocity components, had known bearing acceleration and angular velocity components, the navigation computer can according to the requirements of the corresponding mechanics equations of navigation and attitude parameters.

Voxel-SIFT feature extraction algorithm

Due to the disorder of LiDAR point cloud data, it is a major research difficulty to extract and identify the feature points. The selection of feature extraction method will seriously affect the registration of LiDAR point cloud, thus affecting the accuracy of LiDAR SLAM overall positioning and mapping. This article focuses on voxel-SIFT feature extraction algorithm based on LiDAR point cloud.

SIFT²¹ is a common feature of 2-D image feature detection, whose application scope includes image mosaic, target recognition, object tracking and 3-D model construction, and so on. Different from SIFT’s application in dense point cloud of 2-D plane image, this article presents a feature point extraction method based on SIFT of voxel lattice, aiming at the spatial sparsity characteristics of 3-D point cloud of LiDAR. Detailed steps of voxel-SIFT feature extraction method are described below:

Step 1: On the 3D LiDAR point cloud data to construct the 3-D voxel grid, on the basis of the Z-axis point cloud information from outside to inside space of point cloud layer division, assuming that each layer of the original model for voxel grid, then the layer of the scale of the space model for the primitive voxel grid model with 3-D Gaussian filter convolution, as shown in the following equations

M_{k} = M (\begin{matrix} x, & y, & z \end{matrix}) \otimes G (\begin{matrix} x, & \begin{matrix} y, & z, & k δ \end{matrix} \end{matrix})

G (\begin{matrix} x, & \begin{matrix} y, & z, & k δ \end{matrix} \end{matrix}) = \frac{1}{{(\sqrt{2 π} k δ)}^{3}} e^{- (x^{2} + y^{2} + z^{2}) / 2 {(k δ)}^{2}}

where $G (\begin{matrix} x, & \begin{matrix} y, & z, & k δ \end{matrix} \end{matrix})$ is Gaussian function, $δ$ is spatial scale parameter, and $k$ is scale adjustment parameter. Different $k$ values represent different scale spaces.

Step 2: Calculate the Gaussian difference model of adjacent scale layers to ensure the scale invariance of voxel mesh. Refer to formula (5)

D_{O} G_{k} = W_{k} (\begin{matrix} x, & y, & z \end{matrix}) - W_{k - 1} (\begin{matrix} x, & y, & z \end{matrix})

Step 3: Detect the extreme point from different directions of Gaussian difference space, which is the maximum point or minimum point in all Gaussian difference models.

Step 4: The spatial region containing 8 × 8 × 8 small voxel meshes were redivided with the extreme point as the center, and each subregion was further divided into a 4 × 4 × 4 grid region. The normal vector of each point in the point set was calculated in each grid region after the partition.

Step 5: Use the histogram to statistics the amplitude and direction angle of the normal vector in each grid area. The horizontal axis of the histogram statistics the direction angle, while the vertical axis statistics the accumulated value of the amplitude corresponding to the direction angle. Select the vector value corresponding to the histogram peak as the characteristic vector of the subgrid area.

Figure 3 is the schematic diagram of voxel-SIFT feature extraction algorithm, which can guarantee that the extracted feature points have rotation invariance and feature vectors with high resolution, and can distinguish the feature areas with slight changes from those with less obvious information.

Figure 3.

Voxel-SIFT feature extraction algorithm. SIFT: scale-invariant feature transform.

KF-based LiDAR/IMU-integrated navigation

Because the GPS lost its signals in the indoor environment, it was unable to positioning, individual LiDAR navigation will produce cumulative errors over time; thus, the traditional navigation system can be improved by integrating other positioning sensors. In a word, LiDAR/IMU-integrated navigation system adopts the classical KF to decrease the LiDAR error and improve the positioning precision of robot navigate autonomously indoors.

The Kalman filtering is realized by the method of error estimation, mainly state error and observation error. This indicates that the filter estimates the error of the designated attitude position, and velocity of IMU, it continuously estimates the output error of various sensors to optimize the information of each sensor.²²

Equation of state model

To integrate inertial information and LiDAR observation, KF of the translation, rotation, velocity, gyroscope, and accelerometer’s first-order Markov process with a total of 16-dimensional states is selected, and the error-state representation method is adopted to linearize it. Then, the state equation established is as follows

\dot{\tilde{X}} = F \tilde{X} + G W

\tilde{X} = {[δ R, δ θ, δ V, ∊_{r}, \nabla, Δ λ]}^{T}

F = [\begin{matrix} \begin{matrix} 0_{3 \times 3} & I_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & - C ({\hat{q}}_{I}^{W}) {\hat{a}}_{x} \end{matrix} & \begin{matrix} 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 1} \\ 0_{3 \times 3} & - C ({\hat{q}}_{I}^{W}) & 0_{3 \times 1} \end{matrix} \\ \begin{matrix} 0_{3 \times 3} & 0_{3 \times 3} & - {\hat{ω}}_{x} \\ 0_{7 \times 3} & 0_{7 \times 3} & 0_{7 \times 3} \end{matrix} & \begin{matrix} - I_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 1} \\ 0_{7 \times 3} & 0_{7 \times 3} & 0_{7 \times 1} \end{matrix} \end{matrix}]

G = [\begin{matrix} \begin{matrix} 0_{3 \times 3} & 0_{3 \times 3} \\ - C ({\hat{q}}_{I}^{W}) & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} \end{matrix} & \begin{matrix} 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} \\ - I_{3 \times 3} & 0_{3 \times 3} \end{matrix} \\ \begin{matrix} 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & I_{3 \times 3} \\ 0_{1 \times 3} & 0_{1 \times 3} \end{matrix} & \begin{matrix} 0_{3 \times 3} & I_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{1 \times 3} & 0_{1 \times 3} \end{matrix} \end{matrix}]

W = {[n_{a}^{T} n_{b_{a}}^{T} n_{w}^{T} n_{b_{w}}^{T}]}^{T}

where $F \in R^{16 \times 16}$ is the system state transition matrix, $G \in R^{16 \times 12}$ is the noise covariance matrix, and $W \in R^{12 \times 1}$ is the system noise vector. $δ R, δ θ, δ V$ refer to the translation, rotation, and velocity error of the inertial system relative to the world system. $ε_{r}, \nabla$ refer to the error of the triaxial gyroscope and accelerometer, respectively. $Δ λ$ refers to the measurement scale error of the LiDAR. $p_{I}^{W}, q_{I}^{W}$ represent the transformation from {I} to {W}, expressing the position and attitude information of IMU in the{W}. $n_{b}_{w}$ and $n_{b}_{a}$ are the random walk noise of gyroscope and accelerometer, respectively.

Measurement equation model

The pose of LiDAR is calculated and then used as observation quantity for KF status update.

Firstly, it is assumed that the uncertainty of LiDAR pose can be obtained, and its measured noise $n = {[n_{p}, n_{q}]}^{T}$ is defined. The noise vector is similarly divided into two parts: translation n_p and rotation n_q . To build the measurement model, the measurement residuals are defined as follows

\tilde{Z} = Z - \hat{Z} = H \tilde{X} + n

Since we use the error quaternion, residuals can also be divided into translation and rotation. For the translation part, the residual has the following form

Z_{p} = p_{l}^{L} = C (q_{W}^{L}) (p_{I}^{W} + C (q_{I}^{W}) p_{l}^{I}) λ + n_{p}

{\tilde{Z}}_{p} = C (q_{W}^{L}) (p_{I}^{W} + C (q_{I}^{W}) p_{l}^{I}) λ + n_{p} - C (q_{l}^{L}) ({\hat{p}}_{I}^{W} + C ({\hat{q}}_{I}^{W}) p_{l}^{I}) \hat{λ} + n_{p}

where $p_{l}^{L}$ and $q_{l}^{L}$ are the position and attitude of LiDAR in {L} reference frame, $q_{W}^{L}$ represents the rotation vector of the {W} to {L}, and $p_{L}^{I}, q_{L}^{I}$ represents the transformation from {L} to {I} and represents the position and attitude relationship between LiDAR and IMU which are determined during system initialization.

For the rotating part, using the definition of error quaternion, we have the following equation

Z_{q} = q_{l}^{L} = q_{W}^{L} \otimes q_{I}^{W} \otimes q_{l}^{I}

\begin{matrix} {\tilde{Z}}_{q} = Z_{q} - {\hat{Z}}_{q} \\ = q_{W}^{L} \otimes q_{I}^{W} \otimes q_{l}^{I} \otimes {(q_{W}^{L} \otimes {\hat{q}}_{I}^{W} \otimes q_{l}^{I})}^{- 1} \\ = q_{I}^{l} \otimes δ q_{W}^{I} \otimes q_{I}^{l}^{- 1} \end{matrix}

Linearized error quaternion $q_{I}^{W}$ , equation (15) can be written as follows

{\tilde{Z}}_{q} = H_{q}^{W I} δ q_{I}^{W}

where $H_{q}^{W I}$ is an approximation, which is defined as follows

H_{q}^{W I} = [\begin{matrix} 1 & 0_{1 \times 3} \\ 0_{3 \times 1} & {\hat{H}}_{q}^{W I} \end{matrix}]

Finally, we get a complete LiDAR observation Z and residual model.

KF equation

The system regards the LiDAR as a six-degree-of-freedom pose sensor and then uses the KF algorithm to fuse inertial information. As shown in Figure 2, in the whole process, IMU measured value and LiDAR output value are used as the input of KF for state prediction and update.

The combined forecasting model and observation model get the traditional KF calculation process, in particular, KF filter through the acceleration and angular velocity information of IMU measurement condition forecasting, through LiDAR measurement of position and posture information, for status updates, constantly updated and revised, the ultimate state estimation results are obtained.²³

The following equation of state and observation equation can be used

x_{k} = A_{k - 1} + B_{k - 1} u_{k - 1} + w_{k - 1}

z_{k} = H_{k} x_{k} + v_{k}

where in equations (18) and (19), k as discrete time, $x_{k} \in R^{n}$ for k time-discrete system state vector, $A_{k} \in R^{n \times n}$ is the state transition matrix of the discrete system, $B_{k} \in R^{n \times l}$ is the input control matrix of the discrete system, $u_{k} \in R^{l}$ is the input vector of the discrete system, $z_{k} \in R^{m}$ is the measurement vector of the discrete system at time k, $H_{k} \in R^{m \times n}$ is the observation matrix of the discrete system, $w_{k} \in R^{n}$ is the process noise vector of the discrete system, and $v_{k} \in R^{m}$ is the measurement noise vector of discrete system.

Due to the unstable calculation height, the sensor is fixed at an initial height. The designed KF model integrates the position and attitude information of LiDAR sensor and IMU sensor and then uses these information to correct the trajectory generated by LiDAR data and compensate the errors accumulated over time.

Validation of the voxel-SIFT algorithm and LiDAR/IMU-integrated navigation algorithm

Voxel-SIFT algorithm verification experiments

To verify the voxel-SIFT feature extraction algorithm for the test result of laser point cloud data, choose three typical scenarios single frame of point cloud data for experiments, including passes through the point cloud pretreatment operation Karlsruhe Institute of Technology and Toyota Technological Institute (KITTI) open-source data set (16 line R-fans sensor) and the actual collected data sets (16 line R-fans sensor), Figure 4 is voxel-SIFT feature extraction algorithm of building space, indoor gallery, and indoor corridor the feature extraction effect of three kinds of situations. The blue point cloud in the figure is the original point cloud, and all the red punctuation marks on the figure are the extracted feature points.

Figure 4.

Feature extraction results of data processed by voxel-SIFT algorithm in different scenarios. (a) Building space; (b) indoor gallery; (c) indoor corridor; and (d) to (f) feature detection results. SIFT: scale-invariant feature transform.

In Table 1, the experimental results (point cloud number, number of feature detection, and time of feature detection) of the two feature detection algorithms are compared, SIFT feature detection algorithm and voxel-SIFT feature detection algorithm. The main evaluation index is the detection quantity and detection time of feature points. To compare the detection time more accurately, the time of each detection of 100 feature points is selected for the same comparison. In the table 1, the number of feature points extracted by voxel-SIFT algorithm is more than that by SIFT algorithm in each kind of scene test, and the overall detection time is shorter. The detection time of the voxel-SIFT algorithm is relatively stable, and the more feature points detected in general, the longer the time is spent. In the comparison of feature detection time presented in Table 1, the voxel-SIFT feature detection algorithm proposed in this article takes the least time, so considering the detection time, the voxel-SIFT algorithm is more suitable than SIFT algorithm for later data processing. The proposed feature extraction method based on voxel lattice improved SIFT has high extraction efficiency and can be applied to the fast registration of laser point cloud.

Table 1.

Comparison of feature point detection results.

Scenarios	Number of point cloud	SIFT feature detection (one)	Detection time/100 points (s)	Voxel-SIFT feature detection (one)	Detection time/100 points (s)
Building space	12,009	267	0.12	378	0.10
Indoor gallery	27,552	122	2.57	172	0.12
Indoor corridor	7131	149	0.13	187	0.13

SIFT: scale-invariant feature transform.

Integrated positioning experiment

A turtlebox2 mobile robot experimental platform with high-performance laptop, LiDAR, and IMU sensor integrated navigation system was designed. During the whole real test operate online experiment, the robot moves at a speed between 0.15 m/s and 0.25 m/s, Rfan-LiDAR has the advantages of common LiDAR sensors, and with high positioning accuracy. For verifying the precision of trajectory under different experimental conditions, two experiments were performed. First, the coordinates of some key points before the relevant path of the experiment were recorded and then the coordinates were compared with those of the experiment to obtain the positioning effect of the coordinates. In Figure 5, the data acquisition of the robot platform.

Figure 5.

The data acquisition of the robot mobile platform.

Indoor linear corridor experiment

Figure 6 shows an indoor scene in which the robot moves a distance along the ground in the experiment.

Figure 6.

The corridor scene map in the experiment.

In Figure 7 and Table 2, with LiDAR and with LiDAR/IMU, the maximum offset of the robot trajectory is 0.17 m and 0.11 m on the Y-axis, respectively. The average errors respectively in the X-axis are 0.02 m and 0.01 m, in the Y-axis are 0.14 m and 0.08 m with LiDAR and with LiDAR/IMU in Table 3.

Figure 7.

The straight trajectory of corridor scene map in the experiment.

Table 2.

Comparison among control points position for LiDAR and LiDAR/IMU in the straight line experiment (in meters).

Control Points	1	2	3	4	5
True Values	(0.00,0.00)	(3.92,1.01)	(6.87,1.88)	(9.90,2.76)	(12.92,3.90)
LiDAR	(0.00,0.00)	(3.94,1.12)	(6.88,2.02)	(9.92,2.90)	(12.94,4.07)
LiDAR/IMU	(0.00,0.00)	(3.92,1.09)	(6.87,1.96)	(9.91,2.85)	(12.93,4.01)

LiDAR: light detection and ranging; IMU: inertial measurement unit.

Table 3.

Absolute values of the coordinate errors (in meters).

	LiDAR (X)	LiDAR (Y)	LiDAR/IMU (X)	LiDAR/IMU (Y)
Average	0.02	0.14	0.01	0.08

LiDAR: light detection and ranging; IMU: inertial measurement unit.

Indoor polygon trajectory experiment

The experiment in which the robot moves clockwise in an octagon and returns to the starting point. The precise position of the reference points coordinates in the carrier frame was obtained by using the tachometry. Figure 8 shows the polygon corridor scene map in the experiment.

Figure 8.

The polygon corridor scene map in the experiment.

According to the trajectories in Figure 9 and coordinates of the reference points in Table 4 that LiDAR’s measured trajectory has an offset to the reference point and ultimately cannot return to the origin, whereas the trajectory of the LiDAR/IMU algorithm is relatively closer to the control points. In Table 4, with LiDAR, the offset of the trajectory at the X-axis and Y-axis, respectively, is −0.13 m and is −0.06 m. With LiDAR/IMU, the offset of the robot trajectory on the X-axis and on the Y-axis, respectively, is 0.08 m and 0.04 m. In the X-axis, the average errors respectively are 0.21 m and 0.12 m, in the Y-axis are 0.26 m and 0.14 m with LiDAR and with LiDAR/IMU in Table 5. In Table 6, it can be concluded that the standard deviation of the integrated measurement of LiDAR/IMU is optimal. The standard deviation from 0.46 m to 0.40 m in X-axis, 0.30 m to 0.22 m in Y-axis.

Figure 9.

The polygon trajectory of corridor in the experiment.

Table 4.

Comparison among control points position for LiDAR and LiDAR/IMU in the polygon experiment (in meters).

Control points	1	2	3	4	5	6	7	8	9	10
True values	(0.00,0.00)	(4.00,0.00)	(12.00,8.00)	(12.00,16.00)	(4.80,23.20)	(−4.80,23.20)	(−12.00,16.00)	(−12.00,8.00)	(−4.00,0.00)	(0.00,0.00)
LiDAR	(0.00,0.00)	(3.98,0.16)	(11.75,8.21)	(11.54,16.23)	(4.53,23.16)	(−5.12,22.98)	(−12.39,15.39)	(−12.15,7.38)	(−4.11, −0.49)	(−0.13, −0.06)
LiDAR/IMU	(0.00,0.00)	(4.01,0.08)	(11.85,8.16)	(11.73,16.15)	(4.62,23.23)	(−4.93,23.09)	(−12.22,15.79)	(−12.10,7.77)	(−3.91, −0.29)	(0.08,0.04)

LiDAR: light detection and ranging; IMU: inertial measurement unit.

Table 5.

Absolute values of the coordinate errors (in meters).

	LiDAR (X)	LiDAR (Y)	LiDAR/IMU (X)	LiDAR/IMU (Y)
Average	0.21	0.26	0.12	0.14

LiDAR: light detection and ranging; IMU: inertial measurement unit.

Table 6.

LiDAR, LiDAR/IMU integrated and real measurement error analysis (in meters).

	X direction std	Y direction std
LiDAR	0.46	0.30
LiDAR/IMU	0.40	0.22

LiDAR: light detection and ranging; IMU: inertial measurement unit.

According to the above, the LiDAR/IMU KF fusion positioning is very significant in compensating the cumulative error of LiDAR and correcting the trajectory and is higher positioning precision. Considering that the robot platform itself may slip sideways during operation and other factors interfere, there are some errors in the experimental results. These results show that error accumulation is small and the positioning precision is high for the short distance fusion positioning. The results show that, compared with a single LiDAR, the average error on the X-axis decreases from 0.21 m to 0.12 m and the standard error decreases from 0.46 m to 0.40 m, and the average error on the Y-axis decreases from 0.26 m to 0.14 m and the standard error decreases from 0.30 m to 0.22 m. Other tests have shown similar results. These results verify that compared with single sensor, the proposed method can improve the positioning accuracy for the short distance fusion positioning.

Conclusions

In this article, the fusion positioning system is studied by combining LiDAR sensor with IMU sensor through KF. The integrated IMU reduces the trajectory errors of the LiDAR and corrects the accumulated error of IMU by using LiDAR; thus, the positioning precision of indoor robot is improved. Contributions are summarized as follows:

With the characteristics of LiDAR point cloud, a SIFT feature based on voxel-SIFT feature is proposed. To highlight the effectiveness of the proposed method in the comparison experiment, the voxel-SIFT feature extraction algorithm was firstly studied and analyzed in detail, and the feature detection effect diagram of the feature extraction algorithm was given. Secondly, this article selected three types of laser point cloud data sets under different scenarios for experimental testing and compared the number of feature point detection and detection time of the above two feature extraction algorithms to give a detailed analysis of experimental results. The improved SIFT method based on voxel lattice proposed in this article has high extraction efficiency and can be applied to the fast registration of LiDAR point cloud.

KF was used to loosely combine LiDAR and IMU data to make up for the shortage of single LiDAR sensor. The above experimental results demonstrate that compared to a single sensor, the proposed method can improve the measurement accuracy.

However, this method has still the precision limitation, the next step is to increase the measurement range of the experiment and compare multiple experiments, improving the calculation precision of navigation parameters to further eliminate errors and increase the measurement distance. At the same time, because the laser point cloud data of different scenarios characteristics vary, all kinds of the fusion feature extraction method is more sensitive to the parameter settings, the follow-up research work can be from the perspective of deep learning, for a variety of scenarios of laser point cloud data to a large number of training, to produce a complete set of deep learning network, can realize the automatic extraction of laser point cloud, improve the various disadvantages of traditional algorithm.

Footnotes

Declaration of conflicting interests

The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: The article was supported by the projects of the National Key Technologies Research and Development program (no. 2016YFB0502002) and the National Natural Science Foundation of China (nos 41764002 and 61803175), in part by the Shan Dong Key Research and Development Program (no. 2019GGX104026), in part by a project ZR2018LF010 supported by Shan Dong Provincial Natural Science Foundation, and in part by the China Postdoctoral Foundation (no. 2017M622204).

ORCID iD

Hang Guo

References

Sithole

Zlatanova

. Position, location, place and area: an indoor perspective. ISPRS Ann Photogramm Remote Sens Spat Inf Sci 2016; 3: 89.

Wang

Xiong

Liu

, et al. Stepwise fusion algorithm with dual correction for multi-sensor navigation. Int J Adv Robot Syst 2018; 15(3): 1–11.

Baglietto

Sgorbissa

Verda

, et al. Human navigation and mapping with a 6DOF IMU and a laser scanner. Robot Auton Syst 2011; 59(12): 1060–1069.

Crocoll

Caselitz

Hettich

, et al. Laser-aided navigation with loop closure capabilities for micro aerial vehicles in indoor and urban environments. In: 2014 IEEE/ION position, location and navigation symposium-PLANS 2014, Monterey, 5–8 May 2014, pp. 373–384. IEEE.

Chowdhary

Michael

Pravitraz

. Self-contained ranging sensor aided autonomous guidance, navigation, and control for indoor flight. J Guid Control Dyn 2011; 59(12): 1060–1069.

Wang

Xiong

Liu

, et al. Chi-square and SPRT combined fault detection for multisensor navigation. IEEE Trans Aerosp Electron Syst 2016; 52(3): 1352–1365.

Zhao

Luo

Han

. Survey on robot multi-sensor information fusion technology. In: 2008 7th world congress on intelligent control and automation, Chongqing, 25–27 June 2008, pp. 5019–5023. IEEE.

Lan

Yuan

, et al. A novel Kalman filter with state constraint approach for the integration of multiple pedestrian navigation systems. Micromachines 2015; 6(7): 926–952.

Wang

Xiong

Liu

, et al. A robust astro-inertial integrated navigation algorithm based on star-coordinate matching. Aerosp Sci Technol 2017; 17(12): 1–10.

10.

Zhou

Changjun

Cui

, et al. A tracking filter in spherical coordinates enhanced by de-noising of converted Doppler measurements. Chin J Aeronaut 2012; 25(5): 757–765.

11.

Liu

Hang

, et al. Optimization design technology of environmental worthiness for MEMS inertial attitude reference system. J Nanjing Univ Aeronaut Astronaut 2012; 44(5): 669–676.

12.

Horaud

Dornaika

. Hand-eye calibration. Int J Robot Res 2011; 14(3): 195–210.

13.

Zhang

Singh

. Laser–visual–inertial odometry and mapping with high robustness and low drift. J Field Robot 2018; 35(8): 1242–1264.

14.

Liu

. LIDAR-IMU time delay calibration based on iterative closest point and iterated sigma point Kalman filter. Sensors 2017; 17(3): 539.

15.

Ying

Cui

, et al. Research on the technology of LIDAR data processing. In: 2017 first international conference on electronics instrumentation & information systems (EIIS), Harbin, 3–5 June 2017, pp. 1–5. IEEE.

16.

Jensfelt

Christensen

. Laser based position acquisition and tracking in an indoor environment. In: International symposium on robotics and automation-ISRA, Gaithersburg, USA, 1–4 January 1998, vol. 98, pp. 331–338.

17.

Wang

Liu

, et al. Laser-based online sliding-window approach for UAV loop-closure detection in urban environments. Int J Adv Robot Syst 2017; 13(2): 61.

18.

Deilamsalehy

Havens

. Sensor fused three-dimensional localization using IMU, camera and LIDAR. In: 2016 IEEE Sensors, Orlando, 30 October–3 November 2016, pp. 1–3. IEEE.

19.

Mirzaei

Roumeliotis

. A Kalman filter-based algorithm for IMU-camera calibration. IEEE Trans Robot 2008; 24(5): 1143–1156.

20.

Bortz

. A new mathematical formulation for strapdown inertial navigation. IEEE Trans Aerosp Electron Syst 2007; AES-7(1): 61–66.

21.

Gopaul

Wang

. Loosely coupled visual odometry aided inertial navigation system using discrete extended Kalman filter with pairwise time correlated measurements. In: 2017 forum on cooperative positioning and service (CPGPS), Harbin, 19–21 May 2017, pp. 283–288. IEEE.

22.

Wen

Guo

, et al. Research into Kinect/inertial measurement units based on indoor robots. Sensors 2018; 18(3): 839.

23.

Liu

Zhang

, et al. LIDAR/MEMS IMU integrated navigation (SLAM) method for a small UAV in indoor environments. In: 2014 DGON inertial sensors and systems (ISS), Karlsruhe, 16–17 September 2014, pp. 1–15. IEEE.

Light detection and ranging/inertial measurement unit-integrated navigation positioning for indoor mobile robots

Abstract

Keywords

Introduction

Coordinate system

Employed sensors

LiDAR

IMU

LiDAR/IMU integrated navigation positioning

Algorithm framework

Principle of SINS

Voxel-SIFT feature extraction algorithm

KF-based LiDAR/IMU-integrated navigation

Equation of state model

Measurement equation model

KF equation

Validation of the voxel-SIFT algorithm and LiDAR/IMU-integrated navigation algorithm

Voxel-SIFT algorithm verification experiments

Integrated positioning experiment

Indoor linear corridor experiment

Indoor polygon trajectory experiment

Conclusions

Footnotes

Declaration of conflicting interests

Funding

ORCID iD

References