2011 International Conference on Mechatronic Science, Electric Engineering and Computer

August 19-22, 2011, Jilin, China

Path Planing and Tracking for Multi-robot

System Based on Improved PSO Algorithm

Mao Yang Chun-zhe Li

Electrical Engineering College Kuan cheng Branch
Northeast Dianli University, NEDU Changchun Power Supply Company
Jilin, China Changchun, China
[email protected] [email protected]

Abstract— A new algorithm based on improved particle path planning is equivalent to parameter optimization of
swarm optimization (PSO) of cubic splines is proposed for particular cubic splines. In section 3, a simple PD
multiple mobile robot path planning. The center path is controller is adopted to tracking the center optimal path.
described by string of cubic splines, thus the path planning is Simulation results are presented in section 5 to verify the
equivalent to parameter optimization of particular cubic effectiveness of proposed control strategy. The paper ends
splines. Improved PSO is introduced to get the optimal path with conclusions and future research directions in section 6.
for its fast convergence and global search character. PD
controller is adopted to tracking the center optimal path. II. PATH PLANNING
Experimental results show that a collision-avoidance path
can be found effectively among obstacles and each other by A. Improved PSO
the proposed algorithm.
PSO is a relatively new optimization tools. In this
Keywords-path planning; path tracking; multi-robot;
algorithm, each feasible solution in the search space is seen
PSO;cubic splines as a particle. First generating the initial swarm, i.e.
randomly initialized particles in solution space. Changes to
the position of the particles within the search space are
I. INTRODUCTION based on the social psychological tendency of individuals
Path planning is the key technology of mobile robot to emulate the success of other individuals. A swarm
navigation. Robot needs to find a feasible, non-collision, consists of a set of particles, where each particle represents
optimal path from the start to target point. There exist lots a potential solution. The position of each particle is
of path planning methods including: grid method, viewable changed according to its own experience and that of its
method, and artificial potential field [1-7]. neighbors.
With the constantly expanding of the robot application The standard mathematical representation of PSO is as
areas, it is very difficult for the single robot to meet the follows:
demand on application. Multiple robots form a system and Let the search space is D-dimensional, the total number
completing a more complex task through collaboration is of particles is N, the ith particle position is represented as a
increasingly becoming the focus problem in the robotics vector X i ( xi1 , xi 2 ,..., xiD ) , the best position for the ith
and intelligent science field [8-17]. particle so far is Pi ( Pi1 , Pi 2 ,..., PiD )  the best position for
Path tracking is a comparatively effective control
strategy to realize multi-robot foraging mission and map all the particles so far is Pg ( Pg1 , Pg 2 ,..., PgD )  the
detection, and it is mainly divided into two stages: the velocity for the ith particle is Vi (Vi1 , Vi 2 ,..., ViD )  the
generation of reference path and path tracking. Under the
condition that the multi-robot model is known, the track velocity and position for each particle is updated as
meet dynamic stability can be generated offline. The follows:
investigation of multi-robot origins from the behavior of Vid (t  1) ZVid (t )  c1r1 j (t )( PiD  xiD )  c2 r2 j (t )( PgD  xiD )

swarm organism in nature which can finish a complex X id (t  1) xid (t )  Vid (t  1)ˈ 1 d i d n,1 d d d D
work though coordination such as flocks of birds, school
of fish. Biologists have been working on understanding Where c1 and c2 are the acceleration
and modeling of swarming behavior for a long time. coefficients; r1 j (t ), r2 j (t ) ~ U (0,1) , Z called inertia weight,
Particle Swarm Optimization (PSO) also is an
evolutionary computational model which is based on which is used to balance the relationship between
swarm intelligence. PSO is developed by Kennedy and exploration and exploitation. When Z is larger, the
Elberhart [8] who have been inspired by the research of the algorithm has strong global search ability; When Z is
artificial livings. smaller, the algorithm focuses on local search. In addition,
The paper is organized as follows. In section 2, A new the particle velocity will not be too large. Let the upper
algorithm based on improved PSO of cubic splines is limit velocity is Vmax when Vid ! Vmax let Vid Vmax ;when
proposed for multiple mobile robot path planning. The
Vid  Vmax  let Vid Vmax  The second part of above
center path is described by string of cubic splines, thus the
formula is “cognitive” part, represents the thinking of

