A Cooperative Path Planning Algorithm for a Multiple Mobile Robot System in a Dynamic Environment

A practical path planning method for a multiple mobile robot system (MMRS) requires handling both the collision-free constraint and the kinematic constraint of real robots, the latter of which has to date been neglected by most path planning methods. In this paper, we present a practical cooperative path planning algorithm for MMRS in a dynamic environment. First, each robot uses an analytical method to plan an obstacle-avoidance path. Then, a distributed prioritized scheme is introduced to realize cooperative path planning. In the scheme, each robot calculates a priority value according to its situation at each instant in time, which will determine the robot's priority. Higher-priority robots can ignore lower-priority robots, whereas lower-priority robots should avoid collisions with higher-priority robots. To minimize the path length for MMRS, a least path length constraint is added. The priority value is also calculated by a path cost function that takes the path length into consideration. Unlike other priority methods, the algorithm proposed is not time consuming; therefore, it is suitable for dynamic environments. Simulation results are presented to verify the effectiveness of the proposed algorithm.


Introduction
As autonomous mobile robots are being deployed in increasingly complex scenarios, it is essential for a team of robots to be able to work in parallel and perform more complex tasks than can be accomplished by a single robot [1].A well-known and essential requirement for any MMRS is that the robots must be able to navigate safely through the environment in which the tasks are to be performed [2]; thus, proper path planning must be developed.
The goal of path planning is to find a continuous trajectory for a robot from the initial state to the goal state without colliding with obstacles, while maintaining robot-specific constraints [3].As a result of its importance, path planning has attracted much attention in the field of robotics.
The potential field method is a classic and useful method for path planning.In Reference [4], a potential field is utilized to represent the influence of complex-shaped obstacles.The influence of all obstacles is calculated by a weighted average of the circular obstacle potential fields.The potential field method is extremely fast and also striking, because of its mathematical elegance and simplicity.Nevertheless, a shortcoming of this method is the existence of local minima in the potential function, which cause the robot to become trapped.In addition, the optimality of this method is not assured [5].The grid-based method combined with the A * algorithm is also considered a good choice for path planning [6].This method uses a grid-based representation of the environment.The A * algorithm can then be used to solve grid-based path planning problems [7].The algorithm uses a heuristic concept based on the Euclidean distance to search for the shortest paths.Alternatively, the breadth-first search is a graph search algorithm that is also suitable for grid-based path planning.This algorithm begins at the root node and explores all neighbouring nodes, then explores all neighbouring nodes of each nearest node.Grid-based methods can effectively represent the environment; however, a shortcoming of grid-based path planning is that higher precision requires more computation, making this method unsuitable for complex environments [8].
The probabilistic roadmap method is also a popular path planning scheme for mobile robots.This method uses a sampling technique to discover a sparse representation of obstacles in a configuration space.Reference [9] makes use of a probabilistic roadmap to avoid occlusions of the target and any obstacles.The probabilistic roadmap method is easily implemented and can be applied to complex environments.However, this method may fail when the environment does not have a sufficient number of free points with which to construct a probabilistic map.
There are also many heuristic methods that can be used for path planning, such as rapidly exploring random trees [10], neural networks [11], genetic algorithms [12], simulated annealing [13], ant colony optimization [14], particle swarm optimizer and fuzzy logic [15,16].These methods can yield feasible schemes; however, their optimality cannot be assured with any of the above-mentioned methods.
Mobile robots are usually non-holonomic systems; on the other hand, most path planning methods are merely developed to address geometrical constraints, while neglecting kinematic ones.However, Reference [17] presents a feasible algorithm for addressing kinematic constraints.
In Reference [17], a new analytical solution to mobile robot path planning is proposed.This method can provide a real-time collision-free path for a car-like mobile robot moving in a dynamically changing environment.Based on the explicit consideration of the robot's kinematic model and the collision avoidance condition, a family of feasible trajectories and their corresponding steering controls are derived in terms of one adjustable parameter.This method exhibits excellent performance with respect to precision and real-time ability for mobile robot path planning.
All of the abovementioned methods concern path planning for single robots, which in turn form the basis of path planning for MMRS.The MMRS path planning problem can be defined as follows: given a set of m robots in a k-dimensional workspace, each robot has an initial starting configuration (e.g., position and orientation) and a desired goal configuration; thus, the path should be planned for each robot to reach its goal and avoid collisions with obstacles, as well as other robots in a given workspace [18].
The difference between single robot path planning and MMRS path planning is that various planning approaches are incorporated in the latter to handle conflict between autonomous robots.The path planning approaches for MMRS can mainly be categorized into two classes [19].
The first class is referred to as coupled centralized approaches [20], which apply global information and plans directly.These approaches treat an entire team of robots as a single composite system, to which the classical single-robot path planning algorithms are applied.These approaches pursue both optimality and completeness; thus, they are too computationally intensive to use in practice.
The second class is referred to as decoupled approaches [21].These approaches can find good solutions quickly, but at the cost of losing optimality and completeness.Decoupled approaches are typically further sub-divided into two broad categories: path coordination and prioritized planning.
Path coordination methods first plan independent paths for robots separately; then, they seek to plan the robots' velocities to avoid collisions along these paths [22].In path coordination, the area the robots travel across is treated as a shared resource.The decomposition of path planning and velocity planning incorporates an additional time dimension into calculations.
In prioritized planning approaches, different priorities are assigned to each robot [23], either randomly or determined by motion constraints; for example, more highly constrained robots are given higher priority.The sequence of path planning in MMRS is then determined by the priority.This method can prevent collisions between robots.The advantage of prioritized planning approaches is that they reduce a single planning problem in a very high-dimensional space into a sequence of planning problems in a much lower-dimensional space.In this approach the paths must be computed in a sequence and the sequence is determined by the priority.The computation will last until the robot with the lowest priority plans its path.Thus, it can be observed that the disadvantage of the prioritized planning approach is time consumption.
In this paper, we reconstruct an analytical path planning method to accommodate multiple mobile robot systems with the assistance of the prioritized planning scheme.A practical and cooperative analytical path planning algorithm is presented for MMRS.This algorithm is completely distributed and each robot plans its path by utilizing local information.To realize cooperative path planning, a distributed prioritized scheme is introduced.First, each robot calculates a priority value according to its situation at each instant and then the priority value is sent to the other robots.The robot's priority is then determined to realize cooperative path planning.Higher-priority robots can ignore lower-priority robots, whereas lower-priority robots should regard higher-priority robots as obstacles.After a confirmation of each robot's priority, the robots plan their paths using the basic analytical path planning method, while taking into consideration the path length constraint.To minimize the path length for MMRS, the priority value is also calculated by a path cost function that takes the path length into consideration.
A traditional priority scheme is time consuming; computation will last until the robot with the lowest priority has planned its path.The priority scheme used in this paper overcomes the time-consumption issue.That is because once the priority has been ensured, the path will be planned in parallel, thereby accelerating the algorithm's execution.
Although only a car-like mobile robot is used in this paper, the cooperative analytical path planning method is also suitable for mobile robots with other kinematic models.
The rest of the paper is organized as follows.Section 2 describes the kinematic model of the car-like non-holonomic mobile robot.Section 3 describes the analytical path planning algorithm for one non-holonomic mobile robot.The formulation of path planning for multiple mobile robot systems is described in section 4.Then, the prioritized path planning method for MMRS is presented in section 5.The experimental results are presented in section 6. Section 7 concludes the paper and outlines future research.

