# Collaborator: A Nonholonomic Multiagent Team for Tasks in a Dynamic Environment

Authors: Jing Ren; Mark Green
Journal: Journal of Robotics
 (2009)

---

## 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.


---

## Body

## 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 Section2, 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 Figure1. 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.Figure 1
Agent-based navigation software architecture. The architecture of a single agent is structured in four hierarchical layers:DecisionMaking (DM), KnowledgeBase (KBase), Interaction and Communication. Local and remote sensor information is integrated into the knowledge base, and no distinction is made between them during motion planning.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.TheDecision 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.TheKnowledge 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 theCoordination 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(ax,ay) are represented by the negative Gaussian attractor function:
(1)fA(x,y)=1-e-((x-ax)2+(y-ay)2)/2σ2.Repulsive obstacles and other agents centered at(rx,ry) are modeled with the circular, two-dimensional generalized Gaussian repulsor functions:(2)fR(x,y)=e-(1/2)(((x-rx)2+(y-ry)2)/σ2)C.
For some positive integer C. The variance, σ, is a measure of the size of the obstacle. The parameter C 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 parameterC, generalized Gaussian function modeling can be used to construct potential fields that are free of undesired local minima. Figure 2(a) uses a small C and there are local minima among the obstacles, by changing C to a larger value and thereby highly localizing the effects of the obstacles, local minima disappear in Figure 2(b).Effect of parameterC.
(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, ViXi(q) for agent i in mode Xi (where Xi can be any of the modes defined above). To construct the navigation function for a given mode, we use the three part formula:(3)ViXi(q)=VAXi(qi)+VOXi(qi)+∑j≠iVR(qi,qj).
In (3), VAXi(qi) represents the sum of the effects on agent i of all the NA attractors in the system during mode Xi, and thus(4)VAXi(qi)=∑k=1NA(1-e-(((xi-(ak)x)2+(yi-(ak)y)2)/2σ2).VOXi(qi) represents the sum of the effects on agent i of all the known static obstacles in the system during mode Xi. The number of known static obstacles is NO; so we can state this as(5)VOXi(qi)=∑k=1NO(e-(1/2)(((xi-(rk)x)2+(yi-(rk)y)2)/σ2)C).
Finally, the functions VR(qi,qj) represent the repulsor functions between pairs of agents i and j. Note that VR is not mode dependent, since the number of agents is assumed to be constant. Thus, in general
(6)VR(qi,qj)=e-(1/2)(((xi-xj)2+(yi-yj)2)/σ2)C.


## 4. Motion Control Law

Using the technique of artificial potential fields, we construct anavigation function, ViXi(q) for each agent i in mode Xi (where Xi can be any of the modes defined above). The (mode-dependent) kinematics of each agent are then given by:(7)q̇i=-α∂ViXi/∂qi|∂ViXi/∂qi|.
In (7), the operator ∂ViXi(q)/∂qi represents the gradient of ViXi(q) with respect to only qi. Thus, we use a gradient descent method to generate a unit vector direction for q̇i 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 Section7 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:(8)gj(q)=|qi-Pj|2-Dobs2≥0,
where Pj is the position of the moving obstacle j, and Dobs 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 distanceDobs 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 dfi are the directions that can stabilize the system and move robots away from the activated moving obstacles and thus satisfy the following conditions:(9)dfiTdngi>0,(10)dfiTdgj≥0,j=1,2,…,m,
where dgj=∂gj/∂qi represents the gradient of the constraint gj and dngi=-(∂ViXi(q)/∂qi)/∥∂ViXi(q)/∂qi∥ is the negative gradient direction of the navigation function ViXi(q) of robot i. In (9), we require any control which can stabilize the system to have a positive projection on dngi (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 dgj (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 [Θmin,Θmax] is the set of feasible directions in the following form of a rotation angle with respect to the negative gradient direction dngi. From basic geometry (refer to Figure 13), we can see that there will be no feasible solution if Θmin>Θmax. If there exists a feasible solution and dngi is in the feasible set, then dngi is the optimal feasible gradient direction, otherwise the optimal feasible gradient direction can be obtained by rotating dngi to the feasible direction set with smallest rotation.If we useΘri to denote the optimal rotation angle, the following formula for Θri is given by(11)Θri={0ifΘmin≤0,Θmax≥0,ΘminifΘmin>0,ΘmaxifΘmax<0.
We define the optimal feasible gradient direction dfngi to be
(12)dfngi=dngiBriT(q),
where the rotation matrix Bri(q) is of the following form:(13)Bri(q)=[cosΘri-sinΘrisinΘricosΘri].
The control law is then given by(14)q̇=k·dfngi.
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.Figure 3
The darker line is the trajectory of a robot trying to reach the goal (@ at the top of the figure). It must achieve this goal while avoiding the moving obstacle (lighter line).In Figure4, 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 x and y direction, we can see the differences are the order of 10-15, which is not significant.Figure 4
Histogram of position differences of 350 steps in bothx and y directions shows that the trajectories generated from two algorithms are the same.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 Figure3, 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 O(1), 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θngi as(15)θngi=arctan(-∂ViXi∂yi,-∂ViXi∂xi),
and define the angle between the negative gradient and the heading to be θni=min{∡(θngi,θi),∡(θngi,-θi)}. θni is in the set [-π/2,π/2] by construction (please refer to Figure 5). The angular velocity control law is given by(a) The robot forward direction has a positive projection on the negative gradient direction; the robot will move forward to guarantee theθni is in the set [-π/2,π/2]. (b) The robot backward direction has a positive projection on the negative gradient direction; the robot will move backward to guarantee that the θni is in the set [-π/2,π/2].
(a)(b)(16)θ̇i=βθngi-θi|θngi-θi|,
and the linear velocity control law is given by
(17)q̇i=-α(∂ViXi/∂qi)BniT(q)|∂ViXi/∂qi|,
where α and β are constant speed parameters and(18)Bni=[cosθni-sinθnisinθnicosθni].Figure6(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.Motion planning for nonholonomic robots with multiple 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Θoi to denote the optimal rotation angle, the following formula for Θoi is given by(19)Θoi={0ifΘmin≤0,Θmax≥0,ΘminifΘmin>0,ΘmaxifΘmax<0,
(refer to Figure 13) and we define the feasible steepest descent direction di to be(20)di=dngiBoiT(q),
where Boi(q) is defined as follows:(21)Boi(q)=[cosΘoi-sinΘoisinΘoicosΘoi].Motion Control Law
The (mode-dependent) kinematics of each agent is then given by(22)θ̇i=βθroi|θroi|,
where θroi is defined as the angle from the orientation θi to the optimal feasible direction di:
(23)q̇i=-α(∂ViXi/∂qi)BaiT(q)|∂ViXi/∂qi|,
where the rotation matrix Bai(qi) is defined as
(24)Bai(q)=[cosΘai-sinΘaisinΘaicosΘai],
where Θai 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 Θai to be in the set [-π/2,π/2]; the robot will move forward if forward direction is a feasible direction, move backward if backward direction is a feasible direction. Bai 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.Figure7 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) shows the robot motion with nonholonomic constraints, no obstacle. (b) shows the robot motion with nonholonomic constraints, with a random moving obstacle, note that to fully test our algorithm, we keep the moving obstacle activated for each step.
(a)(b)Figure8 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) shows the robot motion with nonholonomic constraints, with a static obstacle at (55, 40), (b) shows the robot motion with nonholonomic constraints, with a moving obstacle moving around (55, 40).
(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. Figure9 shows the results of a typical simulation, played in a 10×10 grid containing 6 static obstacles (represented with the # symbol) and 2 moving obstacles (represented with the M symbol) with a team of 3 agents trying to find 3 small objects (represented by the @ symbol) and 3 big objects (represented by the O symbol). The O 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 R1 starts at position (1,0), the robot R2 starts at position (3,0), and the robot R3 starts at position (5,0). The small object collection location is (9,0) and the big object collection location is (1,0).Figure 9
This figure is the snapshot after the task is completed. All three big objects are returned to the big object collection position and all three small objects are returned to the small object collection position. The robots are back in their home position. The dashed line represents the trajectory of agentR3.In Figure9, we illustrate the search-and-transport task. Only R3's trajectory is shown (represented by dashed line) in Figure 9 to reduce clutter.Figures10(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 R1, R2, and R3 are represented by the solid line, dash-dot line and dashed line, respectively. From this figure, we can see that R2 found a big object at the “O” point, R3 came to help and they returned the big object back to the big object collection position together. R1 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.This figure is to show the agent team coordination in a big object carrying task. In (a)R2 finds a big object, stops searching and calls for help. In (a) R3 comes to help after receiving the confirm message. In (b) R3 arrives at the big object and R2 and R3 are carrying the big object home together. Finally, in (c) R2 and R3 reached the object home.
(a)(b)(c)Figure11 illustrates our moving obstacle avoidance technique. When R1 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, R1 can make progress to the goal and avoid collision with the obstacle at the same time.(a)R1 spots a moving obstacle in the way. and start moving obstacle avoidance algorithm for their navigation. (b)–(e) shows for each step, the robot is moving away from the current position the obstacle therefore avoiding collision. Meanwhile the total effects of attractive “force” from the unsearched cells and the objects attract the robot towards to the goal.
(a)(b)(c)(d)(e)In Figure12 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 M 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 R1 starts at position (5,0), the robot R2 starts at position (15,0), and the robot R3 starts at position (25,0). The small object collection location is (45,0) and the big object collection location is (5,0).Figure 12
We show the simulation results with a more complex environment. Obstacles are larger and take on different shapes, and the workspace is more crowded. This figure is the snapshot after the task is completed. All three big objects are returned to the big object collection position and all four small objects are returned to the small object collection position.Figure 13
This figure illustrates our optimal feasible direction scheme. Note that the filled area is the resulting feasible area.


## 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:(25)VX(q)=∑i=1QViXi(q)-∑i=1Q∑j=i+1QVR(qi,qj)(26)=∑i=1Q(VAXi(qi)+VOXi(qi))+∑i=1Q∑j=i+1QVR(qi,qj),
where the step from (25) to (26) is justified by the fact that VR(qi,qj)=VR(qj,qi). In these equations Q is the number of robots.To show intramodal Lyapunov stability, we are required to showVX(q)≥0 for all q and V̇X(q)<0 for all q,t. VX(q)≥0 follows naturally from the definition of VX(q) in (26). To show that VX(q) is always decreasing, we begin by using (25). For convenience, in the derivations that follow, we have replaced terms of the following form VR(qi,qj) with the short form VRij:
(27)V̇X(q)=∑i=1Q∂ViXi(q)∂qiq̇iT+∑i=1Q∑i≠j∂ViXi(q)∂qjq̇jT-∑i=1Q∑j=i+1Q(∂VRij∂qiq̇iT+∂VRij∂qjq̇jT).Now, observe that sinceqj only appears in ViXi(q) through the VRij terms, the second term of (27) can be rewritten as
(28)∑i=1Q∑i≠j∂ViXi∂qjq̇jT=∑i=1Q∑i≠j∂VRij∂qjq̇jT=∑i=1Q∑j=i+1Q∂VRij∂qjq̇jT+∑i=1Q∑j=1i-1∂VRij∂qjq̇jT=∑i=1Q∑j=i+1Q∂VRij∂qjq̇jT+∑p=1Q∑s=p+1Q∂VRps∂qpq̇pT=∑i=1Q∑j=i+1Q(∂VRij∂qiq̇iT+∂VRij∂qjq̇jT).Thus, the second and third terms in (27) will cancel, and we will have(29)V̇X(q)=∑i=1Q∂ViXi(q)∂qiq̇iT.If we substitute forq̇i using the dynamics defined in (7), we will have
(30)V̇X(q)=-α∑i=1Q∂ViXi(q)∂qi(∂ViXi/∂qi)T|∂ViXi/∂qi|=-α∑i=1Q|∂ViXi(q)∂qi|,
and we will have V̇X(q)<0 for all t,q as required.Remark 1.
The constructed Lyapunov functionVX(q) is a generalized energy function. The first term ∑i=1QVAXi(qi) decreasing means that the agents get closer to the attractors. The second term ∑i=1QVOXi(qi) decreasing means that the agents move away from the static obstacles. The third term ∑i=1Q∑j=i+1QVR(qi,qj) 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 q̇i using the dynamics defined in (17), we will have(31)V̇X(q)=-∑i=1Q(∂ViXi(q)∂qi)αBni(q)(∂ViXi/∂qi)T|∂ViXi/∂qi|.If we denotevxi=-∂ViXi(q)/∂xi and vyi=-∂ViXi(q)/∂yi,(32)(∂ViXi(q)∂qi)Bni(q)(∂ViXi∂qi)T=[vxivyi]TBni[vxivyi]=[vxivyi]T[cosθni-sinθnisinθnicosθni][vxivyi]=(vxi2cosθni+vyi2cosθni).Note that by construction,θni⊆[-π/2,π/2]. Therefore we will have V̇X(q)<0 for all t,q as required.


### 9.3. With Moving Obstacles

From (29), we have(33)V̇X(q)=∑i=1Q∂ViXi(q)∂qiq̇iT.If we substitute forq̇i using the dynamics defined in (14), we will have(34)V̇X(q)=∑i=1Q∂ViXi(q)∂qikdfngi.According to the definition of feasible directions (refer to (9)), we have V̇X(q)<0 for all t,q as required.


### 9.4. With Moving Obstacles and Nonholonomic Constraints

If we substitute forq̇i using the dynamics defined in (23), we will have(35)V̇X(q)=(-α)∑i=1QX(∂ViXi(q)∂qi)Bai(q)(∂ViXi/∂qi)T|∂ViXi/∂qi|.If we denotevxi=-∂ViXi(q)/∂xi and vyi=-∂ViXi(q)/∂yi,(36)(∂ViXi(q)∂qi)Bai(q)(∂ViXi∂qi)T=[vxivyi]TBai(q)[vxivyi]=[vxivyi]T[cosθai-sinθaisinθaicosθai][vxivyi]=(vxi2cosθai+vyi2cosθai).Note that by construction,θai⊆[-π/2,π/2]. Therefore we will have V̇X(q)<0 for all t,q as required.


## 9.1. Stability without Constraints

Within each mode, we can define a mode-specific Lyapunov function of the following form:(25)VX(q)=∑i=1QViXi(q)-∑i=1Q∑j=i+1QVR(qi,qj)(26)=∑i=1Q(VAXi(qi)+VOXi(qi))+∑i=1Q∑j=i+1QVR(qi,qj),
where the step from (25) to (26) is justified by the fact that VR(qi,qj)=VR(qj,qi). In these equations Q is the number of robots.To show intramodal Lyapunov stability, we are required to showVX(q)≥0 for all q and V̇X(q)<0 for all q,t. VX(q)≥0 follows naturally from the definition of VX(q) in (26). To show that VX(q) is always decreasing, we begin by using (25). For convenience, in the derivations that follow, we have replaced terms of the following form VR(qi,qj) with the short form VRij:
(27)V̇X(q)=∑i=1Q∂ViXi(q)∂qiq̇iT+∑i=1Q∑i≠j∂ViXi(q)∂qjq̇jT-∑i=1Q∑j=i+1Q(∂VRij∂qiq̇iT+∂VRij∂qjq̇jT).Now, observe that sinceqj only appears in ViXi(q) through the VRij terms, the second term of (27) can be rewritten as
(28)∑i=1Q∑i≠j∂ViXi∂qjq̇jT=∑i=1Q∑i≠j∂VRij∂qjq̇jT=∑i=1Q∑j=i+1Q∂VRij∂qjq̇jT+∑i=1Q∑j=1i-1∂VRij∂qjq̇jT=∑i=1Q∑j=i+1Q∂VRij∂qjq̇jT+∑p=1Q∑s=p+1Q∂VRps∂qpq̇pT=∑i=1Q∑j=i+1Q(∂VRij∂qiq̇iT+∂VRij∂qjq̇jT).Thus, the second and third terms in (27) will cancel, and we will have(29)V̇X(q)=∑i=1Q∂ViXi(q)∂qiq̇iT.If we substitute forq̇i using the dynamics defined in (7), we will have
(30)V̇X(q)=-α∑i=1Q∂ViXi(q)∂qi(∂ViXi/∂qi)T|∂ViXi/∂qi|=-α∑i=1Q|∂ViXi(q)∂qi|,
and we will have V̇X(q)<0 for all t,q as required.Remark 1.
The constructed Lyapunov functionVX(q) is a generalized energy function. The first term ∑i=1QVAXi(qi) decreasing means that the agents get closer to the attractors. The second term ∑i=1QVOXi(qi) decreasing means that the agents move away from the static obstacles. The third term ∑i=1Q∑j=i+1QVR(qi,qj) 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 q̇i using the dynamics defined in (17), we will have(31)V̇X(q)=-∑i=1Q(∂ViXi(q)∂qi)αBni(q)(∂ViXi/∂qi)T|∂ViXi/∂qi|.If we denotevxi=-∂ViXi(q)/∂xi and vyi=-∂ViXi(q)/∂yi,(32)(∂ViXi(q)∂qi)Bni(q)(∂ViXi∂qi)T=[vxivyi]TBni[vxivyi]=[vxivyi]T[cosθni-sinθnisinθnicosθni][vxivyi]=(vxi2cosθni+vyi2cosθni).Note that by construction,θni⊆[-π/2,π/2]. Therefore we will have V̇X(q)<0 for all t,q as required.


## 9.3. With Moving Obstacles

From (29), we have(33)V̇X(q)=∑i=1Q∂ViXi(q)∂qiq̇iT.If we substitute forq̇i using the dynamics defined in (14), we will have(34)V̇X(q)=∑i=1Q∂ViXi(q)∂qikdfngi.According to the definition of feasible directions (refer to (9)), we have V̇X(q)<0 for all t,q as required.


## 9.4. With Moving Obstacles and Nonholonomic Constraints

If we substitute forq̇i using the dynamics defined in (23), we will have(35)V̇X(q)=(-α)∑i=1QX(∂ViXi(q)∂qi)Bai(q)(∂ViXi/∂qi)T|∂ViXi/∂qi|.If we denotevxi=-∂ViXi(q)/∂xi and vyi=-∂ViXi(q)/∂yi,(36)(∂ViXi(q)∂qi)Bai(q)(∂ViXi∂qi)T=[vxivyi]TBai(q)[vxivyi]=[vxivyi]T[cosθai-sinθaisinθaicosθai][vxivyi]=(vxi2cosθai+vyi2cosθai).Note that by construction,θai⊆[-π/2,π/2]. Therefore we will have V̇X(q)<0 for all t,q 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.



---

*Source: 986207-2010-02-01.xml*