Distributed motion planning and sensor fusionfor cooperative behavior of mobile robots

Gerasimos G. Rigatos

Unit of Industrial Automation

Industrial Systems Institute

26504 Rion Patras, Greece

email: grigat@isi.gr

Abstract: The paper analyzes two important issues in the design of multi-robot systems: (i) motionplanning with the use of distributed algorithms, (ii) sensor fusion with the use of Extended Kalman orParticle Filtering. First, distributed gradient for motion planning of a multi-robot system is examined.The dynamic model of the multi-robot system is derived and its convergence to the desirable position isproved using Lyapunov analysis of the distributed gradient algorithm. Next, the problem of localization ofa mobile robot through the fusion of measurements that come from distributed sensors is examined. Twoapproaches are studied: sensor fusion using Extended Kalman Filtering and sensor Fusion using ParticleFiltering. Simulation tests are provided to evaluate the performance of the distributed motion planningalgorithm for the multi-robot system, as well as the performance of Extended Kalman and Particle Filter-ing in mobile robotics sensor fusion problems.

Keywords: multi-robot systems, swarm-bots, motion planning, distributed gradient algorithms, sensorfusion, Extended Kalman Filtering, Particle Filtering.

1 Introduction

In the recent years there has been growing interest in multi-robot systems. As the cost of robots goes downand as robots become more compact, the number of military and industrial applications of multi-robotsystems increases. Possible industrial applications of multi-robot systems include hazardous inspection,underwater or space exploration, assembling and transportation. Some examples of military applicationsare guarding, escorting, patrolling and strategic behaviors, such as stalking and attacking. This paperwill analyze two important issues in the design of multi-robot systems: (i) motion planning with the useof distributed gradient algorithms, (ii) sensor fusion with the use of Extended Kalman or Particle Filtering.

Of primary importance in the operation of multi-robot systems are distributed motion planning algorithmswhich enable motion towards a goal state through a dynamic environment and permit the robot to operateautonomously instead of being part of a centralized control structure [1]. To this end distributed gradientalgorithms are proposed. As in the case of potential field methods, the motion of each robot is determinedby a potential function [2-4]. The potential of each robot consists of two terms: (i) the cost V i due to thedistance of the i-th robot from the goal state, (ii) the cost due to the interaction with the other M − 1robots [5]. Moreover, a repulsive field, generated by the proximity to obstacles, is taken into account. Thedifferentiation of the aggregate potential provides the kinematic model for each robot [6,7]. It is provedthat the velocity update equation is equivalent to a distributed gradient algorithm [8,9]. The convergenceto the goal state is studied with the use of Lyapunov stability theory. It is shown that in the case of a

quadratic cost function V i the mean position of the multi-robot system converges to the goal state x∗ whileeach robot stays in a bounded area close to x∗ [11,12].

Another important topic in the operation of multi-robot systems is distributed sensing and fusion of mea-surements from various sources, such as odometric sensors and sonars. Sensor fusion can result in improvedlocalization of each robot. At a second stage the estimated state vector of the robot can be used by anonlinear controller in-order to make the mobile robot track a desired trajectory [13-15]. Suitable methodsfor state estimation based on the fusion of measurements that come from distributed sensors are ExtendedKalman Filtering and Particle Filtering [16,17].

The Extended Kalman Filter (EKF) is an incremental estimation algorithm that performs optimizationin the least mean squares sense and which has been successfully applied to data fusion for mobile robotlocalization [18]. In the EKF approach the state vector of the mobile robot is approximated by a Gaussianrandom variable, which is then propagated analytically through the first order linearization of the robot’smodel. EKF processes rapidly the incoming data and is thus recommended for real-time applications suchas the navigation of the mobile robots. On the other hand, Particle Filtering (PF) is a relatively new non-linear filtering method, which has improved performance over the established nonlinear filtering approaches(e.g. the EKF), since it can provide optimal estimation in nonlinear non-Gaussian state-space models [19,20]. PF can succeed better accuracy in the estimation of the mobile robot’s state vector, provided that thenumber of particles (estimations of the state vectors which evolve in parallel) is large [13]. However, dueto computational complexity the method is slower than the EKF.

The structure of the paper is as follows: In Section 2 the kinematic model of multi-robot system that con-sists of point-mass robots is derived and distributed gradient is proposed for the robots’ motion planning.Stability analysis of the distributed gradient algorithms is performed with the use of Lyapunov theory. InSection 3 the Extended Kalman Filter (EKF) for the nonlinear state-measurement model is presented anddata fusion with the use of Extended Kalman Filtering is discussed. In Section 4 the Particle Filteringalgorithm for state estimation of the mobile robots is introduced. Particle filtering based on sequentialimportance resampling is analyzed. The prediction and correction stages are explained. Issues for improvedresampling and substitution of the degenerated particles are discussed. In Section 5 simulation experimentsare carried out to: (i) study the convergence of distributed gradient algorithms to the goal state in the caseof a swarm of M mobile robots which are randomly initialized in the 2D plane (ii) evaluate the performanceof the Extended Kalman Filter and the Particle Filter in sensor fusion for motion control of the mobilerobots. Finally, in Section 7 concluding remarks are stated.

2 Distributed motion planning for cooperative behavior of mobile robots

2.1 Kinematic model of the multi-robot system

