A perception-driven exploration hierarchical ... - SAGE Journals

Research Article

A perception-driven exploration hierarchical simultaneous localization and mapping for mobile robots adapted to search and rescue environments

Advances in Mechanical Engineering 2018, Vol. 10(3) 1–18 Ó The Author(s) 2018 DOI: 10.1177/1687814018765874 journals.sagepub.com/home/ade

Hongling Wang1, Chengjin Zhang1,2 , Yong Song2 and Bao Pang1

Abstract Perception-driven hierarchical simultaneous localization and mapping approach is proposed based on a distributed particle of percolator model adapted to search and rescue post-disaster environment. The proposed hierarchical analysis reduced the complexity; it can only highlight the interesting three-dimensional objects marked on the two-dimensional map, which adapted to urgent search and rescue post-disaster scenarios. Object identification, area coverage, and loop closure are the key issues that simultaneous localization and mapping faces when mobile robots detect and reach survivors in search and rescue collapsed environments. We developed hierarchical analysis using iterative extended Kalman filter method to improve the localization accuracy in simultaneous localization and mapping of mobile robots with an iteration process. An analytic hierarchical map-building mechanism efficiently constructs a globally consistent topological map using graph-based nodes and data association. An improved iterative extended Kalman filter procedure is used to estimate positions and the covariance matrix. The proposed algorithm incorporates a prediction step and a measurement update step, which performs the state vector time update in constant time interval and the correction of the state vector based on the new sensor measurement. The hierarchical mapping process is divided into three levels. The bottom level perceives the surroundings and collects input. The middle level receives and processes data from the bottom level. The upper level organizes the feature information from the previous levels to form a global, topologically consistent map. Simulations and experiments using the mobileRobot platform, Pioneer LX robot, and a crawler mobile robot validate the efficiency of this hierarchical simultaneous localization and mapping approach adapting to search and rescue post-disaster scenarios. Keywords Hierarchical simultaneous localization and mapping, loop closure, perception-driven exploration, distributed particle percolator, iterative extended Kalman filter

Date received: 1 August 2017; accepted: 26 February 2018 Handling Editor: Fakher Chaari

Introduction Simultaneous localization and mapping (SLAM) is vital process for mobile robots conducting search and rescue (SAR) tasks while extracting external features from a series of images. Earlier versions of the SLAM approach ignored damage to a building’s interior.1 The

1

School of Control Science and Engineering, Shandong University, Jinan, China 2 School of Mechanical, Electrical and Information Engineering, Shandong University, Weihai, Weihai, China Corresponding author: Chengjin Zhang, School of Mechanical, Electrical and Information Engineering, Shandong University, Weihai, #180 West Wenhua Road, Weihai 264209, China. Email: [email protected]

