Real-time cooperative collision avoidance method of multiple mobile robots

IAV2004 - PREPRINTS 5th IFAC/EURON Symposium on Intelligent Autonomous Vehicles Instituto Superior Técnico, Lisboa, Portugal July 5-7, 2004

REAL-TIME COOPERATIVE COLLISION AVOIDANCE METHOD OF MULTIPLE MOBILE ROBOTS ABE Yasuaki ∗ and MATSUO Yoshiki ∗ ∗

Graduate School of Science and Engineering, Tokyo Institute of Technology

Abstract: In a previous study, the authors have proposed the Cooperative Collision Avoidance (CCA) method. The method extended the Velocity Obstacle to multiple mobile robot systems by introducing the concept of the Common Velocity Obstacle, and achieved cooperative collision avoidance among multiple mobile robot systems without supervisor or communication. The problem of the method was the enormous computation caused by the high dimensional search, and two improved CCA method are introduced in this paper. The first method transforms the search structure from a massive monolithic search into two separate light searches and this leads to significant reduction of the computation from O(a4 ) to O(a2 ), where a is the resolution of the search. The second method solves the search analytically by applying an assumption that every robot has a direction oriented objective. The effectiveness of the two methods are confirmed by the computer simulations. Keywords: Autonomous mobile robots, Robot navigation, Obstacle detection, Obstacle avoidance.

1. INTRODUCTION

The primary problem of the previous CCA method is the computational load that is invoked by the search of the optimal solution. In this paper, two improved methods that shrink the problem are introduced. The first method modifies the structure of the search, and this reduces the computation from O(a4 ) to O(a2 ), where a is resolution of the search. The second method solves the search analytically by applying some reasonable assumptions. The required computation for this method is small enough for the implementation. The organization of this paper is as follows. Section 2 summarizes related work, and section 3 briefly introduces fundamentals of the previous study. Section 4 introduces two improved CCA methods. In the following section 5, three CCA methods are demonstrated and the computer simulations are performed. Section 6 concludes the results and states future plans.

For mobile robot systems, collision avoidance is inevitable issue. Especially, consideration of other mobile robots is important, because methods that do not care of other robots easily lead to “inconsistency” among robots, such as deadlock and flapping. Our goal is to design a collision avoidance method for multiple autonomous mobile robot systems, where cooperation among robots is the most important issue. In a previous study, the authors have proposed the Cooperative Collision Avoidance (CCA) method (ABE and MATSUO, 2001; ABE and MATSUO, 2003). The method provides a scheme of implicit communication that allows cooperation without any communication. Besides, the method does not impose any restriction on velocity selection, thus mobile robots are not prevented from performing their original objectives. Instead of these advantages, the computational load of the method is huge, and this problem makes the CCA method impractical.

2. RELATED WORK In this section, some collision avoidance methods and corresponding viewpoints are summarized. Typ-

30

for some robot systems, however, the method may not suit for other robot systems.

ical keywords used here to distinguish some collision avoidance method from others are online, autonomy, communication, navigation subtask, etc. The navigation subtask means locomotive objective of mobile robot, such as migration to some point, moving toward some direction, wandering, and so on.

3. THE FUNDAMENTALS OF THE CCA METHOD In this section, firstly make the architecture of the CCA method clear, then brief derivation of the CCA method follows. As the CCA method is derived from the Velocity Obstacle (VO) method, introduction of the VO method is also given.

2.1 The Trajectory Oriented Method One of the most common collision avoidance method is the trajectory oriented method, where reference trajectories are determined at first without consideration of the dynamic obstacles, then collision avoidance against other robots are performed along the predefined trajectories (BROCK and KHATIB, 1999; ERDMANN and LOZANO-PEREZ, 1986; FRAICHARD and LAUGIER, 1993). The advantage of this method is that the resulting trajectories, hence the performance, can be expected in advance of the execution, and in most cases the performance is sub-optimal. The major problems are the hardness of the trajectory planning among multiple robots and the computational cost. The former problem is caused by the fact that every robots should be results in the same trajectory. The meanings of the latter problem is that the trajectory planning requires time consuming over time calculation, and this computation should be performed every time the situation has changed, for example some robot has changed its navigation subtask. Thus, the trajectory oriented method is appropriate for well-known stable environment, for example carrying robots in factories, however, the method is not suit for multiple autonomous mobile robot systems.

