An Adaptive Unscented Particle Filter Algorithm through Relative Entropy for Mobile Robot Self-Localization

Abstract

Self-localization is a basic skill for mobile robots in the dynamic environments. It is usually modeled as a state estimation problem for nonlinear system with non-Gaussian noise and needs the real-time processing. Unscented particle filter (UPF) can handle the state estimation problem for nonlinear system with non-Gaussian noise; however the computation of UPF is very high. In order to reduce the computation cost of UPF and meanwhile maintain the accuracy, we propose an adaptive unscented particle filter (AUPF) algorithm through relative entropy. AUPF can adaptively adjust the number of particles during filtering to reduce the necessary computation and hence improve the real-time capability of UPF. In AUPF, the relative entropy is used to measure the distance between the empirical distribution and the true posterior distribution. The least number of particles for the next step is then decided according to the relative entropy. In order to offset the difference between the proposal distribution, and the true distribution the least number is adjusted thereafter. The ideal performance of AUPF in real robot self-localization is demonstrated.

1. Introduction

Autonomous mobile robots have increasingly been used in a wide range of applications [1, 2]. Self-localization is a fundamental and challenging task for the autonomous mobile robots [3]. In order to successfully make decisions or perform the missions, the mobile robots have to know their poses precisely [4]. The self-localization of mobile robot can be defined as the estimation for the pose of the mobile robot relative to the environment [5].

Over the past years many different solutions for mobile robot self-localization have been presented, and they can be divided into two classes: the sensor-based method and the sensor assisted probabilistic method. The sensor-based method only uses the sensor information to realize the mobile robot self-localization. Reference [6] presents a natural landmark-based indoor localization algorithm. The natural landmarks are extracted from the input image which is obtained by the vision sensor. Then the map positions of the natural landmarks are identified by comparing the input image with the reference data set to acquire the mobile robot self-localization. Reference [7] has suggested an algorithm that only utilizes the Bluetooth device to realize the self-localization of the mobile robot. Although the sensor-based method is easy to implement, it has an inherent limitation. The range and resolution of the sensors are subject to the physical law, and the sensor measurements can be perturbed by the noises. These limitations influence the sensors' ability to extract the useful information from the measurements. On the other hand the sensor-based probabilistic method can utilize probabilistic law to deal with the noise and the uncertainty of the sensor information [5].

Bayesian filter is the most important probabilistic method for self-localization which is a state estimation problem for nonlinear system with non-Gaussian noise [8, 9]. Bayesian filter is a class of statistical filter techniques via Bayesian inference, and it can recursively calculate the posterior probability, the estimated state, and the predicted state [10]. Bayesian filter is easy to implement and has a wide range of applications, so it has been extensively studied in recent years [11, 12].

There are several commonly used Bayesian filter algorithms that can be applied in the mobile robot self-localization. Kalman Filter (KF) is a recursive algorithm for the linear system state estimation with Gaussian noise. KF has high computational efficiency and good accuracy [13]. In [14], the applicability of KF to the mobile robot self-localization is analyzed. This algorithm is only suitable for linear systems. Extended Kalman Filter (EKF) is another kind of Bayesian filter for the mobile robot self-localization. In [15] EKF is used to localize the four-wheeled mobile robot equipped with the wheel encoders and a laser-rangefinder sensor. EKF is derived for the nonlinear systems, which linearizes the measurements and evolution models using the Taylor series expansions. EKF only uses the first-order term of the Taylor series expansions of the nonlinear functions. This linearization often induces large errors in the estimated statistics of the posterior distributions. This is especially evident when the models are highly nonlinear, and it can lead to filter divergence [16]. Unlike EKF, the Unscented Kalman Filter (UKF) does not approximate the nonlinear process and the observation model. It uses the true nonlinear models. UKF approximates the distribution of the state random variables by the scaled unscented transformation [17]. In [18], the performances of EKF and UKF are compared for the localization of an autonomous robot. The conclusion is that UKF has shown better performance in terms of average localization accuracy. UKF does not need to solve the Jacobian matrix, so it has fewer computations and higher precision.

