A Modified Particle Filter for Simultaneous Localization ... - Springer Link

J Intell Robot Syst (2006) 46: 365–382 DOI 10.1007/s10846-006-9066-0

A Modified Particle Filter for Simultaneous Localization and Mapping N. M. Kwok · A. B. Rad

Received: 26 May 2004 / Accepted: 3 May 2006 / Published online: 3 August 2006 © Springer Science + Business Media B.V. 2006

Abstract The implementation of a particle filter (PF) for vision-based bearing-only simultaneous localization and mapping (SLAM) of a mobile robot in an unstructured indoor environment is presented in this paper. Variations, using techniques from the genetic algorithm (GA), to standard PF procedures are proposed to alleviate the sample impoverishment problem. A monochrome CCD camera mounted on the robot is used as the measuring device and a measure on the image quality is incorporated into data association and PF update. Since the bearing-only measurement does not contain range information, we add a pseudo range to the measurement during landmark initialization as a hypothesised pair and the non-promising landmark is removed by a map management strategy. Simulation and experimental results from an implementation using real-life data acquired from a Pioneer robot are included to demonstrate the effectiveness of our approach. Key words particle filter · sample impoverishment · SLAM · vision

1. Introduction In mobile robot applications, it is a fundamental requirement that the robot should be able to locate its position within its operating environment. This can be achieved by using odometer measurements and referencing to a map of the environment. However, odometer measurements tend to accumulate errors over time; therefore other measuring devices are often required. A detail and precise map is also not

N. M. Kwok Faculty of Engineering, University of Technology, Sydney, Australia e-mail: [email protected] A. B. Rad (B) Department of Electrical Engineering, The Hong Kong Polytechnic University, Hong Kong, PR China e-mail: [email protected]

366

J Intell Robot Syst (2006) 46: 365–382

always available so it is required to build the map as the robot navigates. Hence, we have to solve the simultaneous localization and mapping (SLAM) problem. A solution to SLAM was provided by [7] using a range-and-bearing laser scanner and formulated the problem in the extended Kalman filter (EKF) framework. The laser scanner is accurate but is heavy, expensive and power consuming. The EKF operates satisfactorily if the process non-linearity is not severe and the noise is largely confined to Gaussian characteristics. If such conditions do not hold true, one has to employ particle filters (PF) [11] for a better representation of the non-linearity. Alternatively, a multiple hypotheses approach was applied in [18] to account for uncertainties in initial state estimations and landmarks are deleted subsequently according to their estimation quality. This approach is powerful but the storage requirement and map management complexity are demanding. Apart from using a laser scanner as the measuring device, cameras are popular alternatives because of their reduced cost, weight and power consumption. Related work can be found in [4] where a pan-tilt camera was used for SLAM which also used the EKF. Work on a combination of laser and vision was reported in [21] and [1]. Use of vision and odometer was adopted in [3], however, landmarks are known and listed in a database. Similarly, in [20], pre-stored landmarks were also used to localize a mobile robot with a single camera. Another approach by pre-recording image sequences was found in [15], but all these approaches restrict the operating area of the robot. Although the use of vision has its many advantages, detection and selection of landmarks from image sequences are challenging [17]. On the other hand, some research work for outdoor SLAM was presented in [9] as an alternative to indoor environments. Furthermore, the use of sonar as an alternative measurement device for simultaneous localization and mapping was implemented in [10]. The PF is based on the Monte Carlo simulation technique that represents the system probability distribution function (PDF) by samples. This filter out-performs the EKF in cases of non-linearities in the process and measurement model. However, the application of PF is hindered by its need for a large number of samples, consequently, this imposes a heavy computation load. The PF also suffers from the socalled sample impoverishment problem that samples tend to converge to a confined region in the solution space, making state estimations trapped in local optima. Some insights into the sample impoverishment problem was reported in [16] and mitigation to this problem using the evolutionary computing approach was proposed. In our work, we conduct preliminary research on the use of a fixed camera mounted on a mobile robot for simultaneous localization and mapping in an unstructured indoor environment. There will be no known a priori landmark locations or their identifications. Landmarks are extracted from edges in the scene and stored as subsequent matching templates while the robot is moving. We firstly ensure that the PF operates over an extended period of time by a modification in the conventional implementation procedures. We tackle the sample impoverishment problem by introducing techniques from genetic algorithms (GA). Similarities between PF and GA have been pointed out in [14]. Further references are also available in [2] where multi-objective estimation was treated. Variations to the PF using GA techniques were proposed in [5] and enhancements to the PF is achieved with a crossover operator. Another way of modifying PFs was found in [19] where the PDF is truncated resulting in an extended tail that allows for dynamic inclusion of newly observed landmarks. Here, we use the BLX-α operator which fills the undefined supports of the PDF [13] and expands the coverage of the solution space.