Kinematic Model of the Car-like Non-holonomic Mobile Robot
In this paper, the kinematic model of a car-like mobile robot is used.The main feature of the kinematic model of the car-like mobile robot is its non-holonomic constraints, because a rolling-without-slipping condition exists between the wheels and the ground.
The generalized coordinates of the car-like robot model is shown in Figure 1, where the rear wheels are driving wheels and the front wheels are steering wheels.The radius of the wheels is ρ .The car-like robot is round; O is the centre, where the Cartesian coordinate of O is (x, y) and the radius of the car-like robot is Ro.Point W is the midpoint of the rear wheel axis and point V is the midpoint of the front wheel axis.l is the distance between M and V. θ measures the orientation of the car body with respect to the x -axis.ϕ is the steering angle.Let 1 u be the angular velocity of the driving wheels and 2 u be the steering rate of the front wheels; thus, the kinematic model for the car-like robot can be obtained by equation ( .

Figure 1. Generalized coordinates of the car-like robot
There is a model singularity at , where the car may become jammed.It is supposed that . The importance of this singularity is limited due to the restricted range of the steering angle of ϕ in most practical cases.
To reveal the potential linear characteristic of the kinematic model, the model can be translated into the chained form as follows: where From the chained form shown in (2), it can be observed that the chained form has a strong underlying linear structure, which is nonlinear.

Analytical Path Planning for One Non-holonomic Mobile Robot
Analytical path planning takes into consideration both the boundaries and the obstacle-avoidance criterion and then describes the feasible paths by sixth-order piecewise-constant polynomials.Thus, the path planning problem is transformed into a problem of calculating the parameters of the polynomials.

Feasible Path Planning without Obstacles
If there is no obstacle, the feasible path can be described as follows: Without taking the obstacles into consideration, the feasible path only has to satisfy the initial boundary condition x y θ ϕ at time 0 t and the goal boundary condition x y θ ϕ at the end time f t .
According to (2) and the boundary conditions, the boundary states can be obtained.

Obstacle-Avoidance Criterion
The environment in which the mobile robots move contains obstacles; thus, the collision avoidance criterion should be considered during path planning.
The obstacles are assumed to be dynamic and the i th obstacle's coordinates are ( ( ), ( )) ) , where s T is the sampling period.
During this period, the i th obstacle is moving at a known velocity , , [ , ] Thus, the obstacle-avoidance criterion is expressed as follows: The velocity of the mobile robot along the x-axis is assumed to be a constant C, which can be calculated as follows:

Procedure for Feasible Path Planning in a Dynamic Environment
The function 4 1 ( ) z F z = is chosen to be a six-order polynomial.Thus, the function where k a is a constant vector to be determined.
Therefore, the path can be calculated analytically by adopting the three following procedures and explicit proof of the procedure can be found in [17].
Step 1: utilize the initial boundary condition and the goal boundary condition to represent the feasible paths in terms of the adjustable parameter 6 k a .
First, the boundary conditions 0 z and f z should be ensured and the chained form should be obtained.
Then, during time interval a a a a a can be determined by 6 k a .

The World Model for the Mobile Robot in MMRS
It is assumed that in MMRS, there are many robots moving in a two-dimensional environment.All of the robots have to avoid colliding with both moving obstacles and other robots.Each robot has limited sensing and communication abilities; thus, not all of the information about the environment can be obtained.Usually, information about obstacles and other robots is essential for any robot to plan its path; therefore, the world model of the mobile robots mainly maintains information about the obstacles in the environment and the robots.
Suppose that there is a team of moving robots where j o q is the position of the j th obstacle in the set S i B and i q is the position of i R .
where l q is the position of l R , {1,..., } l n ∈ and l i ≠ .
Usually, the communication range of one robot is greater than its sensing range.
where i B is the set of all obstacles and robots in the i th robot world model that need to be considered during path planning.  19and robots 2 R and 3 R satisfy equation ( 20) and ( 21), the world model of 1 R only maintains information about the objects as shown in Figure 3.
Assumption 1: in order to facilitate the avoidance criterion, the robots and obstacles are represented by circles.
Assumption 2: all of the robots exhibit accurate self-localization and can exhibit accurate obstacle localization.• Satisfying the goal configuration for each robot, such as the position and orientation at the goal point.
goal i c is the goal configuration for robot i r .
• Avoiding collisions with obstacles.
• Avoiding collisions with other moving robots.
• Minimizing the total path length of the MMRS.

Prioritized Path Planning Method for Multiple Mobile Robots
The cooperative analytical path planning algorithm is a decoupled strategy.Given a team of robots, each robot can be assigned a priority value that can determine its priority.When planning the path, a robot with high priority can neglect robots with lower priority, whereas a robot with low priority should avoid robots with higher priority.This scheme can coordinate the paths of robots and prevent collisions between robots.
For the cooperative analytical path planning algorithm for MMRS, the priority has a serious effect on the quality of the planning path; thus, the priority should be determined appropriately.In this paper, a priority function is proposed to calculate the priority value.This priority value is transmitted to all robots and priority is then determined through the distributed, prioritized planning scheme.

The Calculation of Priority
A priority function is proposed to calculate the priority value of the mobile robots.When robot i R is located at point p, the priority value can be calculated by equation ( 23): where ( , ) f p i is the path cost of the i th robot when it is located at point p; ( , ) h p i is the cost of moving from the initial point 0 0 ( , ) x y to point p; ( , ) g p i is the cost of moving from point p to the goal point f f ( , ) x y .In this function, the effect of orientation is neglected.The path cost can be measured by the path length.

Distributed Prioritized Planning Scheme for MMRS
Each robot utilizes local information to calculate a priority value by (23).Then, each robot has to verify the order of the priority.The order will determine the way each robot coordinates path planning with other robots.
Assume that the mobile robots within the world model have complete communication.The robots exchange messages , , , , where t i p is the priority value of the i th robot at time t.The principle for determining the order of the robot's priority t i D can be described as follows: Where t i B means the number of obstacles in i B at t.

The Constraint for Minimizing the Path Length of Multiple Mobile Robot Systems
Suppose a path for robot i needs to be planned from time t to time tf.The planned path is described by (26).f ( ) The function F has a continuous derivative and the element of the arc length can be expressed as follows: where i t L is the path length of the i th robot planned at time t.
Thus, the length of the path can be calculated by the definite integral of the function F: Hence, let m be a function that defines all path lengths of the multiple mobile robot system: 1 ( ) Therefore, one objective of path planning for multiple mobile robot systems is minimizing the function m.
Based on equation (29), we can observe that only centralized planning can resolve this optimization problem.Because centralized planning can create heavy communication and computation loads, the constraint for minimizing the path length of multiple mobile robots can be divided into n sub-constraints: Therefore, during the path planning of each robot, both the path length constraint and the criterion for avoiding dynamic obstacles and other robots should be taken into consideration.

Cooperative Analytical Path Planning Algorithm for MMRS
In the cooperative analytical path planning algorithm, the analytical path planning method is combined with the distributed priority scheme to obtain a new multiple mobile robot path planning method that can plan sub-optimal obstacle-avoidance paths in real time.
In the cooperative analytical path planning algorithm, each robot uses the same planning scheme at each instant.First, the initial and goal conditions should be verified; then, the path from the current time position to the goal position is determined and each robot will move along its planned path at this time.At the next time interval, the boundary condition is modified and a new trajectory that can also satisfy the obstacle-avoidance criterion is planned.The flow of the cooperative analytical path planning algorithm for MMRS can be illustrated by the i th robot path planning at time  23) and then determine the order of the priority value through the communication between the robots within their limited communication range.3 Determine which robots can be regarded as obstacles and which robots can be ignored.Then, apply the obstacle-avoidance criterion to determine the range of the constant 6 k a of the i th robot.
4 Choose the best 6 k a that can satisfy the constraint for minimizing the path length.5 Determine the feasible path and calculate the steering inputs.

