Lin Jiang, Han Wang, Bin Lei*, Jianyang Zhu, Huaiguang Liu and Hui Zhao

(1. Key Laboratory of Metallurgical Equipment and Control Technology, Wuhan University of Science and Technology, Wuhan430081, China； 2. Institute of Robotics and Intelligent Systems, Wuhan University of Science and Technology, Wuhan 430081, China)

Abstract: In order to solve the problem of localization loss that an autonomous mobile robot may encounter in indoor environment, an improved Monte Carlo localization algorithm is proposed in this paper. The algorithm can identify the state of the robot by real-time monitoring of the mean weight changes of the particles and introduce more high-weight particles through the divergent sampling function when the robot is in the state of localization loss. The observation model will make the particle set slowly approach to the real position of the robot and the new particles are then sampled to reach the position. The loss self-recovery experiments of different algorithms under different experimental scenarios are presented in this paper.

Keywords: indoor mobile robot; self-recovery; localization loss; improved Monte Carlo localization algorithm

1 Introduction

In the field of service robotics, autonomous robots are required to complete the tasks of family service[1], elderly escort, and scene monitoring independently in indoor environments[2], where indoor localization technology is a prerequisite.

Monte Carlo localization (MCL) algorithm was first proposed in 1999in Ref. [3], which is obtained by substituting the appropriate probabilistic motion and perception model into the particle filter[4]. The MCL algorithm uses a large number of particles to represent the confidence of the robot’s position. The measurement model determines the importance weight of the particle, and then samples the particle that can represent the position of the robot from the motion model. Although the MCL method can deal with global localization, the “kidnapped problem” or global localization loss is still an issue, which is the situation when a well-tracked robot suddenly moves to somewhere else without being told during an interior mission[5]. Once such a situation occurs, the robot may execute a wrong action using wrong information, which will damage the environment or hurt itself. The “kidnapped problem” is a complex variant of the global localization. The practical significance of this issue comes from the observation of the most advanced localization algorithms. However, current algorithms are not always stable, so it is essential for robots to possess the ability to recover from localization loss by themselves[6]. To solve the problem of robot localization loss and “kidnapped problem”, foreign and domestic scholars have achieved some results in the research of two-dimensional map[7], three-dimensional map[8-9], and localization based on ROS[10-11]. Scholars from China Central South University proposed an improvement algorithm named Adaptive Monte Carlo localization (AMCL) for multi-hypothesis tracking[12]. Based on particle clustering, the algorithm can reduce the impact of “kidnapping” on robots by reducing particle degradation. He[13]proposed a self-localization algorithm based on genetic algorithm. By introducing divergence parameters, the algorithm can maintain particle diversity and reduce the particle deprivation effect to make localization more accurate, which allows the robot to have the specific ability to recover when it is “kidnapped.” Scholars from Ritsumeikan University[14]proposed an algorithm for detecting and identifying double “kidnapping problems”. Further analyses detected the existence of “kidnapping problems” and analyzed them in Ref. [15] only when it is under conditions that are better resolved in Ref. [16]. Since the indoor environment is complex and diverse, the “kidnapping problem” is different.

Although many scholars have improved methods to solve the “kidnapping problem”, few have discussed the exhaustive analysis of various positioning failures and the corresponding solutions. In this paper, an improved MCL (IMCL) algorithm is proposed on the basis of the existing MCL with the combination of the characteristics of particle resampling and the analysis of different situations of localization loss. When a robot is under the condition of positioning failure, different particle sampling methods can help it to recover its positioning function. The theoretical analysis and experimental verification of the invalidation of the robot are performed in this paper to verify the effectiveness of the proposed algorithm.

2 IMCL Algorithm

2.1 Theoretical Background

The AMCL algorithm is a further improvement of the MCL algorithm[17]. By tracking the likelihoodp=(zk|z1:k-1,u1:k,m) of the short-term likelihood (wfast) and the long-term likelihood (wslow) of the likelihood value, the resampling random divergence is dynamically adjusted based on changes in likelihood[18]. When the robot is in the “kidnapping” state, the short-term likelihood (wfast) falls faster than the long-term likelihood (wslow), and the resampling increases the diversity of new particles based on the difference[19]. Although the AMCL algorithm will generate new particles, it cannot solve the robot’s “kidnapping problem” due to its limitations which can be effectively solved by the IMCL algorithm proposed in this paper.

When the robot is in the “kidnapping” state, the matching degree between the environment of the observed robot and the actual map environment will decrease, which will affect the weights of the sample particles. The controller can then judge whether the robot is in the “kidnapping” state by changing the weight. In this way, a self-recovery localization mechanism is used to help the robot relocate. The process of the localization is shown in Fig.1 and the comparison of the MCL and IMCL relocalization processes is shown in Fig.2.

