Next Article in Journal
A Wireless Passive LC Resonant Sensor Based on LTCC under High-Temperature/Pressure Environments
Next Article in Special Issue
A Method for Oscillation Errors Restriction of SINS Based on Forecasted Time Series
Previous Article in Journal
Towards the Development of a Smart Flying Sensor: Illustration in the Field of Precision Agriculture
Previous Article in Special Issue
An Inertial and Optical Sensor Fusion Approach for Six Degree-of-Freedom Pose Estimation
 
 
Font Type:
Arial Georgia Verdana
Font Size:
Aa Aa Aa
Line Spacing:
Column Width:
Background:
Article

LiDAR Scan Matching Aided Inertial Navigation System in GNSS-Denied Environments

1
GNSS Research Center, Wuhan University, 129 Luoyu Road, Wuhan 430079, Hubei, China
2
Department of Remote Sensing and Photogrammetry, Finnish Geospatial Institute, Kirkkonummi FI-02431, Finland
3
Department of Navigation and Positioning, Finnish Geospatial Research Institute, Geodeetinrine 2, Kirkkonummi FI-02431, Finland
*
Author to whom correspondence should be addressed.
Sensors 2015, 15(7), 16710-16728; https://doi.org/10.3390/s150716710
Submission received: 13 May 2015 / Revised: 22 June 2015 / Accepted: 3 July 2015 / Published: 10 July 2015
(This article belongs to the Special Issue Inertial Sensors and Systems)

Abstract

:
A new scan that matches an aided Inertial Navigation System (INS) with a low-cost LiDAR is proposed as an alternative to GNSS-based navigation systems in GNSS-degraded or -denied environments such as indoor areas, dense forests, or urban canyons. In these areas, INS-based Dead Reckoning (DR) and Simultaneous Localization and Mapping (SLAM) technologies are normally used to estimate positions as separate tools. However, there are critical implementation problems with each standalone system. The drift errors of velocity, position, and heading angles in an INS will accumulate over time, and on-line calibration is a must for sustaining positioning accuracy. SLAM performance is poor in featureless environments where the matching errors can significantly increase. Each standalone positioning method cannot offer a sustainable navigation solution with acceptable accuracy. This paper integrates two complementary technologies—INS and LiDAR SLAM—into one navigation frame with a loosely coupled Extended Kalman Filter (EKF) to use the advantages and overcome the drawbacks of each system to establish a stable long-term navigation process. Static and dynamic field tests were carried out with a self-developed Unmanned Ground Vehicle (UGV) platform—NAVIS. The results prove that the proposed approach can provide positioning accuracy at the centimetre level for long-term operations, even in a featureless indoor environment.

1. Introduction

