Adaptive Formation Control and Collision Avoidance Using a Priority

ARTICLE International Journal of Advanced Robotic Systems

Adaptive Formation Control and Collision Avoidance Using a Priority Strategy for Nonholonomic Mobile Robots Regular Paper

Yanyan Dai1, Kyung Sik Choi1 and Suk Gyu Lee1,* 1 Department of Electrical Engineering, Yeungnam University, Gyongsan, Gyongbuk, Korea * Corresponding author E-mail: [email protected]

Received 30 May 2012; Accepted 12 Nov 2012 DOI: 10.5772/55019 © 2013 Dai et al.; licensee InTech. This is an open access article distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

Abstract This paper presents four novel collision avoidance processes for nonholonomic mobile robots to generate effective collision‐free trajectories when forming and maintaining a formation. A collision priority strategy integrates the static and dynamic collision priorities to avoid a collision efficiently and effectively. In addition, it minimizes the turning angle of the follower robot and decreases system computation time. When avoiding collisions between robots, a novel collision avoidance algorithm is used to find a safe waypoint for the robot, based on the velocity of each robot. An adaptive tracking control algorithm, using the Lyapunov analysis, guarantees that the robotʹs trajectory and velocity tracking errors converge to zero considering parametric uncertainties of both the kinematic and dynamic models. The simulation and experiment results validate the effectiveness of the proposed method. Keywords Formation Control, Collision Priority, Collision Avoidance Algorithm, Adaptive Control www.intechopen.com

1. Introduction Over the past decades, multi‐robot formation forming and maintaining has been regarded as a major topic of interest because of its benefits of feasibility, accuracy, robustness and flexibility. The existing formation control approaches include the behaviour‐based approach [1], the virtual structure approach [2, 3] and the leader‐follower approach [4, 5]. Due to its easiness to understand and implement, the leader‐follower approach has stimulated a great deal of research. When using the leader‐follower approach, one or more robots are designated as the leader robots, while the follower robots keep track of the leader robots to form and maintain the formation. Collision avoidance control is a problem in multi‐robot formation systems. To solve this problem, four collision avoidance processes are presented, consisting of collision detection, collision judgement, collision priority and the collision avoidance algorithm. This paper focuses on the third and fourth processes.

Int J Adaptive Adv Robotic Sy, 2013, Vol. 10, Yanyan Dai, Kyung Sik Choi and Suk Gyu Lee: Formation Control and140:2013 Collision Avoidance Using a Priority Strategy for Nonholonomic Mobile Robots

1

With respect to a collision priority process, a static collision priority strategy is coordinated using a dynamic collision priority strategy. Firstly, after avoiding the collisions, a block in reforming formation may happen, that may cause a deadlock problem increasing the collision avoidance trajectory. Therefore, the static collision priority strategy is proposed to increase the efficiency and effectiveness of the collision avoidance. Secondly, when designing the trajectory for the robot, the term ʹsmoothʹ is an essential point to be considered. If the robot turns 90 degrees in a time step, the guarantee of smooth trajectory is impossible, and the wheels of the robot slip easily, causing the increase of localization errors for the robot. Hence, the follower robot minimizes the turning angle using the proposed dynamic collision priority strategy in this paper. In addition, since the dynamic collision priority is based on the actual situation during the motions, the computation time is increased within the system. The solution to this problem will be outlined in section 3. The documented priority strategy studies are as follows. Using the global knowledge, [6‐8] propose the static priority strategy, where the priority is assigned to each robot beforehand and cannot be changed during the motion. The dynamic priority strategy in [9] is based on occupancy of local searching space, where each robot recalculates its priority and exchanges the information with its nearby robot, but the strategy cannot solve the disorder and deadlock problem. In [10], the dynamic priority is introduced, where the network structure is presented based on the motion conflicts amongst the robots. However, there are difficulties in establishing a conflict graph, which includes all robots in the system, at each time step. The proposed collision priority strategy has the following attributes. (1) The static collision priority is related to the formation model and it is very simple; (2) the proposed dynamic collision priority aims to obtain a smooth trajectory by minimizing the turning angle of the follower robot; and (3) due to the integration of the static and dynamic collision priorities, the follower robot does not need to calculate the priorities continuously, while avoiding collisions. Hence, the computation and the complexity of the system can be extensively decreased. With respect to a collision avoidance algorithm process, a novel collision avoidance algorithm is coordinated with multi‐robot formation, based on the velocity and velocity orientation of each robot. In other research, the artificial potential method has been widely used to plan a collision‐free path, where the motion of the robot is controlled by the negative gradient of a mixture of attractive and repulsive potential functions as control inputs. However, since the path is described in the form of a high order 2

Int J Adv Robotic Sy, 2013, Vol. 10, 140:2013

polynomial, the computations referring to the analytical motion planning are complex and even unsolvable [2]. The dynamic motion planning method to deal with the collision avoidance problem is also used in [11, 12], requiring full knowledge of the obstacle’s trajectory. Motion control is another important function in multi‐ robot systems. The trajectory tracking problem is widely solved using the kinematic model of a mobile robot [13‐15], where ‘perfect velocity’ tracking is assumed to generate the actual velocity control inputs. However, since it is difficult for the dynamics of the robot to produce the perfect velocity as the kinematic controller [16], the torque input is used [17]. This paper presents an adaptive tracking control algorithm which integrates an adaptive kinematic controller and a torque controller for the nonholonomic mobile robot, based on [18, 19]. This paper is organized as follows. Section 2 gives three research models. Section 3 discusses the four collision avoidance control processes. Section 4 outlines the robust adaptive controllers. Section 5 shows the simulation results. Finally, in section 6, the experimental results are presented. 2. Research Models 2.1 A Nonholonomic Mobile Robot Model The nonholonomic mobile robot is shown in Fig. 1. The robot R i ( i is the robot number i  1,2,...,N ) consists of a passive wheel and two actuated wheels (e.g., DC motor) to achieve the motion and orientation. The radius of both of the actuated wheels is ri . The distance between the two actuated wheels is denoted by 2bi . The centre of mass of the mobile robot is located at Pic , and Pio is located in the middle point between the right and left driving wheels. The distance between Pic and Pio is denoted by di . {o,x, y} is an inertial Cartesian and {Pic ,X, Y} is the local frame fixed on the robot. Three generalized coordinates of the robot configuration can be described as q i  [xi ,yi ,i ]T in an inertial Cartesian frame, where (x i , y i ) are the coordinates of the point Pic in the inertial Cartesian frame and  i is the orientation of the basis {Pic ,X, Y} with respect to the inertial basis. Define vi and i as the linear and angular velocities of the robot, the ordinary form of the robot R i kinematic model is as (1).

 x i  cosi q i   y i    sin i i   0   