Initialization is introduced to represent the robot’s possible position on the grid map by scattering particles. According to the distance information returned by the laser radar, the direction information returned by the gyroscope, and the displacement information carried by the encoder, the sampling of the particles is completed and the particles that can represent the real position of the robot can thus be obtained. The key step is divergent sampling that when the robot fails localization due to hardware or artificial displacement, it is necessary to generate new particles that can represent the real position of the robot through divergent sampling.

Fig.1 Flowchart of localization

Fig.2 Comparison of MCL and IMCL relocalization processes

2.2 Derivation of IMCL Algorithm Theory

The main idea of the IMCL is to introduce divergent sampling.The purpose of robot localization is to obtain the real-time state of the robot. The solving recursive equations are expressed as follows:

(1)

For nonlinear systems, the analytical solution of this formula does not exist or is difficult to obtain.

Posterior distributed (M) particles can be expressed as

(2)

where the confidence

bel(x0:k)=p(x0:k|u1:k,z1:k)

(3)

and specifically

p(x0:k|z1:k,u1:k)=

ηp(zk|x0:k,z1:k-1,u1:k)p(x0:k|z1:k-1,u1:k)=

ηp(zk|xk)p(x0:k|z1:k-1,u1:k)=

ηp(zk|xk)p(xk|x0:k-1,z1:k-1,u1:k)·

p(x0:k-1|z1:k-1,u1:k)=

ηp(zk|xk)p(xk|xk-1,uk)·

p(x0:k-1|z1:k-1,u1:k-1)

(4)

(5)

ηp(zk|xk)

(6)

(7)

When robot localization is completed,p(xk|z1:k,u1:k) will maintain a high value, but when the robot is in the “kidnapping” state,p(xk|z1:k,u1:k) will suddenly drop and the dynamic Bayesian network recursion will be in an invalid state as follows:

p(xk|z1:k,u1:k)∝0

(8)

bel(x0:k)∝0 orp(xk|z1:k,u1:k)

(9)

At this point, the particles are in a degenerate state, so the position information carried by the particles cannot represent the real position of the robot.

When the robot is in alocalization loss or a “kidnapping” state, the AMCL algorithm will generate random particles in the map, which will increase the computational burden on the computer and is inefficient for relocalization. Although random particles are generated, they cannot represent the real position of the robot and will be iterated during the random sampling process, resulting in a state that the robot is still in the initial position loss.

However, the proposed IMCL algorithm associates the generated random particles with the observations of the robot, detects the changes in the weights of the particles, and controls the number of random particles at all times. The new particles will slowly move close to the high-weight particles and iterate at any time so more and more particles will represent the robot’s real position.

When the robotk+1 moments move (dx,dy,dθ), then in the AMCL algorithm, the particle motion produces the particle sample set at timek+1 as follows:

(10)

From the generated particle sample set, particles are sampled and the robot is still in a loss state.

In the process of matching iterations, the weights of the particles are constantly changing and the state of the robot can be determined by detecting this change, which requires the robot to be able to detect the state of the robot by itself. Thus we need to use relevant parameters of the robot as bases to determine whether the robot is in a localization loss state.

Based on previous research, it has been found that there is no change in the weight of the normalized particles that can be used to determine the state of the robot. When the robot is in a localization loss, the sample set of particles can be well changed by the divergent sampling, so that more highly weighted particles are generated.

When the robotk+1 move (dx,dy,dθ) at a time in the IMCL algorithm, the particle motion produces the particle sample set at timek+1 as follows:

(11)

(12)

where theDvalue represents the status of robot localization loss and it will adopt adaptive changes according to different situations of localization losses.

In order to ensure the feasibility and efficiency of the algorithm, multiple sets of correlation parameters are used to determine the loss situation and to ensure that the program does not fall into a local optimum when running. At the same time, the classification of different localization losses can speed up the efficiency of particle optimization.

3 Classification of Localization Losses

Situations of localization loss can be divided into following types as shown in Fig.3.

Situation (A): The robot is “kidnapped” near the real localization;

Situation (B): The robot is “kidnapped” far away from the real localization;

(B1): The characteristics of the nearby environment are different;

(B2): The characteristics of the nearby environment are similar.

Fig.3 Various situations of robot localization loss

When the robot is in a localization loss, the average weight of the particles will decrease and the magnitude of the decline in different situations will be different. When the robot is in the loss under situation (A), the weight of the particles that represents the position of the robot will quickly drop to a lower value. Compared with situation (A), the weight of the particles in situation (B) will decrease by a large margin closed to zero. Situation (B) can be divided into similar situation (B1) and dissimilar situation (B2). There are many similar features in a similar environment which makes the data association more complex and the relocalization more difficult.