High-precision dynamic positioning is in great demand in Unmanned Aerial Vehicle (UAV) and Unmanned Ground Vehicle (UGV) applications. The most popular technology is the integration of carrier-phase-based differential global navigation satellite systems (GNSS) and commercial-grade or tactical-grade Inertial Navigation Systems (INS) to provide centimetre-level high-accuracy navigation with good GNSS availability [1]. However, a GNSS signal cannot be always available; in a GNSS-denied environment, INS can offer an accurate position solution for a short period, although drift will accumulate and errors will increase without bounds.
Various methods to reduce or bound INS drift have been addressed in the past. A feature-aided INS with external sensors can be placed in odometers, magnetometers, or cameras for Light Detection and Ranging (LiDAR) as an alternative to satellite-based navigation technology in a GNSS-denied environment. The most popular feature-aided INS solutions are visually aided and LiDAR-aided systems [2]; they offer affordable solutions that are not affected by RF signal blockage. To achieve a high level of positioning estimation in a close-range area, a visual-aided system uses a texture feature that matched consecutive images that are captured by a calibrated camera. Various feature extraction and matching methods and algorithms based on a passive sensing solution have been developed in previous works: a stochastic projection was proposed by Veth, Michael J. to track features [3,4]; Scale-Invariant Feature Tracking (SIFT), SURF, Center Surround Extrema (CenSURE), and RANSAC algorithms were introduced in a visual odometry application for feature tracking by Scaramuzza & Fraundorfer [5]; and a Hough Transformation (HT) and photogrammetric algorithms have been frequency adopted in feature tracking [6]. Moreover, Inertial Measurement Units (IMU) have been applied in some monocular camera-based visual SLAM (Simultaneous Localization and Mapping) systems, primarily for attitude estimation [7,8,9,10]. Though SLAM systems focus on localization and mapping, the essence of feature matching in SLAM is identical to visually based navigation systems. However, this passive sensing solution extensively relies on the lighting situation of the detected environment, which restricts its applications. Conversely, LiDAR is an active ranging sensor with a laser source that can be used in environments where natural or artificial light sources are not available.
Most existing LiDAR-aided INSs currently use similar positioning methods with a visually aided system; they extract geometric features (points, lines, or planes) from laser scans [11,12,13,14,15,16,17], which increase computing complexity and decrease system stability and availability. Stefan etc. carried out pilot research by proposing a LiDAR scan matching method with a Gauss-Newton algorithm; the matching results were fused with the measurements of an IMU to estimate a full 3D-motion of a moving platform. It is now available as an open-source package for Robot Operating Systems (ROS) [12]; the accuracy of the proposed method is unknown. GMapping has also been widely adopted by various indoor mapping/SLAM programs that use long-range raw LiDAR range data and odometry. However, it has not been optimized for short-range laser scanners, and accurate odometry measurements are absent from most low-cost platforms [18]. As already discussed in [19], the position of some fine-featured objects cannot actually be precisely measured due to the footprint size of the deployed laser scanner. This means that the measured coordinates of the geometry features already contain measurement errors, and these errors will propagate into the final mapping and positioning results.
Feature-aided inertial navigation can be divided into two categories: relative-based systems and absolute-based systems (also called optic flow and feature- or landmark-based visual navigation). Features in consecutive images or scans are detected and relatively matched to determine the rigid-body transformation (translation and rotation) of the sensing platform [4,15,20]. A photogrammetry method is applied while in absolute navigation to rectify position and attitude [11,20] on the premise that the positions of landmarks detected from the images or scans in a global coordinate reference system are already known. After the system is fused with IMUs, it is able to overcome the inherent limitations and drawbacks of each standalone system. This study assumes that the system can work in an unknown environment, and a relative feature-aided method is thus investigated.
Previous research has shown that an Extended Kalman Filter (EKF) is a relatively robust and efficient error estimating framework suitable for dynamic motion systems [21]; in particular, it is widely used in visually based SLAM and navigation systems [6,7,8,9,10,11]. Thus, the EKF is used in this research on the fusion of IMU and LiDAR scan matching.
A global scan matching aided INS method is thus proposed to establish an efficient navigation system for GNSS denied environment; the entire system consists of 2D LiDAR and a commercial-grade IMU sensor. The low-cost IMU provides a short-term coarse transformation of position and attitude. A 2D laser scanner with a self-developed improved probabilistically motivated Maximum Likelihood Estimation (IMLE) algorithm [22] then uses these transformations to refine the search scope to estimate an accurate position and attitude. Finally, these standalone positioning results are loosely coupled with EKF to obtain the final result. Compared with existing LiDAR-aided INS positioning solutions, this paper offers several major contributions. First, a non-feature extracted-grid map-based global scan matching algorithm is applied to aid the inertial system. It is more accurate and stable while providing low computational complexity. Second, the IMLE algorithm is a brute global optimum search method, and an IMU sensor is used to provide an accurate initial position and narrow search scope that can assure the IMLE algorithm avoids a local optimum and accelerates the computation for subsequent real-time applications. Third, LiDAR scan matching depends heavily on environmental features, and IMUs can assist system navigation in a featureless “outage” environment for a short period to sustain a highly accurate positioning solution until geometric features are detected to aid the inertial system. Finally, the fused result can also rectify the initial state of position, velocity and attitude of the INS to sustain long-term running applications.
The rest of this paper is organized as follows: Section 2 describes the system workflow and error models of the INS and LiDAR; Section 3 discusses the field tests and experimental results; and Section 4 offers conclusions.

2. INS and LiDAR Fusion Modelling

2.1. INS Modelling