Self-localization is a state estimation problem for nonlinear system with non-Gaussian noise. Neither EKF nor UKF can deal with non-Gaussian noise, but particle filter (PF) can handle the state estimation problem of nonlinear system with non-Gaussian noise [19]. The key idea of PF is to use particles to represent the posterior distribution of the state. As new information arrives, these particles are reallocated to update the estimation of the state of the system [20]. As a useful tool for dealing with the nonlinear and non-Gaussian problems, PF has been widely used in various fields [21, 22].

In PF, the particles are usually sampled from the proposal distribution which neglects the new observation, and this can cause particle degeneracy and influence the accuracy of PF. Then the unscented particle filter (UPF) algorithm is proposed to resolve the problem. In UPF, UKF is used to generate the new proposal distribution [23]. This proposal can incorporate the latest observation and match the true posterior distribution more closely, so UPF improves the accuracy of state estimation evidently. In traditional UPF the number of particles is fixed, and in each step each particle utilizes UKF to obtain the new importance proposal that can bring huge computation and influence the real-time capability of the filter. During filtering fixed number of particles may bring unnecessary computation, so the computation will be reduced if the number of particles can be dynamically adjusted according to the estimation quality during filtering.

Mobile robots usually work under a dynamic environment; real-time self-localization is thus needed to be realized [24]. So UPF should reduce its computation to accommodate mobile robots self-localization. To the best of our knowledge there is no related work about how to reduce the computation cost of UPF and meanwhile maintain accuracy. The computation cost of UPF depends on the number of particles, so in order to reduce the computation cost of UPF the number of particles should be adaptively adjusted during filtering.

In this paper an adaptive unscented particle filter algorithm through relative entropy (AUPF) is proposed to enhance the real-time capability of UPF and meanwhile ensure its accuracy. In AUPF, the relative entropy is used to measure the distance between the empirical distribution and the true posterior distribution; the least number of particles for the next step is then decided according to the relative entropy. In order to offset the difference between the proposal distribution and the true distribution the least number is adjusted thereafter. AUPF can adaptively adjust the number of particles during filtering to reduce the necessary computation and hence improve the real-time capability of UPF.

The rest of the paper is organized as follows. Section 2 describes the dynamic state-space model of mobile robot self-localization problem. In Section 3 the concept of UPF is introduced. In Section 4 an adaptive UPF algorithm through relative entropy is proposed. Then Section 5 shows the experiment results which include the simulation results and the utilization of AUPF in a real robot self-localization. Finally, Section 6 gives the conclusions.

2. Dynamic State-Space Model of Mobile Robot Self-Localization

To solve the mobile robot self-localization problem, we need a self-localization model for robot. In general, such a model can be described using a set of discrete-time equations given as follows: 𝑥_𝑡=𝑓𝑥_𝑡−1,𝑤_𝑡,𝑧_𝑡=ℎ𝑥_𝑡,𝑣_𝑡, (1)(2) where 𝑥_𝑡∈𝑅^𝑛𝑥 denotes the self-localization state vector of the mobile robot at time step 𝑡; 𝑛_𝑥 means the dimension of 𝑥_𝑡; 𝑤_𝑡∈𝑅^𝑛𝑤 denotes the process noise at time step 𝑡 which is usually non-Gaussian; 𝑧_𝑡∈𝑅^𝑛𝑧 is the observation vector at time step 𝑡; 𝑛_𝑧 is the dimension of 𝑧_𝑡; 𝑣_𝑡∈𝑅^𝑛𝑣 is the measurement noise at time step 𝑡 which is also usually non-Gaussian; 𝑛_𝑣 is the dimension of 𝑣_𝑡; the states follow a first-order Markov process, and the measurements are assumed to be independent. The function 𝑓 represents the state transition model, 𝑓∶𝑅^𝑛𝑥∗𝑅^𝑛𝑤↦𝑅^𝑛𝑥. The function ℎ represents the state measurement model, ℎ∶𝑅^𝑛𝑥∗𝑅^𝑛𝑣↦𝑅^𝑛𝑧.

3. Unscented Particle Filter Algorithm

In PF, it is hard to design the proposal distribution which can approximate the true posterior distribution reasonably well. The common strategy is to sample the particles according to the state transition. This strategy cannot consider the new observations, so it can not generate an ideal proposal distribution. UPF is proposed to resolve this problem. In UPF, UKF is introduced to produce the proposal distribution of particle filter which considers the new observation, and the produced proposal distribution is close to the true posterior distribution.