4 Localization Simulation and Experimental Data Analysis

4.1 Simulation Analysis of Localization

In order to verify the effectiveness of the proposed algorithm, this paper establishes the positioning and “kidnapping” of the robot in a simulation environment, in which the IMCL algorithm is verified by comparing the experimental data of IMCL and AMCL.

Robot’s motion model is expressed as

(13)

where (xt,yt) is the position of the robot at time stept,vis initialized to 1 for the robot motion velocity,ΔTis the robot motion step length,WxandWyare the Gaussian noise terms of the corresponding position components, andθis the heading declination angle.

The initial state of particles distribution is shown in Fig.4, in which the number of the particles is 2 000. The green arrow shows the robot’s real position in the environment, the grey small circle indicates the initial position of the robot represented by the particle, and the black squares indicate the obstacles.

It can be seen from Fig.4 that the particles at the initial time were distributed throughout the space.

Fig.4 Initial state of particles distribution

The localization algorithm needs to predict the possible position of the robot based onits latest observations, and at the same time combines the real position of the robot at the next moment and performs a dynamic adjustment to trace the green arrow (Fig.4). Comparing the simulation data of the IMCL algorithm with the AMCL, as shown in Fig.5, the IMCL algorithm data was quickly recovered after localization loss.

Fig.5 Simulation data forAMCL and IMCL algorithms

It can be seen from Fig.5 that the error between the estimated value and the true value during the initial localization was big. With the running time, particles gathered near the real position of the robot and the error gradually decreased, while when the robot was “kidnapped”, the error increased rapidly. The simulation data shows that the IMCL is superior to the AMCL in robot “kidnapping” detection, so it can be applied to solve the problem.

4.2 Experimental Analyses of Localization

Thispaper selects a laboratory of 10 500 mm×6 600 mm for the experimental scenario as shown in Fig.6 and a robot localization experiment platform as shown in Fig.7.

The experimental analyses of different scenarios with different levels of complexity and different “kidnapping” situations were conducted to demonstrate the effectiveness of the proposed algorithm.

Fig.6 Robot localization test scenario

Fig.7 Robot localization experiment platform

The mobile robot in this experiment mainly includes the following functional units.

Main control system: Based on Ubuntu 16.04 ROS operating system, the hardware supports 8 GB of DELL inspiration 5420 laptop.

Data acquisition sensor:The SICK lms111 laser radar is used as the environmental measurement sensor, which is in the 180-degree scanning range of the minimum angular resolution of 0.5 degrees and the maximum measurement distance of 20 m to meet the needs of indoor mobile robot localization.

Direction and displacement sensorsare encoder and FY-AHRS model gyroscope.

4.2.1Analysisoftheexperimentaldataindifferentsituationsandenvironments

The experimental data was taken as the reference for the actual position of the robot and the position of the robot in the grid map during the experiment as shown in Fig.8 and Fig.9. The effectiveness of the IMCL algorithm was demonstrated by analyzing the changes in the weights of the particles during the experiment. The data provided in this paper were processed without affecting the true and valid data. The data analyses of the MCL, AMCL, and IMCL algorithms are shown in Fig.10 and Fig.11.

(a)Denotes the initial position of the robot in a simple environment

(b)Denotes the position of the robot after being “kidnapped” in a simple environment

(c)Denotes the initial position of the robot in a complex environment

(d)Denotes the position of the robot after being “kidnapped” in a complex environment

Fig.8 Scenarios for different algorithms under situation (A)

4.2.1.1Dataanalysisundersituation(A)

In situation (A), the robot was “kidnapped” near the real localization and it could observe the same road sign as before it was kidnapped.

The “kidnapping” data of the MCL, AMCL, and IMCL algorithms in situation (A) is shown in Fig.10. The data of MCL and AMCL had consistency in simple and complex environments, which is also consistent with the variation of the weights of the particles in the standard MCL algorithm.

(a)Denotes the initial position of the robot in a simple environment

(c)Denotes the initial position of the robot in a complex environment

(b)Denotes the position of the robot after being “kidnapped” in a simple environment

(d)Denotes the position of the robot after being “kidnapped” in a complex environment

Fig.9 Scenarios for different algorithms under situation (B)

When the robot was in the situation of localization loss, the weights of the particles suddenly dropped, failed to recover in the MCL and AMCL localization algorithms, and fluctuated near a lower weight. However, from the data curve of the IMCL (Fig.10), it can be seen that after a period of iterations, the weights returned to a stable state.

The experiments show that the improved robot had the ability to recover from situation (A) in both simple and complex environments.

Fig.10 Comparison of relocalization results of different algorithms for situation (A) in different environments