0  v  0   i  (1)  1  i 

Define  i  ( i1 , i2 )T as the angular velocities of the right and left wheels of the robot R i . The relationship between vi , i and  i1 ,  i2 is as follows. www.intechopen.com

  1 1 bi   v i   i   i1       (2)  i2  ri 1  bi  i 

y

Y

X

2ri

di 2bi P io

Pic

i

2.2 The Leader‐Follower Formation Model x

o

Figure 1. Two–wheeled nonholonomic mobile robot model

The robot can only move in the direction normal to the axis of the driving wheels, and the wheels do not slip. Define ir and il as the angles of the right and left driving wheels of the robot R i . These nonholonomic constraints can be written as (3)‐(5).

y i cosi  x i sin i  0 (3)

x i cos  i  y i sin  i  bii  riir (4)

x i cos  i  y i sin  i  bii  riil (5)

Since the trajectory of the mobile robot base is constrained to the horizontal plane, the gravitational vector G(q) is zero. The motion of the nonholonomic mobile robot R i are given by

q i  S(q i ) i (t) (6)

M(q i )i  V(q i ,q i ) i   id  B i (7)

cosi r  where S(q i )  i  sin i 2  1 bi   r  M   i   2bi 

2

 0   V 2   ri m d   2bi ic i i

x

li 0

Ri 1

i 0

  , 2 (m i bi  Ii )  I iw 

(m i bi 2  I i )

i (t )

i (t  1) Riw Ri

Figure 2. The leader‐follower formation model

2.3 The Follower Robot Control Model

2

m i  m ic  2m iw , Ii  m icdi  2m iw bi  Iic  2Iiw . In these expressions,  i  ( i1 , i2 )T is the torque applied on the robot R i ’s right and left wheels. m ic and m iw are the mass of the body and wheel with a motor. Iic , I iw , and Iim are the moment of inertia of the body with respect to the vertical axis through Pic , the wheel with a motor with respect to the wheel axis, and the wheel with a motor with respect to the wheel diameter, respectively.  id is the bounded unknown disturbances of robot R i including unstructured and unmodelled dynamics. www.intechopen.com

 xiw (t)   xi 1(t)  li0 cos( i0  i (t))       yiw (t)   y i 1 (t)  li0 sin( i0  i (t))  (8) iw (t)    i 1 (t)    

i 1 (t )

 ri 2 m icdii  2bi 1 0   , B  0 1  ,    0  2

In the formation control method, n robots are controlled to move in a stable mode. A robot is assigned to be the leader robot, which determines the follower robot’s motion by defining the desired distance and the desired bearing angle. The follower robot calculates the waypoint by the desired distance and the desired bearing angle. Let R i 1 and R i be the leader robot and the follower robot, respectively. li is denoted as the actual distance between R i 1 and R i , li0 as the desired distance. i is denoted as the actual bearing angle from the orientation of the follower robot to the axis connecting R i 1 and R i , and  i0 as the desired bearing angle. The formation for the leader robot R i 1 ‐waypoint R iw ‐the follower robot R i with the desired distance and the desired bearing angle is shown in Fig.2, where  i (t)  i 1 (t) . The waypoint of the follower robot R i is denoted by q iw  (xiw ,yiw ,iw )T as (8).

y

cosi   sin i  , 1 bi 

(m b 2  I )  I iw  i i 2i  (m i bi  I i )