UKF is a minimum mean square error estimator for non-linear system. It utilizes the scaled unscented transformation to calculate the statistics of a random variable. UKF has higher accuracy than EKF especially when the models are highly nonlinear. In UPF, before sampling of the particles UKF is utilized to generate the proposal distribution which can consider the new observation. The proposal distribution generated by UKF has a bigger overlap with the true posterior distribution than PF, so UPF has relatively high accuracy, while, in UPF, at each time step each particle will utilize UKF to generate the proposal distribution, so the computation cost of UPF is huge.

The basic UPF algorithm is described as in Algorithm 1: 𝑥_𝑡=1𝑁^𝑁_𝑖=1𝑥_𝑡^(𝑖). (3)

(1) Initialization: 𝑡=0   𝑖=1   do      draw the particles 𝑥𝑖0 from the prior 𝑝(𝑥0) and set              𝑥_0(𝑖)=𝐸[𝑥0(𝑖)]             𝑃0(𝑖)=𝐸[(𝑥0(𝑖)−𝑥_0(𝑖))(𝑥0(𝑖)−𝑥_0(𝑖))𝑇] 𝑥_0(𝑖)𝑎=𝐸[𝑥(𝑖)𝑎]=(𝑥_0(𝑖))𝑇00𝑇                   𝑃0(𝑖)𝑎=𝐸[(𝑥0(𝑖)𝑎−𝑥_0(𝑖)𝑎)(𝑥0(𝑖)𝑎−𝑥_0(𝑖)𝑎)𝑇] =𝑃0(𝑖)000𝑄000𝑅      // 𝑁 is the number of particles in UPF. 𝑥_0(𝑖) is the initial mean value of 𝑥0(𝑖).      // 𝑃0(𝑖) is the initial covariance of 𝑥0(𝑖). 𝑥_𝑎0 is the initial mean value of 𝑥𝑎0.      // 𝑄 is the covariance of 𝑤𝑡. 𝑅 is the covariance of 𝑣𝑡.   While ( 𝑖++=<𝑁 ) (2) For 𝑡=1,2,…,𝑡𝑓   // 𝑡𝑓 is the run time of UPF.   (1) Importance sampling   𝑖=1   do      Update the particles with UKF, obtain 𝑥_𝑡(𝑖) and 𝑃𝑡(𝑖).           𝑥𝑡(𝑖)∼𝑞(𝑥𝑡(𝑖)∣𝑥(𝑖)0∶𝑡−1,𝑧1∶𝑡)=𝑁(𝑥_𝑡(𝑖),𝑃𝑡(𝑖))      // The particles 𝑥𝑡(𝑖) are sampled from 𝑁(𝑥_𝑡(𝑖),𝑃𝑡(𝑖)).   while ( 𝑖++=<𝑁 )   (2) Computing the importance weights    𝑖=1    do             𝑤𝑡(𝑖)∝𝑝(𝑧𝑡∣𝑥𝑡(𝑖))𝑝(𝑥𝑡(𝑖)∣𝑥𝑖𝑡−1)𝑞(𝑥𝑡(𝑖)∣𝑥(𝑖)0∶𝑡−1,𝑧1∶𝑡)    // 𝑤𝑡(𝑖) is the importance weight of the 𝑖th particle at the time step 𝑡.    // The state transition probability 𝑝(𝑥𝑡(𝑖)∣𝑥𝑖𝑡−1) can be obtained from (1).    // The observation model 𝑝(𝑧𝑡∣𝑥𝑡(𝑖)) can be obtained from (2).    // 𝑞(𝑥𝑡(𝑖)∣𝑥(𝑖)0∶𝑡−1,𝑧1∶𝑡) is the proposal distribution.    while ( 𝑖++=<𝑁 )   (3) Normalize the importance weights    𝑖=1    do                𝑤𝑡(𝑖)=𝑤𝑡(𝑖)∑𝑁𝐼=1𝑤𝑡(𝑖)    while ( 𝑖++=<𝑁 )   (4) Resampling step    𝑖=1    do    Multiply/Suppress particles 𝑥𝑡(𝑖) according to the importance weights 𝑤𝑡(𝑖) to obtain 𝑁    random particles 𝑥𝑡(𝑖).    while( 𝑖++=<𝑁 )   (5) Output    The output 𝑥𝑡 is the state estimation of 𝑥 at the time step 𝑡. It can be approximated by (3).

