#### Abstract

In our previous work, we proposed a potential field-based hybrid path planning scheme for robot navigation that achieves complete coverage in various tasks. This paper is an extension of this work producing a multiagent framework, Collaborator, that integrates a high-level negotiation-based task allocation protocol with a low-level path planning method taking into consideration several real-world robot limitations such as nonholonomic constraints. Specifically, the proposed framework focuses on a class of complex motion planning problems in which robots need to cover the whole workspace, coordinate the accomplishment of a task, and dynamically change their roles to best fit the task. Applications in this class of problems include bomb detection and removal as well as rescuing of survivors from accidents or disasters. We have tested the framework in simulations of several tasks and have shown that Collaborator can satisfy nonholonomic constraints, cooperatively accomplish given tasks in an initially unknown dynamic environment while avoiding collision with other team members. Finally we prove that the proposed control laws are stable using the Lyapunov stability theory.

#### 1. Introduction

Motion planning is a fundamental and important issue in robotics. A special type of path planning includes tasks such as demining [1] and searching for victims after a tragedy, which are extremely crucial jobs and require complete coverage in the process of exploration. If time allows, robot teams should perform a thorough search and cover every square foot of the workspace. While covering too much area is not a concern, insufficient coverage could result in disastrous consequences. Similar criteria apply to many other robotic applications such as vacuum robots and land mine detection. In some more complex scenarios, robots are often interrupted by other tasks. For example, in jobs such as bomb detection and removal [2], or rescuing of survivors from accidents or disasters [3], search is the typical mode. However when a robot discovers an object, such as a survivor, its search phase is paused and then it attends to its rescue task. It can only resume searching after the rescue task is finished. These tasks require some sort of *role change* such as the transformation from rescue to search to successfully complete their mission. Moreover when the task cannot be accomplished by a single robot, coordination or task allocation issues arise. In this instance, robots may negotiate to solve the issues and distribute the tasks among themselves.

The robotics literature contains a number of motion planning strategies for this class of tasks using neural networks, fuzzy logic, approximate cellular decomposition, exact cellular decomposition, and artificial potential fields. Most prior work focuses on either high-level task allocation or low-level task execution. Few works combine task allocation with potential-field based motion planning to provide a holistic solution. Parker [2] provides a behavior-based architecture that facilitates fault tolerance: the effectiveness of the architecture is demonstrated through a team of mobile robots performing a laboratory version of hazardous waste cleanup. Gerkey and Mataric [4] presents an auction-based task allocation, which can achieve a distributed approximation to the global optimum for resource usage. The primary contribution of the work is to empirically demonstrate that the distributed negotiation mechanism is viable and effective for coordinating a physical multirobot team. [5] showed that the coordination of reactive robots can be obtained through the exploitation of local interactions; however, it basically remains at the level of task allocation and does not propagate to the level of task execution. In our previous work, a novel hybrid navigation scheme is proposed for a multiagent team aiming to bridge the gap between high-level task coordination and low-level path planning [6]. This work did not focus on low-level path planning, assumed ideal working conditions, and did not take into account moving obstacles and nonholonomic constraints. Those aspects of low-level path planning however, are critical to a physical implementation. In a real-life work environment, moving obstacles are often inevitable. For instance, cleaning robots often find themselves in situations where they need to avoid collision with people going about their own business. Moreover, unlike simulated robots, many physical robots can only move forward and backward, subject to nonholonomic constraints, similar to that of vehicles. In this paper, we extend our previous results to a more realistic environment considering each single constraint and both constraints simultaneously.

