Multilayer Matching SLAM for Large-Scale and Spacious Environments

Abstract

In large-scale and spacious environments, keeping a reliable data association and reducing computational complexity are challenges for the implementation of Simultaneous Localization and Mapping (SLAM). Focused on these problems, a multilayer-matching-based incremental SLAM algorithm is proposed in this article. In this algorithm, SLAM is simplified as a problem composed of a least-square-based optimization problem and data association. Then, it is solved in two steps. Firstly, a multilayer matching method is applied to deal with the data-association problem. Both matching between observation and local map and matching between different local maps are carried out. The uncertainty of the results-matching is described by the Fisher information matrix. Secondly, the robot pose is optimized through an incremental QR decomposition method. This algorithm effectively avoids the local minima caused by the limited observation information, and can build a consistent map of the environment. Meanwhile, the characters (hierarchical and incremental) of the proposed algorithm ensure low computational complexity. Experiments on simulation environments and two kinds of real environments with different sparse features verify that the algorithm is applicable for real-time application in large-scale and spacious environments.

Keywords

SLAM Multilayer Matching ICP Data Association

1. Introduction

Reliable concurrent map building and localization and navigation based on the maps built are the basis for mobile robots to fulfil their assignments. Simultaneous localization and mapping (SLAM) is one of the key enabling technologies for mobile-robot navigation [1, 2, 3]. SLAM addresses the problem of acquiring a spatial map of the environment while simultaneously localizing the robot relative to this model.

After decades of development at the theoretical or conceptual level, SLAM could be regarded as a mature problem [1]. In large-scale and spacious environments, however, there are still numerous problems to be solved [2, 4] from the practical standpoint. A hybrid map is widely used to deal with large-scale environments. For example, in a recent work, SDP-SLAM [5] which combines the benefits of the SegSLAM algorithm [6] and the DP-SLAM method [7], was shown to generate more accurate maps with low alignment errors in submap combinations. Besides the accuracy problem in map-building, high computational complexity is also an important problem in large-scale environments. In order to reduce the computational effort required for solving large-scale SLAM problems, in reference [8] a decoupled SLAM (D-SLAM) algorithm for large-scale-environment map-building that combines local maps is presented; it has significant computational advantages, which are illustrated through computer simulations.

The algorithm's computational complexity can be reduced by reducing posture numbers as described in reference [9].

However, most of the existing methods mentioned above have not taken into the consideration spatial characteristics in large-scale environments. Spacious environments widely exist in urban environments (such as gardens, campuses and squares) and industry environments (such as electric transformer substations). A laser range finder (LRF) has been widely used in these situations for reasons of precision. However, due to the character of spacious environments, only a small amount environmental information is received by an LRF. This brings a great challenge to SLAM in the areas of data association and computational complexity.

Several approaches have been reported in the literature to investigate the SLAM problem in such spacious and large-scale environments. Based on sparse extended information filtering, Thrun [10] successfully solves the SLAM problem in a park environment. However, the study focuses on the localization filter of some special environmental features (trees) and cannot be applied to environments with no such obvious characteristics. Huang [11] proposes sparse local submap joining filter (SLSJF) for map-building in large-scale environments. However, this also depends on environmental features. Estrada [12] presents a hierarchical mapping method with efficient maintenance of loop consistency. Another category of SLAM approaches [13,14, 15] matches the raw LRF data with the environment map directly. These approaches do not extract environmental features, and offer universal and practical methods for general environments. As with the improvement algorithms of FastSLAM [16] and FastSLAM 2.0 [17], GMapping [18] is more effective in terms of both accuracy and time consumption of the algorithm. Data association is solved through the traversing method, which uses raw laser-range-finder data to acquire accurate grid maps instead of predefined landmarks. Map matching and data correlation are resolved with the help of a local histogram [14], which reduces the computational complexity and can be used in large-scale environments. The above methods have shown good performance in campuses, parks and urban environments. However, since they do not explicitly take characteristics of a spacious environment into consideration in the system design explicitly, they can still fall into local minima or even risk failure when the robot is placed in a spacious environment, due to the scarce observation information with large amounts of sensor noise.

Based on the analysis mentioned above, the SLAM problem is still an open issue in large-scale and spacious environments requiring higher localization precision and lower computational cost. A multilayer-matching-based Incremental SLAM algorithm (M2ISLAM) is proposed in this article. In this algorithm, the data-association problem is solved by the multilayer matching method, and the uncertainty is described with the Fisher information matrix. The key innovation of this article is that the multilayer matching method effectively avoids the local minima caused by scarce environmental information, establishes a consistent environmental map and ensures low computational complexity of the algorithm.

The proposed algorithm is tested and compared with the GMapping algorithm in two environments with different sparse features. The results demonstrate the high precision and real-time performance of the proposed method in large-scale and spacious environments.

2. SLAM Problem Definition and Algorithm Architecture

2.1 Preliminaries

The SLAM problem, as defined in the rich body of literature on SLAM, is described as the estimation of posterior probability P( x _1t, m | z _1t, u _1t) [1], where m is the map of the environment.