Proof for the Effectiveness of the Cooperative Analytical Path Planning Algorithm
Theorem 1: if a multiple mobile robot system has complete communication, the cooperative analytical path planning algorithm can produce paths that can prevent collisions with moving obstacles and other robots according to the path length constraints.
Proof: the proof follows the rules of logical induction.
For a team consisting of only one robot, it is clear that the robot can produce the shortest path and avoid collisions with any moving obstacles.
Consider a team of mobile robots 1 { ,..., } n R R R = composed of n autonomous mobile robots.Each robot can avoid collisions with obstacles using the analytical path planning method.At time t, the i th robot calculates a priority value i t p ; suppose the order of the priorities satisfies equation (31) and that the priority can be determined according to this equation....
Robot n R has the lowest priority; therefore, the path generated by n R will prevent collisions with all robots The priority of robot therefore, the path generated by The rest of the paths can be deduced by analogy up to robot 1 R .
Robot 1 R has the highest priority; therefore, it can ignore all other robots and plan its path according to the path length constraints. 1 R does not have to account for collisions with other robots because all other robots will actively avoid it.
Therefore, it can be observed that if priority can be transmitted through the entire multiple mobile robot system, each robot will utilize the distributed prioritized planning scheme to produce a feasible path that can prevent collisions with moving obstacles and other robots.
It is worth mentioning that although each robot plans its path according to the path length constraints, the path length of the MMRS is not guaranteed to be the shortest.
The path length constraint can only ensure that each robot can produce the shortest path when it treats other robots with higher priority as obstacles.Thus, the algorithm presented in this paper is a sub-optimal algorithm.