The INS navigation frame (n-frame) is a local geodetic frame with the x-axis pointing towards geodetic north, the z-axis orthogonal to the reference ellipsoid pointing down, and the y-axis completing a right-handed orthogonal frame. It is also called a north-east-down (NED) system. The body frame (b-frame) is defined at the IMU centre. The dynamic equations in the n-frame are given by [23,24]:
r ˙ n = D 1 v n
v ˙ n = C b n ( f b b a ) ( 2 ω i e n + ω e n n ) v n + g n
C ˙ b n = C b n ( ω n b b × )
ω n b b =   ω i b b C n b ( ω i e n + ω e n n ) b g
D 1 = [ 1 / ( M + h ) 0 0 0 1 / ( N + h c o s φ ) 0 0 0 1 ]
where the position in the navigation frame is r n = [ φ , λ , h ] , φ is the latitude, λ is the longitude, and h is the height above the earth surface; v n = [ v N , v E , v D ] is the platform velocity; C n b is the transformation matrix from the b-frame to the n-frame and vice-versa for C b n ; f b is the specific force; ω i b b is the body angular rate measured by gyroscopes expressed in the b-frame; ω i e n and ω e n n are the Earth turn rate in n-frame and the turn rate of the n-frame with the respect to the Earth; (   ω n b b × ) is the skew symmetric matrix of ω n b b ; g n is the local gravity vector; M and N are the radii of curvature in the meridian and prime vertical; and b a   a n d   b g are the drift of the accelerometer and gyroscope, respectively.
If there were no additional errors, the above mechanization equations could estimate the position and velocity of the system from the raw data of the IMU. However, the IMU outputs contain errors and cause the navigation results to rapidly drift; compensating for this drift is difficult. Thus, an error propagation model must work alongside the system motion model to further correct and obtain a better navigation solution; this is also the theoretic basis of the INS and LiDAR EKF fusion model of this paper. A classic perturbation analysis via a first-order Taylor series expansion is applied, and the error state vector and state-space model are defined as follows:
δ x = [ δ r n , δ v n , ε n , δ b a , δ b g ]
u = [  δ f b , δ ω i b b ]
δ x ˙ = F δ x + Gu
δ x k + 1 = k δ x k + w k
k = exp ( F t ) I + F t
Q = diag ( δ g 2 , δ a 2 )
Q k k G Q G T k T t
where the error state δ x are errors of position ( δ r n ), velocity ( δ v n ), attitude ( ε n ), accelerometer ( δ b a ), and gyroscope drift ( δ b g ); F is the dynamic matrix, G is a design matrix and u is the forcing vector of white noise, according to the system motion model and concrete formation of F , G that can be found in the works of Shin, 2001 and 2005 [25,26]; k is the state transition matrix and w k is the driven response of the input white noise at time t k + 1 , i.e., w k ~ N ( 0 , Q k ) ; Q k is the covariance matrix; Q is the spectral density matrix, and δ g , δ a are the standard deviations of accelerometers and gyroscopes, respectively.

2.2. LiDAR Scan Matching

LiDAR is an active range measuring sensor with a laser source that can detect the geometric information of the environment. However, most existing LiDAR-aided inertial navigation systems use feature (point, line, or plane) extraction and matching methods for assisted navigation. The workflow is more complicated and the extraction process may eliminate effective matching features. Moreover, this method is unstable and unreliable, particularly in featureless environments. Thus, a full scan-matching aided INS is proposed in this paper.
The proposed LiDAR scan-matching algorithm-IMLE is a probabilistic scan matching method based on the feature uncertainty model of a LiDAR sensor [27,28]. A likelihood map M stores the likelihood value created from the previous laser scans and the incoming new scans S t are then matched against the map to find the best body transformation T * , where the entire scan provides the maximum likelihood value P ( S t | M ) . The likelihood value P ( p i | M ) of a single point p i on a map M is proportional to the distance d to the nearest environmental feature F, according to the Gaussian probability model of laser-measured noise with scale parameter σ:
P ( p i |M ) e ( d ( p i , F ) / σ )
P ( S t |M ) =   i = 1 n P ( p i |M )
T * = argmax ( P ( T S t | M ) )
As shown in Figure 1, in an IMLE scan matching model, the likelihood map M is organized as a quad-tree pyramid structure to store the likelihood value with multi-resolutions for a large area. It is geo-projected to the INS navigation frame (n-frame) with a Universal Transverse Mercator (UTM) 35N project coordinate reference to fuse the output of each standalone system into a universal local reference; the map grid cell is populated with a series of pre-defined likelihood values: 0.1, 0.3, 0.6, and 0.9. These are empirical values based on the Gaussian probability model shown in Equation 13.
Based on the above model, a brute search algorithm can be deployed to estimate the best body transformation T * within the entire map M. However, this is a time-consuming process; a more practical approach is to search in a refined local search scope extrapolated from the previous state, which can be obtained from the INS. Figure 2 shows an example of an IMLE scan matching algorithm. The red rectangle points indicate the current scan, which searches in the background likelihood map to determine the optimum position and attitude with maximum likelihood values.
Figure 1. The pyramid structure of likelihood map and the pre-defined likelihood values.
Figure 1. The pyramid structure of likelihood map and the pre-defined likelihood values.
Sensors 15 16710 g001
Figure 2. An example of IMLE scan-matching algorithm.
Figure 2. An example of IMLE scan-matching algorithm.
Sensors 15 16710 g002

2.3. EKF Fusion Modelling