CCA Method Collision Decomposed CCA Method Avoidance Simplified CCA Method Full CCA Method CVO Method

Collision Detection

VO Method

Fig. 1. Architecture of the CCA method Figure 1 shows the architecture of the whole CCA method. The method consists of two parts, the collision detection and the collision avoidance. In our context, the collision detection is to examine whether some velocity causes future collisions or not, and the collision avoidance is to decide some velocity that can avoid future collisions. The Velocity Obstacle method (FIORINI and SHILLER, 1996) is a wellknown collision detection method, where the novel concept of the VO are proposed based on relative velocity between robots. The Common Velocity Obstacle (CVO) method is an extension of the VO method for multiple mobile robot systems, where all robots involving some collision can share the information of the collision without communication. The Full CCA (FCCA) method is a straightforward collision avoidance method derived from the CVO method, where crucial collisions that can not be solved by the VO based collision avoidance method are cooperatively proved. The Decomposed CCA (DCCA) method and the Simplified CCA (SCCA) method are derived for the low computation and detailed introductions are shown in the following section.

2.2 The Cooperative Methods The methods explicitly utilize cooperation are also common for multiple mobile robot systems, where communications or common rules (FUJIMORI, 2000) are often used. The communication based methods are appropriate for robot systems equipped with such capability, however, most robot systems are not. The common rule is a rule every robots should obey and effective to avoid collisions at any rate, however, functional collision avoidance considering navigation subtasks is hard to obtain. For example, although some robot prefers to go right according to its navigation subtask, the common rule may force to go left. In addition, either methods demand strict uniformity of mobile robots. As for the autonomous robot systems, such completeness is not good manner and should be eliminated.

3.1 The Collision Detection Method 3.1.1. The Velocity Obstacle Method Since detailed description of the VO method can be found in the original paper (FIORINI and SHILLER, 1993; FIORINI and SHILLER, 1996), only the results are introduced here. In fig. 2(a), Robot A and B are moving at velocities of VA and VB , respectively. Each robot’s shape is represented by a disk of radius R. As the coordinates of the fig. 2 are velocities, R should be written as R/Δt actually, however, R is used for simplicity. Robot A’s radius can be regarded as 0 by enlarging Robot B’s radius by Robot A’s actual radius. Clearly,

2.3 Summary There are variety of collision avoidance methods, however, no established method for multiple mobile robot systems has been proposed. One reason for this is that the diversity of the multiple mobile robot systems. Some collision avoidance method is appropriate

31

Robot A VB VA VAB

Robot A VA

Robot A R

VAB

VA

Velocity Obstacle for VA Robot AVB VA RVA RAVA

RVA Common Velocity Obstacle for VAB VOAB

VOB VOA

2R

VB

2R

Robot B R

(a) Setup

Robot B

Robot B VB

VOA

VA

VOAB

VB

Robot B

VB

Velocity Obstacle for VAB

(b) VOAB

RVB

Robot B VB VA

Velocity Obstacle for VB

Velocity Obstacle for VA

(a) VOA , VOB

(c) VOA

RVAB RAVAB Robot A VA VAB VB

(b) VOAB

Fig. 3. Introduction of the Common VO method

Fig. 2. Introduction of the Velocity Obstacle method

3.2.1. The Full CCA Method The Full CCA (FCCA) method is an evaluation function based method, where the navigation subtask, the VO and the RV are all described as functions of velocity. Then, overall evaluation function is given as summation of these sub functions, where the velocity that minimizes the function is regarded to be optimal. However, to obtain an optimal velocity is not a simple problem, because the function is not analytic in many cases. Accordingly, the FCCA method utilize the searching to obtain an optimal velocity.