Property 1: M(q i ) is symmetric and positive definite.   2V) is skew symmetric. Property 2: (M Assumption 1: The bounded disturbances  id satisfies  id  c i0  c i1  i , with positive constants c i0 and ci1 [19].

The collision‐free trajectory of the leader robot is predefined and the follower robots track the leader robot while avoiding the collisions. Fig. 3 shows the block diagram to control the follower robot. The follower robot communicates with the leader robot and checks possible collisions with other follower robots. If there is collision danger, the collision judgement process decides whether or not the follower robot starts a collision avoidance algorithm. Based on the waypoint from formation control or collision avoidance control, the adaptive controllers are calculated for the follower robot.

Yanyan Dai, Kyung Sik Choi and Suk Gyu Lee: Adaptive Formation Control and Collision Avoidance Using a Priority Strategy for Nonholonomic Mobile Robots

3

Case

Dij (t)

dDij (t) / dt

Collision danger

1

Dij (t)  (si  s j )

0

Danger decreases.

2

Dij (t)  (si  s j )

0

Danger increases.

Table 1. The two follower robots collision danger with respect to

Dij (t) and dDij (t) / dt .

3.3 The Collision Priority

Figure 3. The block diagram of the follower robot control

3. The Collision Avoidance Control In this section, follower robots try to complete a collision avoidance task in forming and maintaining formation with a leader robot effectively and efficiently. The task includes four processes: collision detection, collision judgement, collision priority and the collision avoidance algorithm. The four processes are detailed as follows. 3.1 The Collision Detection

Let R i and R j denote two follower robots, with their states as (xi , yi ,i )T and (x j , y j , j )T , respectively. The distance Dij between two follower robots is shown in (9). Dij  (x i  x j )2  (y i  y j )2 (9)

As shown in Fig. 4, based on moving speed and physical size of the follower robot, two pseudo protected shells are defined surrounding the follower robot R i and R j , with the radius of s i and s j , respectively. If the distance Dij satisfies the equation (10), the two follower robots must consider the collision avoidance. Dij  (xi  x j )2  (y i  y j )2  ( si  s j ) (10)

Dij Ri

Rj

si

Figure 4. The protected shells of follower robot R i and R j

4

3.3.1 The static priority

After avoiding the collisions, due to the longer desired distance with the leader robot, the follower robot may block other follower robots from reforming the formation. Hence, a collision may occur again, causing the increase of the collision avoidance trajectory and the deadlock problem. To overcome these problems, the static priority psi ( i is the robot number i  1,2,...,N ) for each follower robot is initially defined. The value of the static priority is given by the reciprocal of the desired distance li0 with the leader robot as (11).

psi  1 / li0 (11)

From (11), if the desired distance of the follower robot R j with the leader robot is shorter than the follower robot R i , psj is higher than psi . If the desired distances of the two follower robots R i and R j are the same, psi  psj . 3.3.2 The dynamic priority

sj

y x

Following the collision detection and the collision judgement processes, to avoid collision efficiently and effectively, and to minimize the turning angle of the follower robot, there are two main considerations: 1. Which follower robot should be selected to apply the collision avoidance algorithm; 2. Which follower robot should continue tracking the formation. Therefore, in the collision priority process, the static priority is integrated with the dynamic priority to select which follower robot will perform the collision avoidance algorithm.

To minimize the turning angle of the follower robot, a dynamic priority strategy is introduced to give orders to the robots. Based on the collision avoidance algorithm, define the dynamic priority pdi ( i is the robot number, i  1,2,...,N ) function n( j i) 0  i   j (12) pdi   j1 jp , j   1  i   j

3.2 The Collision Judgement

After detecting a collision, the follower robots judge whether to start the collision avoidance algorithm or not, based on the method in [20]. The judgement of a collision danger relies on the distance Dij (t) and its derivative dDij (t) / dt . As the classifications in Table 1, if the collision danger increases, the two follower robots are required access to the collision avoidance algorithm.

where  i and  j are estimated turning angles of the follower robot R i and R j , respectively, based on the collision avoidance algorithm. p is a positive constant in (12). If the follower robots share the same static priority, the dynamic priority approach is used to choose the


www.intechopen.com

follower robot for addressing the collision avoidance algorithm. If there are multiple robots, the term n( j i) pdi   j1 jp means the accumulation of all turning angle comparisons between the follower robot R i and the other follower robot R j (j  1,2,...,N, j  i) . 3.3.3 The cooperation of the static and dynamic priorities

In order to increase collision avoidance efficiency and minimize the robot’s turning angle, the follower robot is selected to apply the collision avoidance algorithm, based on the cooperation of the static and dynamic priorities. The cooperation flowchart is illustrated in Fig. 5.

Ri



p dj

The collision avoidance algorithm is presented in Fig.6. Referring to the collision detection process, the collision judgement process and the collision priority process, the follower robot R i is assumed to avoid collision with the follower robot R j . As shown in Fig.6, the follower robot R i and R j are currently located at A and B, with the position state Pi  (xi , yi )T and Pj  (x j , y j )T , respectively. 1 is the angle from the axis x to the axis AB .

pis  p sj

pis  p sj

pid



 y j  yi   (13)  x j  xi   

 1  arctan 

Define  j  i as the relative angle of the orientation of the follower robots R i and R j , the angle  2 is written as (14). For the velocities of R i and R j given by P i and P j , the angle  3 is denoted as (15).

p dj

Rj

Ri

Ri

Rj

3.4 The Collision Avoidance Algorithm

Rj

pis  p sj

pid

collision avoidance problem, the follower robot directly applies the collision avoidance algorithm with the selection from steps 1 and 2. In this way, the calculation and the comparison time can be decreased by skipping the comparison of the static and dynamic collision priorities.

 2   | j  i | (14) 

Ri

Rj

Ri

Rj

   (15)  (P i )2  (P j )2  2P i P j cos  2   

 3  arccos 

P i  P j cos  2

 4 is the angle between the orientation of the follower robot R i and the axis AB at point A . The angle  4 can be calculated as (16). Figure 5. The cooperation flowchart of the static and the dynamic collision priorities

After the collision detection and judgement processes, the follower robot is chosen to avoid the collision via the following steps. Firstly, the selection of whether to implement the collision avoidance algorithm or to keep the formation is based on the comparison between the static priorities of the follower robots. If psi  psj , the follower robot R i continues tracking the formation, and the follower robot R j goes into the collision avoidance algorithm process. Secondly, if the static priorities of the follower robots are the same, the follower robot is chosen to implement the collision avoidance algorithm by the comparison result of the dynamic priorities. If pdi  pdj , the follower robot R j starts the collision avoidance algorithm, while the follower robot R i follows the leader robot. Thirdly, if the follower robots continue to have the www.intechopen.com

 4 |i  1 | (16)

The goal of the collision avoidance algorithm is to enlarge the distance between the follower robots until the distance is equal to the sum of the protected shell radius of the follower robots ( si  s j ). Considering the smoothness of the trajectory, the waypoint for follower robot R i is located at C to satisfy the requirement |i (t)  i (t  1)|  / 2 .The distance of AC is calculated as

rwp  Dij cos  5  (si  s j )2  Dij2 sin 2  5 (17)

where  5   3   4 . For the parameters discussed above, the waypoint equation to avoid collision is given by

 xiw (t)   xi (t  1)  rwp cos(i (t  1)   3 )       y iw (t)    yi (t  1)  rwp sin(i (t  1)   3 )  (18)  iw ( t)   i (t  1)   3    


5

where rwp is the distance to move for the next step and i (t  1)   3 is the angle to turn. Based on the orientation of the follower robot R i , note that when the follower robot R j is on the right‐side of the follower robot R i , select i (t  1)   3 in (18), otherwise, select  i (t  1)   3 . y

C

x si

Ri

A

i

 ri   ri  ri ri yiE  yiE      b b 2 2 2 2 i i     xiE   viw cos iE     ri ri      (25)  yiE     2b xiE  i1   2b xiE  i 2   viw sin iE     i i iE    iw      ri ri      2bi 2bi    

si  s j

 j 4 1

sj

Dij

B Rj

Figure 6. The collision avoidance algorithm of the follower robot

Ri

In this section, adaptive controllers are designed for the robot with two actuated wheels based on kinematic and dynamic models. The designed controller can guarantee that the robot’s trajectory and velocity tracking errors converge to zero asymptotically stably under the parametric uncertainties of the kinematic and dynamic models. 4.1 Adaptive Control of the Kinematic Model

Considering the kinematic model in (6), an adaptive tracking controller for the kinematic part is designed based on [18]. The waypoint equations are defined as

x iw  v iw cosiw (19)

y iw  v iw sin iw (20)

iw  iw (21)

where v iw and iw are the waypoint input. Comparing the waypoint state q iw with the current measured state q ic , the tracking error posture can be described as (22).  xiE   cosic    Eiq   y iE     sin ic    iE   0

sin ic

cosic 0

If the parameters ri and bi are not known, the angular velocities of the left and right wheels cannot be obtained by the linear and angular velocities in (2). Based on [19], set a i1  1 ri and a i2  bi ri ,  i1 and  i2 are chosen as   aˆ aˆ i2   v if  a i1  a i1 a i2  a i2   v if   ic   i1c    i1        (26) ˆ a aˆ i2  if  a i1  a i1 a i2  a i2  if     i2c   i1

4. Robust Adaptive Controllers Design

where K i1 , K i2 , and K i3 are positive constants. Using (2), (23) is transformed to

iw

Riw rwp  2  P j 3 Pi

0   xiw  xic    0   y iw  y ic  (22) 1  iw  ic 

where aˆ i1 and aˆ i2 are the estimate of ai1 and a i2 , and a i1  aˆ i1  a i1 , a i2  aˆ i2  a i2 . (25) can be described as (27).  x iE 

 x iE   1  yiE   v iw cosiE          E iq   y iE    0  v ic    xiE  ic   v iw sin iE  iE   0   1   iw         

iE   

Based on [18], the linear and angular velocity v ic and ic are given as v if and if , respectively, 6


 y iE 

 v iw cosiE 



i1



0   

i2



 1   

 

iw

 

To design aˆ i1 and aˆ i2 , based on [21], the Lyapunov function is chosen as (28), where a i1  aˆ i1 , a i2  aˆ i2 . V1  1  xiE 2   y iE 2   2





1  cos  iE  K i2



1 1 a 2  a 2 (28) 2 i1a i1 i1 2 i2a i2 i2

 i1 and  i2 are positive constants. The differential of V1 is    x x  y y   iE sin  iE  1 a a  1 a a V 1 iE iE iE iE K i2  i1a i1 i1 i1  i2a i2 i2 i2   a = xiE  K i1xiE  i1 v if   y iE v iw sin iE a i1     sin iE a   K i2 y iE v iw  K i3 sin  iE  i2 if  a i2   K i2 1 1 a a  a a   i1a i1 i1 i1  i2a i 2 i2 i2 =  K i1  xiE   2



(23)

 1

 y iE    1  a i1  v if  0    1  a i2  if  xiE    v iw sin iE  (27)       a     a  

The derivative matrix E iq can be derived as (23).

 v iw cosiE  K i1xiE  v if    (24)       K v y K sin  i2 iw iE i3 iE   if   iw

K i3  sin  iE 

2

K i2



a i1

 i1a i1

aˆ

i1

  i1xiE v if

(29)



 sin iE  a i2    aˆ i2   i2 if  K i2 

 i2a i2 

The parameter update laws are chosen as (30) and (31).

aˆ i1   i1x iE v if (30)

aˆ i2   i2

if sin  iE K i2

(31)

www.intechopen.com

(29) becomes (32).

 =  K  x 2  V 1 i1 iE

    V   ET ME  1 ET ME  T 1  c i0c i0  c i1c i1 V 2 1 ic ic ic  Pi  Pi  2 ic  i3  i4

K i3  sin iE  K i2

4.2 Adaptive Control of the Dynamic Model

Define the velocity tracking error of the robot R i as (33).     Eic   i   ic   i1 i1c  (33)  i2   i2c 

(7) can be rewritten as (34).

M(ic  E ic )  V( ic  E ic )   id  B i (34)

Therefore, the differential of E ic multiply M can be described as

ME ic  (Mic  V ic )  VEic   id  B i =  Yic Pi  VEic   id  B i

(35)

where Mic  V ic  Yic Pi and

 i2c Yic   i1c    i2c i1c

 r2 Pi   i 2 (m i b i 2  I i )  I iw  4b i

ri 2 4b i

i i2c  , i i1c  T

(m i b i 2  I i ) 2

 ri 2 m d . 2b i ic i 

Based on [19], the torque controller for the dynamic model is designed as (36).

 i  B1 ( K idEic  Yic Pˆ i  uis ) (36)

K id is a diagonal positive‐definite design matrix. Pˆ i is the estimate of Pi and P i  Pˆ i  Pi . uis  Asgn(Eic ) , where sgn(Eic ) is a sign function and A is a positive‐definite controller gain. Define A  cˆ i0  cˆ i1  i , cˆ i0 and cˆ i1 are the estimate of c i0 and ci1 in Assumption 1, and c i0  cˆ i0  ci0 , c i1  cˆ i1  c i1 . To design cˆ i0 , cˆ i1 and Pˆ i , the Lyapunov function is chosen as (37).

c 2 c 2 1 1 V2  V1  ETic MEic  P iT  1P i  i0  i1 2 2 2 i3 2 i4 (37)

 i3 and  i4 are positive constants. The differential of V2 is denoted as (38). www.intechopen.com



 0 (32)

As t   , Eiq is shown to be a stable equilibrium point. Using Barbalat’s lemma, xiE and  iE tend to zero as t   . Since a i1 and a i2 are bounded, x iE and iE are bounded.

   V  E  P T 1P  c i0c i0  c i1c i1   ET  Y P    B  ET  1 M =V  ic 1 ic ic i id i ic  i i  i3  i4 2      ET Y P    K E  A sgn(E )  P T  1P  c i0c i0  c i1c i1 =V 1 ic ic i id id ic ic i i

2







 i3

(38)

 i4

    ET K E  P T 1 (P  YT E )  ET  AET sgn(E )  c i0c i0  c i1c i1 =V 1 ic id ic i ic ic ic id ic ic

 i3

 i4

The parameter update laws are selected as (39)‐(41).

 Pˆ i  YicT Eic (39)

cˆ i0   i3 Eic (40)

cˆ i1   i4  i Eic (41)

Therefore, (38) can be rewritten as (42).    V   ET K E  ET  AET sgn(E )  c i0 c i0  c i1c i1 V 2 1 ic id ic ic id ic ic



  ET K E  A    V 1 ic id ic id

 Eic



c i0c i0

 i0

 i0



c i1c i1

 i1

 i1

(42)

        ET K E  c  E  c i0   c   E  c i1   V 1 ic id ic i0  ic i1  i ic     i0  i1    T  E K E 0 =V 1 ic id ic

V2 is non‐increasing with time, so it is bounded. Using Barbalat’s lemma, xiE ,  iE and E ic tend to zero as t   .  iE and iE are bounded. This implies iE is uniformly continuous. From Barbalat’s lemma, iE  0 as t   . According to (23), (24) and (33), variable iE can be described by the equation K iy v iw yiE  0 . v iw is a bounded function of time. These imply that y iE has to converge to zero. As t   , Eiq  0 is an asymptotically T stable point. Therefore, the error E  ETiq ETic  0 is an asymptotically stable equilibrium point.





5. Simulation Results In this section, the physical parameters of the robots are as bi  0.125m , di  0.15m , ri  0.08m , mic  4 kg , miw  1kg , I ic  2.325 , I iw  0.005 and I im  0.0025 . The parameters for the adaptive controllers are chosen as K i1  0.1 , K i2  0.2 , K i3  0.28 , K id  diag{0.5,0.5} ,  i1   i2  0.001 ,  i3   i4  0.01 and   diag{0.01,0.01, 0.01} . 5.1 First Simulation

Fig. 7 shows the trajectories of three robots forming a line formation at t=2.9s, t=3.3s, t=3.7s and t=6.0s. The desired distance of follower robots 1 and 2 with the leader robot are equal to l10  0.9m and l 20  0.45m , respectively. The desired bearing angles are 10   20  0 . The protected shell radius of the follower robots are 0.16m . The initial state of the leader robot, follower robot 1 and follower robot 2 are as (0.5,1.5,0)T , ( 0.65,2.5,0)T and


7

( 0.65,0.5,0)T , respectively. The leader robot’s trajectory is defined as

yL  1.5 (44)

0

-0.5

-1 0

4

6

Time (sec)

leader robot follower robot1 follower robot2

2.5

2

2 y-axis (meter)

Collision danger

1.5

8

10

12

0

0.5 x-axis (meter)

1

1.5

2

-1

-0.5

0

0.5 x-axis (meter)

3


1

1.5

2

y-axis (meter)

y-axis (meter)

Follower robot 1 tracks formation.

1.5


2.5

2

2 0.9m 0.45m

1.5 1

1

0.5 0.5 -1

-0.5

0

0.5 x-axis (meter)

1

1.5

0 -1

2

angular velocity error

-0.4 -0.6 -0.8 0

0

1

x-axis (meter)

2

3

Along the trajectories in Fig. 7(d), the trajectory and velocity tracking errors of follower robots 1 and 2 are shown in Figs. 8‐11. The simulation was run 50 times to take average values and input noise is  id  3 . From the results, the trajectory and velocity tracking errors approach to zero. 1

x error(meter) y error(meter)  error(rad)

0.5

Error

0

-0.5

-1

4

6

Time (sec)

0.12

8

10

12

angular velocity error of left wheel angular velocity error of right wheel

0.1 0.08 0.06 0.04 0.02 0

2

4

6

Time (sec)

8

10

12

To better show the performance of the static priority strategy in reducing the collision path and in solving the deadlock problem, Fig. 12 is compared with Fig. 7 (d). In Fig. 12, two follower robots track the same leader robot as in Fig. 7 (d), without the static priority comparison result. This means that follower robot 2 must avoid collision using the collision avoidance algorithm, and follower robot 1 keeps tracking the formation trajectory, when the collision danger occurs. As seen from Fig. 12, follower robot 2 cannot reform the formation with the leader robot. Therefore, the static priority strategy is essential to reduce the collision path and solve the deadlock problem. 3

6

Time (sec)

8

10

Figure 8. The trajectory errors of follower robot 1


12


2.5

y-axis (meter)

Figure 7. The trajectories of three robots avoiding collision based on the static collision priorities of two follower robots. (a) t=2.9s, (b) t=3.3, (c) t=3.7, (d) t=6.0s

2

4

Figure 10. The velocity errors of follower robot 1

(c) (d)

-1.5 0

2


(a) (b)

2.5

0 -0.2

-0.06 0

0.5 -0.5

0.2

-0.04

1.5

1

0.5


0.4

-0.02

Follower robot 1 avoids collision.

1

-1


2.5

0.6


8

2


Based on the collision priority process, the two follower robots’ static priorities are not the same so that the follower robot is selected to execute the collision avoidance algorithm by the static priorities comparison result. Follower robot 1’s static priority is smaller than follower robot 2’s. Therefore, follower robot 1 avoids the collision with follower robot 2 using the collision avoidance algorithm, while follower robot 2 keeps tracking the formation.

y-axis (meter)


0.5

Error

xL  0.4t  0.5 (43)

1

2 0.9m

1.5 1 0.5 -1

0

1

x-axis (meter)

2

3

Figure 12. The trajectories of three robots avoiding collision without the static priority comparison result

5.2 Second Simulation

Fig. 13 shows three robots‘ trajectories forming a triangle formation at t=2.7s, t=3s, t=3.5s, t=5s and t=10s. The desired distance of follower robots 1 and 2 with the leader robot are equal to l10  l 20  0.45m , and the desired bearing angle are 10    4 and  20   4 , respectively. The protected shell radius of follower robots 1 and 2 are 0.16m , 0.16m , respectively. The initial state of the leader robot, and follower robots 1 and 2 are as www.intechopen.com


2.5

y-axis (meter)

Collision danger

1.5

Error

-1

-1.5 0

0

0.5

x-axis (meter)

1

1.5

2

Two follower robots are safe.

1.5 1

-1

-0.5

0

0.5

x-axis (meter)

1

-0.5

0

0.5

1

x-axis (meter)

1.5

2

2

4

6

8

Time (sec)

10

12


The follower robot 1 tracks formation.

1

-1

-0.5

0

0.5

0.05

0

-0.05

1

x-axis (meter)

1.5

2

2

4

6

8

Time (sec)

10

12

2.5

Comparing with Fig. 13 (e), Fig. 18 shows that the three robots track the same trajectories as in Fig. 13 (e), without the dynamic comparison result. When collision danger happens, follower robot 2 must avoid collision using the collision avoidance algorithm, and follower robot 1 keeps tracking the formation trajectory. Table 2 shows the performance of the dynamic priority strategy in minimising the turning angle of the follower robot more clearly. 2.5

2.5



2 y-axis (meter)

2


(c) (d)

y-axis (meter)


-0.1 0

1.5

2

-0.1

0.1

2

1.5

0 -0.05


0.5

0.5

12

0.05

0.15

2.5

y-axis (meter)

y-axis (meter)

2

10


-0.2 0

Follower robot 1 avoids collision.

-1


8

-0.15

(a) (b)

2.5

6

Time (sec)

0.1

0.5 -0.5

4

0.15

1

0.5

2


1.5

1

-1

0


y-axis (meter)

2


-0.5


2.5

2

1

0.5


(0.5,1.5,0)T , ( 0.65,2.5,0)T and ( 0.65,0.5,0)T , respectively. The leader robot‘s trajectory is defined as (43) and(44). In this case, because the two follower robots‘ static priorities are the same, the selection for the follower robot to do the collision avoidance algorithm is based on the dynamic priority comparison result. Due to the smaller dynamic priority, follower robot 1 is required to avoid the collision with follower robot 2.

1.5

1.5

1

1 -0.5

-0.5

0

0.5

1

1.5

2

x-axis (meter)

2.5

3

3.5

4

4.5

(e) Figure 13. The trajectories of three robots avoiding collision based on the dynamic collision priorities of two follower robots. (a) t=2.7s, (b) t=3s, (c) t=3.5s, (d) t=5s, (e) t=10.0s

For the trajectories in Fig. 13(e), the trajectory and velocity tracking errors of follower robots 1 and 2 are shown in Figs. 14‐17. The simulation was run 50 times to take average values and input noise is  id  3 . The trajectory and velocity tracking errors approach to zero clearly. 1.5

0.5

1

1.5

2

x-axis (meter)

2.5

3

3.5

4

4.5

Table 2 demonstrates the incremental collision path length and turning angle during forming and maintaining of the formation in Fig. 13 (e) and Fig. 18. Collision avoidance within the priority comparison result without the priority comparison result

Incremental path length (metres) FR1 FR2

Incremental turning angle (rad) FR1 FR2

3.3299

×

1.9769

×

×

6.7401

×

5.1945

Table 2. The comparison results of incremental collision path length and turning angle during forming and maintaining the formation within and without priority comparison results.


1

0

Figure 18. The trajectories of three robots avoiding collision without the dynamic priority comparison result

Error

0.5

0

-0.5

-1 0

2

4

6

Time (sec)

8

10

Figure 14. The trajectory errors of follower robot 1 www.intechopen.com

12

Using the dynamic priority strategy, the incremental collision path length in Fig. 13 (e) is shorter than the incremental collision path length in Fig. 18, and the turning angle in Fig. 13 (e) is quite substantially minimized. Yanyan Dai, Kyung Sik Choi and Suk Gyu Lee: Adaptive Formation Control and Collision Avoidance Using a Priority Strategy for Nonholonomic Mobile Robots

9

5.3 Third Simulation

1


xL  0.55t  0.5 (45)

yL  1.5 (46)

-1 0

1

2

3

4

5

Time (sec)

6

collision avoidance 3

0.9m 0.45m collision avoidance 1


Error

0

-1 0

1

2

3

4

5

Time (sec)

6

0.5 -1

0

7

8

9

10


The velocity errors of follower robots 1‐4 with the trajectories in Fig. 19 are shown in Figs. 24‐27. From the results, the velocity errors approach to zero. 0.3


0.2 0.1 0 -0.1 -0.2 -0.3 -0.4 0

1

2

3

4

5

Time (sec)

6

7

8

9

10

Figure 24. The velocity errors of follower robot 1 angular velocity error of left wheel angular velocity error of right wheel

0.05 0 -0.05 -0.1 -0.15

collision avoidance 2

1

10

-0.5

-0.2 0

1

2

x-axis (meter)

3

4

5

6

Figure 19. The trajectories of five robots avoiding collision based on four collision avoidance processes

The trajectory errors of the follower robots 1‐4 with the trajectories in Fig. 19 are shown in Figs. 20‐23. From the results, the trajectory errors stably converge to zero.

1

2

3

4

5

Time (sec)

6

7

8

9

10

Figure 25. The velocity errors of follower robot 2 0.02 0 -0.02 angular velocity error

1.5

9

0.5


2

8

1

0.1

leader robot follower robot1 follower robot2 follower robot3 follower robot4

7


0.15

2.5

y-axis (m eter)

0

-0.5


The third simulation demonstrates that a large team of robots can benefit from the proposed collision avoidance methods. In Fig. 19, the desired distances of the four follower robots with the leader robot are equal to l10  0.85m and l 20  0.45m , l 30  0.4m , l 40  0.4m , respectively. The desired bearing angles are 10   20  0 ,  30   / 4 and  40   / 4 . The protected shell radius of the follower robots are 0.16m . The initial state of the leader robot and follower robots 1‐4 are designed as (0.5,1.5,0)T , ( 0.55,2.5,0)T , ( 0.55,0.5,0)T , ( 0.6,2,0)T and T ( 0.6,1,0) , respectively. The leader robot’s trajectory is defined as (45) and (46). The collision danger firstly takes place between follower robot 3 and follower robot 4. Based on the dynamic priority strategy, follower robot 3 does the collision avoidance algorithm. Follower robot 2 and follower robot 3 must secondly solve the collision avoidance problem. According to the static priority strategy, follower robot 2 is required to do the collision avoidance algorithm. Finally, follower robots 1 and 2 must take the collision avoidance into consideration. Based on the static priority strategy, follower robot 1 avoids the collision.

Error

0.5

-0.04 -0.06 -0.08 -0.1 -0.12 -0.14 -0.16 0

angular velocity error of left wheel angular velocity error of right wheel 1

2

3

4

5

Time (sec)

6

7

8

9

10


1


0.5

0.15


0.1 angular velocity error

Error

0

-0.5

-1

-1.5 0

1

2

3

4

5

Time (sec)

6

7

8

9

10

Error

-0.5

3

4

5

Time (sec)

6

7

8

9

Figure 21. The trajectory errors of follower robot 2 10 Int J Adv Robotic Sy, 2013, Vol. 10, 140:2013

2

3

4

5

Time (sec)

6

7

8

9

10

5.4 Fourth Simulation

0

2

1



1

-0.1

-0.2 0

0.5

-1 0

0 -0.05

-0.15

Figure 20. The trajectory errors of follower robot 1 1

0.05

10

To show the validation of the adaptive controllers, the fourth simulation presents a large group of robots forming and maintaining formation avoiding collisions in sinusoid trajectories. In Fig. 28, the desired distances of the four follower robots with the leader robot are equal to www.intechopen.com

L

x  0.35t (47)

L

y  1.5sin(0.15t) (48)

3

Error

-1 -1.5 -2 -2.5 -3 0

2

4

6

8

10

Time (sec)

12

14

16

x error(meter) y error(meter)  error(rad) 18 20


The velocity errors of follower robots 1‐4 with the trajectories in Fig. 28 are shown in Figs. 33‐36. From the results, the velocity errors approach to zero. 1


0.5

0

-0.5

2

4

6

8

10

Time (sec)

12

14

16

18

20

Figure 33. The velocity errors of follower robot 1 0.15


0.1

0

0.05 angular velocity error

y-axis (meter)

1

0 -0.5

-1 0

leader robot follower robot1 follower robot2 follower robot3 follower robot4

2

0.5


l10  1.2m and l 20  0.6m , l 30  l 40  0.8m , respectively. The desired bearing angles are 10   20   / 4 ,  30   / 6 and  40   / 2 . The protected shell radius of the follower robots are 0.16m . The initial state of the leader robot and the follower robots are given by (0,0,0)T , ( 1.2, 1.2,0)T , ( 0.8, 1.7, 6)T , ( 0.9,0.6, 6)T and ( 0.8,0.3,0)T , respectively. The leader robot’s predefined trajectory is denoted as (47) and (48). Based on the static priority strategy, follower robot 1 avoids the collision with follower robot 2. In addition, follower robot 3 is required to avoid collision with follower robot 4 by using the dynamic priority strategy.

-1 -2

0 -0.05 -0.1 -0.15 -0.2 -0.25

0

2

4

6

8

x-axis (meter)

10

12

Figure 28. The trajectories of five robots avoiding collision based on four collision avoidance processes

The trajectory errors of follower robots 1‐4 with the trajectories in Fig. 28 are shown in Figs. 29‐32. From the results, the trajectory errors stably converge to zero.

-0.3 0

2

4

6

8

10

Time (sec)

12

14

16

18

20

Figure 34. The velocity errors of follower robot 2 0.15


0.1 0.05 angular velocity error

-3

0 -0.05 -0.1 -0.15 -0.2 -0.25 0

4


3

2

4

6

8

10

Time (sec)

12

14

16

18

20


2 0.2

Error

1

0

-1 -2 -3 0

2

4

6

8

10

Time (sec)

12

14

16

18

20


Error

-0.6 -0.8 -1

angular velocity error of left wheel angular velocity error of right wheel 2

4

6

8

10

Time (sec)

12

14

16

18

20

6. Experimental Results

1 0

6.1 The Robot Platform

-1 -2

2

4

6

8

10

Time (sec)

12

14

16

18

20

Figure 30. The trajectory errors of follower robot 2 3


2.5 2 1.5 Error

-0.4

-1.2 0


2

1 0.5 0 -0.5 -1 -1.5 0

-0.2


3

-3 0


0

2

4

6

8

10

Time (sec)

12

14

16

18


www.intechopen.com

20

The positions and attitudes of the three nonholonomic robots as shown in Fig. 37 are measured using a CCD camera (640×480) on the ceiling. In addition, the diameter of each wheel is 4.3 cm, and the distance between the left and right wheels is 6.9cm. The weight of the robot is 0.4kg. Each robot is equipped with five modules, including communication module, sensor module, MCU module, motor module and power module. The specifications of the modules are shown in Table 3. Yanyan Dai, Kyung Sik Choi and Suk Gyu Lee: Adaptive Formation Control and Collision Avoidance Using a Priority Strategy for Nonholonomic Mobile Robots

11

follower robot by Bluetooth. (4) The follower robot drives the DC motors based on the control law. (5) The follower robot transmits encoder values and the angle value to the server PC. (6) Based on the encoder values and the angle value, a localization process is performed on the PC. (7) The states of the follower robots are changed according to the adaptive controllers. In Fig. 38, the processes inside the dash block are executed in the PC, while the other blocks are processed by the onboard modules of the follower robots.

Figure 37. Three nonholonomic robots in the experiment Module Communication Sensor MCU Motor Power

6.3 Experiment

Name Specifications Bluetooth 10ms circuit Compass ±180.00 deg Encoder 2,048purse/circle Ultrasonic 0.03m~3m ATmega 2560 Comm. & Sensors Control DSP Motor Control DC motor Gear ratio 8:1 Battery 1.5 hr

Table 3. Specifications of each robot

6.2 The System Implementation

The information of the leader robot, such as encoder values and compass sensor value, is sent to server PC by Bluetooth.

In the experiment, three robots are executed to form and maintain a line formation, while avoiding collisions between follower robots. The radius of the protected shell of each robot is 8cm. The other parameters for the adaptive controllers are chosen as K i1  0.1 , K i2  0.2 , K i3  0.28 , K id  diag{0.5,0.5} ,  i1   i2  0.001 ,  i3   i4  0.01 and   diag{0.01,0.01,0.01} . The initial states of the leader robot and follower robots 1 and 2 are (26cm,15cm,0)T , (0,0,0)T and (0,30cm,15deg)T , respectively. The desired distances for follower robots 1 and 2 with the leader robot are defined as l10  60cm and l 20  30cm , respectively. The desired angles of the follower robots are 10   20  0 . Fig. 39 shows the trajectories of the three robots forming and maintaining a line formation. Based on collision priority strategy and the collision avoidance algorithm, when two follower robots form the formation, follower robot 1 avoids the collision with follower robot 2. 40

leader robot follower robot 1 follower robot 2

y-axis (cm)

30 20 10 0 -10 0

20

40

60 80 x-axis (cm)

100

120

140

Figure 39. The formation tracking trajectories of the three robots while avoiding collision

Figure 38. The overview of the system implementation for each follower robot

As shown in Fig. 38, the following steps are the executions for the follower robot to track the leader robot while avoiding collisions. (1) Based on the ultrasonic sensor values, if both of the follower robots are close enough, they send the collision flag to the server PC. (2) When the server PC receives a collision flag, it calculates the waypoint based on the four collision avoidance processes. Otherwise, the waypoint is calculated using the leader robot’s information. (3) The server PC calculates torques and sends motor commands to the 12 Int J Adv Robotic Sy, 2013, Vol. 10, 140:2013

The state errors of follower robots 1 and 2 are shown in Figs. 40 and 41. In Fig. 40, when follower robot 1 avoids the collision, there is an angle error which is reduced after avoiding collision. 0.2


0.1 Error

0

-0.1

-0.2 0

5

10

15

time (s)

20

25

30

35

Figure 40. The state errors of follower robot 1 www.intechopen.com

0.2

0.1 Error

Korea (NRF) funded by the Ministry of Education, Science and Technology ( 2012R1A1B3002240).


9. References

0

-0.1

-0.2 0

5

10

15

time (s)

20

25

30

35

Figure 41. The state errors of follower robot 2

Video snapshots are presented in Fig. 42, where three robots form a line formation while avoiding collision.

Figure 42. Snapshot of three robots forming a line formation and avoiding collision between the follower robots: (a)t=0s, (b)t=3s, (c)t=5s, (d)t=10s, (e)t=16s, (f)t=20s, (g),t=24s (h),t=30s (right, then down).

7. Conclusions In this paper, the proposed four collision avoidance processes were used to generate collision‐free trajectories for nonholonomic mobile robots when forming and maintaining formation. In order to minimize the turning angle of the robot, solve the dead‐lock problem and find the shortest collision avoidance path, the collision priority strategy was coordinated with the collision avoidance algorithm. The adaptive control algorithm, based on Lyapunov analysis, is designed for the trajectory and velocity tracking errors of each robot to converge to zero. Finally, the simulation and experiment results demonstrate the effectiveness of the proposed methods. 8. Acknowledgements This research was supported by the Basic Research Programme through the National Research Foundation of www.intechopen.com

[1] Balch T, Arkin R C (1998) Behavior‐based Formation Control for Multi‐robot Teams. IEEE Trans. on Robot. and Autom., vol. 14, no. 6, pp. 926‐939. [2] Chen X, Li Y M (2006). Smooth Formation Navigation of Multiple Mobile Robots for Avoiding Moving Obstacles. Int. J. Control, Autom., Syst., vol. 4, no. 4, pp. 466‐479. [3] Chen X, Li Y M (2008) Stability on Adaptive NN Formation Control with Variant Formation Patterns and interaction Topologies. Int. J. Adv. Robot. Syst., vol. 5, no. 1, pp. 69‐82. [4] Desai J P, Ostrowski J, Kumar V (1998) Controlling Formations of Multiple Mobile Robots. IEEE Int. Conf. Robot. Autom. pp. 69‐82. [5] Tran V H, Lee S G (2011) A Stable Formation Control using Approximation of Translational and Angular Accelerations. Int. J. Adv. Robot. Syst., vol. 8, no. 1, pp. 65‐75. [6] Berg J, Overmars M (2005) Prioritized Motion Planning for Multiple Robots. IEEE/ RSJ Int. Conf. Intell. Robots and Syst., pp. 430‐435. [7] Bennewitz M, Burgard W, Thrun S (2002) Finding and Optimizing Solvable Priority Schemes for Decoupled Path Planning Techniques for Teams of Mobile Robots. Robot. and Autonomous Syst., vol. 41, no. 2, pp. 89‐99. [8] Zheng T, Zhao X (2006) A Novel Approach for Multiple Mobile Robot Path Planning in Dynamic Unknown Environment. IEEE Int. Conf. Robot., Autom. and Mechatron., pp. 1‐5, Bangkok. [9] Clark C, Bretl T, Rock S (2002) Applying Kinodynamic Randomized Motion Planning with a Dynamic Priority System to Multi‐robot Space Systems. IEEE Aerospace Conf., USA. [10] Liu S, Sun D, Zhu C, Shang W (2009) A Dynamic Priority Strategy in Decentralized Motion Planning for Formation Forming of Multiple Mobile Robots. IEEE/ RSJ Int. Conf. on Intell. Robots and Syst., pp. 3774‐3779, USA. [11] Berg J P, Overmars M H (2005) Roadmap‐based Motion Planning in Dynamic Environments. IEEE Trans. on Robot., vol. 21, no. 5, pp. 885‐897. [12] Stentz A (1995) The Focussed D Star Algorithm for Realtime Replanning. Int. Joint Conf. on Artificial Intell. [13] Kanayama Y, Kimura Y, Miyazaki F, Noguchi T (1991) A Stable Tracking Control Method for a Nonholonomic Mobile Robot. IEEE/RSJ Int. Workshop Intell. Robots and Syst., pp. 1236‐1241. [14] Yuan G F, Yang S X, Mittal G S (2001) Tracking Control of a Mobile Robot using a Neural Dynamics based Approach. IEEE Int. Conf. Robot. Autom., pp. 163‐168, Seoul, Korea.


13

[15] Jiang Z P, Nijmeijer H (1997) Tracking Control of Mobile Robots: A Case Study in Backstepping. Atomatica, vol. 33, no. 7, pp. 1393‐1399. [16] Das T, Kar I N (2006) Design and Implementation of an Adaptive Fuzzy Logic‐Based Controller for Wheeled Mobile Robots. IEEE Trans. on Control Syst. Tech., vol. 14, no. 3, pp. 501‐510. [17] Fukao T, Nakagawa H, Adachi N (2000) Adaptive Tracking Control of a Nonholonomic Mobile Robot. IEEE Trans. on Robot. and Autom., vol. 16, no. 5, pp. 609‐615. [18] Kanayama Y, Kimura Y, Miyazaki F, Noguchi T (1990) A Stable Tracking Control Method for an Autonomous Mobile Robot. IEEE Int. Conf. Robotic. And Autom., pp. 384‐389.

[19] Wu J B, Xu G H, Yin Z P (2009) Robust Adaptive Control for a Nonholonomic Mobile Robot with Unknown Parameters. J. Control Theory Appl., vol. 7, no. 2, pp. 212‐218. [20] Fujimori A (2005) Navigation of Mobile Robots with Collision Avoidance for Moving Obstacles. IMechE. Part I: J. Syst. and Control Engineering, vol. 219, pp. 99‐110. [21] Krstic M, Kanellakopoulos I, Kokotovic P (1995) Nonlinear and Adaptive Control Design, New York: Wiley.

14 Int J Adv Robotic Sy, 2013, Vol. 10, 140:2013

www.intechopen.com