\begin{array}{l} P (x_{1 : t}, m | z_{1 : t}, u_{1 : t}) \\ = P (m | z_{1 : t}, x_{1 : t}, u_{1 : t}) P (x_{1 : t} | z_{1 : t}, u_{1 : t}) \\ = P (m | z_{1 : t}, x_{1 : t}) P (x_{1 : t} | z_{1 : t}, u_{1 : t}) \end{array}

(1)

A robot's pose at time t will be denoted as x _t; the observation of sensor is z_t, the input of robot is u _t, and x _1:t={ x _1:t ⋯, x _t} is the navigation trajectory from time 1 to t.

As shown in equation (1), the map m depends on the robot trajectory and the observation of corresponding sensors. Furthermore, SLAM will be simplified to the problem of robot trajectory estimation [1]:

\begin{array}{l} P (x_{1 : t} | z_{1 : t}, u_{1 : t}) = P (x_{0}) • P (z_{t} | x_{t}) • \\ P (x_{t} | x_{t - 1}, u_{t}) • P (x_{0 : t - 1} | z_{1 : t - 1}, u_{1 : t - 1}) \end{array}

(2)

P{ x _1:t, m | z _1:t, u _1:t) is not calculated directly in the SLAM algorithm based on Pose Graph [18]. Therefore, the maximum likelihood estimation will be obtained as follows:

\begin{array}{l} x_{t}^{*} = \underset{x_{1 : t}}{argmax} (\log P (x_{t} | x_{t - 1}^{*}, u_{t}) + \log P (z_{t} | x_{t}) + \log (x_{t - 1}^{*})) \\ m^{*} = {(x_{t}^{*}, z_{t}), \dots, (x_{1}^{*}, z_{t})} \end{array}

(3)

where P( x _t | x _t−1^*, u _t) is the motion model, parameterized by the control input u _t,P(z_t | x _t) is the measurement model, m^* is the maximum likelihood estimation of environment's map, and x_t^* is the maximum likelihood estimation of robot pose from time 1 to t.

Kinematics and observation models of robot are generally assumed such that they meet the Gaussian distribution. Then, they are converted to the following form:

x_{1 : t}^{*} = \underset{x_{1 : t}}{argmin} \sum_{(i, j) \in ℂ} {‖ x_{i} \oplus T_{i j} - x_{j} ‖}_{Σ_{i j}}^{2}

(4)

In equation (4), the operator ⊕ expresses the coordinate transformation. T_ij and Σ_ij represent respectively the mean and the information matrix of a constraint relating the parameters x_i and x_j. In the following paragraphs, as the main contribution of this article, T _ij, and Σ_ij are obtained through a multilayer matching method.

According to equations (3) and (4), the following is obtained:

x_{1 : t}^{*} = \underset{x_{1 : t}}{argmin} \sum_{(i, j) \in ℂ} {‖ x_{i} \oplus T_{i j} - x_{j} ‖}_{Σ_{i j}}^{2}

(5)

In equation (5), ℂ is all of the possible values of i and j. Previously, SLAM has been simplified to a least-square problem based on graph optimization [19]. f_j( x _i) = x _i ⊕ T _ij is a non-linear function. It is linearized as follows:

f_{j} (x_{i}) = F_{i j} \partial x_{i} + f_{j} (x_{i}^{0})

(6)

In equation (6), $F_{i j} = {\frac{\partial f_{j} (x_{i})}{\partial x_{i}} |}_{x_{i}^{0}}$ is the Jacobian matrix of f_j(x_i).

After the linearization, the linear least-square problem is obtained:

\begin{matrix} δ x_{1 : t}^{*} = \underset{x_{1 : t}}{argmin} \sum_{(i, j) \in ℂ} ‖ H_{i j} F_{i} δ x_{i} - H_{i j} δ x_{j} \\ {^{}}^{} {^{}}^{} {^{}}^{} {^{}}^{} {^{}}^{} {^{}}^{} {^{}}^{} {^{}}^{} {^{}}^{} {+ H_{i j} x_{j}^{0} + H_{i j} f (x_{i}^{0}) ‖}^{2} \end{matrix}

(7)

In equation (7), $H_{i j} = Σ_{i j}^{- \frac{1}{2}}$ . Set $X = {(δ x_{1}^{T}, δ x_{2}^{T}, \dots, δ x_{t}^{T})}^{T}$ can be updated as an optimal trajectory estimation problem:

\begin{matrix} X^{*} = \underset{X}{argmin} {‖ A X + b ‖}_{}^{2} \\ = \underset{X}{argmin} {‖ Q {(\begin{matrix} R & 0 \end{matrix})}^{T} X + {(\begin{matrix} b_{1} & b_{2} \end{matrix})}^{T} ‖}_{}^{2} \\ = R^{- 1} Q^{T} b_{1} \end{matrix}

(8)

Q (R 0)^T is the QR decomposition of A . The incremental method [18] is used for QR decomposition.

2.2 Algorithm Architecture