The objective is to lead a swarm of M mobile robots, with different initial positions on the 2-D plane, toa desirable final position. The position of each robot in the 2-D space is described by the vector xi ∈ R2.The motion of the robots is synchronous, without time delays, and it is assumed that at every time instanteach robot i is aware about the position and the velocity of the other M − 1 robots. The cost functionthat describes the motion of the i-th robot towards the goal state is denoted as V (xi) : Rn → R. Thevalue of V (xi) is high on hills, small in valleys, while it holds ∇xiV (xi) = 0 at the goal position and atlocal optima. The following conditions must hold: (i) The cohesion of the swarm should be maintained,i.e. the norm ||xi − xj || should remain upper bounded ||xi − xj || < εh, (ii) Collisions between the robotsshould be avoided, i.e. ||xi − xj || > εl, (iii) Convergence to the goal state should be succeeded for eachrobot through the negative definiteness of the associated Lyapunov function V i(xi) = ei(t)T

ei(t) < 0. [8].The interaction between the i-th and the j-th robot is taken to be

2

g(xi − xj) = −(xi − xj)[ga(||xi − xj ||)− gr(||xi − xj ||)] (1)

where ga() denotes the attraction term and is dominant for large values of ||xi − xj ||, while gr() denotesthe repulsion term and is dominant for small values of ||xi − xj ||. Function ga() can be associated withan attraction potential, i.e. ∇xi

Va(||xi − xj ||) = (xi − xj)ga(||xi − xj ||). Function gr() can be associatedwith a repulsion potential, i.e. ∇xiVr(||xi − xj ||) = (xi − xj)gr(||xi − xj ||). A suitable function g() thatdescribes the interaction between the robots is given by 12

g(xi − xj) = −(xi − xj)(a− be−||xi−xj ||2

σ2 ) (2)

where the parameters a, b and c are suitably tuned. It holds that ga(xi − xj) = −a, i.e. attraction has

a linear behavior (spring-mass system) ||xi − xj ||ga(xi − xj). Moreover, gr(xi − xj) = be−||xi−xj ||2

σ2 whichmeans that gr(xi − xj)||xi − xj || ≤ b is bounded. Applying Newton’s laws to the i-th robot yields

xi = vi

mivi = U i (3)

where the aggregate force is U i = f i +F i. The term f i = −Kvvi denotes friction, while the term F i is thepropulsion. Assuming zero acceleration vi = 0 one gets F i = Kvvi, which for Kv = 1 and mi = 1 givesF i = vi. Thus an approximate kinematic model is

xi = F i (4)

According to the Euler-Langrange principle, the propulsion F i is equal to the derivative of the total po-tential of each robot, i.e.

F i = −∇xi{V i(xi) + 12

∑Mi=1

∑Mj=1,j 6=i[Va(||xi − xj || − Vr(||xi − xj ||)]} ⇒

F i = −∇xi{V i(xi)} −∑Mj=1,j 6=i[∇xiVa(||xi − xj ||)−∇xiVr(||xi − xj ||)] ⇒

F i = −∇xi{V i(xi)}+∑M

j=1,j 6=i[−(xi − xj)ga(||xi − xj ||) + (xi − xj)gr(||xi − xj ||)] ⇒F i = −∇xi{V i(xi)}+

∑Mj=1,j 6=ig(xi − xj).

(5)

2.2 Distributed gradient for multi-robot motion planning

Substituting Eq. 5 in Eq. (4) one gets

xi(t + 1) = xi(t) + γi(t)[−∇xiV i(xi) + ei(t + 1)] +∑M

j=1,j 6=ig(xi − xj), i = 1, 2, · · · ,M , (6)

with γi(t) = 1, and ei(t) being a stochastic noise term. This verifies that the kinematic model of a multi-robot system is equivalent to a distributed gradient search algorithm.

Motion planning of multi-robot systems can be solved with the use of distributed stochastic search algo-rithms. These can be multiple gradient algorithms that start from different points in the solutions spaceand interact to each other while moving towards the goal position. Distributed gradient algorithms, stemfrom stochastic search algorithms treated in [8, 9] if an interaction term is added:

xi(t + 1) = xi(t) + γi(t)[h(xi(t)) + ei(t)] +∑M

j=1,j 6=ig(xi − xj), i = 1, 2, · · · ,M (7)

The term h(x(t)i) = −∇xiV i(xi) indicates a local gradient algorithm, i.e. motion in the direction of de-crease of the cost function V i(xi) = 1

2ei(t)Tei(t). The term γi(t) is the algorithm’s step while the stochastic

disturbance ei(t) enables the algorithm to escape from local minima. The term∑M

j=1,j 6=ig(xi−xj) describesthe interaction between the i-th and the rest M − 1 stochastic search algorithms. Convergence analysis

3

based on the Lyapunov stability theory can be stated in the case of distributed gradient algorithms. Thisis important for the problem of multi-robot motion planning.

Denoting the goal position by x∗ , and the distance between the i-th robot and the mean position of themulti-robot system by ei(t) = xi(t)− x the objective of distributed gradient for robot motion planning canbe summarized as follows:

(i) limt→∞x = x∗, i.e. the center of the multi-robot system converges to the goal position,

(ii) limt→∞xi = x, i.e. the i-th robot converges to the center of the multi-robot system,

(iii) limt→∞ ˙x = 0, i.e. the center of the multi-robot system stabilizes at the goal position.

If conditions (i) and (ii) hold then limt→∞xi = x∗. Furthermore, if condition (iii) also holds then all robotswill stabilize close to the goal position.

Stability analysis using LaSalle’s theorem shows that the mean of the multi-robot system converges exactlyto the desirable state x∗ while the position xi of the individual robots remains close to x∗, in a circle ofradius ε [10].

Figure 1: LaSalle’s theorem: C: invariant set, E ⊂ C: invariant set which satisfies V (x) = 0, M ⊂ E:invariant set, which satisfies V (x) = 0, and which contains the limit points of x(t) ∈ E, L+ the set of limitpoints of x(t) ∈ E. The individual robots will stay in the area defined by the invariant set M

3 Fusion of distributed measurements using the Extended Kalman Filter

The next issue to be analyzed is that of fusion of measurements coming from distributed sensors, with theuse of nonlinear filtering methods such as Extended Kalman Filtering (EKF). The fused data are used to re-construct the state vector of a mobile robot, and the estimated state vector is in turn used in a control-loop.

First, Extended Kalman Filtering for the nonlinear state-measurement model is revised. The followingnonlinear time-invariant state model is now considered [18]:

4

x(k + 1) = φ(x(k)) + w(k)z(k) = γ(x(k)) + v(k) (8)

where w(k) and v(k) are uncorrelated, zero-mean, Gaussian zero-mean noise processes with covariance ma-trices Q(k) and R(k) respectively. The operators φ(x) and γ(x) are given by, φ(x) = [φ1(x), φ2(x), · · · ,φm(x)]T ,and γ(x) = [γ1(x), γ2(x), · · · , γp(x)]T , respectively. It is assumed that φ and γ are sufficiently smooth inx so that each one has a valid series Taylor expansion. Following a linearization procedure, φ is expandedinto Taylor series about x:

φ(x(k)) = φ(x(k)) + Jφ(x(k))[x(k)− x(k)] (9)

where Jφ(x) is the Jacobian of φ calculated at x(k):

Jφ(x) =∂φ

∂x|x=x(k) =

∂φ1∂x1

∂φ1∂x2

· · · ∂φ1∂xN

∂φ2∂x1

∂φ2∂x2

· · · ∂φ2∂xN

......

......

∂φk

∂x1

∂φk

∂x2· · · ∂φk

∂xN

(10)

Likewise, γ is expanded about x−(k)

γ(x(k)) = γ(x−(k)) + Jγ [x(k)− x−(k)] + · · · (11)

where x−(k) the prior to time instant k) estimation of the state vector x(k), and x(k) is the estimation ofx(k) at time instant k). The Jacobian Jγ(x) is