this conversion does not affect the collision condition. Furthermore, viewing from coordinates fixed on Robot B, Robot A seems to be moving at a relative velocity of VAB = VA − VB . Hence, if VAB points at the hatched region in fig. 2(b) and both robots keep their velocities, Robot A and B will collide in the future. Thus the region is called a “Velocity Obstacle”. VOA in fig. 2(c) is the VO for velocity VA , which can be derived from VOAB by adding VB to VAB and VOAB . When Reachable Velocity RVA which means a feasible velocity set considering robot’s dynamics and kinematics is given, Reachable Available Velocity RAVA = VOA ∩ RVA yields possible collision avoidance velocities. The most problem of the VO method is that the method implicitly assumes that VB does not vary, and this assumption is not appropriate for the multiple mobile robot systems.

4. SIMPLIFICATION FOR IMPLEMENTATION 4.1 Problems of the Full CCA Method The FCCA method searches VA and VB simultaneously. Each velocity requires quadratic search, thus, combined search space of two velocities is 4 dimensional and the computational effort of the method is O(a4 ), where a is resolution of the search. As a result of a pilot study, it is found that the sufficient resolution leads to the massive computation, therefore the FCCA method can not be implemented on the real-time systems.

3.1.2. The Common Velocity Obstacle Method To vanish the above problem, the CVO method was introduced. Figure 3(a) shows VOA and VOB in the same velocity field. Noting that the arrangement of VOA for Robot A and that of VOB for Robot B are just symmetrical each other, both can be depicted as a single figure as fig. 3(b), by rotating and overlaying one onto the other. Then, the VO is renamed as VOAB and called “Common Velocity Obstacle”, because a collision will occur if VAB points inside of it. Collision avoidance can be regarded as that Robot A and B vary their relative velocity VAB cooperatively so that VAB points outside VOAB . The most notable feature of the method is that RVAB is the additive set of RVA and RVB and larger than RVA or RVB , and this feature is just a benefit of cooperation. Actually, the shape of RVA , RVB are more complicated and so is RVAB , however, this notable feature will still remain.

4.2 The Decomposed CCA Method To avoid fraught quartic search, the Decomposed CCA (DCCA) method divides the search into two quadratic searches. The first search is for VAB and the second search is for VA or VB . Once VAB is given by the first search (O(a2 )), combination of VA and VB can be determined by quadratic search. Thus, overall computational effort is O(2a2 ) and this is small enough for implementation as shown in the next section. The only issue of the DCCA method is how to determine VAB , because the sub function of the evaluation function does not give any suggestion for VAB . However, as the evaluation function for VAB can be arbitrary form, this is not so hard problem to guarantee just collision avoidance but performance. Example of the evaluation function is shown in the following section.

3.2 The Collision Avoidance Method As described above, neither the VO method nor the CVO method designate a certain collision avoidance velocity. Therefore, some means to determine the collision avoidance velocity are required. The navigation subtask is general name of such meanings corresponding to navigation, and sometimes called the goal or the objective.

4.3 The Simplified CCA Method To reduce the computation, the Simplified CCA (SCCA) method utilizes the analytical approach instead of the search.

32

5.2 The FCCA method

In the broad sense, the SCCA method may be considered as a rule based method, however, this understanding is not correct accurately. The classical rule based methods put collision avoidance ahead of of the navigation subtask and may lead to undesired results, e.g. deadlock, while the SCCA method can treat collision avoidance and the navigation subtask simultaneously and can overcome nasty situations. In return for this advantage, the design of the SCCA method is more complicated and versatility of the rule based method has been lost. As the SCCA is a general term for the analytical CCA method, detailed example will be given in the following section.

In addition to for navigation subtask, for the VO should be determined The entire evaluation function can be written as summation of the sub functions as follows: E(VA , VB , rA , rB ) = EV,A (VA , rA ) + EE,A (VA ) + EV,B (VB , rB ) + EE,B (VB ) + EVO (VA , VB ) Although considering weighted summation of sub functions is a curious issue, these are fixed in this paper. As mentioned in the previous section, evaluating every velocities inside RVA , RVB can find optimal collision avoidance velocities.