For a complex system such as the one previously described, the central objective is to achieve system stability. Systems are considered stable when convergence to the global minimum has occurred, which can only be achieved when the entire task is completed. Robots often need to work in an environment which is dynamic, noisy and unpredictable. Nevertheless, they should be reliable in their ability to finish the task despite all of the distractions including the intervention of moving obstacles. This guarantee of convergence is necessary for critical tasks such as rescuing victims after a disaster. Stability for single agent navigation in a static environment can be achieved by constructing a simple Lyapunov function. However, stability for multiagent team navigation in a dynamic environment is inherently more difficult due primarily to the interaction among agents and the presence of moving obstacles. In this paper, we propose a cooperative hybrid navigation scheme and construct a mode-specific team Lyapunov function, which shows that the system is stable at all times by using arguments from hybrid systems theory and Lyapunov stability theory.

The rest of the paper is divided into a number of sections that address individual aspects of this problem. In Section 2, we describe the hierarchical agent design that is the basis for this work. Section 3 presents a potential field-based hybrid navigation scheme. In Section 4, a motion control law is derived and Section 5 extends the framework to environments with moving obstacles. Section 6 incorporates nonholonomic constraints in the agent design, while Section 7 considers the robots with both nonholonomic constraints and moving obstacles. Section 8 presents simulation results for a multirobot system subject to nonholonomic constraints in a dynamic environment. In Section 9 team stability is analyzed. Finally in Section 10, we make concluding remarks and discuss future research directions.

#### 2. Agent Design

The multilayer agent architecture used in this work is depicted in Figure 1. Each of the robots in the team use the same architecture and there is no centralized control for the team. The highest level is the task coordination or knowledge level. At this stage of abstraction, agents analyze and manage tasks using an inherent coordination mechanism. The middle level entails path planning, where agents choose the suitable navigation function to pursue ideal objectives based on the accumulation of knowledge or team decisions. Lastly, the lowest level interacts with an actuator and sensors to govern motion control.

Our agent team is completely decentralized, where there is no leader or coordinator for the team and each agent analyzes and makes navigation and coordination decisions autonomously based on its own knowledge of the world. This design is inherently fault tolerant and easy to reconfigure in the event that task requirement changes [6].

Our previous work describes the agent architecture in detail and how it is used to complete high-level tasks [6]. Here we provide a brief description of its main components and how they assist with low-level path planning. The architecture of a single agent consists of four hierarchical layers: *DecisionMaking (DM)*, *KnowledgeBase (KBase)*, *Interaction* and *Communication*. In the following paragraphs, we define the functionality of each layer as well as a the interface between the layers. For each of these layers, the control software is multithreaded, with each main object running in its own thread.

The *Decision Making (DM)* layer is the main source of intelligence in the System. It performs navigation planning based on the information stored in the *Knowledge Base* using the hybrid approach described in [6]. At any point in time a robot can be in one of a finite set of modes, which represent high-level navigation goals. A rule-based system is used to transition between the modes based on the current state of the task. The modes define the navigation goals for the low-level path planning system.

The *Knowledge Base* contains the agent's most recent knowledge about the world map, the “group knowledge” as well as the inference rules for coordination. This information includes the location of all known obstacles and any goals that have been located. The information in the *Knowledge Base* is built up from the robot's own experience, plus that of the other robots it is communicating with.

Devices at the *Coordination* layer, discussed in [7] separate the interaction tasks from the main periodic motion planning task. The interaction between agents is performed using an unambiguous communication protocol [7]. We define interfaces between the interaction devices and both the *Knowledge Base* and *Communication* layers. At the *Interaction* layer, devices perform coordination as well as other interaction tasks and store the results in the *Knowledge Base*, without interfering with regular decision making. As a result, it is a simple matter to add new interaction devices if necessary for new applications.

In summary, the higher levels of the architecture provide the locations of the obstacles and goals which are the input to the low-level path planning system. The higher level ensures that the whole space is searched and all the intended tasked are completed, while the lower level deals with navigating robots from their current locations to goal locations.

#### 3. Potential Field-Based Navigation Scheme

A local minimum free potential field can be constructed using generalized spherical potential functions or harmonic functions [8]. In this paper we use a two-dimensional generalized Gaussian function which is much easier to construct and more effective for obstacles whose protective areas can be superscribed by circles without overlapping.