367

After the PF is made operative over extended time steps, we incorporate the image quality [22] into data association in addition to the nearest-neighbour validation test. Image quality, the similarity between consecutive frames, is used to modify the probability of landmark detection and included in the PF update as motivated by the data association method in [6]. Due to initial uncertainty of landmarks locations, one-one data association is not robust. Here, we propose a diversified approach. A sub-sample of landmark particles is associated with the measurement and the best match is used. When initializing landmarks, we do not have the range information from the bearing-only measurement when a camera is used. The initialized landmark can be well away from its true location in the operating environment. We propose to add a pseudo range to the measurement, giving a pair of near–far initialized landmarks. This approach reduces the initial landmark discrepancy and will accelerate the estimation convergence. The non-promising landmark in the pair will be deleted subsequently by applying the principle of Gambler’s Ruin problem. In map management as in [8], we remove landmarks by an index obtained from the hit-count. In the PF framework, we need a sufficient particle density to cover the operating environment. Double initialization increases the density and reduces sample impoverishment. From another point of view, we reduce the number of particles required in the PF by breaking landmark into pairs while maintaining sufficient particle density. The rest of this paper is arranged as follows. In Section 2, we briefly review the SLAM problem. Then, the PF is introduced in Section 3. In Section 4, modifications to the particle filter will be presented. Data association using diversity and image qualities will be discussed in Section 5 followed by the landmark initialization and map management strategy described in Sections 6 and 7. Simulation and experiment results will be presented in Section 8 and the conclusion drawn in Section 9.

2. SLAM Problem Given a mobile robot deployed in its operating environment, we denote its position at time zero as the origin of the world co-ordinate system. That is xv,0 = [xv,0 yv,0 φv,0 ]T = [0 0 0]T ,

(1)

where xv , yv are the location in Cartesian co-ordinate and φv is the heading with reference to the x-direction and subscript (0 ) denotes the time zero index. Figure 1 depicts the system definitions. When a control u, consisting of speed and turn rate, is issued; the robot state changes according to the process model equation    xv,k+1 xv,k + v1t cos(φv,k + γk 1t) + ηx,k  yv,k+1  =  yv,k + v1t sin(φv,k + γk 1t) + η y,k  , φv,k+1 φv,k + γk 1t + ηφ,k 

(2)

where k is the time index, v is the velocity command, γ is the turn rate, 1t is the time step and ηx , η y , ηφ are noise terms lumping in the effects of control response, wheel slip, etc., the noises are characterized by N(0, σx2 ), for example.

368


Figure 1 System definition.

landmark 1 xf1 ,yf1

landmark 2 xf2 ,yf2

range and bearing to landmark 1, r1 θ1

range and bearing to landmark 2, r2 θ2

robot orientationφv robot location x v,yv origin of world co-ordinate

While moving, the robot makes measurements to landmarks in the operating environment. The measurement model is given by y f i,k − yv,k θi,k = arctan − φv,k + ηθ,k , (3) x f i,k − xv,k where x f i , y f i is the location of a stationary landmark indexed by subscript (i ), ηθ is the measurement noise characterized by N(0, σθ2 ). The SLAM problem is to estimate the aggregated robot and landmark locations, in the form of a state vector, with the use of measurement. The state vector is given as x = [xv x f i ]T = [xv yv φv x f i y f i ]T ,

i = 1, 2, · · · .

(4)

