^{1}

^{*}

^{1}

^{2}

^{*}

^{1}

^{1}

^{1}

This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution license (http://creativecommons.org/licenses/by/3.0/).

Mobile autonomous systems are very important for marine scientific investigation and military applications. Many algorithms have been studied to deal with the computational efficiency problem required for large scale Simultaneous Localization and Mapping (SLAM) and its related accuracy and consistency. Among these methods, submap-based SLAM is a more effective one. By combining the strength of two popular mapping algorithms, the Rao-Blackwellised particle filter (RBPF) and extended information filter (EIF), this paper presents a Combined SLAM—an efficient submap-based solution to the SLAM problem in a large scale environment. RBPF-SLAM is used to produce local maps, which are periodically fused into an EIF-SLAM algorithm. RBPF-SLAM can avoid linearization of the robot model during operating and provide a robust data association, while EIF-SLAM can improve the whole computational speed, and avoid the tendency of RBPF-SLAM to be over-confident. In order to further improve the computational speed in a real time environment, a binary-tree-based decision-making strategy is introduced. Simulation experiments show that the proposed Combined SLAM algorithm significantly outperforms currently existing algorithms in terms of accuracy and consistency, as well as the computing efficiency. Finally, the Combined SLAM algorithm is experimentally validated in a real environment by using the Victoria Park dataset.

Mobile autonomous system are often required to operate in highly uncertain and dynamic environments, such as a battle zone, with minimal or no external help such as GPS or human intervention. Other applications range from disaster relief in areas such as the 9/11 WTC site, to underwater and planetary exploration. Hence, the development of true autonomy in such systems can have very wide military, societal and scientific impact. In order to be “truly autonomous”, any such autonomous system needs to be capable of accomplishing two sub-tasks: (1) it needs to model its environment, along with its attendant uncertainties, as well as find its own location within the environment, using only the noisy observations from its on-board sensors, and (2) it needs to use the environment model to intelligently plan its actions within the environment such that it accomplishes its objectives in a robust and timely fashion. Problem 1 above is known as the Simultaneous Localization and Mapping (SLAM) problem, while the addition of planning to the problem of SLAM in Problem 2 results in the so-called Simultaneous Planning, Localization and Mapping (SPLAM) problem [

After the first closed formulation was introduced, the SLAM problem has attracted immense attention in the mobile robotics fields [

Some scientists have studied the submap-based solution to SLAM problem in large scale environments, e.g., by mixing RBPF with UKF [

Based on the theoretical analysis, the Combined SLAM, a novel submap-based solution to SLAM problem, is proposed for mobile system sensing in large scale environments. The proposed algorithm combines the strength of two popular mapping algorithms: RBPF and EIF. As RBPF-SLAM can avoid linearization of the robot model during operation and provide a robust data association, it is used to produce local maps which are periodically fused into an EIF-SLAM algorithm. Specifically, the sparse information matrix together with the recovery of state vector and covariance submatrix of EIF algorithm significantly improves the whole computational efficiency, and simultaneously avoids the tendency of RBPF-SLAM to be over-confident.

In contrast with sequential submap joining strategy, a binary-tree-based decision-making strategy is proposed to merge the submaps efficiently in order to reduce the computational cost and improve the real-time performance. It has been illustrated that the strategy will result in a total computation cost that is less than

The remainder of this paper is structured as follows: after an overview of related work in the next section, the overall structure of the proposed Combined SLAM algorithm is outlined in Section 3. Section 4 describes the probability distribution of RBPF-SLAM algorithm and its conversion, while in Section 5 we provide a brief review of the EIF-SLAM algorithm for fusing local submaps into a global map. Section 6 presents the binary-tree-based decision-making strategy, as well as its computational cost. In Section 7 we carry out a set of experimental comparisons in terms of accuracy and consistency by using simulated and the Victoria Park datasets, while the results are also discussed. Finally, Section 8 summarizes the most important conclusions of this work.

Considering the estimation algorithm for SLAM problem, the most popular one is EKF. The effectiveness of the EKF approach comes from the fact that it estimated a fully correlated posterior over feature maps and robot poses. However, the EKF-based SLAM algorithm suffers from three well-known drawbacks which complicate its application to large real-world environments: quadratic complexity with respect to the size of the map; inconsistency due to its linearization approximation and sensitivity to failures in data association. Many approaches have been developed to overcome these shortcomings, and the most common ones are: (a) RBPF-SLAM, first proposed by Murphy [

In order to improve the computational efficiency of SLAM, with the goal of being able to map large scale environments in real time, the global map is decomposed into smaller submaps in the popular algorithms, and then submap joining is implemented to form a global map [^{2}) per step. Two recent algorithms have provided important reductions in computational cost: with an amortized cost of O(n) per step, SLSJF SLAM [^{1.5}) per step in the worst cases. The Treemap algorithm [

The aforementioned works have solved the large scale SLAM problem in a number of different forms. In this paper, we present a novel combined SLAM: a submap-based approach to SLAM problem which combines the strengths and avoids the weaknesses of two popular mapping strategies: RBPF-SLAM and EIF-SLAM. Local map building is carried out by using the RBPF-SLAM algorithm which does not suffer from linearization problems and is much more robust in association ambiguity situations. After a sequence of submaps is built, each of them is assembled in a single multi-dimensional Gaussian fashion for the subsequent process of submap joining from the RBPF-SLAM results. Then, submap joining is implemented for fusing the built local submaps into a global map by using the EIF-SLAM algorithm, which not only has the advantages of EKF-SLAM, but also decreases the computational complexity and improves the consistency of estimation.

The submap-based SLAM problem is how to slice the whole running time of a robot into consecutive periods, and each period has its own beginning. Suppose a local map is expressed by (^{L}^{L}^{L}^{L}^{L}

In this section, we will address the Rao-Blackwellized particle filter used in submap building and particularly discuss the probability distribution of RBPF-SLAM and its conversion.

In probabilistic form, the SLAM algorithm can be described by [^{t}_{1}, _{2}, …, _{t}_{1}, _{2}, …, _{n}^{t}^{t}^{t}

When a submap is fusing into the global map, EIF-SLAM algorithm requires this submap be subject to the above distribution in the form of a single multi-dimensional Gaussian. However, the probability distribution of RBPF-SLAM algorithm is a Gaussian Mixture Model (GMM; it will be presented in Section 4.2). Therefore, we should convert the Gaussian Mixture Model of RBPF-SLAM into a single multi-dimensional Gaussian model for the usage of EIF-SLAM.

In the RBPF-SLAM algorithm,

In order to represent the distribution of each particle as low-dimensional Gaussians, the particle can equivalently be represented as:

A single multi-dimensional Gaussian model with mean _{t}_{t}

The first term in the square bracket of

In the real implementation, every particle of RBPF-SLAM has its own data association decisions, consequently, both the number of features and their ordering may differ between particles. In order to convert the Gaussian Mixture Model (GMM) of RBPF-SLAM into a single multi-dimensional Gaussian model for the usage of EIF,

Assign a unique index to each observation. The form of each particle can be augmented with a set of extra correspondence variables

Apply the maximum likelihood data association in every particle. After the process of data association, we can use its results to implement particle updating and particle augmenting for each particle showed in

Re-arrange and re-modify each particle according to a standard which is decided by the particle with the highest importance weight, so as to produce a common features set. The process is accomplished as follows:

Let _{t}

In such case that a common feature may have no corresponding feature in any particle, then the particle will be discarded. Through the above three steps, each particle obtains a single multi-dimensional mean and covariance, and a single Gaussian distribution can then be produced by using

After submaps are established, the submap joining will be implemented. In this section, we will describe the submap joining algorithm based on an extended information filter.

In the local submap joining, the input is an estimation only related to “local” information. If the features among the sequent local submaps are not dense, then the sparsification can be implemented to achieve the map joining effectively. The most important thing is that when the distance among the submaps becomes farther, the correlation between the features of the submaps will be lower. This will result in an exactly sparse information matrix without any approximation, so the EIF-SLAM algorithm will maintain lower computational complexity than EKFSLAM. The state vector of the global map in the proposed algorithm contains feature positions and robot ending poses of each local submap. As usual, the starting point of the first local map is also taken as the starting point of the global map. After the ^{G}^{G}^{G}

When the Extended Information Filter is applied to the estimation problem, an off-diagonal element of the information matrix is non-zero only when the two related objects (the features and the robot starting/ending poses) are within the same local map. Since the size of each local map is limited, any objects will only link to their nearby objects, no matter how many (overlapping) local maps are fused. This results in an exactly sparse information matrix without any approximation. Since all the objects involved in the local maps are included in the global state vector, no marginalization is required in the map joining process and thus the information matrix ^{G}

When the (

Data association aims to find the features in the (^{T}

Algorithm of data association between the global map and the (

Input: the ( |

(1) Find a set of potentially overlapping local maps |

(i) Compute the distance _{i} |

(ii) Set a threshold TH1 (usually the range of the detection sensor); |

(iii) If _{i} |

(2) Find the set of potentially matched features |

(i) Set another threshold TH2; |

(ii) Find the features of the potential local maps whose Euclidean distance are smaller than the TH2; |

(3) Recover the covariance submatrix associated with the pose and the potentially matched features; |

(4) Use NN method to identify the final match. |

After data association, the new features as well as the ending pose in the (^{G}

Suppose we have got the (^{L}_{k+1}(^{G}_{k+1} is the zero-mean Gaussian “observation noise” with covariance matrix ^{L}^{G}

What we need is not the information vector ^{G}^{G}^{G}

During the global map building, in order to reduce the computational cost and improve the real-time performance, we use a binary-tree-based decision-making strategy to merge the submaps efficiently. In a sense, successful submap building is the first step of successful submap joining. Thus the decision-making strategy will be introduced firstly; it is used to generate a sequence of independent submaps with unequal size, then the built submaps are joined to produce the final global map according to the binary-tree-based strategy.

Before submap joining, we should ensure the effectiveness of submap building, which depends not only on the submap building algorithm itself (RBPF-SLAM), but also whether each submap is completed properly. Otherwise, the accuracy of the built submap building will be remarkably reduced due to some problems such as error accumulation and particle depletion, thus affecting the accuracy of the final global map produced by submap joining. Therefore, in decision-making strategy of submap building, several factors should be considered:

The first concern is the situation when there is no feature to be detected over a long period of time. Since the accumulated errors cannot be eliminated while the robot is running, we must reduce the running time of the current submap as far as possible. In such case, a constant threshold time TIME is set to

Secondly, particle diversity must be considered. Within the threshold time, along with the appearing of features, if the diversity problem occurs, the robot should stop the construction of the current submap and start to build a new one.

The last factor we must consider is the number of features in each submap. Generally, a good rule of thumb is that the ratio of feature number to robot pose dimension should be kept between 5 and 20. If feature number of the selected standard particle used in Section 4.2 arrives at the threshold Num_Feature, then we should cease the construction of current submap immediately. The decision-making strategy is illustrated in the following flow chart (

Decision-making Strategy.

Every built submap should be fused into the global map with a binary-tree-based strategy. The leaves of the binary tree stand for the sequence of the _{1,j}. These maps are joined pairwise to produce _{2,j}, which will be joined pair wise into _{3,j} in turn, until finally two local maps of size _{log2m−1,j} will be fused into one map, which is the final global map. The hierarchical binary tree fashion is depicted in

The total computational complexity of CombinedSLAM with the binary-tree-based decision-making strategy is:
_{num}_{ij}_{2}

It can be proven that the EIF is equivalent to the EKF in algebra. EIF has almost the same performance with the EKF except for improving the computational efficiency. Therefore, the following four algorithms in terms of their accuracy, consistency will be compared:

The conventional single-map-type EKF-SLAM(EKF);

The conventional single-map-type RBPF-SLAM(RBPF);

The submap-based SLAM algorithm which combines EKF and EIF(EKF-EIF);

The proposed submap-based Combined SLAM: combining RBPF and EIF (RBPF- EIF);

For the computational efficiency, the proposed RBPF- EIF algorithm will be compared with the submap-based SLAM algorithm which combines RBPF and EKF (RBPF-EKF). In order to verify the efficiency of the binary-tree-based decision-making strategy, the comparison will be made also between the conventional single-map-type EKF-SLAM (EKF-SLAM), sequential submap-type Combined SLAM (SS-CombinedSLAM) and binary-tree-based submap-type Combined SLAM (BS-Combined SLAM,

The experimental environment consists of 554 randomly-generated point features and simulates a two-dimensional world with a size of 2 km × 2 km, as shown in ^{®} and executed on 2.60 GHz Pentium^{®} Dual-Core CPU E5300 with 2 GB of RAM.

To compare the performance of the above mentioned four algorithms in a large-scale environment, we use 50 Monte Carlo simulation experiments. The four algorithms run on exactly the same data with a loop closure; data association is known in all algorithms. The results of the four algorithms are shown in

Estimation errors of the four algorithms are shown in

Normalized Estimation Error Square (NEES) and Root Mean Square (RMS) error are used to evaluate the consistency of the proposed filter. When the ground truth

Consistency is checked up by using the chi-squared criteria:

When CI < 1, the estimation is consistent with the ground truth, and when CI > 1, the estimation is inconsistent with respect to the ground truth. We tested consistency of the previous four algorithms by carrying out 100 Monte Carlo runs in the simulated experiments.

The mean consistency index CI of robot pose and features for the four algorithms are presented in

The evolution of Root Mean Square (RMS) error on robot position and orientation is shown in

From parts A and B of the experimental results, it can be noted that among the four algorithms our proposed RBPF-EIF SLAM algorithm demonstrates better performance, especially in dealing with problems as accuracy and consistency in large-scale environment. For the computational cost, the corresponding evaluation is reported as below.

In addition to evaluate accuracy and consistency of the proposed Combined SLAM, we still need to simulate and analyze its computational cost.

In order to validate another advantage of this work,

Finally, the Victoria Park dataset (available at

An efficient submap-based Combined SLAM algorithm for large-scale environments which combines the strengths of both RBPF-SLAM and EIF-SLAM had been studied in this paper. The RBPF-SLAM algorithm for local map building provides robust data association and improves the estimate validity, and the EIF-SLAM algorithm for submap joining globally allows uncertainty to be remembered over long robot trajectories. Simulation results clearly show that the proposed submap-based Combined SLAM algorithm has better consistency and accuracy in large-scale environments than the current typical algorithms. Furthermore, the proposed algorithm is still computationally efficient when the binary-tree-based submap joining strategy is introduced. The well known Victoria Park dataset has also been applied to verify the improved validity of the Combined SLAM approach and its computation time advantage.

This work is partially supported by the High Technology Research and Development Program of China (2006AA09Z231), Natural Science Foundation of China (51075377), and the Science and Technology Development Program of Shandong Province (2008GG1055011, BS2009HZ006) and Zhejiang Natural Science Foundation (R1100015) and the Open Foundation (GZKF-201017) of the State Key Lab of Fluid Power Transmission and Control at Zhejiang University.

^{2}) square root unscented kalman filter for visual simultaneous localization and mapping

The overall structure of the Combined SLAM algorithm.

Hierarchical binary-tree-based strategy.

The results of the four algorithms and the zoomed first ten submaps of RBPF-EIF.

Error and 3

Mean consistency index CI of robot pose and features for the four algorithms.

RMS errors of robot pose and feature by using the four algorithms.

The computational time used for submap joining by RBPF-EKF and RBPF-EIF.

The computation time for three algorithms: conventional single-map-type EKF-SLAM (blue), sequential submap-type Combined SLAM (red) and binary-tree-based submap-type Combined SLAM (green).

Results for the Victoria Park dataset.

The detailed CPU time by using algorithms of RBPF-EKF and RBPF-EIF.

RBPF-EKF | 4.4 s | 1,445.1 s | 3.4 s | 1,452.9 s |

RBPF-EIF | 13.2 s | 2.4 s | 7.2 s | 22.8 s |