Algorithm 1 

4. An Adaptive Unscented Particle Filter Algorithm through Relative Entropy

It can be seen that UPF described in Section 3 may provide high accuracy for estimation. However, it is computationally expensive due to the utilization of UKF for each particle at each time step. In this section, in order to reduce the computation cost and meanwhile maintain the accuracy of UPF, we propose an adaptive unscented particle algorithm by adaptively adjusting the number of particles during the process. Specifically, we introduce the relative entropy to bind the error induced by sample-based representation of UPF and correspondingly determine the number of particles for each filter iteration.

4.1. Relative Entropy Based Adaptive Adjustment of UPF

The sample-based representation of UPF will bring the error. If the error can be bounded, the accuracy of UPF will be insured. So the adaptive adjustment of the number of particles needs to ensure that the error introduced by the sample-based representation must be within a bound. The error can be measured by the distance between the empirical distribution and the true posterior distribution. In order to maintain the accuracy of UPF, it is assumed that the error must be within a prespecified value 𝜀.

The relative entropy is a measure of the distance between two distributions [25]. The relative entropy between two probability functions 𝑝₁ and 𝑝₂ can be defined by 𝐾𝑝₁,𝑝₂=𝑝₁𝑝log¹𝑝₂. (4)

In UPF 𝑁 particles are drawn from the proposal distribution with 𝑘 different bins. 𝑋 denotes the number of particles drawn from each bin, 𝑋=(𝑋₁,…,𝑋_𝑘). 𝑝 means the true distribution. 𝑝 specifies the probability of each bin, 𝑝=(𝑝₁,…,𝑝_𝑘). The maximum likelihood estimate of 𝑝 is given by 𝑝=𝑁⁻¹𝑋. (5)

The likelihood ratio statistic 𝜆_𝑁 for testing 𝑝 is given by log𝜆_𝑁=^𝑘_𝑗=1𝑋_𝑗𝑝log^𝑗𝑝_𝑗=𝑁^𝑘_𝑗=1𝑝_𝑗𝑝log^𝑗𝑝_𝑗=𝑁𝐾𝑝,𝑝. (6)

Reference [25] gives a conclusion that the likelihood ratio statistic can satisfy a chi-square distribution convergence: 2log𝜆_𝑁⟶𝜒²_𝑘−1𝑛⟶∞. (7)

So𝑃𝐾𝑝,𝑝≤𝜀=𝑃2𝑁𝐾𝑝,𝑝𝜒≤2𝑁𝜀=̇𝑃²_𝑘−1≤2𝑁𝜀. (8)

Then we can obtain 2𝑁𝜀>𝜒²_{𝑘−1,1−𝛿}, (9) where 𝛿 is a given probability; 𝜒²_{𝑘−1,1−𝛿} can be obtained according to 𝛿 and 𝑘𝜒𝑁>^{2𝑘−1,1−𝛿}.2𝜀 (10)

Equation (10) is transformed from (9). From (10) it's found that when the number of particles 𝑁 is big enough the relative entropy between the maximum likelihood estimate and the true distribution is less than 𝜀 with the probability 1−𝛿.

4.2. Compensation for the Influence of the Importance Proposal

In Section 4.1, 𝑝 is assumed to be the true distribution, while the true distribution cannot be obtained, and it is approximately described by the proposal distribution. Furthermore the quality of the match between the proposal distribution and the true distribution is one of the main elements that determines the accuracy of the filter, hence the suitable number of particles. To fix this problem, we need a way to quantify the degradation in the estimation using samples from the proposal distribution.