An Extended Kalman Filter is selected to fuse the measurements of the INS and LiDAR scan matching; an overview of the system architecture is shown in Figure 3. The Kalman filter algorithm involves predicting the state based on the system model and updating the state based on the measurements [29,30]. However, the output frequency of an IMU is higher than LiDAR measuring. For example, the output rate of an Xsens MTi IMU is approximately 100 Hz, whereas the adopted Hokuyo LiDAR measuring rate is only 40 Hz. Thus, the IMU predicts the state r IMU n   v IMU n   C b   IMU n at every period by mechanization; EKF filters the results only when the periods of LiDAR observation information r LIDAR n   C b   LIDAR n are obtained. The state error corrections δ x are then estimated and fed back to the IMU mechanization for estimating the final navigation state r EKF n   v EKF n   C b   EKF n , which will be the initial state for the LiDAR scan-matching search at the next period. The next filter iteration then continues.
The EKF observation functions are as follows:
z k =   H k δ x k + v k
z k = [ r I M U n r L i D A R n ϵ I M U n ϵ L i D A R n ]
H k = [ I 3 × 3 0 0 0 0 I 3 × 3 0 0 0 0 ]
R k = diag ( δ r 2 ,   δ ϵ 2 )
where r I M U n is the predicted position from the IMU mechanization; r L i D A R n is the observed position from the LiDAR scan matching; ϵ I M U n and ϵ L i D A R n are the predicted and observed attitudes, which are expressed as Euler angles, respectively; H k describes the relation between the state vector and the measurements; v k is the driven response of the input white noise at time t k + 1 , i.e., v k ~ N ( 0 , R k ) ; and R k is the covariance matrix. δ r , δ ϵ are approximate values based on the properties of the laser scanner device and the angle and range searching intervals of IMLE scan matching.
The estimates of the EKF prediction functions are:
δ x k + 1 = k   δ x k 1  
P k + 1 = k   P k     k T   +   Q k
The Kalman gain is
G k = P k H k T S k 1 = P k H k T ( H k   P k   H k T +   R k ) 1
The state vector is updated as
δ x k = δ x k +   G k ( z k H k δ x k )
P k = ( I   G k H k )   P k
where δ x k and P k are the prior estimate and its error covariance.
Finally, the estimated error δ x k is fed back to the navigation state of position, velocity, and attitude as follows:
r k n = r k n   δ x k n
v k n = v k n   δ v k n
C b n = ( I + ( ε k n × ) ) C b n
b g = b g δ b g
b a = b a δ b a
where r k n , v k n , and C b n are the prior navigation state of position, velocity, and attitude; ( ε k n × ) is the skew symmetric matrix of attitude error ε k n ; and b g and b a are the prior drift of the accelerometer and gyroscope.
Figure 3. The system architecture of the LiDAR-aided Inertial Navigation System.
Figure 3. The system architecture of the LiDAR-aided Inertial Navigation System.
Sensors 15 16710 g003

3. Results and Discussion

3.1. System Overview

A series of field tests were performed to evaluate the proposed LiDAR-aided inertial navigation system based on [22]. As shown in Figure 4, an Xsens MTi-G IMU and a Hokuyo UTM-30LX-EW laser scanner were installed on a rigid platform and horizontally carried by a cart. They were connected to a laptop with a serial port and a USB port, respectively. The Xsens is a MEMs-based six Degree of Freedom (DOF) miniature commercial grade IMU with an output rate of 100 Hz, an Angular Random Walk (ARW) of 3 degree / h , a Velocity Random Walk (VRW) of 0.12 m / s / h , and a Gyro and Accelerometer Bias Instability of 200 degree/h and 2000 mGal (1 Gal = 1 cm/s2) [6,31]; The coverage of the LiDAR sensor was approximately 0.1 m to 30 m with a 270° scan angle and an angular resolution of 0.25°. A software platform programmed with C++ and Qt was designed for recording the raw data and post-processing navigation; Figure 5 shows the Graphic User Interface (GUI) of the NAVIS software.
Figure 4. The field test cart platform.
Figure 4. The field test cart platform.
Sensors 15 16710 g004
Figure 5. The software platform.
Figure 5. The software platform.
Sensors 15 16710 g005
Two groups of experiments were carried out on the second floor of the Finnish Geospatial Research Institute (FGI) main building: the first group were stationary tests and the second group were dynamic tests along the corridor to evaluate the precision and effectiveness of the position and attitude estimates.

3.2. Evaluation of Stationary Estimation