Attractive objects centered at are represented by the negative Gaussian *attractor* function:

Repulsive obstacles and other agents centered at are modeled with the circular, two-dimensional generalized Gaussian *repulsor* functions:

For some positive integer . The variance, , is a measure of the size of the obstacle. The parameter determines the *effective range (steepness)* of the obstacle and can be varied to modify the obstacle's repulsive effect. We represent obstacle shapes with a superscribed circle.

By specifying a minimum clearance between obstacles and highly localizing the influence of the obstacles through modifications to the parameter , generalized Gaussian function modeling can be used to construct potential fields that are free of undesired local minima. Figure 2(a) uses a small and there are local minima among the obstacles, by changing to a larger value and thereby highly localizing the effects of the obstacles, local minima disappear in Figure 2(b).

**(a)**

**(b)**

In [6], we proposed a mode-switching technique, where mode switches are based on events such as unsearched sectors decreasing and the number of found objects/obstacles increasing. Using the technique of artificial potential fields, we construct a *navigation function*, for agent in mode (where can be any of the modes defined above). To construct the navigation function for a given mode, we use the three part formula:

In (3), represents the sum of the effects on agent of all the attractors in the system during mode , and thus

represents the sum of the effects on agent of all the known static obstacles in the system during mode . The number of known static obstacles is ; so we can state this as

Finally, the functions represent the repulsor functions between pairs of agents and . Note that is not mode dependent, since the number of agents is assumed to be constant. Thus, in general

#### 4. Motion Control Law

Using the technique of artificial potential fields, we construct a *navigation function*, for each agent in mode (where can be any of the modes defined above). The (mode-dependent) kinematics of each agent are then given by:

In (7), the operator represents the gradient of with respect to only . Thus, we use a gradient descent method to generate a unit vector direction for and the constant velocity parameter to determine agent speed. We use a unit gradient since our Gaussian-like potential functions decay very rapidly.

Note that the gradient decent method gives the direction of motion towards the goal, but the speed of this motion, represented by , is a function of the robot design. That is, each robot has a maximum speed that must be accounted for by the navigation system.

#### 5. Extension to Environments with Moving Obstacles

Moving obstacle avoidance is of great importance in robotics research and is inherently harder than dealing with only static obstacles. Many works have been devoted to this problem [9–12]. The most related work in literature is presented by Esposito and Kumar in [12]. In that work, Esposito and Kumar propose a nonlinear programming method for computing optimal feasible directions for the mobile robots. By moving along feasible directions instead of the negative gradient direction, robots can successfully avoid all the moving obstacles while retaining a stable trajectory. In other words, the optimal direction decreases the potential function value most rapidly within the feasible direction set. It is not optimal in term of time or shortest distance though. Although that method is effective in many cases, it may fail to converge for some steps, especially in cases where the number of moving obstacles increases (i.e., more nonlinear constraints need to be considered). Also this method is computationally expensive due to the iterations in the optimization process (please refer to [13]).

Our algorithm uses geometry transformation to solve the moving obstacle avoidance problem. Similar to the work of Esposito and Kumar, this algorithm achieves moving obstacle avoidance by finding optimal feasible solutions. However, there is no approximation in the proposed method and only several geometry transformations are involved even for the most complex scenarios; so it can achieve accurate solutions with less computation. More importantly, the proposed method can guarantee to find an optimal solution when there is one.

Another advantage of the proposed method is that it is easier to incorporate the algorithm into various motion planning algorithms. For example, we will show in Section 7 that the proposed algorithm can be easily incorporated into the nonholonomic robot's motion planning [14] and achieve moving obstacle avoidance.

We associate with each moving obstacle a dynamic constraint [12] of the following form:

where is the position of the moving obstacle , and is the minimum safe distance between the robot and the moving obstacle.

When moving obstacles are far away from robots, they are ignored by the planning algorithm. When the robot moves within the threshold distance of a moving obstacle, the constraints become activated, and robots move along the feasible motion directions. If the path is totally blocked by obstacles (static and moving), the robot will simply halt until it is cleared, which is the same strategy that is used in [15].

