Sensor Fusion Using Improvement of Resampling Algorithm Particle Filtering for Accurate Location of Mobile Robot

Abstract

Presented chapter deals with the information fusion through used a Divisional Resampling of Particle Filter algorithm. The Divisional Resampling is based on the Multinomial Resampling and the Stratified Resampling, which divides the random number any arrangement into several sub-interval arranged by ascending. The chapter combined Divisional Resampling algorithm and the feedback control algorithm and then integrated the measurement information from odometer and sonar sensor, so that estimating the state vector of a mobile robot to achieve the aim of accurate location.

1 Introduction

More recently, the Extended Kalman Filter (EKF) is an estimation algorithm that performs optimization in the least mean squares sense [1]. So EKF has been successfully applied to neural networks training and sensor fusion problems. However, EKF algorithm can result to unsatisfactory representations of the nonlinear functions.

To overcome these shortcomings [2], Particle Filter (PF) is mentioned. The particle filter is an alternative nonparametric implementation of the Bayes filter. In particle filters, the samples of a posterior distribution are called particles and are denoted. $X_{t} : = \{ x_{t}^{1} ,x_{t}^{2} , \ldots ,x_{t}^{M} \}$, each of particle $x_{t}^{m}$ (with 1 < m < M) is concrete instantiation of the state at time t. The input of this algorithm is the particle set $x_{t - 1}$, then the most recent control $u_{t}$ and the most recent measurement $z{}_{t}$.

The algorithm of Particle Filtering can provide optimal estimation in nonlinear non-Gaussian state models and nonlinear models which has improved its performance over the established nonlinear filtering approaches [3]. The basic idea of Particle Filter is based on state space experience conditional distribution of random sample to produce a set of these samples, which is called Particle and then on the basis of measurements constantly to adjust to the weight and position of Particle. In this chapter the Particle Filter has been employed for the localization of mobile robot by fusing data coming from odometer and sonar sensors which are collecting environmental information to realize the position of mobile robot’s velocity and angle information, so as to achieve target tracking.

2 The Nonlinear Measurement Model Particle Filter

2.1 The Divisional Resampling of Particle Filter

The algorithm of Particle Filter has a drawback which is that after some numbers of iteration k, the particles’ weight $w_{k}^{i}$ will become 0 [4]. In the ideal case all the weights should converge to the value $1/N$. So particles are degenerated, they become unnecessary to modify the algorithm.

The basic idea of Resampling is removing small weight particle, so focused on right big particle particles. At present a wide range of present application of heavy sampling algorithms are Multinomial Resampling Residual Resampling, Stratified Resampling, and Systematic Resampling 7.

This chapter uses a kind of compromise resampling algorithm—the Divisional Resampling.

The steps are as follows:

• Step1: $d = \left\lfloor {\sqrt N } \right\rfloor ,R = N\% d$, N is the size of the particle collection

• Step2: (1) if $R = 0$, then $d = \left\lfloor {\sqrt N } \right\rfloor ,N^{d} = N/d,x = 1/N^{d}$

• For $i = 1, \ldots ,N^{d} A_{i} = \left\{ {S_{j}^{i} } \right\}_{1 \le j \le d} ,\;S_{j}^{i} \sim U\left( {\left( {\left( {i - 1} \right)x,ix} \right]} \right)$,

• END

• (2) else

• $N^{d} = \left\lfloor {N/d} \right\rfloor + 1,x = (1 - R/N)/(N^{d} - 1)$

• For $i = 1, \ldots ,(N^{d} - 1)$

• $A_{i} = \left\{ {S_{j}^{i} } \right\}_{1 \le j \le d} ,S_{j}^{i} \sim U\left( {\left( {\left( {i - 1} \right)x,ix} \right]} \right)$

• END

• $A_{{N^{d} }} = \left\{ {S_{j}^{{N^{d} }} } \right\}_{1 \le j \le R} ,S_{j}^{{N^{d} }} \sim U\left( {\left( {\left( {i - 1} \right)x,1} \right]} \right)$

• Step3:

• $U = \{ U_{i} \}_{1 \le i \le N} = \bigcup\limits_{i = 1}^{{N^{d} }} {A_{i} } ,I^{i} = D_{w}^{inv} (U^{i} ),\mathop \xi \limits^{\sim i} = \xi^{{I^{i} }} ,\mathop w\limits^{\sim i} = 1/n,i = 1, \ldots ,n$

The process of Resampling as follows: after calculating the accumulative of particle probability, i is random sampling index, then we use i onto the probability of accumulative and regional, projection position and indicate the position of accumulative and corresponding domain, j is a new particles index 6. As shown in figure one shows.

The purpose of resampling is to reduce the number of particles less weight to focus on the larger particle weight. Basic idea is posteriori probability density function of sampling N times again [5]. The posteriori probability density function is $p(x_{k} \left| {z_{1:k} } \right.) \approx \sum\limits_{i = 1}^{N} {w_{k}^{i} } \delta (x_{k} - x_{k}^{i} )$, Where $x_{k}^{i}$ is the state of a sample of robot position in the K time. $w_{k}^{i}$ is the robot state $x_{k}^{i}$ probability in the K time. Resampling N times, resulting in a new set of support points $\left\{ {x_{k}^{{i^{*} }} } \right\}_{i = 1}^{N}$ of particles, with $p(x_{k}^{{i^{*} }} ) = p(xi_{k}^{{}} ) = w_{k}^{i}$. Resampling is an independent distribution, so the weight is resetting to $w_{k}^{i} = 1/N$ (Fig. 13.1).

Fig. 13.1

Resempled index

2.2 The Prediction Stage

In the prediction stage, we mainly calculate $p(x(k)\left| {Z^{ - } } \right.)$ where $x(k)$ is time sequence of states in the k time, $Z^{ - } = \{ z(1), \ldots ,z(n - 1)\}$is observation sequence from 1 to t time. Through calculation of the recursive estimate edge distribution $p(x_{k} \left| {z_{1:k} } \right.)$, then there is no need to save the system state historical value.

2.3 Update Stage

Measurement update, given [6, 7]$z(k)$, and compute the new value of the state vector

$$ p(x(k)\left| z \right.) = \sum\limits_{i = 1}^{N} {w_{k}^{i} } \delta (x_{k} - x_{k}^{i} ) $$

(13.1)

$$ w_{k}^{i} = \frac{{(w_{{k^{ - } }}^{i} p(z(k)\left| {x(k)} \right. = \xi_{{k^{ - } }}^{i} ))}}{{\sum\limits_{j = 1}^{N} {w_{{k^{ - } }}^{j} p(z(k)\left| {x(k)} \right. = \xi_{{k^{ - } }}^{j} )} }} $$

$$ \xi_{k}^{i} = \xi_{{k^{ - } }}^{i} $$

In the process of practical operation of weights, we need to normalize weights. After for normalization $w_{k}^{{i^{*} }} = w_{k}^{i} /\sum\limits_{i = 1}^{N} {w_{k}^{i} }$. A posteriori probability density in the system in k−1 time is $p(x_{k} \left| {z_{k - 1} } \right.)$. Time measurement information obtained in the k time, in accordance with this article resampling algorithm selected a random sample of N points, after time and status update, N particles of the posterior density can be approximately considered $p(x_{k} \left| {z_{k} } \right.)$.

3 Simulation Results

3.1 Improved Particle

Filter will be applied to mobile robot localization

• (1) Mobile robot motion model

The object of the chapter studied is commonly used in two rounds of indoor robot, shown as follows (Fig. 13.2).

Fig. 13.2

Mobile robot motion model

There is an encoder on the driving wheels and it provides a measurement of the incremental angle. The odometer sensor is used to obtain an estimation of the robot’s angular velocity $w(t_{k} )$ and linear velocity $v(t_{k} )$. Given a sampling period is $\Updelta t_{k} = t_{k + 1} - t_{k}$.

Mobile robot’s in continuous-time kinematic equation is:

$$ \mathop {x(t)}\limits^{ \bullet } = v(t)\cos (\theta (t))\,\mathop {y(t)}\limits^{ \bullet .} = v(t)\sin (\theta (t))\,\mathop {\theta (t)}\limits^{ \bullet } = w(t) $$

(13.2)

Where $(x,y)^{T}$ point is mobile robot’s position and $\theta$ is the orientation of robot body rotation movement.

Establish its probability for motion model: $p(l_{t} \left| {l_{t - 1} } \right.,u_{t - 1} )$. The robot in t−1 time relative to the global coordinates the pose is $l_{t - 1} = (x_{t - 1} ,y_{t - 1} ,\theta_{t - 1} )^{T}$, After time t, reached the position $l_{t}$, then the movement model is:

$$ l_{t} = l_{t - 1} + \left[ {\begin{array}{*{20}c} {\cos (\theta_{t - 1} )} & 0 \\ {\sin (\theta_{t - 1} )} & 0 \\ 0 & 1 \\ \end{array} } \right]{\text{u}}_{t - 1} + v_{t - 1} $$

(13.3)

With: ${\text{u}}_{t - 1} = (\delta_{t - 1} ,\Updelta \theta_{t - 1} )^{T}$ is the odometer model input, during the time of $(t - 1, t)$, $\delta_{t - 1}$ is robot’s center displacement, $\Updelta \theta_{t - 1}$ is the angle of the robot rotated, $v_{t - 1}$ obeyed Gaussian measure noise. Defined the control law 11:

$$ \begin{gathered} v = v_{d} \cos (\theta_{d} - \theta ) + k_{1} ((x_{d} - x) \hfill \\ \cos \theta + (y_{d} - y)\sin \theta ) \hfill \\ w = w_{d} + k_{2} v_{d} \frac{{\sin (\theta_{d} - \theta )}}{{\theta_{d} - \theta }} \hfill \\ ((y_{d} - y)\cos \theta - (x_{d} - x)\sin \theta + k_{3} (\theta_{d} - \theta ) \hfill \\ \end{gathered} $$

(13.4)

$k_{1} ,k_{2} ,k_{3}$ is Gain.

Dynamics of the tracking error:

$$ \begin{gathered} e_{x} = x_{d} - x,e_{y} = y_{d} - y \hfill \\ \mathop {e_{x}}\limits^{ \bullet \bullet } + K_{d1} \mathop {e_{x} }\limits^{ \bullet } + K_{p1} e_{x} = 0 \hfill \\ \mathop {e_{y}}\limits^{ \bullet \bullet } + K_{d1} \mathop {e_{y} }\limits^{ \bullet } + K_{p1} e_{y} = 0 \hfill \\ \end{gathered} $$

(13.5)

• (2) Coordinate system and the sensor modeling

$XOY$ is the plane the global coordinate system [8], $X^{\prime} O^{\prime} Y^{\prime}$ is $XOY$ rotated $\theta$. This chapter uses odometer and ranging sonar sensors to collect mobile robot information. Mobile robot collects surrounding environment information through the sonar sensors. Sonar through the transmitter, meet obstacles issued pulse waves reflected back, then be receiver receive, the reflection of the Angle can be sonar sensors detected. Robot sonar sensors through the collection of information in the environment, Sonar pulsing sound through a transmitter, an obstacle to be reflected back, and then received by the receiver, the angle of reflection can be detected by sonar sensors. Sonar sensors in the coordinate system in the position $XOY$$(x_{i} ,y_{i} )$, Fig. 13.3 shows. Its direction angle $\theta_{i}$, $\delta$ is the opening angle with sonar. Sonar feature information extraction environment, it is to get the raw data corresponding to the distance and angle information.

Fig. 13.3

Sonar position in the coordinate system

Sonar sensor model shown in Fig. 13.3:

Suppose $K$ sampling time, the mobile robot pose $X(k) = (x(k),y(k),\theta (k))$ [9], after a rotation of the coordinate transformation, to the only sonar sensors i-coordinate system in the coordinate $OXY$ $(x_{i} ,y_{i} )$ conversion to $O^{\prime} X^{\prime} Y^{\prime}$ coordinate system.

System $(x_{i}^{\prime} ,y_{i}^{\prime} )$, the sonar sensor model is:

$$ \begin{aligned} x_{i} (k) &= x(k) + x_{i}^{\prime} \sin (\theta (k)) + y_{i}^{\prime} (\cos \theta (k)) \\ y_{i} (k) &= y(k) - x_{i}^{\prime} (\cos \theta (k)) + y_{i}^{\prime} \sin (\theta (k)) \\ \theta_{i} (k) &= \theta (k) + \theta_{i} \end{aligned} $$

(13.6)

Mobile robot wall as reflected in your environment $P^{j}$, sonar sensors are available with distance from the planes $P_{r}^{j}$, $P_{n}^{j}$, $P_{r}^{j}$ is the distance between the origin O to the plane.

Sonar i to plane $P^{j}$ distance of obstacles in k time for $d_{i}^{j} (k)$: i is the number of sonar sensors, j is the number of planes, Simplified model used in this article, as shown in Fig. 13.3. $i = 1,j = 1$, then $d_{i}^{j} (k)$ is: $d_{i}^{j} (k) = P_{r}^{j} - x_{i} (k)\cos (P_{n}^{j} ) - y_{i} (k)\sin (P_{n}^{j} )$ (14)

Where $z(k\Updelta t_{k} )$ is the odometer and sonar sensor measurements [10], $v(k\Updelta t_{k} )$ is a Gaussian white noise sequence~$N(0,R(k\Updelta t_{k} ))$, $z((k + 1)\Updelta t_{k} ) = G(X(k + 1)\Updelta t_{k} ) + v(k\Updelta t_{k} )$, $z(k\Updelta t_{k} )$ can be decomposed into two sub-vectors of the form:

$$ \begin{gathered} z_{1} (k + 1) = [x(k) + v_{1} (k), \hfill \\ y(k) + v_{2} (k),\theta (k) + v_{3} (k)] \hfill \\ z_{2} (k + 1) = [d_{1}^{j} (k) + v_{4} (k)] \hfill \\ \end{gathered} $$

(13.7)

(Fig. 13.4).

Fig. 13.4

Mobile robot odometer and sonar sensor model

3.2 Based on Particle Filter Multi-Sensor

Information fusion for mobile robot localization [11]:

Suppose the robot’s initial position in $OXY$: $x(0) = 1m$, $y(0) = 0m$, $\theta (0) = 45^{\circ}$

In the $O^{\prime} X^{\prime} Y^{\prime}$ coordinate system, location of sonar sensors: $x_{1}^{\prime} = 0.5m,y_{1}^{\prime} = 0.5m,\theta_{1}^{\prime} = 0^{\circ}$

Plane position is $P^{1} \,:p_{r}^{1} = 15.5,p_{n}^{1} = 45.00.$

State noise: $w(k) = 0,p(0) = diag[0.1,0.1,0.1]$, $R(0) = diag[0.1,0.1,0.1]$.

Kalman filter gain $K(k) \in R^{3x4}$

4 Simulation Results are as Follows

Figure 13.5, Fig. 13.6

Fig. 13.5

Mobile robot trajectory tracking using particle filter (asterisk * denotes expectations of mobile robot, the green solid line represents the actual robot trajectory tracking)

Fig. 13.6

a Robot distance error e_x. b Robot distance error e_y

5 Conclusion

The chapter introduces a Divisional Resampling based on Particle Filter algorithm which combines to robot feedback control theory through the fusion of odometer and sonar sensors from collecting environmental information. And applied the chapter introduces a Divisional Resampling based on Particle Filter algorithms to mobile robot to localization and tracking. Simulation results show that based on the particle filter in the Divisional Resampling algorithm, application control theory, will be able to realize the mobile robot for dynamic target effective and accurate tracking. While the future work primarily concentrates in sensor information fusion simplified algorithm, improving the ability of anti-interference and the operation precision, as well as the particle filter algorithm efficiency, robustness and real-time and develop more important particle filter resampling algorithm.