5. EXAMPLES AND SIMULATIONS 5.3 The DCCA method

In this section, examples of the three CCA methods are presented and examined using the computer simulations.

The difference between the FCCA method and the DCCA method is existence of the evaluation function for VAB . The following evaluation function is adopted here for the DCCA method. 2Vmax Erel (VAB , rA , rB ) = |VAB | − |∠VAB − (rA + rB )| π This function is designed heuristically, where the first term is to shorten time period of collision avoidance and the second term is derived from the navigation subtask. The coefficient of the second term − 2Vπmax is applied to normalize the ranges of the two terms. The DCCA method searches VAB inside RVAB with the evaluation function of

5.1 Common Items To compare the simulation results of the three CCA methods, the following items are common for all CCA methods. • Physical parameters of mobile robots. · Typical parameters are shown in table 1. Table 1. Configuration of simulations Notation Radius Max. Velocity Sampling Reaction radius Noise coefficient

Value 1.0 [m] 1.0 [m/s] 0.5 [s] 7.0 [m] 0.05

E(VAB , rA , rB ) = Erel (VAB , rA , rB ) + EVO (VAB ), and obtains optimal VAB , then searches VA inside RVA with the evaluation function of

• Initial location of mobile robots. · Orthogonal, face to face, and aslant orientation of the desired direction are examined, and robots are placed corresponding initial locations. • The navigation subtask of mobile robots. · The objective of mobile robots is to move toward some direction. The navigation subtask is given as desired direction and energy consumption and corresponding sub evaluation functions are the following: |V| · cos (∠V − r) Ev (V, r) = , Vmax |Vk − Vk−1 |2 , Ee (V) = − 2 Vmax where r is desired direction, Vmax is maximum velocity of a mobile robot, and k is an index of time step. • Noise model. · Measurement noises are applied at each time step. Assuming visual sensing, amount of the noise is determined by normal distribution of σ = Nl2 , where N is noise coefficient and l is distance between robots.

E(VA , rA , rB ) = EV,A (VA , rA ) + EE,A (VA ) + EV,B (VAB − VB , rB ) + EE,B (VAB − VB ), and this gives conclusive optimal VA , VB . 5.4 The SCCA method The FCCA method does not depend on any assumptions on the evaluation function, because only the search is used. On the contrary, the SCCA method requires some assumptions to solve the problem analytically. In this subsection, such simplification is demonstrated for example by applying the following assumptions: • Circular shape of the RV. • The navigation subtask is given as the desired direction. Figure 4 shows an example of the desired direction rA , velocity constraints RVA and VOA for Robot A. In this case, only two points of PB1 and PB2 are the optimal velocity candidates. Clearly, PC is always a possibly optimal velocity, however, PC is covered with the VO when collision is predicted and cannot be chosen as the optimal velocity. In addition, PB1 or PB2 always have better evaluations than PA , because if PA

33

is preferred than others, PC must not be covered with the VO and this means collision free. Therefore, only PB1 and PB2 are the optimal velocity candidates.

Case 1 Case 2 Case 3

1

Case 1 Case 2 Case 3

0.8

N = 0.01

0.7 0.6

0.9

rA

0.5 0.4

0.8 0.3

PC PB1

0.2 0.7

PB2

FCCA

DCCA

SCCA

PA

Case 1 Case 2 Case 3

1

FCCA

DCCA

SCCA

Case 1 Case 2 Case 3

0.8

N = 0.05

0.7

RVA

0.6

0.9 0.5 0.4

0.8

Fig. 4. Derivation of the RVD (Step 1)

0.3 0.2 0.7