In the context of moving obstacle avoidance, *feasible motion directions* denoted by are the directions that can stabilize the system and move robots away from the activated moving obstacles and thus satisfy the following conditions:

where represents the gradient of the constraint and is the negative gradient direction of the navigation function of robot . In (9), we require any control which can stabilize the system to have a positive projection on (moving towards the goal). In (10) we require any control which can move the robot away from the moving obstacle to have a positive projection on (moving away from the moving obstacles).

Theorem 1. * The feasible motion direction set can be constructed only using the robot gradients of the navigation function and the constraints for moving obstacles. (Please see the appendix for the proof.) *

In our setting, the robot will move along the optimal feasible direction calculated from the constructed feasible set, where the direction is optimal in the sense that it is closest to the negative gradient direction. We will show later in this section that the resulting motion directions are the same as the optimization method in [12] when the optimization method converges. Recall that the set is the set of feasible directions in the following form of a rotation angle with respect to the negative gradient direction . From basic geometry (refer to Figure 13), we can see that there will be no feasible solution if . If there exists a feasible solution and is in the feasible set, then is the optimal feasible gradient direction, otherwise the optimal feasible gradient direction can be obtained by rotating to the feasible direction set with smallest rotation.

If we use to denote the optimal rotation angle, the following formula for is given by

We define the optimal feasible gradient direction to be where the rotation matrix is of the following form:

The control law is then given by

In Figure 3, we illustrate this algorithm in a typical goal reaching task. The robot is required to reach the goal while avoiding the moving obstacle.

In Figure 4, we show that the results obtained from this method are the same as those from optimization method in [12] when the optimization method converges. Figure 4 shows the histogram of the trajectory differences between optimization method and the proposed method in both and direction, we can see the differences are the order of , which is not significant.

The proposed algorithm is much more computationally efficient than the optimization method. We use moving obstacles with randomly generated positions and run the program 50 times (totally 350 steps) for the simple scenario illustrated in Figure 3, the average number of iterations is above 5 for optimization method, and we know that when it comes to some complex scenarios, the number of iterations will increase substantially. The proposed algorithm, however, always has the computation complexity of , so it is at least five times faster than the optimization method on average.

#### 6. Nonholonomic Robots

The nonholonomic robots we consider in this paper are the Hilare type mobile robot, which have the most typical nonslip constraint. This robot has two parallel wheels which can be controlled independently. By commanding the same velocity to both wheels, the robot moves in a straight line. By commanding velocities with the same magnitude but opposite directions, the robot pivots about its axis. The nonslip constraint forces the mobile robot to move only forward or backward. We resolve the constraint by selecting the forward or backward direction based on whichever one has a positive projection on the gradient descent direction, then update the heading as fast as possible to match our heading to the gradient descent heading. If we assume it is a kinematic system, the inputs are the linear and angular velocity.

We define the angle corresponding to the negative gradient of the navigation function as

and define the angle between the negative gradient and the heading to be . is in the set by construction (please refer to Figure 5). The angular velocity control law is given by

**(a)**

**(b)**

and the linear velocity control law is given by where and are constant speed parameters and

Figure 6(a) shows the effectiveness of the algorithm in a multiple obstacle avoidance task. A nonholonomic robot successfully reaches the target after transversing a workspace which is packed with randomly generated obstacles of different size. Figure 6(b) is a close-up view of Figure 6(a) showing fairly smooth motion of the robot around the obstacles.

**(a)**

**(b)**

#### 7. Both Nonholonomic Constraints and Moving Obstacles