Jγ(x) =∂γ

∂x|x=x−(k) =

∂γ1∂x1

∂γ1∂x2

· · · ∂γ1∂xN

∂γ2∂x1

∂γ2∂x2

· · · ∂γ2∂xN

......

......

∂γk

∂x1

∂γk

∂x2· · · ∂γk

∂xN

(12)

The resulting expressions create first order approximations of φ and γ. Thus the linearized version of theplant is obtained:

x(k + 1) = φ(x(k)) + Jφ(x(k))[x(k)− x(k)] + w(k)z(k) = γ(x−(k)) + Jγ(x−(k))[x(k)− x−(k)] + v(k)

Now, the EKF recursion is as follows: First the time update is considered: by x(k) the estimation of thestate vector at instant k is denoted. Given initial conditions x−(0) and P−(0) the recursion proceeds as:

• Measurement update. Acquire z(k) and compute:

K(k) = P−(k)JTγ (x−(k))·[Jγ(x−(k))P−(k)JT

γ (x−(k)) + R(k)]−1

x(k) = x−(k) + K(k)[z(k)− γ(x−(k))]P (k) = P−(k)−K(k)Jγ(x−(k))P−(k)

(13)

• Time update. Compute:

P−(k + 1) = Jφ(x(k))P (k)JTφ (x(k)) + Q(k)

x−(k + 1) = φ(x(k))(14)

The schematic diagram of the EKF loop is given in Fig. 2(a).

5

(a) (b)

Figure 2: (a) Schematic diagram of the EKF-loop (b) Schematic diagram of the PF-loop.

4 Particle filtering for the nonlinear state-measurement model

4.1 Particle Filter with sequential importance resampling

Here, the problem of fusing measurements coming from distributed sensors will be solved using the ParticleFiltering method. The fused data are used again to estimate the state vector of a mobile robot and thereconstructed state vector will be used in closed-loop control.

In the general case the equations of the optimal filter used for the calculation of the state-vector of a non-linear dynamical system do not have an explicit solution. This happens for instance when the process noiseand the noise of the output measurement do not follow a Gaussian distribution. In that case approximationthrough Monte-Carlo methods can used. As in the case of the Kalman Filter or the Extended KalmanFilter the particles filter consists of the measurement update (correction stage) and the time update (pre-diction stage) [16].

4.1.1 The prediction stage

The prediction stage calculates p(x(k)|Z−) where Z− = {z(1), · · · , z(n− 1)}, using:

p(x(k − 1)|Z−) =N∑

i=1

wik−1δξi

k−1(x(k − 1)) (15)

while from Bayes formula it holds p(x(k)|Z−) =∫

p(x(k)|x(k − 1))p(x(k − 1)|Z−)dx. This finally gives

p(x(k)|Z−) =∑N

i=1wik−1δξi

k−(x(k))

with ξik− ∼ p(x(k)|x(k − 1) = ξi

k−1)(16)

6

The meaning of Eq. (16) is as follows: the state equation of the nonlinear system of Eq. (8) is executedN times, starting from the N previous values of the state vectors x(k− 1) = ξi