The stationary positioning experiment was performed at the beginning of the corridor for approximately 3 min. The NAVIS was installed on a cart at an installation height of approximately 1.3 m. As seen in Figure 6a, the likelihood map result of the corridor shows a featureless environment where straight parallel walls dominate the scene; Figures 6b,c show the positioning results of the IMU + LiDAR and LiDAR scan matching, respectively. Figure 6d shows the compared heading result with the two different methods. The result plots provide confirmatory evidence for the following conclusions:
  • The error distribution of the LiDAR scan-matching method is stepwise and the error distribution of the IMU + LiDAR resembles white noise. As previously mentioned, the LiDAR scan-matching algorithm is an IMLE, which is a likelihood grid map-based searching method that determines the optimum position from candidates. The likelihood map is divided into small cells as candidate positions according to the map resolution and the angle searching intervals, which are set to 1 cm and 0.25° in this test. We believe this is the primary reason for stepwise error distribution. However, In the IMU + LiDAR combined method, Gaussian error predominates the EKF model, resulting in a white-noise distribution positioning error.
  • The estimated positioning and heading results of LiDAR scan matching are better than the IMU + LiDAR fused method. In the static condition, the incoming laser scan has no feature changes. The LiDAR range measure noise is the only stochastic noise source, and with this optimized condition the IMLE easily detects the platform as stationary. However, when a commercial grade IMU is integrated with LiDAR, the accumulated drift of the gyroscope and accelerometer undermines the accuracy of the final positioning result, although it is verified by LiDAR in an EKF. However, the heading errors are minor and can be neglected for positioning processing. It is anticipated that when a higher-grade IMU (tactical-grade or navigation-grade IMU) is integrated, the positioning error can be mitigated.
  • The positioning results of the y-axis are better than the results of the x-axis, regardless of whether the IMU is integrated. Table 1 shows the numerical statistics of the stationary experiments. The RMS errors of the x-axis, y-axis, and heading estimation with the IMU + LiDAR solution are 0.009 m, 0.007 m, and 0.065°. However, the corresponding RMS errors with the LiDAR only solution are 0.007 m, 0.004 m, and 0.000°. The RMS error of the x-axis is higher than that of the y-axis because there are more features along the y-axis (along the corridor direction) than the x-axis (across the corridor direction) for scan matching. As shown in Figure 7a, almost all laser scan points are horizontally distributed; only a few points are vertically distributed, which makes the positioning accuracy of the Y direction greater than the X direction. This result proves that environmental features proportionally affect positioning results [19]. The reason that the heading estimation equals 0 is that the search step of the current IMLE is 0.25°, with a maximum detected range of 30 m; a 0.25° heading change will cause a maximum 5.3 cm displacement of the laser point on a 30 m target, and this circumstance never occurs during the stationary test.
In stationary positioning, the overall estimated accuracy of position and attitude is higher in the LiDAR scan-matching solution than in the combined IMU + LiDAR solution. The accumulated gyroscope and accelerator drift, as measured by a commercial-grade IMU, deteriorates the final position result, even though the positioning accuracy is still at the centimetre level and the heading estimate RMS error is under 0.2 degrees. The EKF model can be used for LiDAR-aided inertial navigation in certain GNSS-denied environments. This result shows that when an IMU detects a platform as stationary, a LiDAR standalone solution should be deployed rather than the combined solution.
Table 1. The static positioning error statistics (m).
Table 1. The static positioning error statistics (m).
RMS ErrorMean ErrorMaximum Error
IMU + LiDARX0.0090.0070.033
Y0.0070.0580.037
Heading0.065 (degree)0.055 (degree)0.123 (degree)
LiDARX0.0010.0060.015
Y0.0040.0030.005
Heading0.0 (degree)0.0 (degree)0.0 (degree)
Figure 6. (a) Likelihood map result of static filed test. (b) The positioning result plot with IMU + LiDAR; (c) The positioning result plot with LiDAR scan matching; (d) The heading result of IMU + LiDAR and LiDAR scan matching.
Figure 6. (a) Likelihood map result of static filed test. (b) The positioning result plot with IMU + LiDAR; (c) The positioning result plot with LiDAR scan matching; (d) The heading result of IMU + LiDAR and LiDAR scan matching.
Sensors 15 16710 g006aSensors 15 16710 g006b

3.3. Evaluation of Dynamic Estimation