In [26], Var_true can measure the influence of sampling from the true distribution 𝑝(𝑥), and it is calculated by Var_true=Var_𝑝(𝑥)𝑁,Var_𝑝(𝑥)=𝐸_𝑝𝑥²−𝐸_𝑝(𝑥)²≈∑^𝑁_𝑖=1𝑥²_𝑖𝜔_𝑖∑^𝑁_𝑖=1𝜔_𝑖−𝐸_𝑝(𝑥)²𝐸_𝑝,(𝑥)=∑^𝑁_𝑖=1𝑥_𝑖𝜔_𝑖∑^𝑁_𝑖=1𝜔_𝑖. (11)(12)(13)

In [27], when the particles come from the proposal distribution 𝑞(𝑥), the variance of the estimator Var_𝐼𝑆 can be given by Var_𝐼𝑆=𝜎²_𝐼𝑆𝑁_𝐼𝑆, (14) where 𝑁_𝐼𝑆 is the number of particles coming from the proposal distribution, 𝜎²_𝐼𝑆≈∑^𝑁_𝑖=1𝑥²_𝑖𝜔²_𝑖∑^𝑁_𝑖=1𝜔_𝑖−∑^𝑁_𝑖=1𝑥_𝑖𝜔²_𝑖𝐸_𝑝(𝑥)²∑^𝑁_𝑖=1𝜔_𝑖+∑^𝑁_𝑖=1𝜔²_𝑖𝐸_𝑝(𝑥)²∑^𝑁_𝑖=1𝜔_𝑖. (15)

In order to obtain the similar accuracy, let (11) be equal to (14): 𝜎²_𝐼𝑆𝑁_𝐼𝑆=Var_𝑝(𝑥)𝑁⟹𝑁=Var_𝑝(𝑥)𝑁_𝐼𝑆𝜎²_𝐼𝑆,2𝑁𝜀>𝜒²_{𝑘−1,1−𝛿}⟹2Var_𝑝(𝑥)𝑁_𝐼𝑆𝜎²_𝐼𝑆>𝜒²_{𝑘−1,1−𝛿}⟹𝑁_𝐼𝑆>𝜎²_𝐼𝑆2𝜀Var_𝑝(𝑥)𝜒²_{𝑘−1,1−𝛿}. (16)

So the minimum number of particles 𝑁^min_𝐼𝑆 can be obtained according to 𝑁^min_𝐼𝑆=𝜎²_𝐼𝑆2𝜀Var_𝑝(𝑥)𝜒²_{𝑘−1,1−𝛿}. (17)

4.3. An Adaptive Unscented Particle Filter Algorithm through Relative Entropy

Figure 1 illustrates the relationship between several basic methods.

Figure 1 

The relationship between several basic filter methods.

The adaptive unscented particle filter algorithm through relative entropy is shown in Algorithm 2.

Inputs: 𝑆𝑡−1=𝑥𝑖𝑡−1,𝑤𝑖𝑡−1𝑖=1,…,𝑛; the control measurement 𝑢𝑡−1; the observation 𝑧𝑡;     the bound 𝜀 and the probability 𝜎; the bin size ℏ; 𝑆𝑡=𝜙; 𝑛=0; 𝑘=0; 𝛼=0. Generate the particles: 𝑖=1 do  Sample a particle index 𝑗(𝑛) from the discrete distribution according to the weights in 𝑆𝑡−1.  Obtain the particles 𝑥𝑛𝑡 from the particles 𝑥𝑗(𝑛)𝑡−1 according to (1).  Compute the importance weight:  𝑤𝑛𝑡=𝑝(𝑧𝑡∣𝑥𝑛𝑡)  𝛼=𝛼+𝑤𝑛𝑡  Insert particles into the particle set:  𝑆𝑡=𝑆𝑡∪𝑥𝑛𝑡,𝑤𝑛𝑡  if (𝑥𝑛𝑡 falls into empty bin)     𝑘=𝑘+1  end if  𝑛=𝑛+1 while 𝑛<𝜎2IS/2𝜀Var𝑝(𝑥)𝜒2𝑘−1,1−𝛿 Normalize the importance weight: for 𝑖=1,…,𝑛  𝑤𝑖𝑡=𝑤𝑖𝑡/𝛼 return 𝑆𝑡

Algorithm 2 

5. Experimental Results

In this section, the results of two experiments are presented. In the first experiment the performance of AUPF is compared with UPF and PF through a simulation to validate the effectiveness of AUPF. Then the utilization and performance of AUPF for a real robot self-localization are also illustrated in detail.

