Improved Particle Filtering Based on Biogeography-based Optimization for UGV Navigation

Transcription

1 Improved Particle Filtering Based on Biogeography-based Optimization for UGV Navigation A. Kuifeng Su 1,2, B. Zhidong Deng 1, and C. Zhen Huang 1 1 Department of Computer Science, State Key Laboratory of Intelligent Technology and Systems, Tsinghua National Laboratory for Information Science and Technology, Tsinghua University, Beijing, China 2 Academy of Armored Force Engineering, Beijing, China Abstract The main challenge with particle filtering is particle degeneracy and the accurate estimation cannot be achieved generally because of serious impoverishment problem, although resampling operation could solve degeneracy to a certain extent. In this paper, we propose an improved particle filtering approach based on biogeography-based optimization (BBO) algorithm, called BBO-, for state estimation of nonlinear and non-gaussian dynamic system. The novel BBO- significantly reduces the degeneracy. The experimental results obtained by applying the BBO- to estimate the trajectory of unmanned ground vehicle validate the performance of our approach. Keywords: Unmanned ground vehicle; Particle filtering; Particle impoverishment; Biogeography-based optimization. 1. Introduction The location of unmanned ground vehicle (UGV) determined according to various types of measurements is critical to UGV navigation. In fact, a variety of location algorithms have considerable difference in accuracy, robustness, and computational efficiency. Particle filtering, which is a method based on random samplings, has advantage of being applied to nonlinear and non-gaussian dynamic systems. But there exist particle degeneracy and difficulty of selecting proposal distribution. To solve particle impoverishment problem, researchers have presented a lot of algorithms such as unscented Kalman particle filter (UK)[1] and Gaussian mixture sum particle filter (GMS)[2]. Many improved algorithms based on intelligent optimizations are also proposed to solve sample impoverishment problem, including particle swarm optimization-based particle filter (PSO-)[3], annealing particle filter (A)[4], genetic particle filter (G)[5], evolutionary particle filter (E)[5], and so on. They are used to tackle the difficulty of selecting proposal distribution. Additionally, there are other hybrid algorithms such as MCMC particle filter, kernel smoothing particle filter, rejection particle filter.[6]. In this paper, we propose an improved particle filtering approach based on biogeography-based optimization (BBO) algorithm, called BBO-, for state estimation of nonlinear and non-gaussian dynamic system. Based on the concept of resampling, particles with high weights have move probability to be propagated, and in the BBO-, some particles will join in the refining process after calculating the weight of particles, which means that those particles will move to the region with high weights. This process can be regarded as migration to the habitat with high suitability of BBO algorithm. Although the BBO operation increases the computing complexity of algorithm, the optimized weights may make the proposal distribution more closed to the poster distribution and overcome the degeneracy of particles. The proposed BBO- algorithm is compared to other several filtering algorithms. The experimental results show that the BBO- has better performance due to their lower means and variances. 2. Problem Description The state and measurement models of the UGV location are built in this section. Our intelligent vehicle platform THIV-I developed by Tsinghua University, the location of various sensors is shown in Fig.1. The position system consists of many on-board sensors, including four ABS speed sensors and steering angle sensors, global position system (GPS), inertia measurement unit (IMU), and environment perception sensor such as lidar and camera, which provide the data of landmark. The information of speed and steering angle can obtained through the CAN bus of the vehicle itself. Meanwhile, the encoders on the rear transmission shafts can be used to obtain the accurate travelled distance during a sample period. The GPS/IMU integrated navigation system working on RTK differential mode provides accurate position reference for performance comparison of different algorithms. 2.1 System model In this paper, the origin of the vehicle body coordinate frame locates at the center of the rear axle, i.e., the installation position of the IMU, whose position is P k (x k, y k ) at the time of t k, and the heading angle is φ k. In 2D environment, the UGV s pose can be expressed by the state vector χ k = (x k, y k, φ k ). The position relation of UGV is shown in Fig. 2. In this figure, H represents a half of distance between two rear wheels, while L indicates the distance between a front axle and a rear axle of UGV.