To prove the effectiveness of the LiDAR-aided inertial navigation system, further dynamic field tests were performed. The NAVIS was installed on a cart and the cart was driven along the corridor of the third floor of the FGI main building several times by an operator. The results of the IMU + LiDAR combined solution and the LiDAR standalone solution were analysed and compared.
Figure 7 shows the likelihood map generated with the two different methods. Blue dots represent the map generated with the combined solution; black dots represent the map generated with the LiDAR standalone solution. The two maps are compared with the reference map, which is represented with red dots and generated by a Terrestrial Laser Scanner (TLS). At the beginning of the trajectory, the left sides of the corridors generated with the two methods are aligned and coincide well with the reference point. Then, the map with the black dots begins to deviate from the reference points; the deviation accumulates to approximately 1.2 m at the end of the trajectory. The primary reason for this deviation is the featureless environment at the LiDAR height (1.3 m) consisting of two parallel walls with glass windows and handrails at the small hall (A) and corridor turn (B). Conversely, the blue dots are aligned and coincide well during the entire trajectory. This implies that the estimated errors, inherent in the LiDAR standalone solution, are eliminated by a commercial-grade IMU measurement. These data suggest that the LiDAR-aided inertial system works to mitigate the mapping errors of a LiDAR standalone system.
Figure 7. The Mapping results with IMU + LiDAR (blue dot) and LiDAR (black dot), compared with TLS (red dot).
Figure 7. The Mapping results with IMU + LiDAR (blue dot) and LiDAR (black dot), compared with TLS (red dot).
Sensors 15 16710 g007
It is well known that an inertial navigation system includes both attitude and range estimation. In an IMLE, a normalized N P ( S t | M ) of LiDAR scan likelihood is introduced to evaluate the navigation quality. It has a relative value from 0 to 1, thus denoting an overlap level of the current scan with previous scans. A value closer to 1 indicates a higher-quality navigation solution. N P ( S t | M ) can be calculated as follows:
NP ( S t | M )   = i = 1 n P ( p i |M ) n
A series of N P ( S t | M ) for each navigation period with the IMU + LiDAR solution and LiDAR standalone solution are shown in Figure 8. The patterns of maximum N P ( S t | M ) are the same, which implies that the range estimation (and the displacement) is almost identical with two methods. We conclude that the difference in the final trajectories of the two methods is primarily affected by the heading estimation. The main error corrected by IMU is the attitude estimation, and Figure 9a,b show evidence that proves this result. After approximately 15 s, the cart enters the area of the small hall, where there is a relatively feature-poor environment. The heading estimated error appears with the LiDAR standalone solution and the accumulated error does not remain fixed to the end. At 60 s, the heading differences reach a maximum 3.7 degrees at the turn of the corridor, which is full of glass handrails. However, the results also prove that IMUs significantly contribute to attitude estimations, particularly for short-period heading estimations that can sustain an accurate heading estimation in a feature-poor environment for a short period until the LiDAR scan matching re-enters a feature rich environment.
Figure 8. The Maximum Likelihood Value of IMU + LiDAR and LiDAR scan matching.
Figure 8. The Maximum Likelihood Value of IMU + LiDAR and LiDAR scan matching.
Sensors 15 16710 g008
Figure 9. (a) The estimated heading of the IMU + LiDAR combined solution and LiDAR standalone solution; (b) the estimated heading difference between the IMU + LiDAR combined solution and the LiDAR standalone solution.
Figure 9. (a) The estimated heading of the IMU + LiDAR combined solution and LiDAR standalone solution; (b) the estimated heading difference between the IMU + LiDAR combined solution and the LiDAR standalone solution.
Sensors 15 16710 g009aSensors 15 16710 g009b
Moreover, 36 key points of environmental features are selected as a reference network to evaluate the mapping and positioning accuracy of the proposed method, as shown in Figure 10. The final accuracy result is shown in Table 2. When the system is moving, the results of the combined solution are obviously better than the LiDAR standalone solution; the RMS error of the combined solution is 0.084 m, remaining at the centimetre level. The RMS error of the other system becomes 0.433 m and drifts to 1.2 m by the end of the trajectory.
Table 2. The dynamic positioning error statistics (m).
Table 2. The dynamic positioning error statistics (m).
RMS ErrorMean ErrorMaximum Error
IMU+LiDAR0.0840.0750.188
LiDAR0.4330.3361.195
Figure 10. Key points of environmental features selected for accuracy evaluation.
Figure 10. Key points of environmental features selected for accuracy evaluation.
Sensors 15 16710 g010

4. Conclusions

In summary, this paper proposes a LiDAR scan-matching aided inertial navigation system based on a commercial-grade IMU and LiDAR combined into one system, with raw IMU outputs used to refine the search scope of SLAM to optimize brute search efficiency. The positioning results calculated by an IMU mechanization algorithm are fused with the SLAM results on a navigation frame with a loosely coupled EKF. This combination mitigates the corresponding inherent drawbacks of a standalone solution to establish stable long-term navigation in a GNSS-denied environment. The results of stationary and dynamic field tests confirm that (1) in a feature poor environment, LiDAR scan matching provides more accurate navigation state estimation than a low-cost IMU + LiDAR system can offer under stationary conditions. In addition, (2) IMU mechanization provides better attitude estimation in dynamic tests, regardless of whether environmental features are rich; it can significantly mitigate the inherent heading estimation errors introduced by scanning matching methods in feature-poor environments. Moreover, (3) LiDAR scan matching contributes to range estimation along the moving direction, which is the basis for accurate heading estimation. Finally, (4) the dynamic positioning accuracy remains at the centimetre level with the proposed combined solution, even in a featureless environment. The proposed LiDAR-aided INS system can overcome the drawbacks of each standalone system and achieve centimetre-level positioning accuracy. It can also be successfully applied in a GNSS-denied environment.
Only 2D navigation was investigated in the current platform configuration. In future work, two LiDARs will be installed vertically and horizontally on the mobile platform for 3D position estimation to verify the performance of 3D navigation in more complicated situations. In this paper, we also found that the outputs of a commercial-grade IMU degraded the accuracy of the combined solution in a stationary survey. Several tactical IMUs will be integrated into the current NAVIS setup to evaluate how these higher-accuracy IMUs could benefit indoor mapping accuracy.