k−1 and using Eq. (8). Thismeans that the value of the state vector which is calculated in the prediction stage is the result of theweighted averaging of the state vectors which were calculated after running the state equation, startingfrom the N previous values of the state vectors ξi

k−1.

4.1.2 The correction stage

The a-posteriori probability density was performed using Eq. (16). Now a new position measurementz(k) is obtained and the objective is to calculate the corrected probability density p(x(k)|Z), where Z ={z(1), z(2), , z(k)}. From Bayes law it holds that p(x(k)|Z) = p(Z|x(k))p(x(k))

p(Z) , which finally results into

p(x(k)|Z) =∑N

i=1wikδξi

k−(x(k))

where wik =

wik−p(z(k)|x(k)=ξi

k− )∑Nj=1wj

k−p(z(k)|x(k)=ξj

k− )

(17)

Eq. (17) denotes the corrected value for the state vector. The recursion of the Particle Filter proceeds ina way similar to the update of the Kalman Filter or the Extended Kalman Filter, i.e.:

Measurement update: Acquire z(k) and compute the new value of the state vector

p(x(k)|Z) =∑N

i=1wikδξi

k−(x(k))

with corrected weights wik =

wik−p(z(k)|x(k)=ξi

k− )∑Nj=1wi

k−p(z(k)|x(k)=ξk− )i and ξik = ξi

k−

(18)

Resample for substitution of the degenerated particles.

Time update: compute state vector x(k + 1) according to

p(x(k + 1)|Z) =∑N

i=1wikδξi

k(x(k))

where ξik∼p(x(k + 1)|x(k) = ξi

k)(19)

The stages of state vector estimation with the use of the particle filtering algorithm are depicted in Fig. 2(b).

4.2 Resampling issues in particle filtering

The algorithm of particle filtering which is described through Eq. (16) and Eq. (17) has a significantdrawback: after a certain number of iterations k, almost all the weights wi

k become 0. To avoid this,resampling is performed which substitutes the particles of low importance with those of higher importance.The particles {ξ1

k, · · · , xNk } are chosen according to the probabilities {w1

k, · · · , wNk }. The resampling pro-

cedure of (ξik, wi

k i = 1, · · · , N) is carried out through previous sorting in decreasing order of the particleweights. This will result into ws[1] > ws[2] > · · · > ws[N ]. A random numbers generator is used and theresulting numbers ui:N∼U [0, 1] fall in the partitions of the interval [0, 1]. The width of these partitions iswi and thus a redistribution of the particles is generated. For instance, in a wide partition of width wj

will be assigned more particles than to a narrow partition of width wm (see Fig. 3).

7

Figure 3: Multinomial resampling: (i) conventional resampling, (ii) resampling with sorted weights

5 Simulation tests

5.1 Distributed gradient for multi-robot motion planning

In the conducted simulation tests the multi-robot system consisted of 10 robots which were randomly ini-tialized in the 2-D field. Theoretically, there is no constraint in the number of robots that constitute therobotic swarm. Of course, the number of robots can be increased and if it is sufficiently large then theobtained measurements will be also statistically significant. Two cases were distinguished: (i) motion inan obstacle-free environment (see Fig. 4) and (ii) motion in an environment with obstacles (see Fig. 5).The objective is to lead the robot swarm to the origin [x1, x2] = [0, 0]. To avoid obstacles, apart from themotion equations given in Section 2 repulsive forces between the obstacles and the robots are taken intoaccount.

Results about the motion of the robots in an obstacle-free 2D-plane were obtained. Fig. 4(a) describesthe motion of the individual robots towards the goal state, in an obstacle-free environment, when thedistributed gradient algorithm is applied. Fig. 4(b) demonstrates how the mean position of the multi-robot formation approaches the goal state when the motion takes place in an obstacle-free environmentand the distributed-gradient algorithm is used to steer the robots. Fig. 5(a) demonstrates the motion ofthe individual robots towards the goal state, in an environment with obstacles, when the steering of therobots is the result of the distributed gradient algorithm. The robots are now also subject to attractiveand repulsive forces due to the obstacles. Fig. 5(b) shows how the average position of the multi-robotformation reaches the equilibrium when the motion is performed in a 2D-plane which contains obstaclesand when the robots’ trajectories are generated by the distributed gradient algorithm.

Fig. 6(a) presents the variation of the Lyapunov functions of the robots when the motion takes place inan environments without obstacles, and the distributed gradient approach is applied to steer the robotstowards the attractor. Fig. 6(b) presents the variation of the Lyapunov functions of the mean of themulti-robot system in the obstacle-free environment. It can be observed that the Lyapunov functions arenot monotonous, i.e. and there is change of the sign of the Lyapunov function’s derivative is due to the factthat the robots’ path encircle the target position several times before finally stabilizing at [x∗, y∗] = [0, 0].This means that robots which were initially approaching fast the goal position had to make circles round

8

−10 −5 0 5 10

−10

−5

0

5

10

X

Y

−10 −5 0 5 10

−10

−5

0

5

10

X

Y

(a) (b)

Figure 4: Distributed gradient Particle swarm with robots interaction in an obstacles-free environment:(a) trajectory of the individual robots, (b) Trajectory of the mean of the multi-robot system.

−10 −5 0 5 10

−10

−5

0

5

10

X

Y

−10 −5 0 5 10

−10

−5

0

5

10

X

Y

(a) (b)