2 Four Cameras Velodyne HDL-64E GPS Antenna PDL Receiver 2.2 Observation model The system of autonomous vehicle utilizes odometer and the IMU to achieve the estimation of basic position and orientation. In order to overcome the error accumulation caused by the relative positioning system, the system applies the 64-line HDL lidar, which is mounted in the anterior part of middle top of UGV, and the relative position relations with respect to UGV is shown in Fig.3. IBEO ALASCA XT Encoder Fig. 1: Intelligent vehicle of Tsinghua University. H c c c Laser L L L b y Instantaneous Center of position k 1 R PP k k 1 PP k k 1 R fl R f R fr k 1 k 1 k 1 a L Fig. 3: Installation diagram of UGV sensor. Then the relationship that the rear axle center transforms to the lidar centre is described by: x b y b y k k M r x k k k k k 1 Fig. 2: Schematic diagram of the vehicle s kinematics parameter model. P k P k+1 (R + M r /2) φ k+1 = P k P k+1 2(R + M r /2) sin[ φ k+1 /2] = φ k+1/2 sin[ φ k+1 /2] (1) Thus, the linear distance P k P k+1 between P k and P k+1 is derived below: P k P k+1 = sin[ φ k+1/2] φ k+1 /2 P k P k+1 = sin[ φ k+1/2] D k+1 φ k+1 /2 (2) In this formula, D k+1 refers to the travel distance of the back wheel center measured by an encoder. X(k + 1) = where ω k is system noise. x k + P k P k+1 cos [φ k+1 + φ k+1 /2] y k + P k P k+1 sin [φ k+1 + φ k+1 /2] φ k + φ k+1 x + ω k (3) x L = x c + a cos(φ) + b cos(φ + π/2) y L = y c + a sin(φ) + b sin(φ + π/2) The corresponding measurement equation is given by: [» ] (x z k = h (X, x k, y k ) = k x L ) 2 + (y k y L ) 2 atan Ä (y k y L ) +ν k (x k x L )ä φl + π/2 (5) In this formula, z indicates the measurement vector, i.e., the coordinates of landmark, {x k, y k } represents the location of landmark identified by the 3D lidar, and ν k is the measurement noise. 3. BBO- state estimation algorithm 3.1 Biogeography-based optimization According to the island migration model of biogeography, Dan Simon proposed biogeography-based optimization (BBO) in 28[7]. BBO is an evolutionary algorithm (EA) motivated by the optimality perspective of natural biogeography. Suppose that we have a global optimization problem and a population of candidate solutions, which can be represented by vectors of integers, each integer in the solution vector is considered to be a suitability index variable (SIV). The population consists of NP = n solution vectors. Each individual is considered as a habitat with a habitat suitability index (HSI), which is similar to the fitness of EAs, PSO, etc to evaluate individual effectiveness. A good solution means to be an island with a high HSI, and a poor (4)

3 solution indicates an island with a low HSI. The high HSI solutions tend to share their features with low HSI solutions. The low HSI solutions accept a lot of new features from the high HSI solutions. Just as species migrate back and forth between islands, BBO operates by sharing information between individuals in a population of candidate solutions. In BBO, each individual has its own immigration rate λ and emigration rate µ. A good solution has higher µ and lower λ. The immigration rate and the emigration rate are functions of the number of species in the habitat. They can be calculated as follows: µ k = E k n Å λ k = I 1 k ã n where I denotes the maximum possible immigration rate, E the maximum possible emigration rate, k the number of species of the k t h individual, and n the maximum number of species[8]. (6) (7) Select H i with probability based on λ i If H i is selected Select H j with probability based on µ i ; If H j is selected Randomly select an SIV s from H j Replace a random SIV in H i with s; increase diversity among the population. We model this in BBO as SIV mutation, and use species count probabilities to determine mutation rates. Mutation can be described as follows[9]: For j=1 to length of SIVs Select an SIV in H i with probability based on a priori probability m; If H i is selected Replace SIV (H i ) with Randomly generated SIV; The BBO algorithm can be described below[9]. I E rate immigration S Number of species emigration Smax Fig. 4: Linear migration curves for an island. There are two main operators, the migration and the mutation, in BBO. During migrating, each solution shares the information probabilistically between habitats through the emigration rate λ i and immigration rate µ i. We use the immigration rate to probabilistically decide whether or not to modify each suitability index variable (SIV) in that solution. If a given solution H i is selected to be modified, then we use the emigration rates of the other solutions to probabilistically migrate a randomly selected SIV to solution H i. The migration process can be given by[9]: For mutation, the main idea is deduced from the species count in habitat change drastically for cataclysmic events. A habitat s HSI can, therefore, change suddenly due to apparently random events. This mutation scheme tends to 1) Initialize the BBO parameters, including the maximum species count n, the maximum migration rates E and I, and the mutation rate 2) Initialize the random parameter of habitat. 3) For each habitat, HIS is mapped as the number and mobility (λ k, µ k ) of species k. Calculate and determine whether HIS satisfy the supervision condition. If HIS dose not satisfy the supervision condition 4) Utilize the immigration rate and emigration rate to restore the habitat, and then recalculate each HIS. 5) For each habitat, refresh the probability distribution of species, update the species according to mutation operator, and then recalculate the fitness. if 6) the algorithm cycle according to the termination condition. The BBO is also a species optimization algorithm. It is not related to the problems of regeneration and producing next generation, compared to both GA and evolutionary strategy optimization algorithms. There is also a remarkable difference between ACO and BBO. ACO produces a new set of solutions at each of iterations, while BBO maintains the solution set to the next iteration and adjusts the solution space according to the migration probability. BBO has more in common with PSO and DE. Compared to PSO and DE, BBO directly updates by the migration of the solution. Thus, the solutions of BBO algorithm can share properties (suitability index variable, SIVs) with each other. BBO mainly consists of the two processes, including species migration and species variation[6], [7], [8], [9]. 3.2 Adaptive particle filtering algorithm based on BBO The main disadvantage of is the so-called particle impoverishment problem, which is the main reason to take