Simulation Results
To verify the effectiveness of the proposed algorithm, we performed a number of simulations.The simulations were carried out on an HP computer with 4G EMS memory and an Intel Core i2-2120 CPU.
Suppose that there are two robots moving within the same two-dimensional environment where three mobile obstacles exist.
In the simulations, the parameters of the robots were set as follows: For Robot 1, Ro1=1, l1=0.8, The initial condition is The final condition is For Robot 2, The initial condition is The final condition is 24, 9, / 4, 0 Assume that there are three obstacles moving within the environment.All of the obstacles are assumed to be circular and their radii are all set to 0.5, and each obstacle moves at different velocities that can change dynamically.The initial position of obstacle 1 is (5, 0).The initial position of obstacle 2 is (9, 4).The initial position of obstacle 3 is (19,10).The velocity of each obstacle in the simulation is shown in Table 1.
At each instant, a new path will be planned for the robot according to the initial position, the goal position and the current state of the environment.It can be seen in Figure 4 that the initial position at each time instant changes, so a new path will be planned for each robot according to the current situation.
To illustrate this characteristic, the planned paths of two robots at six instants are shown in Figure 4.At the first instant, Robot 1 and Robot 2 plan their paths according to the information obtained at time 0. The planned paths can satisfy both the current and goal conditions without colliding with the three obstacles or the other mobile robots.
At the second instant, the obstacles' states change and thus, Robot 1 plans its path again to avoid colliding with obstacle 1.At the third instant, there may be a conflict between Robot 1 and Robot 2. According to equation ( 23), the priority value of Robot 1 is greater than that of Robot 2; therefore, during path planning, Robot 1 can ignore Robot 2, but Robot 2 needs to change its path to avoid colliding with Robot 1.
At the last three instants, Robot 1 and Robot 2 alter their paths to accommodate the movements of the three obstacles.
Briefly, at each time instant, the algorithm proposed in this paper is utilized to plan a new path from the current position to the goal position, and the path planned at the last instant is the true path along which the robots move.
The path planned at the last instant can be called the final path of the robot.
As shown in Figure 4, it is not immediately clear that there are no conflicts between robots and obstacles.To demonstrate the effectiveness of the algorithm, the final paths of the robots and obstacles are depicted as three-dimensional figures.In this paper, a practical and cooperative path planning method for MMRS that can produce sub-optimal or optimal obstacle-avoidance paths in real time within a dynamic environment is presented.This algorithm appropriately combines the analytical path planning method with a priority scheme.The priority scheme used in this paper does not require significant computation; thus, it can accommodate the dynamic environment.
The cooperative path planning algorithm requires accurate robot self-localization and target localization.The paths generated by inaccurate localization information may create conflicts.However, for most real robots, whose navigation systems usually rely on vision systems or laser sensors, accurate self-localization and target localization are hard to accomplish.Therefore, in future studies, we will explore how to modify the algorithm proposed in this paper to tolerate certain localization errors in a bid to expand application of the algorithm. where