Figure 1 shows the system structure of the proposed algorithm. The SLAM problem is simplified into two parts: data association and graph optimization. The robot pose is optimized through the incremental QR decomposition method.

As the key part of SLAM, data association determines the accuracy of SLAM. The constraint relationship T _ij between x _t and x _1:t−1 is estimated by the Iterative Closet Point (ICP) algorithm [20–22] according to the observation of z_t and z_1:t−1 Because ICP cannot deal with the uncertainty, the Fisher matrix is used to estimate the uncertainty of matching results. Then, the covariance matrix Σ_ij between x _t and x _1:t−1 is obtained.

Figure 1.

Architecture of M2ISLAM

Obviously, the matching between z_t and z_1:t−1 cannot be done by ICP due to the computational complexity. The multilayer matching method is proposed in this article to deal with the matching between z_t and the local map, and the matching between different local maps. Therefore, high matching accuracy is ensured and the complexity of the algorithm is reduced at the same time. In a traditional matching strategy [23], only matching between z_t and z_1:t−1 or between z_t and the observation of the neighbourhood is carried out. In contrast, the proposed algorithm effectively reduces the accumulation error caused by incorrect matching [24], and also avoids having the algorithm fall into local minima in spacious environments.

In the second part (graph optimization), the features of environments are not extracted, and the Pose Graph is used to directly establish the appearance-based map of the environment [2,14]. A consistent environmental map with high precision is built, and local minima caused by the scarce observation information are avoided. Meanwhile, the computational complexity of the SLAM algorithm has been greatly decreased due to the characteristics of multilayer matching and incremental QR decomposition.

3. Multilayer Matching Method

Firstly, the matching between reference and observation points is considered. The collection of reference points is marked as M = { m ′₁ ⋯, m _p), and the collection of observation points is set as D = { d ₁ ⋯, d _p}. The transformation between the two collections is marked as T = ( R, t ), where R is the rotation matrix and t is the translation vector. The iterative process of ICP operates as follows:

Step 1: Calculate the corresponding relationship between M ′ and D according to the Nearest Neighbour principle (NN):

{\overset{⌢}{m}}^{'}_{i} = \underset{m_{j} \in M}{\arg \min} {‖ {m^{'}}_{j} - (R d_{i} + t) ‖}^{2}

(9)

Step 2: Calculate the matching error:

E (R, t) = \sum_{i} {‖ {\overset{⌢}{m}}^{'}_{i} - (R d_{i} + t) ‖}^{2}

(10)

Step 3: Search for the optimal ( R, t ) through the SVD method [24] and minimize the matching error E ( R, t ):

(R^{*}, t^{*}) = \underset{(R, t)}{\arg \min} E (R, t)

(11)

Step 4: If ( R ^*, t ) does not vary anymore, suspend the iterative process or return to Step 1.

A searching for the closest points should be carried out several times in one iterative process; this plays a key role in the ICP algorithm's performance. The K-D tree method [26] is used in this article to collect the reference points and reduce the computational complexity. Furthermore, observations which do not match with others due to the noise and unknown obstacles will be discarded. That is to say, they will be discarded in equations (10) and (11) if $‖ {\overset{⌢}{m}}^{'}_{i} - (R d_{i} + t) ‖ > E$ , E is a threshold for comparison between M ′ and D , which were determined empirically during the experiments.

Then, the poses' constraint problem during robot navigation is analysed. z_t is the observation at time t. There are data associations between z_1:t−1 and the environmental information included in z_t. These associations will be divided into two categories according to their continuity (continuous and discontinuous). As shown in Figure 2, the robot starts from x ₀ and returns to the starting point x ₀ after exploring a series of regions. Then, ( x ₈, z ₈) have associations (constraints) with both ( x ₇, z ₇) and ( x ₀, z ₀).

Figure 2.

Example of poses' constraint

For the poses' constraint, whose observations are adjacent on time, as shown in Figure 3-a and Figure 3-b, z _t is matched with local map $l_{t, m_{t}}$ (constructed by $z_{t - m_{t} : t}$ ; m_t denotes the periodic numbers to create the current local map $l_{t, m_{t}}$ ) at time t. Here the Local map is defined as the map constructed by robot pose x and sensor's adjacent observation z during m cycle periods (see Figure 3-c). The initial coordinate of matching uses an odometer or x _t−1 Then $l_{t, m_{t}}$ will be expressed as the collection of $z_{t - m_{t} : t - 1}$ :

l_{t, m_{t}} = {(x_{t - 1}, z_{t - 1}), …, (x_{t - m_{t}}, z_{t - m_{t}})}

(12)

Figure 3.

Matching process between observation and local maps. (a) Red map denotes local map $l_{t, m_{t}}$ and blue contour denotes current observation Z_t. (b) After ICP matching, observation Z_t matches local map $l_{t, m_{t}}$ . (c) Local map $l_{τ, m_{τ}}$ obtained at previous time τ. (d) Current local map $l_{t, m_{t}}$ matches previous local map $l_{τ, m_{τ}}$ after ICP matching.

We will now analyse a situation where the robot returns to the region it has explored at time τ. In spacious environments, there are no significant differences between most observations. Eventually, the accumulated error after long-distance navigation would cause there to be a great difference between the initial value of the iterative matching algorithm and true value. It is therefore easy to fall into local minima if the matching is processed based only on single-frame observations. Therefore, in this article, $l_{t, m_{t}}$ and $l_{τ, m_{τ}}$ (information collection of $z_{τ - m_{τ} : τ}$ ; m_τ denotes the periodic numbers to create the current local map $l_{τ, m_{τ}}$ ) are matched. The matching process firstly discretizes these local maps to grid maps then calculates the overlap ratio between two grid maps. When the overlap between l_{t,m
_t} and l_{τ,m_τ} exceeds a certain threshold, they are regarded as the description of the same region (the robot returns to the region which has been explored). Then, the association between x _t and x _{τm_τ:τ} will be established.

The computational complexity of the matching method is determined by the area of the local map. In this article, τ ∊ {τ_j} has been discretized according to the length of navigation. The local map is built once the robot travels a fixed distance of w_threshold at time τ_j+1.

If the intersection of boundary convex hulls between l_{t,m
_t} and l_{τ,m_τ} is greater than a certain area (it also will be determined by experience during experiments), there is an overlap between l_{t,m
_t} and l_{τ,m_τ}. Then, the ICP matching between l_{t,m
_t} and l_{τ,m_τ} will be carried out.

Figure 3 describes the process of the multilayer-matching algorithm expressed as follows:

Step 1: Initialization: w = 0, t = 0, L_τ = ϕ, l = ϕ, w denotes the distance the robot has covered during the current moment t. L_τ is the set of local maps and l is the current local map.

Step 2: $l_{t, m_{t}} = l_{t, m_{t}} \cup {(x_{t}, z_{t})}$ described by K-D tree, t = t + 1.

Step 3: Matching between z_t and l_{t,m
_t}. As shown in Figure 3-a, T _tj (t-m_t jt) are the matching results.

Step 4: w = w + || x _t - x _t−1||; if w ≥ w_threshold go to Step 5 or return to Step 2.

Step 5: Judging whether there is an overlap between l_{τ,m_τ} and l_{t,m
_t}. If yes, go to Step 6; otherwise, return to Step 7.

Step 6: Matching between the local maps (l_{t,m
_t} and l_{τ,m_τ}). As shown in Figure 3-d, T _tj. (τ - m_τ j τ) are the matching results.

Step 7: $L_{τ} = L_{τ} \cup {l_{t, m_{t}}}, w = 0, l = Φ$ return to Step 2.

If the robot does not return to an area it has already explored, step 3 and one-time ICP matching will be executed; therefore, the computational complexity is a certain value, o(1). If the robot returns to an area it has explored, step 5 and matching between the current local map and all the previous local maps is carried out to obtain the explored area; then, the time consumption of ICP matching is denoted by T_icp′. Thus, the computational complexity will be o(n) + T_icp′. The best condition for the proposed algorithm is that the robot never returns to an area it has already explored; then, the computational complexity is o(n). The worst condition is that the robot constantly regards the current observation as belonging to areas it has previously explored; then, the computational complexity is o(n²) + T_icp′o(n). Therefore, this algorithm could be used in real-time applications, and the reliable constraint T will be obtained if there is a loop closure [27].

4. Uncertainty Estimation

After multilayer matching, the uncertainty of matching results will be estimated. The accurate estimation of uncertainty can effectively improve the accuracy of the established map, and inaccurate estimation can lead to an inconsistent map and even damage to the established map.

Assuming that the uncertainty of matching results meets the Gaussian distribution with zero mean, only the covariance Σ needs to be determined. The inversion of the Fisher information matrix is used as the covariance matrix in this article.

The Fisher information matrix is defined as the function of expected measurement and the surface slope scanned by the laser sensor [28]. After discretization, it is used as the uncertainty estimation of matching results. The Fisher information matrix of observation z_t is shown as follows [28]:

I (P) = \sum_{k = 1}^{n} [\frac{s_{k}}{σ^{2}} {(\frac{Δ r_{k}}{Δ P})}^{T} \frac{Δ r_{k}}{Δ P}]

(13)

P is the pose of the robot. r_k is the measurement of the No. k laser ray. Σ² is the variance of noise in the laser data. s_k is the impact factor of the No.k laser ray. λr_k/ΔP denotes the observation change Δr_k in the laser scan when the robot moves ΔP = [Δx_P, Δy_P, Δθ_P]^T. As the function of observation s_k, it also can be regarded as the contribution to the localizability. The greater the deviation between r_k and ${\bar{r}}_{k}$ (the expectation of r_k), the higher the uncertainty of the matching results. So, s_k is defined as the probability distribution function of the laser-ray finder's observations; it is represented as:

s_{k} = \frac{1}{\sqrt{2 π} σ} e^{- \frac{{(r_{k} - {\bar{r}}_{k})}^{2}}{2 σ^{2}}}

(14)

According to the Cramér-Rao Bound (CRB) principle [29], the overall performance of the current localization is described by matrix I (P). It is also the lower bound of covariance of matching results' probability distribution (we might as well assume that matching results reach the lower bound):

cov (P) = I^{−1} (P) = Σ

(15)

If the robot navigates in a 2-D environment:

\frac{Δ r_{k}}{Δ P} = [\begin{matrix} \frac{Δ r_{k}}{Δ x_{P}} & \frac{Δ r_{k}}{Δ y_{P}} & \frac{Δ r_{k}}{Δ θ_{P}} \end{matrix}]

(16)

Figure 4.

Laser scan with different constraints

As shown in Figure 4, we define the observation as {r_k} when the pose is P and assume the observation is {r′_g} when the pose is P′. Then, the following is obtained:

\begin{array}{l} r_{g}^{'} = \sqrt{{(r_{k} \cos (θ^{'}) + Δ x)}^{2} + {(r_{k} \sin (θ^{'}) + Δ y)}^{2}} \\ θ^{'} = θ + Δ θ + f (k) \\ f (k) = \arctan 2 (r_{k} \sin (θ^{'}) + Δ y, r_{k} \cos (θ^{'}) + Δ x) \end{array}

(17)

In equation (17), f(k) represents the scanning angle corresponding to observation value r_k of the No.k laser ray.

Since the observation when the pose is P′ and P′ = P + ΔP is tenable, {r′_g} is obtained in equation (17). Then, Δr_k/ΔP is also obtained. So, according to the analysis of equations (13) and (15), the covariance Σ can be determined by Δr_k/ΔP. The estimation of matching results' uncertainty is thus completed.

5. Experiments and Results Analysis

5.1 Real Experiments

The real experimental environment in this article is an unmanned substation, a typical large-scale and spacious environment. Usually, the area of a 220 KV substation is around 80,000 m². The scale of a 500 KV substation is larger. In electric transformer substations, the electric plants are usually placed sparsely due to safety issues. A part of a realistic scene and its corresponding scan map in substations for 2D-LRF are shown in Figure 5. The scan range of LRF is 18 m, the scan scale is 270°, and the resolution is 0.5°. The spacious feature of the substation environment is shown in the fact that fewer than 10 poles have been scanned by the LRF sensor and displayed in the local map.

Figure 5.

The realistic scene and scan map of part of a substation

In recent years, some studies have executed the application of intelligent mobile robots for inspecting substation equipment [30]. The inspecting robot is used to improve the quality of detection and accelerate the process of setting up unmanned substations. It can be guided by magnetic guidance, vision navigation, or combined GPS/dead-reckoning (DR) navigation [30]. In this article, SLAM is used to deal with robots' localization and navigation problems in substations.

As shown in Figure 6, the inspection mobile robot is used to verify the proposed algorithm. The mobile robot is equipped with an odometer and LRF (SICK LMS111). Consistent with the setting in Figure 5, the same parameters are set for LRF. A laptop with an Intel Core i5 2410M CPU and a 4G DDR3 RAM is used as the control computer.

Considering the noise of the LRF sensor, set E=0.3m. A larger w_threshold will increase the computational complexity of local map matching, and a smaller w_threshold can cause multilayer matching failure, w_threshold = 2m is used in this experiment. In the Fisher information matrix, Δx_T = Δy_T = 0.1m and Δθ_T = 0.1 are used.

Figure 6.

Prototype of the robot for experiments

In order to verify the validity of the proposed algorithm, the GMapping [11] algorithm is selected to compare with the proposed algorithm (M2ISLAM). GMapping does not rely on predefined landmarks and uses raw laser-range-finder data to acquire accurate grid maps; it is more robust and accurate for generating maps in different scales of environment than FastSLAM 2.0 [17]. Compared to other SLAM algorithms, GMapping shows good performance in a campus environment in Freiburg. The Freiburg campus, with an area of 250 m^*250 m, is similar to a substation environment. Experiments are respectively carried out in two substations (shown in Figure 7) with different environmental features (A: 220 KV substation - electric plants are placed densely; B: 500 KV substation - electric plants are placed relatively sparsely). Because the algorithm of GMapping is non-deterministic, the maps whose results are most general and typical are selected. The localization results are determined by averaging the testing values of repeated measurements.

Figure 7.

Planar map and raw data in substation A and B

The robot is controlled remotely at a speed of 0.4 m/s. Sensor information from the LRF and the odometer readings are gathered and recorded with a frequency of 10 Hz. After the process of remote control, the GMapping and the M2ISLAM algorithms are respectively tested with sensor information. The time-consumption of the process and the accuracy of mapping are respectively recorded to compare their validity, ε is defined as the robot pose error [31], expressed as follows:

\begin{array}{l} ε = \frac{1}{N} \sum_{i} ‖ e ‖ \\ e  = δ_{i} - {\hat{δ}}_{i} \\ {\hat{x}}_{i} = {\hat{x}}_{0} \oplus {\hat{δ}}_{i} \\ x_{i} = {\hat{x}}_{0} \oplus δ_{i} \end{array}

(18)

In equation (18), $x_{i}$ ( ${\hat{x}}_{i}$ is the true (estimated) value of the robot pose; $δ_{i} ({\hat{δ}}_{i})$ ) is the transformation of $x_{i} ({\hat{x}}_{i})$ relative to the original pose $x_{0} ({\hat{x}}_{0})$ . $‖ e ‖$ can be selected as 1-Norm or 2-Norm. When the robot navigates in a 2-D environment, e is represented as (x_e, y_e, θ_e)^T. Then, ε is divided into two parts: ε_t (error in x and y directions) and ε_r (error of angle):

\begin{array}{l} ε_{t} = \frac{1}{N} \sum_{i} ‖ {(x_{e}, y_{e})}^{T} ‖ \\ ε_{t} = \frac{1}{N} \sum_{i} ‖ θ_{e} ‖ \end{array}

(19)

It is difficult to measure and record the true value, x _i, of the robot pose in larger experimental environments. Thus, 50 location points are selected from the robot's trajectory. The localizations at all 50 points of two algorithms are marked in Figure 8. No.1 point (origin point) is regarded as the criterion; the next 49 points are compared to it, respectively. Then, 49 difference values in two algorithms are calculated.

Figure 8.

Measurement of the true pose of the robot

Table 1.

Quantitative results of different approaches/scenarios (errors with 1-Norm and 2-Norm are given). ε_t (error in x and y directions), ε_r (error of angle) and t (time-consumption) of the two algorithms in two substations are shown in Table 1. Through the results in Table 1, compared with the GMapping method, M2ISLAM is more accurate.

			GMapping quantity of particles: 50	M2ISLAM
ε_t	A	L1-Norm	0.062±0.074	0.057±0.063
	A	L2-Norm	0.015±0.043	0.022±0.038
	B	L1-Norm	0.121±0.081	0.074±0.076
	B	L2-Norm	0.075±0.064	0.048±0.061
ε_r	A	L1-Norm	0.9±1.0	0.8±1.0
	A	L2-Norm	1.7±2.7	1.5±2.5
	B	L1-Norm	1.6±1.2	0.9±1.1
	B	L2-Norm	3.2±3.1	0.7±2.7
t	A		489.3 s	49.8 s
t	B		949.7 s	165.4 s

From Table 1, for both the M2ISLAM and the GMapping algorithm, time consumption of information processing is less than the time consumption of information acquisition. Therefore, both could be used in real-time applications. The time consumption of M2ISLAM is only 1/5∼1/9 of that of GMapping. This is because only the optimal trajectory of the robot should be maintained and optimized according to the matching results in the M2LSLAM algorithm, but multiple trajectory assumptions should be maintained in the GMapping algorithm (each particle represents a hypothesis).

We will now analyse the mapping performance of GMapping and M2ISLAM in detail.

5.1.1 Mapping in Substation A

In a 220 KV transformer substation, electric plants are placed densely with no loop closure. The length of trajectory in substation A is 399.8 m. Data on a total of 6858 frames are gathered, and the process costs 690 s. The maps built by the M2ISLAM and GMapping algorithms are shown in Figure 9.

Figure 9.

Raw data and the maps of substation A

As shown in Figure 9, compared to the planar map of substation A (shown in Figure 7) the angle error of the map built by the M2ISLAM algorithm is 3.3°. However, the angle error of the map built by the GMapping algorithm is 7.0°. In addition, compared with GMapping, the pose error ε_t and angle error ε_r of M2ISLAM decrease by about 10% (1-Norm index). For the experiment in substation A, when the robot explores the environment it never returns to an area it has explored: there is no loop closure in substation A, unlike in substation B. In substation A, the proposed algorithm was therefore not effective. The performance of M2ISLAM is worse than GMapping in L2-Norm. In substation B, where there is large loop closure, the robot can observe the repeated information. For the multilayer matching method used in the paper, the matching between different local maps will correct the pose of the robot and obtain more accurate maps, both in L1-Norm and L2-Norm. Therefore, the conclusion is that the M2ISLAM algorithm is able to effectively improve the accuracy of mapping and localization and overcome the match failure problem caused by scarce sensor information in such spacious environments.

5.1.2 Mapping in Substation B

In order to verify the proposed algorithm's effectiveness in loop closure, the experiments in substation B are analysed in this section. Compared with substation A, substation B had the following properties:

The area is larger, and the trajectory is more than 1300 m. There are more loop closures; the robot can arrive at the same destination by different paths.

The electric plants are placed more spaciously. Less information is gathered by the robot. As shown in Figure 7, information on only five poles is detected, and more than 90% of the laser rays reached maximum measurement range.

There is more sensor information noise. On the one hand, the ground is not flat, so the assumption of a 2D plane cannot stand. On the other hand, there are more obstructions, such as bushes, which are difficult to measure with LRF.

Data on a total of 117, 228 frames are gathered during remote control, and the process cost 1180 s. As shown in Figure 7, the robot explores area1 and area2 repeatedly with a ‘U’-type route. Figure 10 shows the raw observation data and the maps respectively built by M2ISLAM and GMapping. Both of the maps in area1 and area2 are consistent and can reflect realistic scenes.

Figure 10.

Raw data and the maps of substation B

The robot enters area3 from site k₁. Then, it moves in the opposite direction after a U-turn. Then, after a rectangular route of a length of about 600 m, it enters and explores area3 from site k₂ (as shown in Figure 7). Due to the significant accumulation of odometer error, the data association is not effectively handled by the GMapping algorithm, and this leads to a map-building failure. As shown in Figure 11-b, Part1’ and Part1” are the same as Part1 in Figure 11-a, and Part2’ and Part2” are the same as Part2 in Figure 11-a. However, reliable constraint is produced by local map matching in the M2ISLAM algorithm. The robot pose is also optimized by an incremental method. Then, as shown in Figure 11-a, a consistent map with higher accuracy is obtained.

Figure 11.

The maps of area3

5.2 Simulation Experiments

In order to verify the performance of the proposed algorithm, an open-source simulation platform [32] is used, which is implemented in Gazebo and ROS. Integrated with kinematics, dynamics, and laser sensor, the robot in the simulation platform can also be controlled manually through a joystick for driver training (manual mode). In this simulation platform [32], the model of LRF (SICK LMS111) is embedded, the flicker noise and white noise [33] are fused in the laser-scan observation, and the error model of the odometer is also built. The ground truth of the robot pose could be outputted directly to be compared with the experimental result.

Figure 12.

Simulation Platform

This platform runs on a laptop with i3 1.9GHz CPU and 4G RAM. A parking scenario (50 m^*70 m), illustrated in Figure 12, is built using SketchUp. There are some cars, trees and bushes. The process is the same as in the real experiments. Figure 13 shows the maps built by the M2ISLAM and GMapping algorithms; the map of area4 built by GMapping observably fails.

Figure 13.

The maps of the simulation scenario

After mapping, the robot navigates to 10 test points (shown in Figure 12) in manual mode in the M2ISLAM and GMapping maps (with 50 and 100 particles). The ground truth and estimated value of the robot pose are recorded. Then, e_t (error in x and y directions) and e_r (error of angle) of the two algorithms are also obtained, as shown in Table 2.

Table 2.

Quantitative results in simulation scenario

		GMapping quantity of particles		M2ISLAM
		50	100
ε_t	L1-Norm	1.31±0.97	0.73±0.81	0.65±0.70
ε_r	L1-Norm	1.7±1.5	1.1±1.0	0.9±0.8

From Table 2, the localization accuracy of M2ISLAM is near to that of GMapping with 100 particles, and is more accurate than that of GMapping with 50 particles. The results of the simulation experiments are similar to those of the real experiments.

The source code of the proposed algorithm is available under the following link: http://robocup.situ.edu.cn/athome/msslam.htm

6. Conclusions

The SLAM algorithm is analysed in large-scale and spacious environments in this article, and a multilayer matching-based incremental SLAM algorithm is proposed. The SLAM problem is simplified into two parts: data association and least-squares optimization. For the data-association problem for SLAM in large-scale and spacious environments, the multilayer matching method is presented, and the Fisher information matrix is used to analyse the uncertainty of matching results. The local minima caused by the scarce information are avoided effectively, and accumulation of incorrect matching is reduced. The incremental QR decomposition is also used to improve the real-time ability and efficiency of the algorithm. The algorithm is tested on a simulation platform and also successfully applied to the inspection robot in transformer substations. As typical large-scale and spacious environments, two substations with different characteristics are selected to verify the proposed algorithm and compare its performance with common SLAM methods. The results show that the proposed algorithm could be applied in actual industrial environments.

Footnotes

7.

This work is partly supported by the National High Technology Research and Development Program of China under grant 2012AA041403 and the Natural Science Foundation of China under grant 61175088.

References

Durrant-Whyte

Bailey

(2006) Simultaneous localization and mapping: part I. IEEE Robotics & Automation Magazine. 13(2): 99–110.

Bailey

Durrant-Whyte

(2006) Simultaneous localization and mapping (SLAM): Part IT IEEE Robotics & Automation Magazine. 13(3): 108–117.

Wang

, and Zhang

(2007) A upf-ukf framework for slam. IEEE International Conference on Robotics and Automation, pp.1664–1669.

Neira

, and Tardós

J D.

(2001) Data association in stochastic mapping using the joint compatibility test. IEEE Transactions on Robotics and Automation. 17(6): 890–897.

Maffei

Jorge

Kolberg

, and Prestes

(2013) Segmented DP-SLAM. IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 31–36.

Eliazar

A I

and Parr

(2004) DP-SLAM 2.0. IEEE International Conference on Robotics and Automation. pp.1314–1320.

Fairfield

Kantor

G A

, and Wettergreen

(2010) Segmented slam in three-dimensional environments. Journal of Field Robotics. 27(1): 85–103.

Huang

S D

Wang

, and Dissanayake

(2010) Mapping large scale environments using relative position information among landmarks. IEEE international Conference on Robotics and Automation. pp.2297–2302.

Kretzschmar

Stachniss

, and Grisetti

(2011) Efficient information-theoretic graph pruning for graph-based SLAM with laser range finders. IEEE/RSJ International Conference on Intelligent Robots and Systems, pp.865–871.

10.

Thrun

Tiu

Koller

. (2004) Simultaneous localization and mapping with sparse extended information filters. The International Journal of Robotics Research. 23(7–8): 693–716.

11.

Huang

Wang

, and Dissanayake

(2008) Sparse local submap joining filter for building large-scale maps. IEEE Transactions on Robotics. 24(5): 1121–1130.

12.

Estrada

Neira

, and Tardos

J D.

(2005) Hierarchical SLAM: real-time accurate mapping of large environments. IEEE Transactions on Robotics. 21(4): 588–596.

13.

Grisetti

Stachniss

, and Burgard

(2007) improved techniques for grid mapping with raoblackwellized particle filters. IEEE Transactions on Robotics. 23(1): 34–46.

14.

Bosse

, and Zlot

(2008) Map matching and data association for large-scale two-dimensional laser scan-based slam. The International Journal of Robotics Research. 27(6): 667–691.

15.

Olufs

, and Vincze

(2009) An efficient area-based observation model for monte-carlo robot localization. IEEE/RSJ International Conference on Intelligent Robots and Systems, pp.13–20.

16.

Montemerlo

Thrun

Koller

. (2002) FastSLAM: A factored solution to the simultaneous localization and mapping problem. AAAI Conference on Artificial Intelligence and Innovative Applications of Artificial Intelligence Conference. pp.593–598.

17.

Montemerlo

Thrun

Koller

. (2003) FastSLAM2.0: an improved particle filtering algorithm for simultaneous localization and mapping that provably converges. International Joint Conference on Artificial Intelligence, pp. 1151–1156.

18.

Kuemmerle

Grisetti

Strasdat

. (2011) g2o: A general framework for graph optimization. IEEE International Conference on Robotics and Automation, pp.3607–3613.

19.

Kaess

Ranganathan

, and Dellaert

(2008) iSAM: Incremental smoothing and mapping. IEEE Transactions on Robotics. 24(6): 1365–1378.

20.

Nüchter

Lingemann

Hertzberg

. (2007) 6D SLAM-3D mapping outdoor environments. Journal of Field Robotics. 24(8–9): 699–722.

21.

Holz

, and Behnke

(2010) Sancta simplicitas-on the efficiency and achievable results of SLAM using ICP-based incremental registration. IEEE International Conference on Robotics and Automation, pp. 1380–1387.

22.

Rodriguez-Losada

, and Minguez

(2007) Improved data association for icp-based scan matching in noisy and dynamic environments. IEEE International Conference on Robotics and Automation, pp.3161–3166.

23.

Pulli

(1999) Multiview registration for large data sets. 2nd International Conference on 3-D Digital Imaging and Modeling, pp.160–168.

24.

Bailey

Nieto

Guivant

. (2006) Consistency of the EKF-SLAM algorithm. IEEE/RSJ International Conference on Intelligent Robots and Systems. pp.3562–3568.

25.

Besl

P J

, and McKay

N D.

(1992) Method for registration of 3-D shapes. IEEE Transactions on Pattern Analysis and Machine Intelligence. 14(2): 239–256.

26.

Bentley

J L.

(1975) Multidimensional binary search trees used for associative searching. Communications of the ACM, 18(9), 509–517.

27.

Gutmann

J-S

, and Konolige

(1999) Incremental mapping of large cyclic environments. IEEE International Symposium on Computational Intelligence in Robotics and Automation, pp. 318–325.

28.

Censi

(2007) On achievable accuracy for range-finder localization. IEEE International Conference on Robotics and Automation, pp.4170–4175.

29.

Bobrovsky

, and Zakai

(1975) A lower bound on the estimation error for Markov processes. IEEE Transactions on Automatic Control. 20(6): 785–788.

30.

Wang

Guo

. (2012) SmartGuard: An autonomous robotic system for inspecting substation equipment. Journal of Field Robotics, 29(1): 123–137.

31.

Burgard

Stachniss

Grisetti

. (2009) A comparison of SLAM algorithms based on a graph of relations. IEEE/RSJ International Conference on Intelligent Robots and Systems, pp.2089–2095.

32.

Liao

Wei

Zhang

. (2013) An open-source development and simulation platform for smart wheelchairs. International Conference on Intelligent Robotics and Applications (ICIRA 2013), Busan, Korea, Sept. 25–28.

33.

Jain

Nandy

Chakraborty

Kumar

. (2011) Error modeling of Laser Range Finder for robotic application using time domain technique. IEEE Conference on Signal Processing, Communications and Computation, pp.1–5.