Figure 5: Distributed gradient with robots interaction in an environment with obstacles, considering aquadratic cost function (a) trajectory of the individual robots, (b) trajectory of the mean of the multi-robot system

9

0 100 200 300 400 500 600 700 800 900 10000

10

20

30

40

50

60

70

80

90

100

time k

Lyapunov o

f th

e r

obots

0 100 200 300 400 500 600 700 800 900 10000

10

20

30

40

50

60

70

80

90

100

time k

Lyapunov o

f th

e m

ean

(a) (b)

Figure 6: Distributed gradient approach in an obstacles-free environment: (a) Lyapunov function of theindividual robots and (b) Lyapunov function of the mean.

the attractor in order to wait for those robots which had delayed. This maintained the cohesion of themulti-robot swarm.

Fig. 7(a) presents the variation of the Lyapunov function of the robots when the motion takes placein an environment with obstacles, and the distributed gradient approach is applied to steer the robotstowards the attractor. Fig. 7(b) presents the variation of the Lyapunov functions of the mean of themulti-robot system in an environment environment with obstacles. In that case the Lyapunov functionstend to become monotonous, i.e. continuously decreasing, and this is due to the fact that the interactionforces between the robots have been made weaker after suitable tuning of the coefficients α and β. Thisenables the robots to approach to the goal state following an almost linear trajectory, since the interac-tion forces that caused curving of the robots path and encircling of the attractor have now been diminished.

Regarding the significance of the mean and the variance of the multi-robot system for evaluating the be-havior of the robotics swarm the following can be stated: although the average position of the multi-robotsystem is not always meaningful, for instance in case that the individual bypass a obstacle with equalprobability from its left or right side, it is a useful parameter that helps to monitor this many-body system.The mean and the variance of the multi-robot formation becomes particularly significant when the motiontakes place at nanoscale. In the latter case, all information about the behavior of the nanoparticles swarmis contained in the mean position of the swarm and its variance.

Finally, simulation results about the motion of the multi-robot swarm in a workspace with polyhedral ob-stacles have been given. Thus additional evaluation for the performance of the proposed motion planningalgorithms is obtained. The obstacles considered in the simulation experiments are not points but poly-hedra with cover certain regions in the 2D plane. Therefore the attractive and repulsive forces generatedbetween the robots and obstacles affect the robots’ trajectories and may also result in local minima. Theseresults are depicted in Fig. 8 to Fig. 9.

For even though the obstacles are not symmetrically placed in the 2D-plane and thus the aggregate forceexerted on the robots in not zero, the multi-robot system converges to the origin [x∗ = 0, y∗ = 0]. The cost

10

0 100 200 300 400 500 600 700 800 900 10000

10

20

30

40

50

60

70

80

90

100

time k

Lyapunov o

f th

e r

obots

0 100 200 300 400 500 600 700 800 900 10000

10

20

30

40

50

60

70

80

90

100

time k

Lyapunov o

f th

e m

ean

(a) (b)

Figure 7: Distributed gradient approach in an environment with obstacles: (a) Lyapunov function of theindividual robots and (b) Lyapunov function of the mean.

−10 −5 0 5 10

−10

−5

0

5

10

X

Y

−10 −5 0 5 10

−10

−5

0

5

10

X

Y

(a) (b)

Figure 8: Distributed gradient for motion planning of the multi-robot system in a 2D-plane with polyhedralobstacles (a) trajectories of the individual robots towards the goal state (b) trajectories of the mean of themulti-robot system.

11

0 500 1000 1500 2000 2500 30000

10

20

30

40

50

60

70

80

90

100

time k

Lyapunov o

f th

e r

obots

0 500 1000 1500 2000 2500 30000

10

20

30

40

50

60

70

80

90

100

time k

Lyapunov o

f th

e m

ean

(a) (b)

Figure 9: Distributed gradient in a 2D-plane with polyhedral obstacles: (i) Lyapunov functions of therobots, (ii) Lyapunov function of the mean of the multi-robot formation.

function is is no longer a convex one. Local minima can be generated due to the proximity to obstaclesand the attractive forces between the individual robots. For instance, local minima can be found in narrowpassages between the obstacles. The inclusion of a stochastic term in the equations that describe theposition update of the robots, i.e. in Eq. (7) may enable escape from minima. Moreover, suitable tuningof the attractive and repulsive forces that exist between the robots may also permit the robots to moveaway from local minima.

5.2 EKF-based Sensor Fusion for Vehicle Localization

The application of EKF to the fusion of data that come from different sensors is examined first [??]. Aunicycle robot is considered. Its continuous-time kinematic equation is:

x(t) = v(t)cos(θ(t)), y(t) = v(t)sin(θ(t)), θ(t) = ω(t) (20)

Encoders are placed on the driving wheels and provide a measure of the incremental angles over a samplingperiod T . These odometric sensors are used to obtain an estimation of the displacement and the angularvelocity of the vehicle v(t) and ω(t), respectively. These encoders introduce incremental errors, which re-sult in an erroneous estimation of the orientation θ. To improve the accuracy of the vehicle’s localization,measurements from sonars can be used. The distance measure of sonar i from a neighboring surface Pj