978-1-61284-722-1/11/$26.00
Authorized licensed use limited to:©2011 IEEE
Wojskowa 1667 on June 04,2024 at 06:25:29 UTC from IEEE Xplore.
Akademia Techniczna. Downloaded Restrictions apply.
particles themselves, and the third part is the "social" part, r (t ) P0 f1 (t )  P1 f 2 (t )  P0c f3 (t )  P4c f 4 (t )
represents the sharing of information between particles and
mutual cooperation. Obviously, two Ferguson splines r (t ) and r (t ) can be
However, there exist some defects for PSO such as easily connected together, as long as the following
local optimum, low accuracy. An improved PSO algorithm conditions:
is used to overcome above defects.
P1 P0 , P1c P0c
If Vid (t  1) ! Vmax ,then Vid (t  1) m Vmax This limits the
So, robot path can be planned of some connected
ability of particles out of local optimum, given the above Ferguson spline, then let
reasons, when the particle velocity exceeds the maximum
speed, the speed is set to have some random values. r (t ) x t , y (t ) a0  a1t  a2t 2  a3t 3
If Vid (t  1) ! Vmax ,then V (t  1) m § 0.5  Random · V  In which,
id ¨ ¸ max
2
© ¹
­a0 2 P0  2 P1  P0c+P1c
If Vid (t  1)  Vmax ,then V ( t  1) m  § 0.5  Random · V  °
°a1 3P0 +3P1  2 P0c  P1c
id ¨ ¸ max
© 2 ¹
Which “Random” is a random number in [0, 1].then the ®
randomization of velocity enhanced, and increased the ° a2 P0c
probability of particles out of local optimum. °¯a3 P0
The improved PSO is called VPSO. Four fitness
The structure of particle is shown in Figure 2.
functions are adopted for comparison. We have
According to the actual demand of path planning, the
n
optimal path should be considered both safety and the
Fitness function1: ¦ x i2  shortest.
i 1 On the one hand, the path should be safety. The
n 1 particle spline should keep a certain distant from the
Fitness function2: ¦ (1 0 0 ( x i  1  x i2 ) 2  ( x i  1) 2 )  obstacles. Let the safe distance are Dsafe , the distance
i 1
n between the spline and obstacles are d min , so we can define
Fitness function 3: ¦ ( x 2  1 0 u c o s( 2 S x )  1 0 )  1
i i the safe function as follows:
i 1

n n
xi ­1 Dsafe  d min
Fitness function4: 1 ¦ x i 2  – c o s ( )1 ° 
4000 i 1 i f1 ® Dsafe  1
i 1
° exp( ) 0  d min d D safe
Figure 1 shows that under the same initial conditions, ¯ d min  1
VPSO can convergence to the optimal value faster than In which d min min min ( x(t )  ox ) 2  ( y (t )  o y ) 2
PSO, especially for a local minimum points. oO t[0,1]

On the other hand, the planned path should be as short

B. Description of Particle
as possible from the start point to the end point. So we can
Mobile robot path planning problem can be regarded as define the shortest function as follows:
searching optimal spline in the cubic spline space. l
Ferguson cubic spline is easy to achieve, with 2 order f2 
continuous derivative. lmin
Let t  [0,1] , Ferguson cubic spline can be defined In which
1
as follows:
l ³ ( xc(t )2 )  ( yc(t )2 )dt 
r (t ) P0 f1 (t )  P1 f 2 (t )  P0c f3 (t )  P4c f 4 (t ) (1) 0

In which P0 , P0c , P1 , P1c are the vector of Ferguson cubic So we define the fitness function as follows:
spline. f1 (t )ˈf 2 (t )ˈf3 (t )ˈf 4 (t ) are Ferguson f f1  f 2
polynomials. It also can be defined as follows: (2) III. DISTRIBUTED CONTROL
Combined with equations (1) and (2), it is easy to
obtain that P0 =r (0), P1 =r (1) are the start point and end A. Swarm Model
point for the spline respectively. Consider a swarm of M members moving in 2-
­ f1 (t ) 2t 3  3t 2  1 dimensional space. For the multi-robot system we have
° assumptions as follows.
3 2
° f 2 (t ) 2t  3t Assumption 1.
® (2) (1)We ignore each robot’s dimension and treat it as a
3 2
° f3 (t ) t  2t  t point mass.
° f (t ) t 3  t 2 (2)All robots move synchronously, i.e., there is no
¯ 4 communication delay between each other.
It is easy to obtain that P0c=r c(0), P1c =r c(1) are the start (3)All robots are homogenous, i.e., there is no difference
point and end point for the tangent vector of the spline in essence.
respectively. Previous theoretical and modeling efforts fall into a
Let another Ferguson cubic spline shows as follows: number of categories. Many of the original ideas about