5.1. Simulation Results

To illustrate the effectiveness of the proposed AUPF, a scalar nonlinear system is considered which is shown in (18) and (19). The simulation is carried out in an HP computer with 4 G memory and Intel Core i2-2120 CPU 𝑥_𝑡=2+sin(0.02𝜋𝑡)+0.3𝑥_𝑡−1+𝑤_𝑡−1,𝑧_𝑡=0.2𝑥²_𝑡+𝑣_𝑡−1,𝑡≤30,0.5𝑥+𝑣_𝑡−1,𝑡>30. (18)(19)

Equation (18) is the state transition model. Equation (19) is the state observation model. 𝑤_𝑡 is white Gaussian noise, and its covariance is 1.5; 𝑣_𝑡 is white Gaussian noise, and its covariance is 1.

We compare the AUPF with traditional PF and UPF. The number of particles in PF and UPF is set to be 100. The initial number of particles in AUPF is also set to be 100. In the experiment we use a value 0.99 for 𝛿 and a fixed bin size Δ 0.02. Random 50-step state estimation results are shown in Figure 2.

Figure 2 

The estimation result of PF, UPF, and AUPF.

The root mean square error (RMSE) is an important value that can be used to reflect the accuracy of state estimation. RMSE can be calculated by RMSE=∑^𝑡𝑓_𝑡=1(𝑥−𝑥)².𝑡𝑓 (20)

We have run AUPF, UPF, and PF for 10000 steps; the RMSE of the three algorithms is shown in Table 1. Meanwhile the average number of particles of the three algorithms is shown in Table 2.

Table 1 

RMSE of AUPF, PF, and UPF.

Table 2 

Average number of particles of AUPF, PF, and UPF.

From Figure 2 and Table 1 we can see that the estimation result of PF is not satisfying. Particle filter uses a set of particles to represent the posterior distribution, so the accuracy of PF is decided by the number of particles. In this experiment the number of particles is 100, and these particles cannot approximate the true posterior density well especially when the nonlinearity of the system is strong. The performance of UPF is the most excellent, because in this algorithm the latest observation is incorporated into the proposal distribution, so 100 particles are enough to approximate the true posterior density. In Table 2 the average number of particles of PF and UPF is 100; it is a fixed number of these algorithms, while in AUPF this number is adaptively adjusted according to (17). According to the adaptive adjustment of the number of particles the runtime of AUPF is reduced which is proportional to this number.

5.2. AUPF for Real Mobile Robot Self-Localization

We hereafter experimentally validate the effectiveness of AUPF for mobile robot self-localization. The robot “E-Robot” which is developed by Chinese Academy of Sciences is utilized for mobile robot self-localization. E-Robot is shown in Figure 3.

Figure 3 

The E-Robot used in the experiments.

During the mobile robot self-localization, the odometer information and the visual information are used. The picture is obtained by the ultrahigh definition color gun camera AMPSON DSC-B468C, and it is processed by FPGA Cyclone II EP2C20. The image pixel of the picture obtained by the camera is 256 ∗ 256. In one second about 30 pictures can be processed.

5.2.1. Mobile Robot Motion Model

The odometer can obtain the offset of robot pose (Δ𝑥𝑟, Δ𝑦𝑟, and Δ𝜃𝑟) from time 𝑡−1 to time 𝑡, so the motion model of mobile robot can be represented by 𝑥𝑟_𝑡=𝑥𝑟_𝑡−1+Δ𝑥𝑟+Δ𝑥𝑟∗𝛿_𝑥𝑟,𝑦𝑟_𝑡=𝑦𝑟_𝑡−1+Δ𝑦𝑟+Δ𝑦𝑟∗𝛿_𝑦𝑟,𝜃𝑟_𝑡=𝜃𝑟_𝑡−1+Δ𝜃𝑟+Δ𝜃𝑟∗𝛿_𝜃𝑟, (21) where (𝑥𝑟_𝑡, 𝑦𝑟_𝑡) is the position at time step 𝑡; 𝜃𝑟_𝑡 is the orientation at time step 𝑡; 𝛿_𝑥𝑟, 𝛿_𝑦𝑟, and 𝛿_𝜃𝑟 are non-Gaussian noise.