4 resampling. This problem appears as the likelihood p ( ) y t x i t is very narrow or likelihood lies in the tail of the proposal distribution q Ä x t x (n) t 1, y 1:tä. As the observation is more accurate and the prior distribution is much boarder than the likelihood (shown in Fig.5), only a few particles have weights of significant importance, which directly leads to the particle impoverishment problem. Prior Fig. 5: Prior and Likelihood. Likelihood The proposal algorithm, called BBO-, incorporate the BBO algorithm into to improve the distribution of samples, speed up the convergence of particle sets, and overcome the impoverishment of particle filtering. The purpose is to take samples to move towards regions of the state space where the posterior probability is significant, which is related to the prior probability and likelihood. Thus, a weighted optimization problem arises to balance the contribution of prior probability and likelihood to the posterior probability. The optimization problem can be described as follows: max F (x) = κ 1F 1 (x) + κ 2 F 2 (x) (8) x S where κ 1 and κ 2 are non-negative weight with κ 1 + κ 2 = 1, S is the search space of particle sampling. Thus, the generated samples x i t, i = 1,, N, are regarded as the initial SIV in the BBO and moved to maximize the objective function F (x). Let ω t and v t be zero-mean Gaussian white noise with variances Q and R, respectively. Considering that (3) and (5) are nonlinear model with additive noise, the first objective consists of the function maximized at high prior regions as follows: F 1 = e 1 2 (x k ˆx k )Q 1 (x k ˆx k ) T (9) The second objective comprises the function maximized at high likelihood regions below: F 2 = e 1 2 (y k ŷ k )Q 1 (y k ŷ k ) T (1) Then the objective function of posterior optimization is given by: F (k) = 1 2 κ 1(x k ˆx k )Q 1 (x k ˆx k ) T 1 2 κ 2(y k ŷ k )Q 1 (y k ŷ k ) T The BBO- algorithm can be summarized below: (11) 1.Initialization the particles X = x (i) N, ω(i) from posterior i=1 distribution p(x ) with associated ω (i) = 1 / N 2.Initialize the BBO parameters, including the maximum species count n, the maximum migration rates E and I, and the mutation rate 3.For time steps t = 1, 2,, T 4.Importance Sampling: for i = 1,, N, draw samples from the importance proposal distribution as follows: x (i) t q Ä x t x (n) t 1, y 1:t 1) Consider each particle as a species of universal habitat. 2) For each habitat, HIS is mapped as the number and mobility (λ k, µ k ) of species k. Calculate and determine whether HIS can satisfy the supervision condition. If HIS dose not satisfy the condition of importance distribution 3) Utilize the immigration rate and emigration rate to restore the habitat, and then recalculate each HIS. 4) For each habitat, refresh the probability distribution of species, update the species according to mutation operator, then recalculate the fitness. 5) Weight update: evaluate the importance weights with (11). if 6) the algorithm cycle according to the termination condition. 5. Normalize the importance weights: ω (i) t = ω (i) t N i=1 ω(i) t 6. Output the statics of the particles. 7. Re-sampling: generate N new particles x (i) t from the set according to the importance weights ω (i) N t i=1 Repeat Step 3 to 7. ä x (i) N t i=1 The BBO moves all particles towards the particles with the best fitness where is the region of having the maximum weight particles. As the best fitness value reaches a certain threshold, the optimal sampling process terminates. 4. Experimental results To verify the effectiveness and efficiency of the proposed BBO- algorithm, this paper carried out experiments on urban road. Ideal two-dimensional environments were chosen as the test sites. In addition, the traffic signs stood on both sides of the road as the landmarks that were detected by the lidar. At first, we measured the accurate position of those landmarks using Span/DGPS integrated navigation system from Novatel. At the same time, the exact location of UGV was recorded during our experiment, i.e., χ (x ref, y ref, θ ref ), in order to compare the state estimation results of various algorithms with such an exact location of UGV. The BBO- algorithm proposed in this paper was compared to both standard particle filtering () and unscented particle filtering (U). The ideal reference trajectory provided by SPAN integrated navigation system and the estimation results are shown in Fig.6. Fig.7 and Fig.8 give