Figure 5 shows the desired directions, RVs, and VOs of both robot A and B, considering the symmetry of their arrangements. As the apex of VOA corresponds to VB and that of VOB does to VA , and the optimal velocity candidates are PB1 and PB2 , VOA and VOB always keep contact with each other as long as a collision is occurring. Hence, the arrangement of two VOs is like either fig. 5(a) or fig. 5(b), where optimal VA corresponds to PB1 and PB2 in fig. 4, respectively. These figures mean that if VA or VB is given, corresponding VB or VA is determined uniquely and automatically and the velocity is the cooperative collision avoidance velocity. Thus, the choosing collision avoidance rA

FCCA

VA

(a) Case 1

DCCA

SCCA

(b) Energy Index

Figure 7 shows the two evaluations with respect to the three CCA method and three situations. Each point is the mean of 10 runs and error bar means the standard deviation. In each graph plot, the standard deviation of the SCCA method is larger than that of the FCCA method and the DCCA method, and this means the instability of the SCCA method. The poor result of the DCCA in case 1 is caused by inadequate evaluation of VAB , and fig. 6(b) indicates this, where slow velocities are usually selected. Figure 8 shows the computational time for simulations versus the search resolution of the FCCA method and the DCCA method. Simulated time period is 20 [s], and a computer used for the simulations is Pentium IV 2.4GHz and is 10 to 20 times faster than supposing robot systems of MMX Pentium 223MHz. Therefore, the search resolution for the FCCA method should be less than 5, however, the search resolution is not sufficient for better performance as shown in fig. 9. The SCCA method is free from the search resolution, and its computational time is 0.007 [s] and fast enough. The dashed lines depicted in fig. 8 are results of least square fitting of f (x) = axb + c, where b = 3.59 for the FCCA method and b = 1.99 for the DCCA method. As the estimated b is 4.0 and 2.0, respectively, this result means that the estimation of the computational effort is approximately correct.

VA VB

RVA /RVB

FCCA

Fig. 7. Evaluation

rB VB

SCCA

(a) Velocity Index

rA

rB

DCCA

RVA /RVB (b) Case 2

Fig. 5. Derivation of the RVD (Step 2) velocities problem is now proved by determining location of line l. The optimal location of l is easily obtained by simple but space consuming calculations, where the following condition exists: rA − ∠VA = rB − ∠VB .

3000 [s] 12 2500 10 8 2000 6 4 1500 2 0 1000

5.5 Simulation Results Computer simulations on two mobile robots are performed and evaluated in this section, where the setup each robot has a unique desired direction and the directions are crossing orthogonally with each other is considered. To evaluate collision avoidance methods is a difficult matter and an established index does not exist. In this paper, according to the navigation subtask, average velocity component along preferred direction during collision avoidance and the energy consumption are adopted as evaluation index. In computer simulations, the energy consumption can not be evaluated, the summation of impulse at each time step is used instead.

0.6

FCCA

DCCA

[s] 0.5 0.4 0.3

2

4

6

8

10 12 0.2

500 0.1

0 -500

0 0

5

10

15

20

25

30

35

40

(a) The FCCA Method

45

50

0

5

10

15

20

25

30

35

40

45

50

(b) The DCCA Method

Fig. 8. Computational Time vs Resolution

6. CONCLUSIONS To decrease the computation, the DCCA method and the SCCA method were introduced from the FCCA

34

6

6

[m]

[m]

4

4

4

Case 1

6 [m]

2

2

0

0

0

-2

-2

-2

-4

-4

-4

1

1

1

0.5

0.5

0.5

0

0

0

-0.5

-0.5

-0.5

2

-1 -6 -6

-4

-2

0

2

4

[m] 6

6

-6 -6

-4

-2

0

2

4

[m] 6

6

-6 -6

[m]

4

4

2

2

2

Case 2

[m]

4

0

0

0

-2

-2

-2

-4

-4

-4

-6

-4

-2

0

2

4

[m] 6

6

-6 -6

-4

-2

0

2

4

[m] 6

6

-6

[m]

4

4

2

2

2

0

0

0

-2

-2

-2

-6

4

[m] 6

-1 -1