is thus taken into account (see Fig. 10(a) and Fig. 10(b). Sonar measurements may be affected by whiteGaussian noise and also by crosstalk interferences and multiples echoes.

The inertial coordinates system OXY is defined. Furthermore the coordinates system O′X ′Y ′ is considered(Fig. 10(a)). O′X ′Y ′ results from OXY if it is rotated by an angle θ. The coordinates of the center of thewheels axis with respect to OXY are (x, y), while the coordinates of the sonar i that is mounted on thevehicle, with respect to O′X ′Y ′ are x

′i, y

′i. The orientation of the sonar with respect to OX ′Y ′ is θ

′i. Thus

the coordinates of the sonar with respect to OXY are (xi, yi) and its orientation is θi, and are given by:

12

(a) (b)

Figure 10: (a) mobile robot with odometric sensors, (b) Orientation of the sonar i

xi(k) = x(k) + x′isin(θ(k)) + y

′icos(θ(k))

yi(k) = y(k)− x′icos(θ(k)) + y

′isin(θ(k))

θi(k) = θ(k) + θi

(21)

Each plane P j in the robot’s environment can be represented by P jr and P j

n (Fig. 10(b), where (i) P jr is

the normal distance of the plane from the origin O, (ii) P jn is the angle between the normal line to the

plane and the x-direction.The sonar i is at position xi(k), yi(k) with respect to the inertial coordinates system OXY and its orien-tation is θi(k). Using the above notation, the distance of the sonar i, from the plane P j is represented byP j

r , P jn (see Fig. 10(b)):

dji (k) = P j

r − xi(k)cos(P jn)− yi(k)sin(P j

n) (22)

where P jn ε [θi(n)− δ/2, θi(n) + δ/2], and δ is the width of the sonar beam. Assuming a constant sampling

period ∆tk = T the measurement equation is z(k+1) = γ(x(k))+v(k), where z(k) is the vector containingsonar and odometer measures and v(k) is a white noise sequence ∼ N(0, R(kT )). The dimension pk of z(k)depends on the number of sonar sensors. The measure vector z(k) can be decomposed in two subvectors

z1(k + 1) = [x(k) + v1(k), y(k) + v2(k), θ(k) + v3(k)]

z2(k + 1) = [dj1(k) + v4(k), · · · , dj

ns(k) + v3+ns(k)]

(23)

with i = 1, 2, · · · , ns, where ns is the number of sonars, dji (k) is the distance measure with respect to

the plane P j provided by the i-th sonar and j = 1, · · · , np where np is the number of surfaces. Bydefinition of the measurement vector one has that the output function γ(x(k)) is given by γ(x(k)) =[x(k), y(k), θ(k), d1

1(k), d22(k), · · · , d

npns ]T . The mobile robot’s state is [x(k), y(k), θ(k)]T and the control in-

put is denoted by U(k) = [u(k), ω(k)]T .

In the simulation tests, the number of sonar is taken to be ns = 1, and the number of planes np = 1, thusthe measurement vector becomes γ(x(k)) = [x(k), y(k), θ(k), d1

1]T . To obtain the extended Kalman Filter

(EKF), the kinematic model of the vehicle is linearized about the estimates x(k) and x−(k) the control

13

input U(k − 1) is applied.

The measurement update of the EKF is

K(k) = P−(k)JTγ (x−(k))[Jγ(x−(k))P−(k)JT

γ (x−(k)) + R(k)]−1

x(k) = x−(k) + K(k)[z(k)− γ(x−(k))]P (k) = P−(k)−K(k)JT

γ P−(k)

The time update of the EKF is

P−(k + 1) = Jφ(x(k))P (k)JTφ (x(k)) + Q(k)

x−(k + 1) = φ(x(k)) + L(k)U(k)

where L(n) =

Tcos(θ(k)) 0Tsin(θ(k)) 0

0 T

, where L(n) =

Tcos(θ(k)) 0Tsin(θ(k)) 0

0 T

while Q(k) = diag[σ2(k), σ2(k), σ2(k)], with σ2(k) chosen to be 10−3 and φ(x(k)) = [x(k), y(k), θ(k)]T , γ(x(k)) =

[, x(k), y(k), θ(k), d(k)]T , i.e.

γ(x(k)) =

x(k)y(k)

θ(k)P j

r − xi(k))cos(P jn)− yi(k)sin(P j

n)

(24)

Assuming one sonar ns = 1, and one plane P 1, np = 1 in the mobile robot’s neighborhood one getsJT

γ (x−(k)) = [Jγ1(x−(k)), Jγ2(x

−(k)),Jγ3(x−(k)), Jγ4(x

−(k))]T , i.e.

JTγ (x−(k)) =

1 0 00 1 00 0 1

−cos(P jn) −sin(P j

n) {x′icos(θ − P jn)− y

′isin(θ − P j

n)}

(25)

The vehicle is steered by a dynamic feedback linearization control algorithm which is based on flatness-basedcontrol [21]:

u1 = xd + Kp1(xd − x) + Kd1(xd − x)u2 = yd + Kp2(yd − y) + Kd2(yd − y)

ξ = u1cos(θ) + u2sin(θ)

v = ξ, ω = u2cos(θ)−u1sin(θ)ξ

(26)

The following initialization is assumed (see Fig. 11): (i) vehicle’s initial position in OXY: x(0) = 0m, y(0) =

0m, θ(0) = 45.0o (ii) position of the sonar in O′X ′Y ′: x′1 = 0.5m, y

′1 = 0.5m, θ

′1 = 0o (iii) position of the plane P 1:

P 1r = 15.5m, P 1

n = 45o, (iv) state noise w(k) = 0, P (0) = diag[0.1, 0.1, 0.1], and R = diag[10−3, 10−3, 10−3, 10−3](v) Kalman Gain K(k) ε R3×4 (vi) desirable trajectory: starts at xd(0) = 0, yd(0) = 0, and forms a 45o with theOX axis.

The use of EKF for fusing the data that come from odometric and sonar sensors provides an estimation of the statevector [x(t), y(t), θ(t)] and enables the successful application of nonlinear steering control of Eq. (26) [22]. Theobtained results are depicted in Fig. 12(a).

5.3 Particle Filtering-based Sensor Fusion for mobile robot ocalization

The particle filter can also provide solution to the sensor fusion problem. The mobile robot model described in Eq.(20), and the control law given in Eq. (26) are used again. The number of particles was set to N = 1000.

14

Figure 11: Desirable trajectory of the autonomous vehicle i

The measurement update of the PF is p(x(k)|Z) =∑N

i=1wikδξi

k−(x(k)) with wi

k =wi

k−p(z(k)|x(k)=ξik− )

∑Nj=1w

jk

p(z(k)|x(k)=ξj

k− )where the

measurement equation is given by z(k) = z(k) + v(k) with z(k) = [x(k), y(k), θ(k), d(k)]T , and v(k) =measurementnoise.

The time update of the PF is p(x(k + 1)|Z) =∑N

i=1wikδξi

k(x(k)) where ξi

k∼p(x(k + 1)|x(k) = ξik) and the state

equation is x− = φ(x(k)) + L(k)U(k), where φ(x(k)), L(k), and U(k) are defined in subsection 5.2. At each run ofthe time update of the PF, the state vector estimation x−(k + 1) is calculated N times, starting each time from adifferent value of the state vector ξi

k. The obtained results are given in Fig. 12(b) [22].

From the simulation experiments depicted it can be deduced that the particle filter has better performance than theEKF in the problem of estimation of the state vector of the mobile robot, without being subject in the constraintof Gaussian distribution for the obtained measurements (see fig. 13). The number of particles influences the per-formance of the particle filter algorithm. The accuracy of the estimation succeeded by the PF algorithm improvesas the number of particles increases. The initialization of the particles, (state vector estimates) may also affect theconvergence of the PF towards the real value of the state vector of the monitored system. It should be also notedthat the calculation time is a critical parameter for the suitability of the PF algorithm for real-time applications.When it is necessary to use more particles, improved hardware and some new technologies, such as making parallelprocessing available to embedded systems, enable the PF to be implemented in real-time systems [20].

6 Conclusions

In this paper the problem of distributed multi-robot motion planning was studied. A M -robot swarm was consid-ered and the objective was to lead the swarm to a goal position. The kinematic model of the robots was derivedusing the potential fields theory. The potential of each robot consisted of two terms: (i) the cost V i due to thedistance of the i-th robot from the goal state, (ii) the cost due to the interaction with the other M − 1 robots.The differentiation of the potential provided the kinematic model for each robot. It was proved that the velocityupdate equation is equivalent to a distributed gradient algorithm. The convergence to the goal state was studiedwith the use of Lyapunov stability theory. It was shown that in the case of a quadratic cost function V i the meanposition of the multi-robot system converges to the goal state x∗ while each robot stays in a bounded area close to x∗.

Distributed gradient for multi-robot motion planning was evaluated through simulation tests. It was observed thatwhen the multi-robot system was evolving in an environment with obstacles, the interaction between the individ-

15

0 2 4 6 8 10 12 14 160

2

4

6

8

10

12

14

16

X

Y

0 2 4 6 8 10 12 14 160

2

4

6

8

10

12

14

16

X

Y

(a) (b)

Figure 12: (a) Desirable trajectory (continuous line) and obtained trajectory using EKF fusion based onodometric and sonar measurements (−.), (b) Desirable trajectory (continuous line) and obtained trajectoryusing PF fusion based on odometric and sonar measurements (−.).

ual robots (attractive and repulsive forces) had to be loose, so as to give priority to obstacles avoidance. Theperformance of the method was satisfactory. The algorithm succeeded cooperative behavior of the robots withoutrequirement for explicit coordination or communication.

On the other hand Extended Kalman and Particle filtering have been tested in the problem of estimation of thestate vector of a mobile robot through the fusion of position measurements coming from odometric and sonar sen-sors. The paper has summarized the basics of the Extended Kalman Filter, which is the most popular approach toimplement sensor fusion in nonlinear systems. The EKF is a linearization technique, based of a first-order Taylorexpansion of the nonlinear state functions and the nonlinear measurement functions of the state model. In theEKF, the state distribution is approximated by a Gaussian random variable. Although the EKF is a fast algorithm,the underlying series approximations can lead to poor representations of the nonlinear functions and the associatedprobability distributions. As a result, the EKF can sometimes be divergent.

To overcome these weekness of the EKF as well as the constraint of the Gaussian state distribution, particle filter-ing has been introduced. Whereas the EKF makes a Gaussian assumption to simplify the optimal recursive stateestimation, the particle filter makes no assumptions on the forms of the state vector and measurement probabilitydensities. In the particle filter a set of weighted particles (state vector estimates evolving in parallel) is used toapproximate the posterior distribution of the state vector. An iteration of the particle filter includes particle updateand weights update. To succeed the convergence of the algorithm at each iteration resampling takes place throughwhich particles with low weights are substituted by particles of high weights.

Simulations have been carried out to give a comparison of the performance of the EKF and the particle filteralgorithm in the problem of mobile robot localization, through the fusion of measurements coming from differentsensors. These simulations have shown that the particle filter is superior than the EKF in terms of the accuracyof the state vector estimation. The performance of the particle filter algorithm depends on the number of particlesand their initialization. It can be seen that the PF algorithms succeeds better estimates of the mobile robot’s statevector as the number of particles increases, but on the expense of higher computational effort.

16

5 5.2 5.4 5.6 5.8 6 6.2 6.4 6.6 6.8 75

5.2

5.4

5.6

5.8

6

6.2

6.4

6.6

6.8

7

X

Y

5.2 5.4 5.6 5.8 6 6.2 6.4 6.6 6.8 75

5.2

5.4

5.6

5.8

6

6.2

6.4

6.6

6.8

X

Y

(a) (b)

Figure 13: Precision of trajectory tracking (a) using EKF-based sensor fusion (−.) with respect to thedesirable trajectory (continuous line) (b) using PF-based sensor fusion (−.) with respect to the desirabletrajectory (continuous line).

References[1] Y. Guo and L.E. Parker, A distributed and optimal motion planning approach for multiple mobile robots, In:

Proc. 2002 IEEE Intl. Conference on Robotics and Automation, Washington DC, May 2002, pp. 2612-2619.

[2] O. Khatib, Real-time obstacle avoidance for manipulators and mobile robots, International Journal of RoboticResearch, vol. 5, no.1, 1986, pp. 90-99.

[3] E. Rimon and D.E. Koditscheck, Exact robot navigation using artifical potential functions, IEEE Transactionson Robotics and Automation, vol. 8, 1991, pp. 501-518.

[4] J.H. Reif and H. Wang, Social potential fields: A distributed behvioral control for autonomous robots, Roboticsand Autonomous Systems, Elsevier, vol. 27, 1999, pp.171-194.

[5] H. Levine and W.J. Rappel, Self-organization in systems of self-propelled particles, Physical Review E, vol.63,2000.

[6] G.G. Rigatos A.P. Tzes and S.G. Tzafestas, Distributed Stochastic Search for multi-robot cooperative behavior,IMACS 2005 International Conference, July 2005, Paris, France.

[7] G.G. Rigatos, Distributed gradient for multi-robot motion planning, ICINCO 05, 2nd Intl. Conference onInformatics in Control, Automation and Robotics, Barcelona, Spain, Sep. 2005.

[8] M. Duflo, Algorithmes stochastiques, Mathematiques et Applications vol. 23, Springer, 1996.

[9] A. Benvensite, P.Metivier and P. Priouret, Adaptive algorithms and stochastic approximations, Springer:Applications of Mathematics Series, vol. 22, 1990.

[10] G.G. Rigatos, Coordinated motion of autonomous vehicles with the use of a distributed gradient algorithm,Journal of Applied Mathematics and Computation, Elsevier, 2007.

[11] H. Khalil, Nonlinear Systems, Prentice Hall, 1996.

[12] V. Gazi and K. Passino, Stability analysis of social foraging swarms, IEEE Transactions on Systems, Man andCybernetics - Part B: Cybernetics, vol. 34, no. 1, pp. 539-557, 2004.

[13] G.G. Rigatos, Particle filtering for state estimation in nonlinear industrial systems, 2nd IC-EpsMs0 2007, Inter-national Conference on Experiments/Process/Systems Modelling/Optimization/ Simulation, Athens, Greece,May 2007.

17

[14] K. Watanabe and S.G. Tzafestas, Learning algorithms for neural networks with the Kalman Filters. Journalof Intelligent Robotic Systems, vol. 3, pp. 305-319, 1990.

[15] G.G. Rigatos, Extended Kalman and Particle Filtering for sensor fusion in motion control of mobile robots,MASCOT07, 7th Meeting on Applied Scientific Computing and Tools, Institute for Calculus Applications,Rome Italy, September 2007.

[16] S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics, MIT Press, 2005.

[17] F. Gustafsson, F. Gunnarsson, N. Bergman, U. Forssel, J. Jansson, R. Karlsson, and P.J. Nordlund, ParticleFilters for Positioning, Navigation and Tracking, IEEE Transactions on Signal Processing- Special Issue onMonte Carlo Methods for Statistical Signal Processing, vol.50, pp. 425-435, 2005.

[18] G.G. Rigatos and S.G. Tzafestas, Extended Kalman Filtering for Fuzzy Modelling and Multi-Sensor Fusion,Mathematical and Computer Modelling of Dynamical Systems, Taylor and Francis, vol. 13, no.3, 2007.

[19] J. Fichou, F. LeGland and L. Mevel, Particle-Based Methods for Parameter Estimation and Tracking: Numer-ical Experiments, Publication Interne IRISA, No 1604, Rennes, France, 2004.

[20] N. Yang, W.F.Tian, Z.H. Jin, and C.B. Zhang, Particle Filter for sensor fusion in a land vehicle navigationsystem, Measurement Science and Technology, Institute of Physics Publishing, vol. 16, pp. 677-681, 2005.

[21] Oriolo G., De Luca A. and Vendittelli M.:, WMR Control Via Dynamic Feedback Linearization: Design,Implementation and Experimental Validation. IEEE Transactions on Control Systems Technology, vol. 10, no.6, pp.835-852, 2002.

[22] G.G. Rigatos, Extended Kalman and particle filtering for sensor fusion in mobile robot localization, PhysCon’07, IEEE International Conference on Physics and Control, Potsdam, Germany, September 2007.

18