5 the comparison of eastward and northward errors, respectively. The experimental results indicated that the estimation accuracy of the proposed BBO- algorithm was greatly improved, compared to that of and U algorithms. The statistical distribution of position errors is shown in Fig.9 and Fig.1, respectively Reference North error(m) North(m) time(s) Fig. 8: Northward deviation comparison East(m) Fig. 6: Comparison of reference trajectory with estimation using, U, and BBO East error(m) Proportion East error(m) 2 3 Fig. 9: Distribution of eastward error time(s) Fig. 7: Eastward deviation comparsion. 5. Conclusions Particle filtering () is sequential Monte Carlo methods based on the particle representation of probability density. It can be applied to nonlinear/non-gaussian system and generalizes Kalman filtering. One of the main disadvantages of is the particle impoverishment problem, which is caused directly by the resampling process of the algorithm. We propose a BBO optimization algorithm to weight the likelihood and the prior for balancing the contribution of prior and likelihood to the posterior estimation. Our experimental results show that the BBO- algorithm can effectively improve the location accuracy of UGVs. In addition, due to Proportion North error(m) Fig. 1: Distribution of northward error.

6 the inherent characteristics of BBO algorithm, its efficiency is higher than other intelligent optimization algorithms such as PSO in solving high-dimensional optimization problems, which has provided a guarantee for real-time application of BBO-. 6. Acknowledgements This work was supported in part by National Natural Science Foundation of China (NSFC) under Grant Nos and 67754, and by the National High-Tech R & D Program of China under Grant No. 211AA411. References [1] Yi. Y, D A.Grejner-Brzezinska, Tightly-coupled GPS/INS integration using unscented Kalman filter and particle filter, in Proceedings of the 19th International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS 26), 21, pp [2] J.H.Kotecha, P.M.Djuric, Gaussian sum particle filtering, IEEE Transactions on Signal Processing., vol. 51, pp , Oct. 23. [3] Y.Hernane, S.Hernane, M.Benyettou, PSO: An Optimised Filtering Approach Based on Sampling, Journal of Applied Sciences., vol. 1, pp , Jun. 21. [4] J.Gall, J.Potthoff, C. Schnörr, B.Rosenhahn, H.P.Seidel, Interacting and annealing particle filters: Mathematics and a recipe for applications, Journal of Mathematical Imaging and Vision., vol. 28, pp. 1 18, Jan. 27. [5] N.M. Kwok,G. Fang, W.Zhou, Evolutionary particle filter: re-sampling from the genetic algorithm perspective, in Intelligent Robots and Systems, 25.(IROS 25), 25, pp [6] Z. Chen. (23). Bayesian filtering: From Kalman filters to particle filters, and beyond. [Online]. Available: jpg/tfc67/chen_bayesian. pdf [7] S. Zhang, C. Zhu, J. K. O. Sin, and P. K. T. Mok, Biogeographybased optimization, IEEE Transactions on Evolutionary Computation., vol. 12, pp , Jun. 28. [8] W.Gong, Z.Cai, C.X.Ling, DE/BBO: a hybrid differential evolution with biogeography-based optimization for global numerical optimization, Soft Computing-A Fusion of Foundations, Methodologies and Applications., vol. 15, pp , Apr. 21. [9] M.Ovreiu, D.Simon, Biogeography-based optimization of neuro-fuzzy system parameters for diagnosis of cardiac disease, in Proceedings of the 12th Annual Conference on Genetic and Evolutionary Computation, 21, pp