1668 on June 04,2024 at 06:25:29 UTC from IEEE Xplore. Restrictions apply.
Authorized licensed use limited to: Wojskowa Akademia Techniczna. Downloaded
group spacing were formulated qualitatively, or using B. Design of the Controller
simple formulate. For each robot, we can establish the The center path is described by string of cubic splines.
model according to the classical Newtonian mechanics law PD controller is adopted for tracking the planned center
as follows: path.
x i vi  u ext k p [ x (t )  xd (t )]  kd [ xc(t )  xdc (t )]

ext in
mi vi u i  u  bi vii  Although the reference path xd (t ) is the center of
multi-robots system, we can design the magnitude of
where i 1, 2,3..., M , xi  ƒ2 is the position of the i th repulsion force and the attraction force in order to keep
2
robot, vi ƒ is the velocity of the i th robot, mi m is cohesion. Note that the swarm size is concerned with the
safe distance Dsafe .
ext
the mass of the i th robot, u i is input which we impose.
in IV. EXPERIMENTAL RESULT
u is the total force on the robot i as a result of inter-
i
M In order to testify the effeteness of the algorithm,
robot interaction, obviously, we have ¦ uiin 0 because simulation experiments have been done by MATLAB. We
i 1 design the parameters as follows:
in Maximum number of iterations: 100; Number of cubic
the internal interaction offset. The term ui is for the splines: 5; Number of particles: 20; step of
cohesion of swarm and is of the form iterations t 0.02 ; Number of obstacles: 7; c1 c2 2 .
M
uiin  ¦ [ g a ( xi  xi )  g r ( x i  x i )]( x i  x i )   Figure 3 shows the optimal path using improved PSO
j 1, j z i to the evolution of the swarm with the initial population.
Figure 3 shows the optimal path using improved PSO to
where g r : ƒ o ƒ and g a : ƒ o ƒ
   
represents the evolution of the swarm with population after100
respectively the magnitude of repulsion force and the iterations. Figure 3 shows the path of multi-robot under the
attraction force. bi vi represents the velocity damping control with magnitude of repulsion force and the
attraction force.
from the environment. It is worth noting that bi b=0 in
V. CONCLUSION
this paper.
Assumption 2. A new algorithm based on improved particle swarm
optimization of cubic splines is proposed for multiple
There exist corresponding functions J a : ƒ o ƒ and

mobile robot path planning. The center path is described
J r : ƒ o ƒ such that for any y ƒ2 by string of cubic splines, thus the path planning is
equivalent to parameter optimization of particular cubic
’y Ja ( y ) yg a ( y ), ’ y J r ( y ) yg r ( y )  splines. Improved PSO is introduced to get the optimal
2
path for its fast convergence and global search character.
Definition 1. The swarm center x ƒ is defined by PD controller is adopted to tracking the center optimal path.
1 M Experimental results show that a collision-avoidance path
x
M
¦x 
i 1
i  can be found effectively among obstacles and each other
by the proposed algorithm.
2
The cohesion and stability of the swarm for multi-robot
Therefore, the velocity of the swarm center v ƒ is system under the control policy are our future work.
derived by time differentiation
M ACKNOWLEDGMENT
1
v
M
¦v  i This work was
   supported by Scientific Research
i 1 Foundation of Northeast Dianli University (No.BSJXM-
The position and velocity can be denoted as follows 201016).
T T T T
x ( x1 ,..., x M )T , v (v1 ,..., v M )T   REFERENCES
Then, the system can be expressed as [1] Dongbing Gu, Huosheng Hu, “Using fuzzy logic to design
i i separation function in flocking algorithms,” IEEE Transactions on
§ x · § 0 1 · § x · § 0 · i i fuzzy systems. vol. 16,NO.4,August 2008,PP.826-836.
¨¨ i ¸¸ ¨ ¸ ¨¨ i ¸¸  ¨ ¸ (u ext  u in )  
[2] Zongyao Wang, Dongbing Gu, “Fuzzy control of distributed
©v ¹ ©
 0 -b ¹©v ¹ © ¹ 1 flocking system,” Proceedings of the 2007 IEEE International on