Acknowledgments

This study was financially supported by the National Nature Science Foundation of China (41304004), the Academy of Finland (project: “Centre of Excellence in Laser Scanning Research” (272195)).

Author Contributions

Jian Tang and Yuwei Chen conceived and designed and performed the experiments, wrote the paper; Xiaoji Niu, Li Wang analyzed the IMU/LiDAR system; Liang Chen and Jingbin Liu contributed reagents/materials/analysis tools; Chuang Shi and Juha Hyyppä reviewed the paper.

Conflicts of Interest

The authors declare no conflict of interest.

References

  1. Fotopoulos, G.; Cannon, M. An overview of multi-reference station methods for cm-level positioning. GPS Solut. 2001, 4, 1–10. [Google Scholar] [CrossRef]
  2. Miller, M.M.; Soloviev, A.; Uijt de Haag, M.; Veth, M.; Raquet, J.; Klausutis, T.J.; Touma, J.E. Navigation in GPS Denied Environments: Feature-Aided Inertial Systems; Air force research lab eglin afb fl munitions directorate: Valparaiso, FL, USA, 2010. [Google Scholar]
  3. Veth, M.M.; Raquet, J. Fusion of Low-Cost Imaging and Inertial Sensors for Navigation. J. Inst. Navig. 2007, 54, 11–20. [Google Scholar] [CrossRef]
  4. Veth, M.J. Fusion of Imaging and Inertial Sensors for Navigation. Ph.D. dissertation, Air Force Institute of Technology (AFIT), Dayton, OH, USA, 2006. [Google Scholar]
  5. Scaramuzza, D.; Fraundorfer, F. Visual odometry [tutorial]. IEEE Robot. Autom. Mag. 2011, 18, 80–92. [Google Scholar] [CrossRef]
  6. Jiang, W.; Wang, L.; Niu, X.; Zhang, Q.; Zhang, H.; Tang, M.; Hu, X. High-Precision Image Aided Inertial Navigation with Known Features: Observability Analysis and Performance Evaluation. Sensors 2014, 14, 19371–19401. [Google Scholar] [CrossRef] [PubMed]
  7. Nützi, G.; Weiss, S.; Scaramuzza, D.; Siegwart, R. Fusion of IMU and vision for absolute scale estimation in monocular SLAM. J. Intel. Robot. Syst. 2011, 61, 287–299. [Google Scholar] [CrossRef]
  8. Lupton, T.; Sukkarieh, S. Efficient integration of inertial observations into visual SLAM without initialization. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, St. Louis, MO, USA, 10–15 October 2009; pp. 1547–1552.
  9. Kneip, L.; Martinelli, A.; Weiss, S.; Scaramuzza, D.; Siegwart, R. Closed-form solution for absolute scale velocity determination combining inertial measurements and a single feature correspondence. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Shanghai, China, 9–13 May 2011; pp. 4546–4553.
  10. Kleinert, M.; Schleith, S. Inertial aided monocular SLAM for GPS-denied navigation. In Proceedings of the IEEE Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI), Salt Lake City, UT, USA, 5–7 September 2010; pp. 20–25.
  11. Klein, I.; Filin, S. Lidar and INS Fusion in Periods of GPS Outages for Mobile Laser Scanning Mapping Systems. ISPRS-Int. Arch. Photogramm. Remote Sens. Spat. Inf. Sci. 2011, 3812, 231–236. [Google Scholar] [CrossRef]
  12. Kohlbrecher, S.; Von Stryk, O.; Meyer, J.; Klingauf, U. A flexible and scalable slam system with full 3d motion estimation. In Proceedings of the IEEE International Symposium on Safety, Security, and Rescue Robotics, Kyoto, Japan, 1–5 November 2011; pp. 155–160.
  13. Soloviev, A. Tight coupling of GPS, laser scanner, and inertial measurements for navigation in urban environments. In Proceedings of the Position, Location and Navigation Symposium, 2008 IEEE/ION, Monterey, CA, USA, 5–8 May 2008; pp. 511–525.
  14. Wang, X.; Toth, C.; Grejner-Brzezinska, D.; Sun, H. Integration of terrestrial laser scanner for ground navigation in GPS-challenged environment. In Proceedings of the XXI Congress of International Society for Photogrammetry and Remote Sensing, Beijing, China, 3–11 July 2008; pp. 3–11.
  15. Borges, G.A.; Aldon, M.J. Optimal Robot Pose Estimation using Geometrical Maps. IEEE Trans. Robot. Autom. 2002, 18, 87–94. [Google Scholar] [CrossRef]
  16. Pfister, S.T. Algorithms for Mobile Robot Localization and Mapping Incorporating Detailed Noise Modeling and Multi-scale Feature Extraction. Ph.D. dissertation, California Institute of Technology, Pasadena, CA, USA, 2006. [Google Scholar]
  17. Horn, J.P. Bahnführung Eines Mobilen Roboters Mittels Absoluter Lagebestimmung Durch Fusion Von Entfernungsbild-und Koppelnavigations-Daten. Ph.D. Thesis, Technical University of Munich, Munich, BY, Germany, 1997. [Google Scholar]
  18. Giorgio, G.; Cyrill, S.; Wolfram, B. Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters. IEEE Trans. Robot. 2007, 23, 34–46. [Google Scholar]
  19. Tang, J.; Chen, Y.; Chen, L.; Liu, J.; Hyyppä, J.; Kukko, A.; Chen, R. Fast Fingerprint Database Maintenance for Indoor Positioning Based on UGV SLAM. Sensors 2015, 15, 5311–5330. [Google Scholar] [CrossRef] [PubMed]
  20. Kim, H.S.; Baeg, S.H.; Yang, K.W.; Cho, K.; Park, S. An enhanced inertial navigation system based on a low-cost IMU and laser scanner. In Proceedings of the SPIE 8387, Unmanned Systems Technology XIV, 83871J, Baltimore, MD, USA, 1 May 2012.
  21. Maybeck, P.S. Stochastic Models, Estimation And Control; Navtech Book & Software Store: Dayton, NY, USA, 1994. [Google Scholar]
  22. Tang, J.; Chen, Y.; Jaakkola, A.; Liu, J.; Hyyppä, J.; Hyyppä, H. NAVIS—An UGV Indoor Positioning System Using Laser Scan Matching for Large-Area Real-Time Applications. Sensors 2014, 14, 11805–11824. [Google Scholar] [CrossRef] [PubMed]
  23. Jekeli, C. Inertial Navigation Systems With Geodetic Applications; Walter de Gruyter: Berlin, Germany, 2001. [Google Scholar]
  24. Titterton, D.; Weston, J. L. Strapdown Inertial Navigation Technology, 2nd ed.; The American Institute of Aeronautics and Astronautics and the institution of electrical engineers: Reston, FL, USA, 2004. [Google Scholar]
  25. Shin, E.-H.; Naser, E.-S. Accuracy Improvement of Low Cost INS/GPS for Land Applications; University of Calgary, Department of Geomatics Engineering: Calgary, AB, Canada, 2001. [Google Scholar]
  26. Shin, E.H. Estimation Techniques for Low-Cost Inertial Navigation. Available online: http://www.geomatics.ucalgary.ca/links/GradTheses.html (accessed on 10 July 2015).
  27. Kurt, K.; Ken, C. Markov localization using correlation. In Proceedings of the Sixteenth International Joint Conference on Artificial Intelligence, San Francisco, CA, USA, 15 August 1999; pp. 1154–1159.
  28. Olson, E.B. Real-time correlative scan matching. In Proceedings of IEEE International Conference on Robotics and Automation (ICRA), Kobe, Japan, 12–17 May 2009; pp. 4387–4393.
  29. Simon, D. Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches; John Wiley Sons: Hoboken, NJ, USA, 2006. [Google Scholar]
  30. Grewal, M.S.; Andrews, A.P. Kalman Filtering: Theory and Practice; Prentice-Hall, Inc.: Hoboken, NJ, USA, 1993. [Google Scholar]
  31. MTi User Manual. Available online: https://www.xsens.com/wp-content/uploads/2013/12/MTi-User-Manual.pdf (accessed on 10 July 2015).

Share and Cite

MDPI and ACS Style

Tang, J.; Chen, Y.; Niu, X.; Wang, L.; Chen, L.; Liu, J.; Shi, C.; Hyyppä, J. LiDAR Scan Matching Aided Inertial Navigation System in GNSS-Denied Environments. Sensors 2015, 15, 16710-16728. https://doi.org/10.3390/s150716710

AMA Style

Tang J, Chen Y, Niu X, Wang L, Chen L, Liu J, Shi C, Hyyppä J. LiDAR Scan Matching Aided Inertial Navigation System in GNSS-Denied Environments. Sensors. 2015; 15(7):16710-16728. https://doi.org/10.3390/s150716710

Chicago/Turabian Style

Tang, Jian, Yuwei Chen, Xiaoji Niu, Li Wang, Liang Chen, Jingbin Liu, Chuang Shi, and Juha Hyyppä. 2015. "LiDAR Scan Matching Aided Inertial Navigation System in GNSS-Denied Environments" Sensors 15, no. 7: 16710-16728. https://doi.org/10.3390/s150716710

Article Metrics

Back to TopTop