In this section, we consider the motion control problem subject to both nonholonomic constraints and moving obstacles. We will first find a feasible direction set such that all directions in this set can move the robot towards the goal and at same time away from the moving obstacles. We define the optimal feasible direction as the direction that is in the feasible set and closest to the gradient descent direction. If the robot's forward direction is in the feasible set, the robot will move forward; if the robot's backward direction is in the feasible set, the robot will move backward (note that backward direction and forward direction cannot be in the feasible set at the same time). When neither of these directions is in the feasible set, the robot will not generate linear velocity but will rotate towards the optimal feasible direction. In this way, the robot can move towards (taking both positions and orientations into account) the goal and avoid moving obstacle at same time.

If we use to denote the optimal rotation angle, the following formula for is given by

(refer to Figure 13) and we define the feasible steepest descent direction to be

where is defined as follows:

*Motion Control Law*

The (mode-dependent) kinematics of each agent is then given by
where is defined as the angle from the orientation to the optimal feasible direction :
where the rotation matrix is defined as
where is defined as the rotation angle from the negative gradient direction to the *actual* motion direction (forward direction or backward direction). Note that we require to be in the set ; the robot will move forward if forward direction is a feasible direction, move backward if backward direction is a feasible direction. is set to a zero matrix when neither the forward direction nor the backward direction is in the feasible set. In this case, the robot will not have any linear velocity.

Figure 7 shows the comparison of two scenarios,the first scenario is the goal reaching task with no obstacles. The second scenario is the goal reaching task with one moving obstacle that is always in the way of the robot at each step of its motion. From the figures, it is clear that the robot can reach the goal in six steps if the route is clear, while needs 80 steps under the influence of the moving obstacle.

**(a)**

**(b)**

Figure 8 shows the difference of robot motions with and without moving obstacles in the task of static obstacle avoidance. The first figure shows that the robot can get around the static obstacle in 60 steps, however with a moving obstacle, the robot in the second figure needs 90 steps.

**(a)**

**(b)**

#### 8. Simulation Results

In this section, we show how this framework succeeds in a typical navigation and coordination task: search-and-transport. We have performed simulations of agent teams in several environments. Figure 9 shows the results of a typical simulation, played in a grid containing 6 static obstacles (represented with the symbol) and 2 moving obstacles (represented with the symbol) with a team of 3 agents trying to find 3 small objects (represented by the @ symbol) and 3 big objects (represented by the symbol). The symbols also represent positions where the big objects are picked up, and the symbols also represent positions where the small objects are picked up. The solid lines near the moving obstacles are trajectories of the moving obstacles. The robot starts at position , the robot starts at position , and the robot starts at position . The small object collection location is and the big object collection location is .

In Figure 9, we illustrate the search-and-transport task. Only 's trajectory is shown (represented by dashed line) in Figure 9 to reduce clutter.

Figures 10(a)–10(c) is part of the simulation. We use it to illustrate coordination among agents by showing the paths followed by the three agents and moving obstacles. The trajectories of agents , , and are represented by the solid line, dash-dot line and dashed line, respectively. From this figure, we can see that found a big object at the “” point, came to help and they returned the big object back to the big object collection position together. found a small object at the “@” point, and is carrying it back to the small object collection position. The figure also illustrates how all agents avoid both static and moving obstacles at all times.

**(a)**

**(b)**

**(c)**

Figure 11 illustrates our moving obstacle avoidance technique. When spots a moving obstacle represented by the M symbol, a moving obstacle avoidance algorithm is incorporated into the navigation function. By always moving along the feasible direction, can make progress to the goal and avoid collision with the obstacle at the same time.

**(a)**

**(b)**

**(c)**

**(d)**

**(e)**

In Figure 12 we illustrate how our method works with a more complex scenario. The environment is enlarged from 10 by 10 to 50 by 50. The workspace is more clustered and the obstacles have a larger size and assume different shapes. It is shown that all the algorithms presented in the paper are not affected by these changes. The three robots can still coordinate and accomplish the search-and-transport task. The new workspace contains eight closely placed static obstacles of different shapes and two moving obstacles (represented with the symbol). The number of large objects remains three and the number of small objects is increased to four. As in the last scenario, the solid lines near the moving obstacles are the trajectories of the moving obstacles. The robot starts at position , the robot starts at position , and the robot starts at position . The small object collection location is and the big object collection location is .

