Hierarchical-map Updating Approach for Simultaneous Localization and Mapping of Mobile Robots

For the tremendously increasing of system state in wild field, the computational complexities of mobile robot system should be taken into account. This paper proposes a hierarchical-map updating approach for simultaneous localization and mapping of robots. The basic idea of hierarchical-map is defining two kinds of maps during the recursive updating process, namely local map (upper map) and global map (lower map). The system states will be updated by the preset maps. The hierarchical-map updating process is just for the upper map and the lower map is updated after a certain running term. In the calculation, the state data of the upper map is far less than that of the lower map. It is validated by the experiments that, the approach is more optimal than others in computational complexities while ensuring the consistency estimate

x y x y  T  has no time index. The robot's movement model is rolling motion constraints (i.e., assuming zero wheel slip) [12]. 1 1 The time interval from 1 k  to k moment is T  , speed k v and driving angle k G are constant, which constitutes controlled quantity

SLAM observation model
The observed model is where ik z is the observation vector at time k and i h is the model of the observation of the i th landmark. The vector ik z is a observation of the landmark location i p relative to the robot's location vk x . This type of observation will be referred to as a vehiclelandmark observation or a VLM observation.
The model is not assumed to be perfect and unmodelled sensor characteristics and noise corruption are lumped into a observation error vector ik  . The observation error vector is again taken to be a temporally uncorrelated and zero mean random sequence.

Landmark model
Landmarks are fixed and conspicuous features within the environment. Landmarks can have many physical forms; corners, planes, rough surfaces, poles, natural or artificial terrain features can all be considered landmarks if they are repeatedly and reliably observed by a Mathematically, landmarks are represented as a vector of parameters that define the location and other properties of the landmark. This thesis generally employs the simplest of all landmark models: a landmark is a stationary point like entity in two dimensions. A point landmark is defined by two parameters specifying its position in cartesian space with respect to some global coordinate frame. A point landmark is visible from all viewing angles. In general, more complex landmarks can be incorporated into the maps and models employed throughout this thesis.
The i th point landmark in the environment will be denoted as i p and will be defined as follows The relationship between the point landmark state at times k + 1 and k is trivial. The landmark is stationary and so Importantly, and in contrast to the vehicle model, there is no additive uncertainty term in the landmark model. The vehicle motion model, the observation model, and the measured values of the control parameters , are not exact, but are subject to noise, which lead to uncertainty in the state estimate. For this reason, we require a probabilistic filter to recursively estimate a distribution over the state given noisy information.

Hierarchical-map updating algorithm for SLAM 3.1. Basic ideas of hierarchical-map
When the robot conducts SLAM in the wild environment, the map data and the system state space will increase exponentially. When the robot is working in the wild, what concerns are the interested areas around and the general objects. Hence, for the map building of the robot, it is available to divide the interested areas and the general objects to hierarchical maps. According to the requirements, the map can be divided into N layers. The top layer is denoted by 1. And the bottom layer is denoted by N. The schematic graph of the hierarchical maps is shown in Figure 1.

Updating for the hierarchical maps
For the problem of computational complexity increase resulted by the increment of the state space, the system area is divided into two parts during the EKF recursion process, namely the upper-map state space and the lower-map state space. During each step of the SLAM recursion step, the updating step just updates the upper-map state space. When the robot navigates out of the upper-map area, then the lower-map is updated. As it is shown in Figure 2, the rectangular Area A is the initial upper-map state space; Area B is the initial lower-map state space. When the sensor of the mobile robot observes the features of the lower-map state space while navigating in the upper-map state space, the lower map will be updated. Besides, the upper-map state space and the lower-map state space are re-divided, as shown in Figure 2.
The HMU-SLAM algorithm is described as follows: Initialisation and prediction: the upper-map area is independent from the lower-map area. The state vectors are divided to two parts.
where, n presents the number of the virtual features. The covariance matrixes are divided as follows: The auxiliary matrix: The initial time 1 k : Different from the EKF-SLAM algorithm, the HMU-SLAM algorithm conducts the prediction in the upper-map area. At this time, only Top TT , X P are involved in the prediction. The auxiliary matrix has a recursive equation as the following.
Updating: the updating is divided to upper-map updating and lower-map updating. The upper-map updating is just for the local areas and the lower-map updating is conducted via the recursion of the auxiliary matrix.
Updating of Low TL LL , , X P P : Once the area of the lower state space is observed (at time K2 here), the lower-map updating is conducted via the recursion formula.