B
represents the set of obstacles that can be sensed by i R .All obstacles in set S i B should satisfy equation(19).

B
is the set of perceptible robots of i R .Information regarding C i B can be acquired through the robot's sensing or communication abilities; therefore, all of the robots in the set C i B should satisfy equation(20) and equation(21). c

Figure 2 . 1 R . Because only obstacles 1 B and 2 B
Figure 2. All of the mobile robots and the dynamic obstacles surrounding R0

1
to the path length constraint.
that satisfy the path length constraint.

Table 1 .Figure 4 .
Figure 4.The paths of Robot 1 and Robot 2 at six instants

Figure 5 (
Figure 5(a) is a normal three-dimensional figure that shows the paths of Robot 1 and the three obstacles from time 0 to time 40. Figure 5(a) is rotated to obtain Figure 5(b) and Figure 5(c).Figure 5(b) shows that there are no conflicts between Robot 1 and obstacle 1 or between Robot 1 and obstacle 3.Figure 5(c) demonstrates that there are no conflicts between Robot 1 and obstacle 2.

Figure 5 (
Figure 5(a) is a normal three-dimensional figure that shows the paths of Robot 1 and the three obstacles from time 0 to time 40. Figure 5(a) is rotated to obtain Figure 5(b) and Figure 5(c).Figure 5(b) shows that there are no conflicts between Robot 1 and obstacle 1 or between Robot 1 and obstacle 3.Figure 5(c) demonstrates that there are no conflicts between Robot 1 and obstacle 2.

Figure 5 (Figure 5 .
Figure 5(a) is a normal three-dimensional figure that shows the paths of Robot 1 and the three obstacles from time 0 to time 40. Figure 5(a) is rotated to obtain Figure 5(b) and Figure 5(c).Figure 5(b) shows that there are no conflicts between Robot 1 and obstacle 1 or between Robot 1 and obstacle 3.Figure 5(c) demonstrates that there are no conflicts between Robot 1 and obstacle 2.

Figure 6 (
Figure 6(a) is a normal three-dimensional figure that shows the paths of Robot 2 and the three obstacles from time 0 to time 40. Figure 6(a) is rotated to obtain Figure 6(b) and Figure 6(c).Figure 6(b) shows that there are no conflicts between Robot 1 and obstacle 1 or between Robot 1 and obstacle 3.Figure 6(c) demonstrates that there are no conflicts between Robot 1 and obstacle 2.
Figure 6(a) is a normal three-dimensional figure that shows the paths of Robot 2 and the three obstacles from time 0 to time 40. Figure 6(a) is rotated to obtain Figure 6(b) and Figure 6(c).Figure 6(b) shows that there are no conflicts between Robot 1 and obstacle 1 or between Robot 1 and obstacle 3.Figure 6(c) demonstrates that there are no conflicts between Robot 1 and obstacle 2.

Figure 6 .
Figure 6(a) is a normal three-dimensional figure that shows the paths of Robot 2 and the three obstacles from time 0 to time 40. Figure 6(a) is rotated to obtain Figure 6(b) and Figure 6(c).Figure 6(b) shows that there are no conflicts between Robot 1 and obstacle 1 or between Robot 1 and obstacle 3.Figure 6(c) demonstrates that there are no conflicts between Robot 1 and obstacle 2.

Figure 7 (Figure 7 .
Figure 7(a) is a normal three-dimensional figure that shows the paths of Robot 1 and Robot 2 from time 0 to time 40. Figure 7(a) is rotated to obtain Figure 7(b).Figure 7(b) clearly shows that there are no conflicts between Robot 1 and Robot 2. ] [ , , , , ] Calculate the priority value of the i th robot according to equation (