Creative Commons CC BY: This article is distributed under the terms of the Creative Commons Attribution 4.0 License (http://www.creativecommons.org/licenses/by/4.0/) which permits any use, reproduction and distribution of the work without further permission provided the original work is attributed as specified on the SAGE and Open Access pages (https://us.sagepub.com/en-us/nam/ open-access-at-sage).

2 hierarchical SLAM procedure employs topological mapping to organize graph-based local sub-maps in unstructured SAR areas. Currently, topological maps, feature-based maps, and grid-based maps incorporate the three basic map types. Topological map adapts well to unstructured areas and is well suited to higher dimensional mapping, but it cannot locate a mobile robot accurately. Feature-based maps are good at extracting obvious landmarks for mapping and localization; however, they do not perceive potential unstructured obstacles. Grid-based maps supply discrete occupancy rates of destroyed areas, but the mobile robot’s position cannot be located.2 Hierarchical SLAM requires the local sub-maps to be statistically independent, which constrains data sharing among local metric sub-maps. Hierarchical local maps (HLMs) contain a set of sub-maps; at each time point, a new sub-map is added indicating and storing a mobile robot’s position and covariance at that moment. However, such an algorithm is not well adapting to SAR post-disaster scenarios. When the HLM SLAM method is used in mobile robot SAR scenarios, closing loops and the computational cost of local sub-maps are the key issues. Hierarchical stereo visual SLAM builds low-level metric maps and high-level topological global map in unstructured SAR scenarios. The whole map is divided into low-level local sub-maps, and the highlevel topological map maintains global consistency. The graph-based hierarchical SLAM framework is designed for SAR scenarios in which mobile robots continuously conduct local level localization and mapping. Global SLAM is primarily implemented for closing loops. The local level SLAM runs simultaneously in each different calculation unit and creates a pose-graph trajectory for sub-maps.3 Conventional structure from motion (SFM) estimates the mobile robot’s motion via sequences of image frames, which is not well adapted to consistent localization.4 A 6-degree-of-freedom (6-DoF) stereo visual system captures landmarks for providing information regarding location and angle relative to single camera. However, MonoSLAM cannot solve the computational complexity of covariance. A divide-and-conquer approach is employed for this hierarchical visual SLAM. Decoupled stochastic mapping (DSM) divides the global map into smaller cells, and each cell is a unit, or part, of the high-level topological map; however, loop closure remains an unsolved issue in this method. Computational complexity is conventionally addressed by working on a constrained space or by increasing selection steps on the total number of identified landmarks.5 Constrained relative sub-map filters (CRSFs) maintain the local map structure, and the local maps then connect with other neighborhood grid maps. The constant time SLAM (CTS) algorithm links the local sub-maps to form a graph-based topological map in

Advances in Mechanical Engineering the Atlas system. The traditional linearized methods of the extended Kalman filter (EKF) cannot extract precise data for mapping, which results in inaccurate estimations in SAR areas and issues with closing loops due to the increased nonlinear uncertainties. Stochastic mapping techniques provide an approach to impose loop consistency on high-level topological maps; additionally, the iterated extended Kalman filter (IEKF) algorithm is optimized for loop closing.5 Although global position system (GPS) is applied to many SLAM systems when operating in normal environments, the robot perception-driven hierarchical SLAM system is designed to be independent of GPS because GPS signal is frequently unavailable and unreliable in SAR post-disaster environments. D. Vivet et al.6 proposed a ground-based radar sensor system for SLAM that is independent of a GPS signal. As a result, the trajectory and location of the mobile robot can be obtained based on proprioceptive sensors (e.g. gyrometers, odometer sensors, and inertial measurement unit (IMU)) in collapsed environments in which GPS is unavailable. O Montiel-Ross et al.7 guided a mobile robot using an adaptive candidate matching window instead of GPS data to accomplish an exploration through an environment with obstacles; there were four main processes (i.e. perception, localization, cognition, and motion control) involved in perception-driven exploration (PDE). Hierarchical GPS-aided visual SLAM only depends on the information captured by a wide-angle camera and uses IEKF and a multi-level relaxation (MLR) algorithm. Visual information is fused with robust scale-invariant feature transform (SIFT) descriptors to ensure loop closure. A low-level stereo visual system provides information for EKF computation. Local sub-maps are related to a mobile robot’s series of positions and, over these fingerprints, a global topological map is formed.8 In a graph-based hierarchical procedure, the fusion of GPS data and dead reckoning IMUs is not highly reliable due to the robot’s periodic trajectory; however, the complexity of the covariance computation in using an EKF algorithm constraints the process to low-level SLAM. A real-time hierarchical SLAM system includes multi-level SLAM, generating a high-level topological map.9 In a hierarchical representation, the hybrid metric–topological map divides the exploration area into sub-maps on the basis of a spectral clustering algorithm. Disasters commonly occur in coal mines, a mobile robot traditionally uses a semi-autonomous controller to conduct exploration in a collapsed area. A hierarchical reinforcement learning (HRL) algorithm is used with the SLAM process in coal mine SAR scenario.10 This HRL system is composed of different sensors and wireless network modules. A method for three-layer, hierarchical multi-mobile robot SLAM has been established in an urban SAR scenario; the piloted robot and

Wang et al. the followed robots constitute of hierarchal SLAM architecture.11 There exist large domains in which high precision mapping is essentially impossible with any reasonable particles. The distribution of trajectories incorporates the effects of observations; these trajectories are returned by the low-level mapping process, and the high-level SLAM process can only weigh the match between the new observations and the existing high-level maps.12 HRL method is used to conduct SLAM exploration for mobile robots in SAR environment, and a two-level hierarchical visual SLAM (VSLAM) is used to build globally consistent elevation maps for loop closure by utilizing radio frequency identification devices (RFID).10,13 However, conventional SLAM methods do not consider morphological characteristics of the environment that result from damage to the interior of the ruined space. The remainder of this article is structured as follows. In section ‘‘Hierarchical architecture analysis methods in SLAM,’’ hierarchical architecture methods in SLAM are analyzed. In section ‘‘Hierarchical map-building approach,’’ an analytic hierarchal map-building mechanism is elaborated. In section ‘‘PDE using a hierarchical SLAM algorithm,’’ a PDE hierarchal SLAM algorithm is derived. In section ‘‘MobileRobot platform for SAR SLAM simulations,’’ a mobileRobot simulator is established, and we conduct hierarchical SLAM simulations with a virtual mobile robot using this platform. In section ‘‘The experiments for hierarchical SLAM in unstructured SAR scenarios,’’ the experiments validate the perception-driven hierarchical SLAM adapting to SAR unstructured scenarios. Section ‘‘Results and discussion’’ provides the experimental results and discussion. Section ‘‘Conclusion and future work’’ discusses conclusions and future work.

Hierarchical architecture analysis methods in SLAM Most traditional SLAM methods depend on an EKF algorithm to estimate the mobile robot’s position, the location of landmarks, and the associated covariance matrix. However, time-consuming calculations and slow quadratic memory impede application of such an approach in urgent urban search and rescue (USAR) scenarios. The hierarchical model structure appropriately adapts to such complex environmental characteristics during the SLAM process. A topological–metric procedure algorithm reduces computational costs, and simultaneously, maintains the map’s global consistency. Building only a global topological map without submaps will result in the lack of detail of the exploration area in the new produced map. To remedy this situation, we use a fusion of hierarchical and topological– metric SLAM; this method is designed for mobile robot

3 performing SLAM task in unstructured post-disaster areas. In this strategy, data association is more robust, global maps maintain consistency, and the loop-closure process ensures that the exploration covers the SAR area. Hierarchical analysis divides the global map into local sub-maps; each sub-map has its own processes. Integrated over these local sub-maps, a higher level topological is formed, which helps maintain the global map’s consistency. Using this analytic hierarchy process (AHP), a hybrid metric–topological map is built during SLAM.

Two-level hierarchical particle invasion for localization and dense maps Two-level hierarchical structures contain a local level for the mobile robot’s location relative to a local reference frame and a global level for maintaining consistency in the topological structure. This method cannot implement real-time mapping and localization. A MLR algorithm is therefore applied to build a global map, and the EKF method establishes the mathematical model for local sub-maps. The distribution of collapsed patches is random and unstructured, and the patches appear to have irregular boundaries in a two-dimensional (2D) plane. Distributed particle simultaneous localization and mapping (DP-SLAM) maintains a joint distribution over robot positions and poses. This approach uses a particle filter with presupposed dense sensor data. In Schleicher et al.,9 a two-level (i.e. metric-topological) SLAM has been proposed. The two-level particle filter models inevitable drift resulting from using a finite number of particles in the particle filtering process. Probabilistic expectation maximization (EM) solves the problem of mapping, and the effects of drift on lowlevel maps can accurately approximate perturbations to the end points of the robot’s trajectory.12 Two-level hierarchical SLAM of local sub-maps and global topological maps is well suited to SAR operations in unstructured areas. All local sub-map grids are integrated and stored in a single global map from which they can be efficiently accessed; this forms a map cache O(PB). The total number of low-level map accesses N is represented as ð1Þ

N = O(PB)

where P is the number of particles and B is the area of the grid squares. The low-level particles state vector XL is defined as XL = ½ Xr ,

Y1 ,

Y2 ,

   T

ð2Þ

where Yi represents all local particles on the sub-map.

4

Advances in Mechanical Engineering

The state vector of mobile robot observations Xr is expressed as follows Xr = ½ Xrob ,

qrob ,

v T

nrob ,

ð3Þ

where nrob and v are the linear and the angular speed of the mobile robot, respectively. Xrob is the mobile robot’s location relative to the local frames and qrob is the pose orientation, which is represented as qrob = ½ q0 ,

qx ,

qy ,

qz 

ð4Þ

Equations (1)–(4) establish a low-level particle filter model. vSLAM uses a monocular camera to provide information. This improved approach is combined EKF and an extended information filter (EIF) for pose and state estimation.9 The low-level processes build feature-based local metric sub-maps, and the high-level processes maintain a graph-based global topological map. With the improved IEKF, the computational complexity is obviously reduced, and loop closures are optimized. The independent local sub-maps are integrated into a global topological map.

A three-layer hierarchical control architecture In Grisetti et al.,14 a three-level hierarchy is constructed using graph-based nodes; the three levels consist of the bottom, intermediate, and top levels. The lowest level perceives the surroundings and collects input; the middle level processes the received data from the bottom level, and the highest level organizes the feature information to form a globally consistent map. The updated local sub-maps result in an optimized upper level global topological map. The resulting eigenvector can be a weight vector that indicates a unique hierarchical ranking. The unique hierarchical rankings for the scheme layer and the criterion layer are calculated in the same way. In this hierarchical approach, a vector of particles Cartesian coordinates is defined as X = ½ x1 ,

,

xn  T

ð5Þ

and the distances between different particles are calculated according to d(xi , xj , zij ) = dij (xi , xj )

ð6Þ

where xi , xj represent the ith and the jth particle, respectively. zij represents the observation from the mobile robot. The probability function of all observations is defined by X dij T -ij dij ð7Þ F(x) = (i, j)2Q

where Q is particle pairs which are included in an observation set, and -ij is the information of covariance matrix associated with an observation of particle j relative to particle i. The configuration of the particles x minimizes the function (7) as follows x = arg min F(x)

ð8Þ

x

The three-layer mobile robot control architecture is composed of three functions: sense, plan, and act. In the sensing layer, the mobile robot collects raw data for building a global model; in the planning layer, the mobile robot produces exploration trajectories to close a loop, and in the acting layer, the mobile robot creates a map and locates itself on this map. The middle level, i.e, the planning layer, which generates a plan for high level (acting layer) exploration, and simultaneously feeds the information back to the bottom level (the sensing layer) to perceive the surroundings; all the outputs are transferred to the top level, the acting layer, to control the mobile robot’s movement for performing SLAM. This three-layer hierarchical analysis combines features of an active exploration system with a middle layer of information regarding the mobile robot’s motion.15,16 In the three-layer architecture, a mobile robot locates itself while it is mapping the exploration area. A monocular camera and EKF algorithm are used in this architecture. While SLAM is in process, the reactive layer rapidly feeds back environmental information to compensate for sensory noise. The mobile robot system combines this reactive characteristic with a state-based layer. Hence, sensors, robot motion, localization, mapping, and exploration must be analyzed hierarchically in this robust system. The lowest task of the acting layer is to operate the mobile robot’s path while exploring.16

Six-degree-of-freedom three-dimensional hierarchical analysis The three-dimensional 6-degree-of-freedom (3D-6DoF) system obtains 3D data from a visual perception system and output 6-DoF pose of the mobile robot.17 The 6-DoF hierarchical SLAM deals with 3D segments using visual information. The fusion of various data association metrics reduces errors in feature matching. Information from three cameras combines to form a stereovision system. A graph-based topological map is formed over a series of local sub-maps. The analytic model adapts to SAR post-disaster scenarios. The local level sub-maps aims at each segment, and the high-level topological map keeps globally consistent. The 3D6DoF approach for establishing the hierarchical models is expressed in equations (9) and (10)

Wang et al.

5 b0i ,

mi = ½ a0i ,

c0i ,

c00i T

b00i ,

a00i ,

ð9Þ

and the covariance matrix equation of two observed 3D extrema is expressed as follows 2

s2a0

i

6 s2 6 b0i a0i 6 2 6s 0 0 6 ca Pmi = 6 2 i i 6 sa00 a0 6 i i 6 s200 0 4 bi ai s2c00 a0 i

i

s2a0 b0

s2a0 c0

i i

s2a0 a00 i i

s2a0 b00

s2b0

s2b0 c0

s2b0 a00

s2b0 b00

s2c0 b0

s2c0

s2c0 a00 i i

s2c0 b00

s2a00 b0

s2a00

s2a00 b00

i i i

i i

i i i

i i

i i i i

i i

i

i

s2a00 c0

s2b00 b0 i

i

s2b00 c0

s2b00

s2c00 b0

i i

s2b00 a00

s2c00 c0

i i

s2c00 a00 i

s2c00 b00

i

i

i i

i

i

i

i

i

i

i

i

i

s2a0 c00

3

i i

s2b0 c00 7 7 i i 7 2 sc0 c00 7 i i 7 7 s2a00 c00 7 i i 7 s2b00 c00 7 5 i i s2c00 i

ð10Þ where mi are the coordinates of the two observed 3D extrema and Pmi represents the covariance matrix the observations of mi ; the each term of covariance matrix in equation (10) is the variance of two extrema observations.

Multi-level hierarchical treemap representation A multi-level sub-map SLAM method recursively segments the whole graph into local sub-maps forming a treemap; the optimization then runs over the whole tree. A report from K Ni and F Dellaert18 described a treemap representation. A node-based optimization procedure speeds up the calculations in the translation of submaps to the global map. The large amount of noise accompanying the measurements is one of the challenges in the SLAM process. The size of the area represents another challenge in decreasing computational costs. Tectonic smoothing and mapping (TSAM) combined with a nested dissection algorithm simplifies the computational complexity. The calculation can be performed using an out-of-core form based on a sub-map approach. Compared with the divide-and-conquer approach, an algorithm that fuses batches can result in better sub-map segments. Nested dissection can be adapted to SAR SLAM in combination with optimal local sub-maps. Linearized processing prevents a SLAM system from converging; therefore, an IEKF algorithm can be employed to compensate for estimation error. DPSLAM is a global occupancy grid-like array method, in which a data structure stores sufficient statistics on the current estimate of the opacity grid cell from a sonar range finder (SRF). The grid updates all nodes in the ancestry tree, which are stored in a single global map, while each node in the tree also maintains a list of all grid squares updated by that time. A pointer to the updated ancestral node is associated with the current opacity estimate. The global map iterates over all grid squares that could be filled in range of the robot’s exploration area, and the observation vector stores all particle nodes which contain the grid squares. Holes in the local

maps are filled by propagating inherited map information. A new observation is added to the observation vector of each grid square that was visited by the particle’s sonar casts. A pointer to each new observation is added to this particle node’s vector. Multi-objective 3D SLAM mobile robots automatically adjust the number of observations to estimate the pose and location. The mobile robot’s pose estimation is a higher priority than map expansion.15 The multi-level hierarchal procedure increases the robustness of the SLAM system.

Hierarchical map-building approach Wavelet hierarchical map representation model of unstructured SAR area A model for mapping collapsed areas needs to describe the damage inside buildings and simultaneously meet the needs of the rescue mission. Based on the AHP, the mechanism by which the map is built determines the estimation and data association methods. However, conventional mapping methods do not generally take into account an environmental model based on morphological characteristics of the collapse site. Hierarchical map building is an effective approach for collapsed scenarios. The SLAM system is composed of three levels. In the lowest level, grid-based sub-maps describe the details of the morphology of the collapsed area. In the middle level, local segments are processed based on spectral clustering. At the highest level, a globally consistent topological map is formed over associated cloud point data from the lower levels.1 The style of map determines the algorithms used for estimation and data association. The AHP determines a particular style of map. Disaster scenarios consist of heavily destroyed and completely damaged partitions. The degree of morphological damage can be divided into five hierarchies: basically intact, minor destruction, moderately destroyed, heavily damaged, and completely collapsed. Therefore, the hierarchical map model representation is appropriate for describing a collapsed disaster environment. An unstructured randomized distribution of destroyed regions appears as irregular edges on a 2D map. This map model requires some criteria which are used to assess the weight factor of the hierarchical level. A sphere-digital elevation model (S-DEM) is used to represent the 3D map. The hierarchical map model describes irregular obstacles in a collapsed disaster scenario. The collapsed scenes are identified by semantic nodes, and destroyed node shapes are described by occupancy grids in local maps. A hierarchical map is used to model the robot’s environment. The semantic map stores information regarding the distribution of collapsed spots to reduce complexity. Grid sub-maps

6

Advances in Mechanical Engineering

Table 1. Notation schemes and criteria: equations (11) and (12). Scheme

S1 S2 S3

Criteria Applicability C1

Accuracy C2

Computational cost C3

Interactivity C4

vSC11 vSC21 vSC31

vSC12 vSC22 vSC32

vSC13 vSC23 vSC33

vSC14 vSC24 vSC34

are used to represent details of each segment. Such a composite map is built to identify the collapsed spots. An SAR post-disaster scene is used as an experimental environment to assess the extent of destruction in a collapsed morphology. Levels of disaster damage are ranked into five hierarchies.1 Among them, the degree of destruction is an especially important criterion. Schematic diagrams indicate the four grades of destruction of building components. Walls and columns with cracks or breakages and deposits from collapsed areas are represented as topological nodes to construct the semantic space.

evaluation criteria for mapping, and the layer is composed of applicability C1 , accuracy C2 , computational cost C3 , and interactivity C4 . Map model representations at the scheme layer include a topological map S1 , metric map S2 , and hierarchical map S3 . A representation of the scheme criteria is shown in Table 1. The weight factor vs between each scheme can be defined as

Map-building construction

The scheme layer is represented by vS = ½ vs1 vs2 vs3 T , and the environmental partitioning criterion is vC = ½ vc1 vc2 vc3 vc4 T , each term of the left matrix on the right side of the equation (11) represents the weight factor. The collapsed deposits are represented by topological nodes to construct the semantic space.1 Formula (12) shows this partition methods

A map model of the collapsed scenario needs to describe the disaster damage inside buildings and simultaneously meet the needs of the rescue mission. Based on the AHP, the mechanism for building the map determines the estimation method and data association. In a hierarchical SLAM system, the architecture is composed of three partitions: semantic, intermediate, and metric layers. The intermediate layer includes several sub-maps, which are obtained by partitioning a larger environment. On each sub-map, damaged structures are identified by a topological node that represents the distribution of collapsed regions. Environmental details inside each sub-map include occupancy rates. Figure 1 shows the metric-topological mapping mechanism, the local sub-maps’ hierarchical representation in a collapsed disaster scenario, which consists of the feature descriptors, metric mechanism, and semantic sub-maps. The global topological map lies over the series of sub-maps. As shown in Figure 1(a), the top layer represents the feature descriptors, the middle layer indicates the metric mechanism, and the bottom layer contains the semantic sub-maps; in Figure 1(b), the upper layer represents local sub-map segments, and the lower layer displays the global structure. Judgment matrices and single hierarchical rankings are calculated; a pairwise comparison is performed to assess the impact of items in the criterion layer on the target layer. Next, the weight factor of each criterion is determined. The model criterion layer is generated according to

2

3

2

vsc11 vs1 s2 4 5 4 v vS = vs2 = c1 s3 v vs3 c1

vsc12 vsc22 vsc32

vsc13 vsc23 vsc33

2 3 3 v vsc14 6 c1 7 vc 2 7 vsc24 56 4 vc 3 5 vsc34 vc 4

N = fRi = Wj [ Dk ji  1, j.0, k.0, j 6¼ kg

ð11Þ

ð12Þ

where N indicates topological nodes, Ri refers to destroyed or collapsed regions, Wj are damaged columns, and Dk represents the unstructured deposits. To fuse higher topological maps with lower featurebased metric sub-maps, the generalized Voronoi graph (GVG) map method employs a global topological map to maintain consistency of local feature-based submaps.2 A globally consistent map can be generated by merging local map patches according to a relative route graph; end points from the laser range finder (LRF) readings are transformed from the mobile robot.13 Sensor noise drastically reduces the useful information of sensor readings; the noise is primarily generated by environmental factors such as surface texture and illumination, and by the measurement principle itself such as interference errors between ultrasonic sensors. The conventional solution is to take multiple reading into account and to employ temporal and multi-sensor fusion. An amortized constant time approach is used to estimate pose states in hierarchical SLAM, which is disA played by fusion Kalman-information filter.19

Wang et al.

7

Figure 1. The architecture of metric-hierarchical SLAM: (a) the local sub-maps’ hierarchical representation and (b) the global topological map.

hierarchical SLAM approach uses a small portion of the robot’s trajectory as input for the low-level mapping process; the output is not only a distribution over maps but also a distribution over robot trajectories.12 Real-time accurate mapping of large environments is described in Estrada et al.5 Elevation mapping and localization is conducted online USAR scenarios. The

effects of drift on low-level maps can be accurately approximated by perturbations to the end points of the robot’s trajectory.

PDE using a hierarchical SLAM algorithm Perception-driven hierarchical SLAM is a method that has been adapted to harsh SAR environments; it

8 integrates local sub-maps into a globally consistent map via mobile robots’ perceptions during their exploration (e.g. obstacle avoidance and perceived surrounding objects). YS Chou and JS Liu20 developed a 3D laser mapping system for obtaining 3D scan information of the surrounding environment via a LRF mounted on a mobile robot; the system allows for rotational motion, providing both horizontal and vertical scanning data. The map produced using an LRF is a local grid map. Chen et al.21 designed a method for speed tracking of a motorized exploration system for mobile robots in dangerous SAR environments. A Bethencourt and L Jauli22 added an IMU to the SLAM system; 2D images are then used to calculate a transformation between 3D point clouds. Conventionally, SLAM is formulated as a passive process that localizes and builds a map using a provided data sequence and exploration trajectory. However, a perception-driven SLAM exploration strategy uses active loop closing to disambiguate position uncertainty and enhance map accuracy without depending on any landmark assumptions. The information is obtained from a state-dependent switching process for mobile robots in unstructured post-disaster areas, which is based on percolation theory. The topology of unexpected neighborhoods frontiers will heavily reduce the efficiency of a robot’s perception-driven SLAM in SAR environment.23 Large areas and urgent time restrictions necessitate a SLAM algorithm to be associated with SAR missions; such an algorithm should have strong computational capabilities to achieve the rapid localization and mapping. Probabilistic approaches are applied to address the basic problem of localization with particle filters. A hierarchical SLAM procedure uses a small portion of the mobile robot’s trajectory as input for the low-level mapping process. The output is not only a distribution over maps, but also a distribution over robot trajectories.12 PDE for SLAM completes area coverage using loop closure, which uses an integrated algorithm to balance exploration and revisitation. This optimal solution maximizes coverage and improves SLAM performance.24 The proposed metric–topological approach produces a series of metric sub-maps. The topological map is built along with local metric sub-maps. The improved iterative extended Kalman filter (IEKF) algorithm is used for metric sub-maps, and MLR algorithm is used for building a global topological map.

Percolation filter algorithm A hierarchical linear distributed percolator-SLAM algorithm maintains topological consistency of each particle filter online. The asymptotic complexity of DPSLAM is simpler than that of pure SLAM with the same number of particles. An invasion percolation

Advances in Mechanical Engineering estimator uses information gained through the statedependent switching process to guide the entropy, which forms filter-based incremental SLAM methodology. This percolation filter enables a reduction in inevitable drift of localization in harsh disaster scenarios. In contrast, in traditional particle filters, a large number of particles track the mapping systems using diffuse posterior distributions, which increases the computational complexity dramatically.12 The PDE requires real-time map of the SAR scenes to be constructed by a mobile robot. Drift properties inherent to the sampling process cause errors in the estimation of a mobile robot’s position. All the current particles are contained in a DP-SLAM filter. A grid-based occupancy map is formed in the process of DP-SLAM. Each particle is associated with a grid of pointers to observation nodes. Using P particles and observations of B grid squares produces a total of O(PB) map access points; observation vectors store all observations of grid squares produced by any particle. Then, we fill the holes of the local sub-maps based on updated mapping information. A new observation is added to each grid square visited by the particle’s LRF. The action of completing a small map grid can be considered a random process with noise. Next, a higher level filter is supplied to track the particle’s pose more accurately. The mobile robot’s trajectory is used to build a low-level metric sub-map. Faisal et al.25 employed EKF algorithm by combining the infrared (IR) sensor information and the dead reckoning to solve the mobile robot localization, while multi-sensors multi-baseline mapping system (3MS)26 mainly depends on the fusion of multi-baseline stereovision and laser range reading data to enhance the accuracy of the point cloud. Though the positioning accuracy is enhanced, these methods increased the complexity in harsh unstructured SAR environments. The current EKF approach is limited by the following aspects: (1) processing time is O(n2 ) relative to the number of features n and (2) cumulative errors result in poorly matching data association. In contrast, the proposed hierarchical DP-SLAM, at both low and high levels, ensures that there is no increase in the total calculational complexity of O(PB). The proposed hierarchical analysis using IEKF method has improved the localization accuracy in SLAM of mobile robots with an iteration process. A comparison between the improved IEKF hierarchal SLAM and conventional EKF and linearized SLAM with respect to closing loops is represented in Figure 2. In IEKF systems, the linear stochastic difference equation (13) is represented as X (k) = AX (k  1) + BU (k) + W (k) The estimated values are calculated as

ð13Þ

Wang et al.

9

Table 2. Procedure: a high-level particle forms a topological map for each loop closure. Step 1 Step 2 Step 3 Step 4 Step 5 Step 6 Step 7 Step 8

Sample a high-level SLAM state Perturb the sampled robot state by adding random drift Sample a low-level trajectory from the distribution over trajectories returned by the low-level SLAM process Compute a high-level weight by comparing the trajectory and robot observations against the sampled high-level map, starting from the perturbed robot state (equations (15)–(19)) Update the high-level map; equation (16) Use DP-SLAM at both low and high levels of the hierarchy to constrain total computational complexity Each low-level SLAM process runs for a set number of iterations, equations (1)–(4) Repeat from Step 1, until end

SLAM: simultaneous localization and mapping; DP-SLAM: distributed particle simultaneous localization and mapping.

Z(k) = HX (k) + V (k)

ð14Þ

where X (k) is the pose state at time k in the Kalman filter; U (k) is the control of time k; A and B are the system matrices. Z(k) is the estimation trajectory at time k; H is computational parameter matrix; W (k) and V (k) are the white Gaussian noise of the Kalman filter and the computational system, respectively, and their covariance matrices are Q and R, respectively. The predicted pose at the current time step k is estimated based on the previous time k  1, which is represented as X (kjk  1) = AX (k  1jk  1) + BU (k)

ð15Þ

where X (kjk  1) is the predicted result based on the previous pose state. X (k  1jk  1) is the optimal state of the previous pose. And then, the covariance matrix P of the predicted pose state is updated according to P(kjk  1) = AP(k  1jk  1)A0 + Q

ð16Þ

where P(kjk  1) is the covariance matrix corresponding to X (kjk  1), and P(k  1jk  1) is the optimized covariance, and A0 is the transpose of matrix of system matrix A. We combine the predicted pose state and computational results to estimate the optimal pose X (kjk) at the current time k are obtained according to X (kjk) = X (kjk  1) + Kg (k)½Z(k)  HX (kjk  1) ð17Þ Kg represents Kalman gain; it is used in the following equation Kg (k) = ½I  Kg (k)HP(kjk  1)

ð18Þ

As shown in Figure 2, the optimal IEKF trajectory is accuracy with the robot’s estimated trajectory; the covariance matrix P(kjk) of the optimal estimated pose state X (kjk) is updated iteratively based on equation (19); hence, the IEKF algorithm can be maintained throughout the process of localization and mapping P(kjk) = P(kjk  1)H 0 =½HP(kjk  1)H 0 + R

ð19Þ

Figure 2. The comparison of IEKF trajectory with traditional and estimated trajectories.

The penetration of rubble by mobile robots is similar to the action of invasion.12 The SLAM loop for each high-level particle trajectory is obtained according to the algorithm presented in Table 2. A hierarchical framework for particle filters is applied to the SLAM process. The drift can easily be represented by a low-dimensional descriptor. However, as in conventional particle filter applications in SLAM, drift should be modeled in higher dimensions. A hierarchical particle filter maintains the necessary uncertainty to produce very accurate maps using an additional level of sampling on top of a series of small particle filters. The occupancy rate of the surrounding space represents the area’s traversable probability. As shown in Table 3, the mobile robots start SLAM from an initial direction within an SAR area produced by Module 1; however, they are periodically controlled by the percolation estimation of connected voids, using Module 3. The robot’s exploration trajectory is used to construct a low-level map. The assumptions of the derivative PDE are displayed in Table 3. According to the assumptions in Table 3, PDE solves area coverage for perception-driven SLAM in

10

Advances in Mechanical Engineering

Table 3. Assumption: the modules for PDE, equation (12). Module 1 Module 2 Module 3 Module 4

The coverage area boundaries of the SAR are given An initial exploration trajectory is planned using the given boundary information The user can define the maximum allowable exploration uncertainty, which will be used to trigger PDE’s revisit decision The SLAM trajectory-graph is built online with the prior boundary information on the disaster scene

SAR: search and rescue; PDE: perception-driven exploration; SLAM: simultaneous localization and mapping.

Table 4. Procedure: the process of tracking features according to image frames, equations (9) and (10). Step 1 Step 2 (a) (b) (c) (d) (e) (f)

The stochastic map predicts the pose of each subsequent image The features are searched with a normalized sum-of-squared-differences algorithm Simplify computational complexity Recursive calculations Feature matching Build local sub-maps Loop-closure detection and optimization Repeat from Step 2, until end

SAR scenarios. Mobile robots explore the whole SAR post-disaster environments in SLAM process, that is, mobile robots should cover the exploration area for mapping the environment while localization themselves accurately within this map.24 A parallel filter makes predictions of the future position and pose based on measurements of the current state of the target goal and the forecast for the step size in the adaptive filter. We consider each robot’s pose as a node of an incrementally built graph whose edges represent the sensed space overlap (SSO) between two observations. A large-scale environment is divided into several local segments with an appropriate scale. Through the segmentation, each local partition is independent; hence, the computational cost is reduced. However, the inherent connection between local maps is also cut off. Thus, the accuracy and consistency of SLAM would decrease without the interregional correlation.

Local sub-maps construction A vital goal during map segmentation is to balance computational complexity and global consistency. An environmental segment is defined based on spectral clustering. The map-building flow includes three layers. The process is started with the preprocessing of original data, which correspond to distances from a LRF to a mobile robot of SAR scenario. Here, we present a model for target motion. Measurement prediction is based on matching an observation with a map. This is usually done by representing the environment with a grid or a topological graph of a finite number of possible position states. During each update, the probability of each state in the entire space is updated, applying probability theory to

robot localization.5 According to a description in Clemente et al.,27 the perception SAR SLAM approach is summarized in Table 4. In this process, a mobile robot tracks the series of images features within the SAR exploration area. A high-level SLAM algorithm maintains global map consistency with almost constant processing time, which is achieved using a MLR algorithm over the fingerprints. MLR assigns a globally consistent set of Cartesian coordinates to the fingerprints of the graph based on locally consistent measurements.9 The whole map is divided into local sub-maps identified by the fingerprints, including the ordinary fingerprints and SIFT fingerprints. Outlier features are excluded from the computation using the random sample consensus (RANSAC) algorithm. Using hierarchical representation based on the hybrid metric–topological map, the environment is divided into sub-maps on the basis of spectral clustering.1 After extraction and estimation, positions of corner features and the robot’s pose are ascertained. The current environment can then be divided into several sub-maps based on spectral clustering. The details of the sub-maps are represented as grids by updating the occupancy probability in the underlying metric layer. Additionally, in the upper semantic layer, the collapsed patches are identified in each sub-map based on the degree of computational aggregation. The distribution of trajectories can be looked on as a distribution of motions in the higher level SLAM; therefore, the output from each sub-map is used as the input for a new SLAM process on a broader time scale. The high-level SLAM process must avoid double counting, and low-level mapping processes run independently from the initialized map. When the observations

Wang et al.

11

Table 5. Algorithm: simple formalization of the data association. Program 1 Program 2 Program 3

Each visual feature is associated with a sub-map grid, equations (20) and (21) The Mahalanobis distance is a threshold probability (i.e. confidence level) at which errors are allowed in matching, equations (5) and (6) Measure d dimensions of the similarity between two fingerprint sets, equations (7) and (8)

associated with a new particle sensor scan are integrated, the asymptotic, per-particle complexity of the SLAM process reduces to pure localization drift.

Data association using SIFT fingerprints The whole map is partitioned into low-level local submaps identified by mobile robot positions, which are called fingerprints. Metric 3D sequential mapping of visual landmarks can be obtained. The high-level topological map maintains consistency with the MLR approach and SIFT fingerprints. Visual landmarks can aid in detecting loop closures by marking revisited positions. A series of low-level local fingerprints produces grid sub-maps that consist of visual landmarks; the high-level topological map is built along with local metric sub-maps. A graph-based topological map is composed of edges and vertices. Fingerprints indicate the pose of a mobile robot, and the edges show the relationship between vertices. The MLR algorithm estimates each pose with the coverage area closed loops. The robot uses landmarks for localization while building topological maps and determining distances between landmarks. The improved fusion of an EIF and EKF is used to estimate the pose and covariance matrix.19 Three-dimensional sequential mapping of visual natural landmarks and the robot location are obtained using a top-down Bayesian method to model the mobile robot’s behavior.9 A fingerprint pose vector X fpl + ! is represented by equation (20), and its covarl+1 iance matrix Pfp Yi Yi is defined in equation (21) fpl + 1 X fpl + 1 = ( Xrob

l+1 Pfp Yi Yi

Y1fpl + 1

Y2fpl + 1

∂Y fpl + 1 = i rob  Prob Yi Yi  ∂Yi

∂Yifpl + 1 ∂Yirob

∂Y fpl + 1 + irob  Prob  ∂Xfpl + 1 fpl + !

T

ð20Þ

!T

ð21Þ

) !T

∂Yifpl + 1 ∂Xfprob l+1

fpl + 1 and Xfprob are the mobile robot’s pose vecwhere Xrob l+1 tor and fingerprint pose vector, respectively. Yifpl + 1 and Yirob are y-axis directional information of each fingerprint pose vector and mobile robot pose vector, respectively. Prob Yi Yi represents the mobile robot pose vector’s covariance matrix. Prob fpl + 1 is the mobile robot’s pose relative to the fingerprint location.

Pure vSLAM is based on the hierarchical SLAM approach, which adopts the inverse-depth method to obtain feature depth from a monocular camera. This data association technique improves robustness in harsh and complex SAR environments.17 Detailed 3D geometric and photometric information is abstracted for SLAM. This approach initializes point features with an IEKF algorithm in real-time. A simple formalization of the data association algorithm is shown in Table 5. The observations associated with a new particle’s sensor scan are integrated into the map. Hierarchical SLAM assumes relatively sparse and unambiguous landmarks that simplify localization and map building. The first assumption in this method is that the robot’s state vector must be highly correlated with the remaining state variables, and the second is that low-level mapping is moderately accurate and locally consistent.

Loop-closure detection Detailed investigation of treemap data structures shows that heavy leaves in the tree, that is, leaves with many poses, lead to poor loop-closure performance. One of the main issues concerning SLAM in unstructured SAR environments is loop closing, that is, to recognize previously visited places. A hierarchical SLAM implementation includes an additional processing level, such as the sum-of-squared differences, to identify a specific place using the SIFT prints. Once a loop closure has been detected, the whole map is corrected according to the recognized location.4 Figure 3 compares mobile robot trajectories between a wandering exploration and a loop-closure exploration in an SAR environment; the loop-closure detection is compared between different trajectories of mobile robots in an uncertain environment. Figure 3(a) shows the wandering exploration trajectory of a mobile robot and Figure 3(b) shows the loop-closure exploration trajectories of a mobile robot. According to Figure 3, the low-level local sub-maps and the high-level topological map form relatively independent stages of SLAM. An optimal loop-closing strategy maintains independence of the lower submaps; simultaneously, the upper topological map maintains global consistency. A series of independent local stochastic sub-maps are associated with the global topological map. The SIFT descriptors match and store

12

Advances in Mechanical Engineering

Figure 3. The loop-closure comparison: (a) wandering exploration trajectory and (b) loop-closure exploration trajectories.

Table 6. Algorithm: map correction via updating hierarchical levels of the mapping, equations (15)–(19). Process 1 Process 2 Process 3 Process 4 Process 5 Process 6

Sample a high topological map Perturb the sampled mobile robot state by adding random noise Select a low-level mobile robot trajectory Calculate a high-level weight by estimating the low-level trajectory Update the high-level global topological map based on the new observations Repeat from Process 1, until end

the graph-pose scheme; key frames are used to decide whether the mobile robot is revisiting a place or not, that is, whether a loop is being closed.4 For implementing a successful large-scale exploration, the mobile robot needs to base on building a topological map and maintaining the map consistency by using hierarchical metric -topological procedure and visual sensors. The low metric level SLAM implementation focuses on the visual system.

Map correction Hierarchical optimization SLAM only corrects coarse structure and only updates those parts related to the overall map. This approach provides accurate nonlinear, efficient map estimation. The poses of mobile robots are represented by nodes in a graph. Error minimization can avoid singularities in the state space parameterization.14 A sparsification procedure uses nonlinear approximation to calculate the nodes’ covariance; the x 2 test and the joint compatibility test are considered to be state-of-the-art data association approaches in map building. The proposed updating process is presented in Table 6.

MobileRobot platform for SAR SLAM simulations The set-up adept mobileRobot platform and its environment consist of a simulated scene for SAR SLAM,

and a virtual Pioneer 3-DX (P-3DX) mobile robot which autonomously explores the given area by running the proposed hierarchical SLAM procedure; the robot can be monitored and controlled by PC control center via TCP port 8101. As shown in Figure 4, a mobile robot conducts localization and mapping using a metric–hierarchical approach in a cluttered environment; the area covered is displayed by blue band, which represents the LRF’s output, and the sonar rays are shown in gray rays from the edge of the mobile robot. The position data display the location, orientation, and coordinates of the mobile robot. The sensor’s readings in Figure 4(a) show the measurements of the mobile robot’s poses and fingerprints, which are displayed as Cartesian coordinates (x, y) and the deviation angular u from x-axis direction; the data of sensor readings are displayed in Table 7 which compared the proposed SLAM with the conventional method. The artificial obstacles and the objects of interest are loaded into the simulation’s environment of mobileRobot platform for hierarchical SLAM. In Figure 4(b), the mobile robot runs the proposed PDE program and plans a trajectory for closing a loop. Figure 4(c) implements a loop closure in a given SAR area using hierarchical PDE, which displays the completed exploration loop-closure trajectory. In Figure 5, a virtual mobile robot, that is, the p3DX robot, is exploring a cluttered, unstructured SAR area, and it indicates that the trajectories almost completely cover the whole exploration SAR area. The SIFT

Wang et al.

13

Figure 4. The virtual mobile robot simulates a hierarchical SLAM process: (a) localization and mapping using the proposed metric– hierarchical approach, (b) the exploration trajectory is closing a loop, and (c) the exploration trajectory completed loop closure.

Table 7. Data comparison of the hierarchical IEKF SLAM with the traditional method. x (cm) coordinates y (cm) coordinates Angle (°) Time (min) IEKF coverage rate Other coverage rate

24.57 21.48 –17.4 0 0 0

15.18 –6.27 5.49 3 0.3 0.25

47.10 –17.1 5.90 6 0.5 0.30

39.47 21.50 –11.9 9 0.6 0.50

4.54 –20.2 –3.07 12 0.7 0.55

–6.84 –15.4 4.83 15 0.75 0.65

–28.2 11.85 0.38 18 0.8 0.7

–31.3 –38.7 0.54 21 0.85 0.75

2.74 11.24 0 24 0.95 0.8

IEKF: iterated extended Kalman filter; SLAM: simultaneous localization and mapping.

algorithm based on perspective projection is generally used on computer vision tasks such as target tracking, image retrieval, and 3D reconstruction.28 The fusion of point-based recognition and localization with those of point-based monocular vSLAM can identify and recover the 3D geometry of objects. The coverage range of the matching keypoints is wider, including the distorted peripheral areas. Once a new SIFT is generated, it is matched with all stored SIFT fingerprints within the SAR exploration area.

The experiments for hierarchical SLAM in unstructured SAR scenarios The set-up of the mobile robots used in the experiments is a Pioneer LX, an MTX-type robot. This mobile

robot employs a protocol for essential communication that is compatible with an arnlServer program and control program, and other software. The Pioneer LX robot can autonomously perception-driven explore the SAR area depending on the embedded industrial PC (IPC). The hardware contains a LRF and front- and rear-facing sonar sensors. The software constitutes an MTX-lynx core, a module containing a robot controller and power management, and an IPC with micro Windows, which are all pre-installed. The map-building–based approach entails exploring the environment and building a representation map of the environment,29 and the robot perceives and detects the landmarks for SLAM mainly depending on the camera’s successive visual scenes. In our method, the LRF combined with the visual system embedded

14

Figure 5. A virtual p3DX mobile robot’s exploration trajectories.

mobile robot. According to Liu et al.,30 a computationally efficient visual–inertial fusion algorithm is designed to an accelerometer and a vSLAM algorithm measurement. The proposed algorithm incorporates a prediction step and a measurement update step, which

Advances in Mechanical Engineering performs the state vector time update in constant time interval and the correction of the state vector based on the new sensor measurement. The URG-04LX sensors embedded mobile robot combining the robot visual system produce the environmental 2D map, and simultaneously, the visual image of the SAR objects was extracted 3D features. The proposed hierarchical analysis reduced the complexity and only highlight the interesting 3D objects marked on the 2D map, which adapted to urgent cluttered SAR scenarios. An outdoor experimental set-up is shown in Figure 6; a crawler-I LUKER is installed with URG04LX which is a laser scanning distance sensor. The sensor is mounted on the middle module of the robot. The height of the sensor from the ground is approximately 35 cm. The range of the sensor is 4 m and the angular range is 240°. An embedded platform contains the core of a USAR system. This wheeled-crawler mobile robot enables rapid mapping in a large environment with rough terrain. The ground truth experiment was conducted in collapsed SAR gypsum environment using the crawler-I LUKER mobile robot, Figure 6(a) shows the components of the crawler-I LUKER robot, and Figure 6(b) shows the crawler explores the collapsed gypsum SAR area. A crawler mobile robot set-up was used to explore the outdoor collapsed unstructured scene with perception-driven SLAM, as shown in Figure 7(a) and (b); in this SLAM process, the robot runs the algorithm of Table 5 to identify 3D color objects of interest, and runs the algorithm of Table 6 and the procedure of Table 2 to build 2D map while exploring SAR area. The mobile robot performs PDE of SAR SLAM in an outdoor collapsed, unstructured area using the proposed AHP methods, and a map of this environment is constructed. In Figure 7(b), a topological map of this environment is generated with local objects of interest, that is, colored marks 1, 2, and 3, and pink rectangle

Figure 6. A crawler-I LUKER experiment using the analytic hierarchy process (AHP): (a) the components of the crawler-I LUKER and (b) exploration in collapsed gypsum scene.

Wang et al.

15

Figure 7. The outdoor experiment: (a) the crawler-I LUKER robot performs SLAM while perception-driven exploring an SAR area and (b) the formed map of this environment.

Figure 8. The indoor experiment of metric–topological hierarchical SLAM process with loop-closure perception-driven exploration: (a) an artificial scene for SAR SLAM and (b) the hierarchical loop-closure exploration.

areas. Comparing with the proposed perception-driven hierarchical SLAM, the produced map of conventional SLAM as shown in Figure 10 does not have any marks of 3D SAR objects. Figure 8(a) displays a simulated indoor SAR environment; a Pioneer LX robot was autonomously performing the proposed hierarchical perception-driven task, as shown in Figure 8(b). In this SLAM process, the robot runs the algorithm of 4.4 Loop-closure detection to identify the revisited place, and runs the algorithm of Table 6 and procedure of Table 2 to build a consistent global 2D map.

In Figure 8, a cluttered and indoor USAR scene was simulated; legs of chairs and tables, books, and cardboard boxes on the ground are randomly distributed: they appear as complex contours in the observation plane, which is displayed in Figure 8(a). A hierarchical SLAM based on a hybrid metric–topological map is presented, as shown in Figure 8(b). The ability of the hierarchical map model to describe irregular obstacles is verified in an artificial indoor environment, and the metric–topological hierarchical SLAM process implements loop-closure PDE with the proposed hierarchical computation in a cluttered experimental SAR environment.

16

Advances in Mechanical Engineering

Figure 9. The SAR exploration area coverage from (a) to (c); the red grids are laser scanning to produce submaps, the red belt is mobile robot’s trajectory, and the green image is mobile robot, and the grey lines emitting from the robot represent the sensor’s information.

Results and discussion Hierarchical SLAM is conducted by the two types of mobile robots in artificial cluttered SAR environments and real unstructured collapsed scenarios. In the simulations, a virtual mobile robot solved the loop-closure and unstructured area coverage issues using a metric– topological approach. The loop-closure exploration was implemented as shown in Figure 4(a)–(c). In Figure 5, full coverage during exploration of an SAR cluttered area was completed with a PDE procedure and distributed percolation SLAM. From the comparison of IEKF trajectory with traditional trajectory in Figure 2 and the experiments in Figure 9, a set of data of robot poses were obtained, including x coordinates, y coordinates, and the corresponding angles, which are shown in Table 7; the data of sensor readings can be obtained, as shown in Figure 4(a). The exploration area coverage rates are compared between the hierarchical IEKF SAR SLAM and the ordinary traditional SLAM, and the experimental effects demonstrate that the proposed methods have higher area coverage rates comparing to the conventional ones.

The experimental results demonstrate that the hierarchical mapping method can accurately describe destroyed, collapsed environments, as shown in Figure 1(a) and (b). Comparing to the traditional EKF, the proposed IEKF algorithm calculates the local sub-maps between two fingerprint features, which improves localization and mapping accuracy; the comparable SLAM trajectories are presented in Figure 2. We have compared our method with traditional approaches with respect to trajectory accuracy. Loopclosure detection is conducted via wandering and perception-driven programs in an artificial, cluttered environment; the trajectories are then compared, as shown in Figure 3(a) and (b), which displayed the loopclosure results by comparing perception-driven SLAM exploration trajectory with traditional SLAM case. In hierarchical metric–topological algorithms, the global map was integrated over local grid maps. Each of the local sub-maps was calculated independently; therefore, the computational efficiency was improved. Simultaneously, at the topological level, a global map organizes a series of local sub-maps; hence, the whole map was maintained consistency with respect to data

Wang et al.

17 Declaration of conflicting interests

map comparison

10000

map2

8000

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

y(mm)

6000

Funding 4000

2000

zero point

0

−2000 −4000

−2000

0

2000 x(mm)

4000

6000

8000

Figure 10. Comparing with Figure 7(b), the conventional 2D map without 3D marks.

association. In Figure 6(a) and (b), the analytic hierarchy process runs a crawler SAR mobile robot in a real collapsed scenario, which enables rapid mapping and localization. Another experimental set-up is shown in Figure 7(a) and (b), in which the SAR SLAM of perception-driven was performed in outdoor collapsed, unstructured scene. A metric–hierarchical SLAM experiment is implemented in an artificial cluttered indoor environment, which is shown in Figure 8(a) and (b). In addition, we compared the experimental results (Figures 4–8) with traditional SLAM methods in performances of loop-closure and exploration area coverage. Figure 10 shows a comparison with the experiment of Figure 7(a) and (b), there are 3D colored objects and areas marked in environmental map of Figure 7(a), which used the proposed hierarchical perception-driven SLAM methods, while there are not identification marks of the objects on the map produced by using conventional approaches as shown in Figure 10.

Conclusion and future work The developed PDE enables mobile robots to conduct hierarchical SLAM adapted to SAR environments such as a damaged interiors following a disaster. This method solves loop-closure and exploration area coverage, achieves consistent 2D/3D map with SAR objects identification, and minimizes computational costs. In particular, the metric–topological hierarchical algorithms can efficiently build a topological global map by integrating over local sub-maps; the distributed particle percolator approach improves the adaptability of mobile robot for adapting to SAR SLAM task. In the future work, the hierarchical map-building mechanism will be further researched since the map building is a vital factor for area coverage, loop-closure, and computational cost issues adapting to harsh SAR scenarios during mobile robot exploration.

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This research is supported by the National Nature Science Foundation of China (NSFC) under grant nos 61573213, 61473174, 61473179, and by the Natural Science Foundation of Shandong Province under grant nos ZR2015PF009, ZR2014FM007, by Shandong Province Science and Technology Development Program under grant no. 2014GGX103038, and by Special Technological Program of Transformation of Initiatively Innovative Achievements in Shandong Province under grant number no. 2014ZZCX04302.

ORCID iD Chengjin Zhang

https://orcid.org/0000-0002-0066-2114

References 1. Wang N, Ma SG, Li B, et al. A hierarchical map building for SLAM used in ruins environments. In: Proceedings of the 11th world congress on intelligent control and automation, Shenyang, China, 29 June–4 July 2014, pp.1409–1414. New York: IEEE. 2. Lisien B, Morales D, Silver D, et al. Hierarchical simultaneous localization and mapping. In: Proceedings of the 2003 IEEE/RSJ international conference on intelligent robots and systems, Las Vegas, NV, 27–31 October 2003, pp.448–453. New York: IEEE. 3. Zhang H, Hou ZF, Li NJ, et al. A graph-based hierarchical SLAM framework for large-scale mapping. In: Proceedings of the 5th international conference on intelligent robotics and applications part II (ICIRA), Montreal, QC, Canada, 3–5 October 2012, pp.439–448. Berlin: Springer. 4. Schleicher D, Bergasa LM, Ocan˜a M, et al. Improvement of speeded-up robust features for robot visual simultaneous localization and mapping. Robotica 2014; 32: 533–549. 5. Estrada C, Neira J and Tardo´s JD. Hierarchical SLAM: real-time accurate mapping of large environments. IEEE T Robot 2005; 21: 588–596. 6. Vivet D, Gerossier F, Checchin P, et al. Mobile groundbased radar sensor for localization and mapping: an evaluation of two approaches. Int J Adv Robot Syst 2013; 10: 1–12. 7. Montiel-Ross O, Sepulveda R, Castillo O, et al. Efficient stereoscopic video matching and map reconstruction for a wheeled mobile robot. Int J Adv Robot Syst 2012; 9: 1–13. 8. Schleicher D, Bergasa LM, Ocan˜a M, et al. Real-time hierarchical GPS aided visual SLAM on urban environments. In: Proceedings of the 2009 IEEE international conference on robotics and automation, Kobe, Japan, 12– 17 May 2009. New York: IEEE.

18 9. Schleicher D, Bergasa LM, Ocan˜a M, et al. Real-time hierarchical outdoor SLAM based on stereovision and GPS fusion. IEEE T Intell Transp 2009; 10: 440–452. 10. Priyadharsini S, Kavitha S and Hemalatha S. Coal mine rescue robot using semi-autonomous controller. IJSART 2015; 1: 48–51. 11. Williamson DA and Carnegie DA. Toward hierarchical multi-robot urban search and rescue: development of a ‘‘Mother’’ agent. Stud Comp Intell 2007; 76: 1–7. 12. Eliazar AI and Parr R. Hierarchical linear/constant time SLAM using particle filters for dense maps. Durham, NC: Department of Computer Science, Duke University, 2005. 13. Kleiner A and Domhege C. Real-time localization and elevation mapping within urban search and rescue scenarios. J Field Robot 2007; 24: 723–745. 14. Grisetti G, Kmmerle R, Stachniss C, et al. Hierarchical optimization on manifolds for online 2D and 3D mapping. In: Proceedings of the 2010 IEEE international conference on robotics and automation, Anchorage, AK, 3–8 May 2010, pp.273–278. New York: IEEE. 15. Miro´ JV, Zhou WZ and Dissanayake G. A strategy for efficient observation pruning in multi-objective 3D SLAM. In: Proceedings of the 2011 IEEE/RSJ international conference on intelligent robots and systems, San Francisco, CA, 25–30 September 2011, pp.1640–1646. New York: IEEE. 16. Fairfield N. Simple landmark localization on a threelayer mobile robot architecture. In: Proceedings of the college of information science and technology, Pennsylvania State University, State College, PA, 30 April 2001, pp.1–24. 17. Marzorati D, Matteucci M and Sorrenti DG. Multi-criteria data association in 3D-6DoF hierarchical SLAM with 3D segments. In: Proceedings of the VISAPP international workshop on robotic perception, Funchal, 22–25 January 2008. 18. Ni K and Dellaert F. Multi-level submap based SLAM using nested dissection. In: Proceedings of the 2010 IEEE/ RSJ international conference on intelligent robots and systems, Taipei, Taiwan, 18–22 October 2010. New York: IEEE. 19. Ila V, Porta JM and Andrade-Cetto J. Amortized constant time state estimation in pose SLAM and

Advances in Mechanical Engineering

20.

21.

22.

23.

24.

25.

26.

27.

28.

29.

30.

hierarchical SLAM using a mixed Kalman-information filter. IEEE Robot Auton Syst 2011; 59: 310–318. Chou YS and Liu JS. A robotic indoor 3D mapping system using a 2D laser range finder mounted on a rotating four-bar linkage of a mobile platform. Int J Adv Robot Syst 2013; 10: 1–10. Chen G, Zhang WG and Zhang XN. Speed tracking control of a vehicle robot driver system using multiple sliding surface control schemes. Int J Adv Robot Syst 2013; 10: 1–9. Bethencourt A and Jauli L. 3D reconstruction using interval methods on the Kinect device coupled with an IMU. Int J Adv Robot Syst 2013; 10: 1–10. Kara M, Erkman AM and Erkmen I. Prioritized mobile robot exploration based on percolation enhanced entropy based fast SLAM. J Intell Robot Syst 2014; 75: 541–567. Kim A and Eustice RM. Perception-driven navigation: active visual SLAM for robotic area coverage. In: Proceedings of the 2013 IEEE international conference on robotics and automation (ICRA), Karlsruhe, 6–10 May 2013. New York: IEEE. Faisal M, Alsulaiman M, Hedjar R, et al. Enhancement of mobile robot localization using extended Kalman filter. Adv Mech Eng 2016; 8: 1–11. Faisal M, Mathkour H, Alsulaiman M, et al. Multi-sensors multi-baseline mapping system for mobile robot using stereovision camera and laser-range device. Adv Mech Eng 2016; 8: 1–18. Clemente LA, Davison AJ, Reid ID, et al. Mapping large loops with a single hand-held camera. In: Proceedings of the 2007 robotics: science and systems (RSS), Atlanta, GA, 27–30 June 2007. Tong G, Chen X and Ye N. A spherical model based keypoint descriptor and matching algorithm for omnidirectional images. Adv Mech Eng 2014; 2014: 1–7. Gu¨zel MS. Autonomous vehicle navigation using vision and mapless strategies: a survey. Adv Mech Eng 2013; 2013: 953–956. Liu C, Prior SD, Teacy WL, et al. Computationally efficient visual-inertial sensor fusion for global positioning system-denied navigation on a small quadrotor. Adv Mech Eng 2016; 8: 1–11.