Notice that the particle filter used in this work is not limited to normal distributions of noises in the process and measurement equations. However, by invoking the Central Limit theorem for independent noisy measurements, we adopt here the normal distribution for the noises.

3. Particle Filter The particle filter (PF) is based on the Bayesian estimation philosophy in which the information about the system is embedded in a probability distribution function (PDF). Using a large number of samples (particles) as in the Monte Carlo methods, the PDF is generated and evolved according to the system process equation. Upon reception of a measurement, the PDF is updated and the estimation process iterates repeatedly. When compared to the widely used EKF, the particle filter out-performs in cases when non-linear process models are employed in the system. In fact, the EKF can be derived from the Bayesian estimation principle and results in a simple recursive procedure based on the linear modelling and Gaussian noise assumptions. However,


369

this attractive feature is only available in a limited problem domain where the above assumptions are satisfied. 3.1. Filtering Procedure At time zero, we have the system PDF given by p(x0 ) which is an impulse signifying the knowledge of the robot at the origin of the world co-ordinate. The system then evolves according to the process equation, giving an intermediate PDF p(x˜ k+1 |xk ). Here, x˜ is the temporal predicted state and x is the estimated state. When a measurement is available, a likelihood of obtaining that measurement can be found as p(zk+1 |x˜ k+1 ), where z is the measurement available at time k. Finally, we combine the PDF according to the Bayes’ rule and obtain a recursive equation p(xk+1 |zk+1 ) ∝ p(zk+1 |x˜ k+1 ) p(x˜ k+1 |xk ) p(xk |zk ).

(5)

Details of the theoretical development if PF can be found in [11] and we will not go further here. Implementation of the particle filter can be described as follows with the time index dropped for clarity of presentation and superscript (i ) denotes the ith particle which represents a possible system state. 1. Randomly initialize particles, xi . 2. In every time step, propagate particles through the process model equation, x˜ i ← xi . 3. Calculate the likelihood (weight) from the measurement, i.e., ! 1 −(θ − θˆ i )2 i w = √ exp , (6) 2σθ2 2πσθ i i y −y i ˆ where θ = arctan xif −xiv − φvi is the expected bearing to a landmark. f

v

4. Re-sample (select) particles according to their normalized weights, giving x j ← x˜ i . 3.2. Sample Impoverishment A major difficulty in implementing a particle filter occurs in the so-called sample impoverishment problem. This is the problem of losing diversities in the particles such that the solution space cannot be adequately represented and estimations become unreliable. The major cause of the problem is the lack of sufficient computational resources available in representing a continuous PDF by a finite number of samples (the particles in the particle filter). An obvious remedy would be to use a large number of particles when initializing the filter, but this would make the computational complexity unacceptable. An alternative approach is to re-locate the particles in the solution space where they are needed. This motivates the present work in modifying the PF with the use of evolutionary computing techniques.

4. Modified Particle Filter There are several research works on investigating the similarities between particle filtering and genetic algorithms. We will make use of GA techniques to modify the

370


PF implementation in order to remove the sample impoverishment problem. An insight into the re-sampling procedure reveals that particles are being multiplied or discarded according to their weights. Viewing from another perspective, discarded particles are being moved on top of selected particles resulting in duplications. 4.1. BLX-α Crossover We adopt the BLX-α operator [13] to move the landmark particles in each iteration. There are numerous method applicable in genetic algorithms for crossover, e.g., the arithmetic average. In this method, the crossed particles are bounded within the distance spanned by the two parent particles. The result is that, although the particles are moved, the particles still tend to concentrate between the parents and are not contributing to the recovery of lost particles outside the bounds. The BLX-α procedure is given below and illustrated in Figure 2. 1. Select, with equally likely probability, two particles xi , x j. 2. Calculate the distance dij between the two particles in Cartesian co-ordinate. 3. Move the particles according to the product of the α parameter and the distance, giving xˆ i = xi − αdij, and xˆ j = x j + αdij. 4. Further move the particles by a uniform random parameter r ∈ [0, 1], giving x˜ i = r xˆ i + (1 − r)xˆ j and x˜ j = r xˆ j + (1 − r)xˆ i , respectively. Here, we have selected parameter α = 0.5. This choice results in filling the gap in the particle space where it is not initially defined when generating the particles. First, there is no guarantee that the previous PDF is the correct one and we need to re-supply particles to represent undefined PDF supports. Second, the spreading of particles only occurs in this interim stage, outliers will be removed by the re-sampling process in the PF. Third, spreading of particles is helpful in reducing the sample impoverishment problem by introducing new potential solutions to the estimation. Referring to [12], this corresponds to jittering and prior editing. To maintain the shape of the previous PDF, we limit the crossover by a crossover probability pc = 0.1. Thus, only pc of the particles will be moved and we put forward our justification as follows. Consider pc particles are removed from the PDF, we effectively perform a normalization on the PDF but the sum of particle weights does not sum to unity. The 1 − pc particles carry the remaining 1 − pc weights but relocate the weights to other particles. The total weight then returns to unity and the relocation is dynamically adjusted by the previous PDF. We believe this dynamic