5.2.2. Mobile Robot Observation Model

In our experiment, the mobile robot can move under an environment with many features, such as the white line and the two-dimension codes. All the features can be extracted from robot vision information.

In this paper, the special two-dimension (2D) codes have been used as the features or landmarks. As shown in Figure 4, the outer contour of each 2D code is regular pentagon. A different 2D code has different dots inside the regular pentagon. The 2D code is developed by Institute of Automation, Chinese Academy of Sciences. It is easy to be processed and can bring plenty of information. In [28], the image processing for 2D code is explained in detail.

(a) Example 1 

(b) Example 2

(c) Example 3

(d) Example 4

Figure 4 

Some examples of 2D code developed by Chinese Academy of Science.

In our experiments, each 2D code is different from the others. Each 2D code is located at a fixed position, and it brings the location information. So once the mobile robot detects one 2D code, it can obtain one numerical value of self-localization with the help of image processing.

Assume that the robot can detect 𝑛 two-dimension codes 𝑑₁,…,𝑑_𝑛, then 𝑛 self-localization value 𝑙𝑣₁,…,𝑙𝑣_𝑛 can be obtained accordingly.

In our experiment, the mobile robot self-localization result is represented by a set of particles, 𝑠^𝑡_𝑗=⟨𝑙𝑝^𝑡_𝑗,𝑝^𝑡_𝑗⟩, where 𝑙𝑝^𝑡_𝑗 means the self-localization value maintained by the 𝑗th particle at time step 𝑡, and 𝑝^𝑡_𝑗 is the importance weight of the 𝑗th particle at time step 𝑡. Utilizing the mobile robot motion model and 𝑙𝑝_𝑗^𝑡−1, the 𝑗th particle can obtain an original self-localization numerical value 𝑙𝑝𝑜^𝑡_𝑗. If the robot is localized at 𝑙𝑝𝑜^𝑡_𝑗, the distance between 𝑙𝑝𝑜^𝑡_𝑗 and 𝑙𝑣_𝑖 can be used to calculate the probability 𝑃(𝑑_𝑖∣𝑙𝑝𝑜_𝑗^𝑡−1) with which the robot localized at 𝑙𝑝𝑜^𝑡_𝑗 can observe the feature 𝑑_𝑖. The probability will become bigger along with the decrease of the distance 𝑃𝑑_𝑖∣𝑙𝑝𝑜_𝑗^𝑡−1=e^{−|𝑙𝑣𝑖−𝑙𝑝𝑜𝑡𝑗|}. (22)

The extraction of the features just depends on the robot's position and orientation, so the features are mutually independent. The observation model 𝑃(𝑑∣𝑙𝑝𝑜_𝑗^𝑡−1) is formulated in 𝑃𝑑∣𝑙𝑝𝑜_𝑗^𝑡−1=𝑃𝑑₁∣𝑙𝑝𝑜_𝑗^𝑡−1∗⋯∗𝑃𝑑_𝑛∣𝑙𝑝𝑜_𝑗^𝑡−1=e^{−|𝑙𝑣1−𝑙𝑝𝑜𝑡𝑗|}∗⋯∗e^{−|𝑙𝑣𝑛−𝑙𝑝𝑜𝑡𝑗|}. (23)

Actually, the mobile robot can detect at least one feature in our experiment, and no more than two features will be chosen which are the nearest to the robot.

5.2.3. The Realization of AUPF for Mobile Robot Self-Localization

In the experiment, the initial number of particles of AUPF is set to be 400. The number of particles during filtering at time instant 𝑡 is represented by 𝑁𝑝^𝑡. The process of the realization for mobile robot self-localization is reported as follows.(1)Obtain the initial position of the mobile robot 𝑙₀ according to the robot vision.(2)Around 𝑙₀ a field of 2 ∗ 2 meters is chosen, and 400 particles are uniformly distributed in this field; the importance weight of each particle is 1/400.(3)Obtain new particles according to UKF algorithm and the mobile robot motion model which utilizes the odometer information.(4)Obtain the vision information, extract the features, and update the importance weight of each particle according to (17).(5)Output the estimation of mobile robot position according to (18), then calculate the orientation of the robot according to (24), and then return to step 3 at the next time step.(6)Calculate the number of particles 𝑁𝑝^𝑡.(7)Resample 𝑁𝑝^𝑡 particles according to their importance weight.(8)Set all the importance weight of particles to be 1/𝑁𝑝^𝑡.