Steps for HMU-SLAM
HMU-SLAM process is performed as follows.
Step1 Initialisation： Initially define and assign values for the prediction location, system covariance matrixes, hierarchical areas, the auxiliary matrix, the system process noise covariance matrix, the system observation noise covariance matrix, control variables, the maximum observation distance and the time interval.
Step2 Location prediction for the robot: predict the current location and covariance matrix of the robot according to the position estimate, covariance matrix, auxiliary matrix and moving model of the former moment in the upper-map area.
Step3 Practical observation: Obtain the practical observations by the observations of the environment features using the sensors of the robot.
Step4 Prediction observation: Calculate the prediction observations of the environment features using the measurement model according to the coordinates of the location prediction and observation environment features on the lower map.
Step5 Data association (relating): conduct the data association for the observation features of the upper map. Step6 State updating: the state updating is divided to upper-map updating and lowermap updating. The upper-map updating is just for the associated features. The data is generally far less than that of the lower map.
The lower-map updating is generally conducted after a certain distance of movement of the robot, to modify the data of the global map.
Step7 State incrementing: Add the new observed features to the map. The HMU-SLAM algorithm is recursive by steps in sequence of prediction, observation, data association, uppermap updating or lower-map updating and state incrementing.

Results and Analysis
The initial state and covariance matrix of the experiments are 0/ 0 , respectively. In the initialisation process, the noise covariance and the observation covariance are  Figure 3 shows classical EKF-SLAM algorithm. Figure 4 shows classical Fast-SLAM algorithm. Figure 5 shows the hierarchical-map SLAM of the proposed algorithm. Figure 6 shows the two-layer map SLAM. The map is built by using the proposed algorithm. The data processing quantity of a single-step calculation is an important index for deciding the efficiency of an algorithm [10]. For the large-scale map, to verify the computation complexity of the algorithm, the experiments also performed the classic EKF-SLAM, FastSLAM and UKFSLAM respectively in the same environment, for comparing with the proposed algorithm in this paper. Figure 7 shows the time consuming averages of the above algorithms under 50 single-step calculations. i presents step length. T presents time (unit: s). For the proposed algorithm, the estimation issue of consistency should be considered. For linear Gaussian filter, the filter performance can be characterized through NEES (normalized estimation error squared) [11].
Under the hypothesis that the filter is consistent and approximately linear-Gaussian, NEES obeys  2 distribution. Consistency of the algorithm is evaluated by performing N times Monte Carlo runs. The algorithm performance indicators are evaluated by the average NEES. When N   ， k  approaches to the state vector dimension. To verify the consistency estimate, as to EKF-SLAM, FastSLAM, UKFSLAM and the proposed algorithm, we conducted 50 MonteCarlo runs on the robot for each. Figure 8 shows the NEES consistency estimate curves. As it can be seen from Figure 8, the NEES curves of the algorithms are almost within [2.36，3.72]. Hence, they are all consistency estimates. From the above, the proposed algorithm is optimal in computational complexity while ensuring the consistency estimate.

Conclusion
For the problem of the ever-increasing computational complexity resulted by the everincreasing state space in the large-scale environment, this paper proposes a hierarchical-map updating algorithm for simultaneous localization and mapping. According the algorithm, the robot will build multiple map layers at map building. During state updating, only a few states of the upper map are updated, thus lowering the computational complexity of the algorithm. The experiments validated that the proposed algorithm has the minimum computational complexity while ensuring the consistency estimate. The ideas of this paper provide a meaningful clue for the map building of the large-scale unknown environment for robot.