4.2.1.2Dataanalysisundersituation(B)

In situation (B), the robot was “kidnapped” far away from the real localization and it could not observe the same road sign as before it was “kidnapped”.

Fig.11 shows the experimental results of situation (B) in different environments. The data obtained under situation (B) had certain similar characteristics to that in situation (A). When the robot was in a state of localization loss, the weights of the particles would decrease, but as the number of iterations increased, only the weights of particles in the IMCL algorithm slowly rose back to a constant value, while that in the other two algorithms remained the same. Volatility around a lower value could not be recovered. Compared with situation (A), the recovery speed of IMCL algorithm in situation (B) was much slower. It is because there was a big change in the long-distance environmental characteristics, which led to a larger range of particle optimization.

Fig.11 Comparison of relocalization results of different algorithms for situation (B) in different environments

4.2.2DataanalysisofIMCL

In order to analyze the relocalization effect of the IMCL algorithm in a complex dynamic environment, further experiments were performed and the data is shown in Fig.12.

Fig.13 shows the experimental results of the IMCL algorithm in the complex dynamic environment. According to the localization data of the IMCL algorithm in complex and complex dynamic environments, the IMCL algorithm achieved self-recovery of localization loss for the two proposed situations in this work. From the results, it can be seen that the data obtained by the IMCL algorithm in different environments are basically consistent in terms of features. However, in the complex and complex dynamic environments, the characteristics of the weights of particles were obviously changed. Moreover, the average weight of ions varied greatly when the robot was in a “kidnapped” or a normal state, but this change was not the same in different environments. Overall, IMCL is proved robust on the issue of relocalization after “kidnapping”.

Through the above three sets of experimental results, it can be concluded that the IMCL algorithm can solve the problem of robot’s self-recovery from localization loss.

(a)Denotes the initial position of the robot in situation (A) in a complex dynamic environment

(c)Denotes the initial position of the robot in situation (B) in a complex dynamic environment

(b)Denotes the position of the robot after being “kidnapped” in situation (A) in a complex dynamic environment

(d)Denotes the position of the robot after being “kidnapped” in situation (B) in a complex dynamic environment

Fig.12 IMCL algorithm for different situations in a complex dynamic environment

Fig.13 Comparison of results obtained by IMCL algorithm under different situations in a complex dynamic environment

4.2.3Analysisofsituation(B)relocalizationefficiencyindifferentsituations

For situation (B), the IMCL had anisotropy in resolving the effectiveness of “kidnapping” relocalization. A more complex environment was selected for this experimental scenario. Fig.14 demonstrates the seven images of the entire experimental process. The experiments in this paper mainly focus on the time needed for “kidnapping” iterations under different situations. Comparing situation (B1) with situation (B2) (Fig.15), situation (B1) had an iterative average (less than 500) for 10 experiments which was significantly less than situation (B2) (greater than 500). During the experiment, it was found that robots achieved same localization loss and then recovered under different situations. Fig.14(a) shows that the particles were in the initialized state. Figs.14(b)-(c) show the robot’s real position on the map by observing the model and sampling the motion model. Figs.14(d)-(g) show the state of the robot being “kidnapped.” The particles representing the robot in Figs.14(d) and (f) began to determine the real position of the machine through the divergent sampling and the observation function weights. Fig.14(e) shows that the particles were in a locally convergent state, but with the iteration of the motion model and observation model, they would re-diverge and finally converge to the real position of the robot.

Fig.14 Dynamic process of relocalization particles by localization loss of IMCL algorithm

As shown in Fig.15, through multiple test results, the self-recovery of localization loss in situation (B1) was much faster. This experiment selected a complex environment and set relatively similar environments. When the robot was in a “kidnapping” state, by observing the particle dynamics, it can be found that the particles were clustered in a similar environment in situation (B2), which results in the proper positioning of the robot and more time for iteration. This makes it difficult for the iterations to increase, but over time, the iterations will increasingly approach the correct position and be accurately localized. The experiments also show that the complexity and similarity of the environments do affect the efficiency of robot localization.

A similar environment may cause the particles to fall into a local optimum, but the improved algorithm relocalization will work well that it guarantees that the robot will not fall into a local optimum during relocalization.

Fig.15 Comparison results of IMCL under different conditions in situation (B)

5 Summary and Outlook

In this paper, we propose an IMCL algorithm to solve the inability of robots to self-recover when their initial localization fails. Experiments conducted in simple and complex dynamic environments show that by improving the particle resampling function and the resampling mechanism, robots can relocate themselves from the “kidnapped” state to their real positions. Future studies can focus on extracting feature data from the indoor environment. A robot recognizable semantic map can be created, which can speed up the localization of the robot and the efficiency of relocalization after localization loss.