Figure 2 Particle crossover scheme.

xj x

i

i i j ~ x = r xˆ + (1 − r ) xˆ

i i ij xˆ = x − α d

j j i ~ x = r xˆ + (1 − r ) xˆ

d

ij j j ij xˆ = x + α d


371

60

60

50

50

40

40

30

30

20

20

10

10

0 2.5

3

3.5

4

4.5

(a)

5

5.5

6

0 2.5

3

3.5

4

4.5

5

5.5

6

(b)

Figure 3 Effect of landmarks PDF with moving. (a) Landmark PDF before move; (b) landmark PDF after move.

approach is more adaptive as compared to the hard truncation in [19]. Figure 3a and b show the described effect. Notice that the undefined locations are filled with low probability particles while the dominant shape of the overall PDF remains almost the same. 4.2. Batch Update In the EKF framework and with the assumption of independent measurements, updates can be implemented in a sequential manner within iterations [7]. But in PF, each update involves the re-sampling process, thus increasing the possibility of sample impoverishment. We try to minimize this possibility by performing batch update in PFs within one iteration and with more than one measurement as follows. 1. Calculate the cumulative weight products for the measurements, that is, no resampling at this step. 2. Perform re-sampling after weighting all measurements. 4.3. Measurement Quality The likelihood function represents that, given the expected robot and landmark locations, the probability of obtaining the real-life measurement. When using a camera as a measuring device, we can incorporate the image quality into the calculation of the likelihood function. We proceed as follows. 1. Calculate the weight for each particle, see Equation (6). 2. Calculate the image quality, Q ∈ [0, 1] of the landmark image, see Section 5.3. 3. Modify the weight by the power law as wi ← (wi ) Q . By modifying the weight, we effectively compress the PDF. When the image quality is poor, most particles will be retained through the re-sampling process. That is, we take a rather conservative approach that if the measurement is not sure then reduce the effect of updating.

372


5. Data Association Data association derives the correspondence between a measurement and a landmark. It is crucial for the operation of any estimation process. In this work, we will use a hybrid association approach by integrating the nearest neighbour with image quality. We also use hysteresis thresholds, (γ1 , γ2 ) in the nearest neighbour test, to reduce ambiguities arising from noisy measurements. 5.1. Nearest Neighbour This is a classical approach that determines the difference between the real-life and the expected measurements. The result is thresholded against some confidence level in the χ 2 sense. That is, if η = ν 2 /σ 2 < γ1 ,

(7)

then declare the measurement being from a particular landmark. Here ν = θ − θˆ is the innovation, σ 2 is the error covariance between the robot and the landmark, γ1 ≈ 3.8 for 95% confidence with 1 degree of freedom (bearing-only measurement). Otherwise, if η > γ2 ,

(8)

then declare the measurement from a new landmark. Where γ2 ≈ 10.8 for 99.9% confidence. 5.2. Diversified Neighbour The nearest neighbour method operates between the robot location and the estimated landmark location. When the estimated landmark is away from the true location, data association may fail. We observe that if the extent of particles can cover the true landmark location then there is better chance if we perform a diversified nearest neighbour and select the best association. We proceed as follows. 1. Obtain a sub-sample of vehicle and landmark particles xiv and xif , i