The robot position (𝑥𝑟_𝑡, 𝑦𝑟_𝑡) is obtained by AUPF; then the orientation 𝜃𝑟_𝑡 can be calculated according to 𝑥𝑙^𝑟_𝑡=𝑥𝑙−𝑥𝑟_𝑡cos𝜃𝑟_𝑡−𝑦𝑙−𝑦𝑟_𝑡sin𝜃𝑟_𝑡,𝑦𝑙^𝑟_𝑡=𝑥𝑙−𝑥𝑟_𝑡sin𝜃𝑟_𝑡+𝑦𝑙−𝑦𝑟_𝑡cos𝜃𝑟_𝑡, (24) where (𝑥𝑙, 𝑦𝑙) is the position of one 2D code in the world coordinate, and this position is already known. All the positions used above are in the world coordinate. (𝑥𝑙^𝑟_𝑡, 𝑦𝑙^𝑟_𝑡) is the position of the 2D code in the robot coordinate, and this position can be easily obtained by the image processing.

5.2.4. Experiment Results and Analysis

In order to illustrate the effectiveness of AUPF, we designed a mobile robot self-localization experiment. In the experiment, the E-Robot randomly moved on the ground, and a trajectory was generated. Then the trajectory was recorded. The trajectory is depicted in Figure 5 by a black line. From Figure 5 we can see that the 2D codes are located on the ground as landmarks. When the robot moved, AUPF was run to calculate the robot position at each time step. The effect of AUPF can be measured by the difference between the calculated position and the trajectory.

Figure 5 

The real trajectory of the E-Robot in the experiment.

Then we adopt AUPF, UPF, and PF to obtain the estimation of self-localization value. The estimation results of the three algorithms are shown in Figure 6.

Figure 6 

The estimation results of PF, UPF, and AUPF.

The mobile robot self-localization result can be measured by the mean relative position error (MRPE) which is defined in ∑MRPE=^{𝑡𝑓𝑡=1}𝑥^𝑡_true−𝑥_𝑡²+𝑦^𝑡_true−𝑦_𝑡²,𝑡𝑓 (25) where 𝑥^𝑡_true and 𝑦^𝑡_true are the true robot positions in Cartesian coordinates at time step 𝑡, 𝑡𝑓 is the total time instant of the three algorithms, and 𝑥_𝑡 and 𝑦_𝑡 are the estimation results.

From Figure 6 and Table 3 we can see that the MRPE of AUPF is better than PF, and the MRPE of UPF is the best. That is, because in UPF each particle moves according to a new proposal distribution which utilizes the new observation, while in PF the particles cannot move appropriately without the direction of new observation. We also find that AUPF can acquire relatively ideal accuracy. The number of particles in AUPF is reduced, and the reduction is according to the estimation quality, so the reduction will not cause severe loss of accuracy.

Table 3 

Experiment results of AUPF, PF, and UPF.

6. Conclusions

In UPF the number of particles is fixed, and the computation cost of UPF is relatively large which influences its realtime capability. In order to accommodate the mobile robot self-localization under dynamic environment, an adaptive unscented particle filter algorithm through relative entropy has been proposed in this paper. This algorithm utilizes the concept of relative entropy to measure the distance between the empirical distribution and the true posterior; then the difference between the proposal distribution and the true distribution is offset to obtain the least number of particles of UPF for the next step. This algorithm can effectively reduce the computation cost and enhance the real-time capability for UPF. In the future, we will research on how to further reduce the computation of UPF at the least cost of the accuracy. Moreover, the influence of the accuracy will be investigated and analyzed.

Acknowledgments

The authors would like to thank Professor Jing Wang for his work on the paper revision. This paper is supported by the National Natural Science Foundation of China (nos. 61071096, 61073103, and 61202342), Research Fund for the Doctoral Program of Higher Education of China (nos. 20110162110042 and 20100162110012), and the Science and Technology Planning Project of Hunan Province, China (no. 2011GK3214).