Mechatronics and Automation August5-8, 2007, Harbin, China, pp.
We have assumption that uiext uext because of 2807-2812.
M [3] Xiaohai Li, Jizong Xiao, Zhijun Cai: Stable Flocking of Swarms
in
¦u
i 1
i 0 , we have Using Local Information, IEEE Xplore Downloaded on December
23, 2008.

mx  bx u ext   [4] Veysel Gazi, Kevin M. Passino: A class of attractions/repulsion
functions for stable swarm aggregations, INT. J. CONTROL. 2004.
VOL. 77.NO.18,1567-1579.

1669 on June 04,2024 at 06:25:29 UTC from IEEE Xplore. Restrictions apply.
Authorized licensed use limited to: Wojskowa Akademia Techniczna. Downloaded
[5] Veysel Gazi, Kevin M. Passino: stability analysis of social foraging [11] R. Olfati-Saber, “Flocking for multi-agent dynamic systems:
swarms, supproted by the DARPA MICA program via the AFRL, Algorithms and theory,”IEEE Trans. Autom. Control, vol. 51, no.
under contract E33615-01-C-3151. 3, pp. 401-420, Mar 2006.
[6] Dan Jin, Lixin Gao: Stabiliy analysis of a double intergrator swarm [12] Gazi V, Passino K M. “Stability analysis of swarms,” IEEE Trans.
model related to position and velocity, Transations of the Institute Autom. Contr.,2003,48(4)692-697.
of Measurement and Control 30, 3/4 (2008) pp. 275-293. [13] Yanfei Liu, Kevin M. Passion, “Stable social foraging swarms in a
[7] Vicsek T, Czirok A, Ben-Jacob E, Cohen I, Shocher O, “Novel noisy environment,” IEEE Transactions on Automatic Control,
type of phase transition in a system of self-driven particles,”Phys. VOL. 49, NO. 1, January 2004.
Rev. Lett.1995.75(6). [14] Z LI, Y JIA, “Flocking for swarm systems with fixed topology in a
[8] Reynolds C, “Flocks, birds, and schools: A distributed behavioral changing environment,” J Control Theory Appl 2008 6(3):333-339.
model,”Comput. Graph,1987,21:25-34. [15] Gazi V, Passino K M. “Stability analysis of social foraging
[9] Jadbabaie A, Lin J, Morse A S, “Coordination of groups of mobile swarms:combined effects of attactrant/replellent profiles”,Proc. Of
autonomous agents using neighbor rules,” IEEE Trans. Autom. Conf.Decision Contr., NV,Dec 2002, pp.114-123.
Contr.,2003.48(6): 988-1001. [16] Gazi V, Passino K M. “Stability analysis of social foraging
[10] H. G. Tanner, A. Jadbabaie,and G. J. Pappas, “Flocking in fixed swarm,” IEEE Trans. On Syst. Man, and Cyber.-part B:
and switching networks,” IEEE Trans. Autom. Control, vol. 52, Cybernetics, vol.34,no.1,pp.539-557,Feb 2004.
no.5 , May 2007pp863-868. [17] Wu XX,Guo BL,Wang J. “Mobile Robot Path Planning Algorithm
Based on Particle Swarm Optimization of Cubic Splines” Robot,
vol.31,no.6,pp.556-560,Nov,2009.

Figure 1. Comparison between VPSO and PSO.

P0 x P0 y P0cx P0cy P1x P1 y P1cx P1cy …

Figure 2. Structure of particle.

1400 1400 1200

1200 1200
1000

1000 1000
800

800 800

600
y

600 600

400
400 400

200
200 200

0 0 0
-400 -200 0 200 400 600 800 1000 1200 1400 1600 -400 -200 0 200 400 600 800 1000 1200 1400 1600 0 200 400 600 800 1000 1200 1400 1600
x

Figure 3. Center path with the initial population and center path with the population after100 iterations and swarm path

1670 on June 04,2024 at 06:25:29 UTC from IEEE Xplore. Restrictions apply.
Authorized licensed use limited to: Wojskowa Akademia Techniczna. Downloaded