#### 9. Team Stability Analysis

The definition of our modal navigation functions, and of our mode-switching rules, allows us to show that the system (entire agent team) is globally stable at all times and in all states [16].

With the mode switching technique defined in [6], we can guarantee the mode switches occur in finite time as follows. Although the order of mode switches cannot be predicted in advance, our switching technique guarantees that the mode-transition graph will be * acyclic* since mode switches only occur when the team has made progress towards solving the overall task and there is never a situation where a given mode is reentered. Therefore it is straightforward to show that our hybrid system is one of the switched system defined in [16].

Now we only need to show that the low-level path planning scheme presented here is stable.

##### 9.1. Stability without Constraints

Within each mode, we can define a mode-specific Lyapunov function of the following form:

where the step from (25) to (26) is justified by the fact that . In these equations is the number of robots.

To show intramodal Lyapunov stability, we are required to show for all and for all . follows naturally from the definition of in (26). To show that is always decreasing, we begin by using (25). For convenience, in the derivations that follow, we have replaced terms of the following form with the short form :

Now, observe that since only appears in through the terms, the second term of (27) can be rewritten as

Thus, the second and third terms in (27) will cancel, and we will have

If we substitute for using the dynamics defined in (7), we will have and we will have for all as required.

*Remark 1. *The constructed Lyapunov function is a generalized energy function. The first term decreasing means that the agents get closer to the attractors. The second term decreasing means that the agents move away from the static obstacles. The third term decreasing means that the agents try to stay away from the other team members.

##### 9.2. Stability Subject to Nonholonomic Constraints

If we substitute into (29) for using the dynamics defined in (17), we will have

If we denote and

Note that by construction, Therefore we will have for all as required.

##### 9.3. With Moving Obstacles

From (29), we have

If we substitute for using the dynamics defined in (14), we will have

According to the definition of feasible directions (refer to (9)), we have for all as required.

##### 9.4. With Moving Obstacles and Nonholonomic Constraints

If we substitute for using the dynamics defined in (23), we will have

If we denote and

Note that by construction, . Therefore we will have for all as required.

#### 10. Conclusions and Future Work

In our previous work, we proposed a software framework and a hybrid navigation scheme for multiple agent navigation and coordination. However, there were several issues that remained unresolved in the previous work. Specifically, the robots and their environment were abstractions of the real world. For instance, the robots had the ability to move in all directions and the environment contained only fixed obstacles. In this paper, we extend our work by eliminating these ideal characteristics and considering the challenges that robots are likely to encounter in the real world. In particular, we propose geometric-based methods to avoid moving obstacles and satisfy nonholonomic constraints. Our methods are suitable for fast online calculation and they are easily combined with one another to solve more complex problems such as nonholonomic constraints with moving obstacles. Furthermore, our control framework applies to a wide range of scenarios. We have demonstrated the search-and-transport problems with interdependencies for a multiagent team. And this framework is applicable to a much wider range of tasks than were discussed in this paper, due to our hierarchical design and fully distributed organization and because our stability proof makes no assumptions about the nature of the team members.

#### Appendix

*Proof. *(For simplicity, we drop the subscript “*i*” which refers to the *i*th robot in this proof.)First we denote the angle corresponding to the gradient of constraint by and define it as follows:
where . To better illustrate our method, we represent all directions in the following form of rotation angles with respect to . is defined as the angle between and . We define this angle to be positive if a counter-clockwise rotation takes to and negative otherwise:
The resultant falls in the interval and we transform it to the interval by the following formula:
Next we construct a feasible direction set for each of the activated moving obstacles:
Note that the feasible direction set for each activated moving obstacle constraint is a subset of because all feasible directions must lie in the same half-tangent plane as . Next, we find the intersection of the feasible direction sets that works for all moving obstacles:
By construction, we know that is also a subset of .