-0.5

0

0.5

1

-4

-2

0

2

4

FCCA

[m] 6

-0.5

0

0.5

1

-4

-2

0

2

4

1

1

1

0.5

0.5

0.5

0

0

0

-0.5

-0.5

-0.5

-1

-1

[m] 6

-1

-0.5

0

0.5

1

-1

-0.5

0

0.5

1

-1

-0.5

0

0.5

1

-0.5

0

0.5

1

-1 -1

-0.5

0

0.5

1

1

1

1

0.5

0.5

0.5

0

0

0

-0.5

-0.5

-0.5

-1 -6

-1 -1

-4

-6

-4

-2

0

2

4

[m] 6

-6 -6

-4

DCCA (a) Trajectories

-2

0

2

4

[m] 6

SCCA

-1 -1

-0.5

0

FCCA

0.5

1

-1 -1

-0.5

0

0.5

DCCA (b) Velocities

1

-1

SCCA

Fig. 6. Resulting Trajectories and Velocities (N = 0.05) REFERENCES Case 1 Case 2 Case 3

1

N = 0.01

Case 3

[m]

-4

2

6

4

-6

0

-6

[m]

-4

-2

6

[m]

-6

-4

Case 1 Case 2 Case 3

0.6

ABE, Y. and Y. MATSUO (2001). Collision avoidance method for multiple autonomous mobile agen ts by implicit cooperation. In: Proc. IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems. ABE, Y. and Y. MATSUO (2003). Simplified cooperative collision avoidance method considering the desired direction as the operation objective of each mobile robot. In: Proc. Intl. Conf. on Control, Automation, and Systems. BROCK, O. and O. KHATIB (1999). High-speed navigation using the global dynamic window approach. In: Proc. IEEE Intl. Conf. on Robotics and Automation. Vol. 1. pp. 341–346. ERDMANN, P. and T. LOZANO-PEREZ (1986). On multiple moving objects. In: Proc. IEEE Intl. Conf. Robotics and Automation. pp. 1419–1424. FIORINI, P. and Z. SHILLER (1993). Motion planning in dynamic environments using the relative velocity paradigm. In: Proc. IEEE Intl. Conf. on Robotics and Automation. Vol. 1. pp. 560–565. FIORINI, P. and Z. SHILLER (1996). Time optimal trajectory planning in dynamic environments. In: Proc. IEEE Intl. Conf. on Robotics and Automation. Vol. 2. pp. 1553–1558. FRAICHARD, T. and C. LAUGIER (1993). Pathvelocity decomposition revisited and applied to dynamic trajectory planning. In: Proc. IEEE Intl. Conf. on Robotics and Automation. Vol. 2. pp. 40–45. FUJIMORI, A. et al. (2000). Cooperative collision avoidance between multiple mobile robots. J. of Robotic Systems 17(7), 347–363.

0.5

0.9 0.4

0.3

0.8

0.2 0.7 2

3

5

10

20 30

50

(a) Velocity Index

SCCA

2

3

5

10

20 30

50

SCCA

(b) Energy Index

Fig. 9. Evaluation of the FCCA Method vs resolution

method by reconstructing the velocity decision procedure and by applying straightforward assumptions. The two CCA methods were examined by computer simulations and their performances were evaluated using the average velocity and the energy consumption. The performances were approximately same as the FCCA, while the computational efforts were dramatically reduced from the FCCA. Consequently, the DCCA method and the SCCA method could be the real-time cooperative collision avoidance method, although the FCCA method were not. In the DCCA method, the 2-step search was introduced, where the evaluation function for VAB was utilized. This evaluation function considered only collision avoidance and the navigation subtask in this paper, however, capability of the function is not limited to these. For example, some classical rule based collision avoidance methods can be rewritten into the DCCA formula, because most rule based methods are based on relative relationship between robots and so is the evaluation function of the DCCA. The most problem of the DCCA method is that the method tends to be heuristic. Organized formulation of the method is interesting problem and will be future study.

35