Formation control for a cooperative multiagent system with a ...
Formation Control for a Cooperative Multi-Agent System using Decentralized Navigation Functions Maria Carmela De Gennaro and Ali Jadbabaie Abstract— We propose a decentralized cooperative controller for a group of mobile agents. The control design is based on the navigation function formalism. The aim of the group control law is to generate a pattern or formation in a given workspace while avoiding obstacles and collisions. The desired goal is specified in terms of distances among the agents. We show that it is always possible to design a control law as the gradient of a suitablydefined navigation function whose minimum corresponds to the desired configuration. Furthermore in certain cases, such as when the topology of the interconnection is an acyclic graph, this minimum is unique. Some simulations are shown to test the strategy.
I. INTRODUCTION Cooperative control and multi-agent robotics is an active research area both in control theory and robotics. Problems such as flocking, consensus, coverage and pattern formation are some of the important problems that have been studied over the past few years. The goal of these problems is to develop distributed protocols and control laws that allow the reaching of complicated global goals [1]–[10]. In this paper, we study the problem of pattern formation among a group of planar agents. The formation is defined as a configuration in a bounded workspace, where each agent is at a desired distance between its neighbors ( i.e. the agents immediately nearest to it). The desired goal is specified in terms of relative distances, so the formation can be achieved in any part of the workspace, and it can have any orientation. The connection among the agents is dependent on their mutual distances, so it evolves over time, affecting the eventual solution of the problem. In the workspace there are also obstacles that have to be avoided by the agents. Formation control for a group of agents is a challenging problem that has been studied in the last few years. The definition of formation is specified in many different ways, as for example the rigid formation [11], or a desired figure in the plane [12]. Also the problem of collision among agents and with some obstacles has been studied; several solutions have been provided in a cooperative framework. In [13] the collision problem is solved for a group of agents moving in the plane, with the use of gyroscopic forces; in [14] an optimization problem is solved for a group of UAVs that search for targets in a common area while avoiding other hazardous areas. Maria Carmela De Gennaro is a PhD Student at the Universit`a del Sannio, Dipartimento di Ingegneria, piazza Roma 21, Benevento, Italy. Email:[email protected] Ali Jadbabaie is with the Department of Electrical and Systems Engineering, University of Pennsylvania, Philadelphia, PA 19104, USA. Email: [email protected]
The strategy implemented in this paper is based on the use of navigation functions. These functions were initially designed [15] in order to steer a single point-mass robot from almost all initial positions toward the desired final position in a bounded workspace, while avoiding obstacles. The navigation function itself is composed of two subfunctions, one that attracts the robot to the goal, the other that repels the robot from obstacles. Moreover, it has some parameters that can be tuned. Early implementations of navigation functions to control groups of robots were centralized. Each robot maintained its own copy of the global navigation function. In this framework each robot required full knowledge of the environment and of the state of the global system. In the recent literature there have been attempts to apply the same idea to “navigate” one or more robots in a workspace, by using a decentralized approach. Each work makes a step toward the final complete decentralization of the problem. In [16] the proposed navigation function is decentralized in the sense that it is different for each agent: it contains only the information about the distances between this agent and all the other agents and obstacles. The goal of each agent in the group is to reach a specific point in the workspace, and the information about the other agents and obstacles is used only for avoiding them; there is not a cooperative task to achieve. In [17] a limited sensing region is considered for each agent, but still without defining any common goal. In [18]- [19] a limited sensing region is defined for the obstacle avoidance, and also a common task is defined for the agents. In fact each agent has to reach a desired formation with the others, and the formation is specified in terms of relative positions and orientations. The connection among the agents is considered fixed and independent of the distances among them. In this paper the last step toward the decentralization is presented. For each agent we propose a navigation function based on the distances with all the other agents and obstacles. Two regions are defined around each agent: the connection region and the collision region, closer to the agent. In the connection region, the goal function drives the agent in order to reach the desired distances with the other connected agents. In the collision region, the collision function avoids the collision between the controlled agent and the other agents and obstacles. We describe the structure of the navigation function, compute the equilibria that can be reached, and analyze the stability properties of these equilibria. We show how the con-
nections among agents influence the equilibria determined by the navigation function. Our analysis assumes that the group remains connected all time. This is not necessarily guaranteed by the navigation function, but it is important to reach the desired final configuration in the workspace. The paper is organized in as follows. In Section II we describe the workspace, agents and obstacles. Also, for each agent we define the connectivity and collision regions. In Section III we describe the expression on the decentralized navigation function. In Section IV we analyze the equilibria of the navigation function. Finally, in Section V we show some simulations to test the effectiveness of the proposed function. II. P ROBLEM F ORMULATION The multi-agent system considered is a group of N mobile agents in R2 , each with a first order dynamics: q˙i = ui , i = 1, ..., N
(1)
where qi and ui are respectively the state and the control input of the agent i. The workspace F is assumed circular and bounded, with radius Rw , and the agents are assumed to be point masses. In the workspace there are M obstacles, modeled as fixed points p1 ,...,pM . The assumption that both agents and obstacles are points is not restrictive, because it was shown in [20] that a large class of shapes can be mapped to single points by suitable coordinate transformations. Each agent has a limited communication capability- that is, it can only communicate with agents within its neighborhood region. The neighborhood region of agent i is a circle of radius Ri (communication radius) around agent i, such that all the other agents inside this region are considered “neighbors”. The set of the i-th agent’s neighbors is given by : Ni , {j 6= i : krij k = kqi − qj k ≤ Ri }. (2) Moreover, for each agent we define a collision region, i.e. a circle of radius δi < Ri around the agent, such that any agent or obstacle inside that radius is considered an obstacle. The set of obstacles of agent i is defined as: Ci , {j 6= i : krij k ≤ δi }
[
{k = 1, .., M : kqi − pk k ≤ δi }. (3)
In the following we assume that all the agents have the same communication radius, Ri = R, and the same collision radius δi = δ, for all i. Fig. 1 shows the i-th agent neighborhood and collision regions. This multi-agent system can be represented by an undirected graph G = (V, E) where V is the set of nodes (agents) and E is the set of edges (connections (i, j)). We call graph G the connectivity graph of the system. Since the connections depend on the distances between agents, the graph G varies over time. The connection between agents can be also represented by an incidence matrix B, whose rows and columns are indexed by the vertices and edges of G respectively. Given an arbitrary orientation for the graph, the incidence matrix B is constructed by assigning the value
Fig. 1.
Neighborhood and collision region for the i-th agent.
1 to the (i, j) element of B if edge j is incoming to vertex i, −1 if edge j is outgoing from vertex i, and 0 otherwise [21]. III. D ECENTRALIZED NAVIGATION F UNCTION The goal for the multi-agent system is to reach a desired formation in the workspace, specified in terms of desired mutual distances among the connected agents. To solve this problem, a local navigation function is defined for each agent i [15]: ϕi (q) =
(γi
(q)k
γi (q) + βi (q))1/k
(4)
where: • γi (q) : F → R+ , the goal function, has only one minimum, reached when the i-th agent is at the desired distances with its neighbors; • βi (q) : F → [0, 1] is the obstacle function, and vanishes when the i-th agent collides with an obstacle or with another agent; • k is a positive tuning parameter. The navigation function is used to control each agent in order to reach the desired final destination and avoid the obstacles. In fact the control action of the i-th agent is: ui = −α∇qi ϕi (q),
(5)
where α is a positive scaling factor. Now we describe the expression of the goal and collision function. A. γi (q): The Goal Function The goal function γi is defined as the sum of pairwise goal functions γij , for all the agents j different from i: γi (q) =
N X
γij (qi , qj ).
(6)
j=1,j6=i
γij (qi , qj ) is dependent on the norm of the distance krij k and on the desired final distance cij : zij 2 ( ) krij k ≤ cij ; cij az2 ij cij < krij k ≤ R; γij = 3 +cz 2 +dz +e bzij ij ij a(R−cij )2 krij k > R, b(R−cij )3 +c(R−cij )2 +d(R−cij )+e (7)
where cij > δ, and zij = krij k−cij . The function γij (qi , qj ) reaches its unique minimum when krij k = cij ; otherwise it is positive and maximum (equal to 1) when the agents are not connected (krij k > R) or when they collide (krij k = 0), as shown in Fig. 2. The constants a, b, c, d, e are chosen to ensure the continuity of γij and its first and second derivatives in the points cij and R. The function γij is twice differentiable ∈ C 2 function on F, as shown in Fig. 3.
(kril k = 0), and its maximum equals to 1 at kril k = δ, as shown in Fig. 4. The coefficients of the polynomials depend on δ, and they are chosen in order to make βil a C 2 function on F.
Fig. 4.
Shape of the collision function βil (kril k), with δ = 5.
The function βiw (qi ) is the obstacle function between the workspace boundary and i, and it is defined in a similar way: Shape of the goal function γij (krij k), with cij = 20 and R = 50. Fig. 2.
1 pI (kqi k) pII (kqi k) βiw (qi ) = pIII (kqi k) 0
kqi k ≥ Rw − δ; Rw − δ < kqi k ≤ Rw − 3δ/4; Rw − 3δ/4 < kqi k ≤ Rw − δ/4; Rw − δ/4 < kqi k ≤ Rw ; kqi k > Rw . (10) Function βiw reaches its unique minimum at the collision with the workspace (kqi k = Rw ), and the maximum equal to 1 at kqi k ≤ Rw − δ, as shown in Fig. 5. The coefficients of the polynomials depend on δ and Rw , and they are chosen in order to make βiw a C 2 function on F.
Fig. 3. First (up) and Second (down) derivative of the goal function
γij respect to krij k.
Shape of the collision function βiw (kqi k), with Rw = 50 and δ = 5. Fig. 5.
B. βi (q): The Obstacle Function The obstacle function βi (q) is the product of pairwise obstacle functions: Y βi (q) = βil (qi , ql )βiw (qi ). (8) l∈Ci
where βil is the obstacle function between agent i and agent or obstacle l, and βiw is the obstacle function between agent i and the boundary. The function βil is given by: p1 (kril k) p2 (kril k) βil (qi , ql ) = p3 (kril k) 1
kril k < δ/4; δ/4 ≤ kril k < 3δ/4; 3δ/4 ≤ kril k < δ; kril k ≥ δ,
(9)
where p1 , p2 and p3 are third order polynomials in kril k. The function βil (qi , ql ) reaches its unique minimum at collision
The function ϕi : F → [0, 1], is a C 2 function on F, and it is admissible on F, i.e. it is uniformly maximum on obstacles and workspace boundary. In order to see if it is a navigation function [15], we have to prove that: • it is polar at the desired formation, i.e. it is minimum only if the agent i is at the desired distance among its neighbors; • and it is a Morse function in F, i.e. its critical points are non-degenerate. We analyze these two properties in the following session. IV. C RITICAL POINTS OF THE NAVIGATION FUNCTION In this paragraph we analyze the critical points of the navigation function previously defined. The navigation function is used to control each agent to reach desired distances with its neighbors, but it does not guarantee the maintenance of
the connection among the controlled agent and its neighbors. For this reason, we assume that a minimum connection level will be always present in the global system, without specifying how it is maintained. Under this assumption, we want to see how the navigation function drives each agent toward its goal. The critical points of the navigation function (4), are studied by analyzing the gradient of the function ϕi : 1 ∇qi ϕi = [kβi ∇qi γi − γi ∇qi βi ], k(γik + βi )1/k
(11)
where the gradients of the functions γi and βi are: ∇qi γi =
N X
∇qi γij ;
(12)
j=1
∇qi βi =
X l∈Ci
S
∇qi βil w
βi . βil
(13)
The critical points of ϕi are obtained when: ∇qi ϕi = 0 ⇔ kβi ∇qi γi = γi ∇qi βi .
(14)
This condition is true in two different cases that we analyze below. Case1) : ∇qi γi = 0 and ∇qi βi = 0 From ∇qi βi = 0 we know that the i-th agent is far from the workspace boundary and the obstacles. If ∇qi γij = 0 for all connected pairs of (i, j), the i-th agent is at the desired distances among the neighbors. This is the desired equilibrium, and we can prove that this equilibrium is a minimum, by evaluating the Hessian of ϕi , as shown in the following proposition. Proposition 1 The navigation function has a nondegenerate minimum at the desired formation. Proof. The Hessian of ϕi is: ∇2qi ϕi = −
1 {(γik + βi )[k∇qi βi (∇qi γi )T k(γik + βi )1/k+2 −∇qi γi (∇qi βi )T + kβi ∇2qi γi − γi ∇2qi βi ]
1 + 1 [kβi ∇qi γi − γi ∇qi βi ][kγik−1 ∇qi γi + ∇qi βi ]T } k
At the desired equilibrium, the Hessian becomes: ∇2qi ϕi = where ∇2qi γi = 2I
kβi ∇2qi γi − γi ∇2qi βi k(γik + βi )1/k+1
(15)
X 1 , c2ij
(16)
∇2qi βi = 0,
j∈Ni
and I is the identity matrix in R2X2 . The Hessian ∇2qi ϕi > 0, so the equilibrium point is a minimum. If ∇qi γi = 0 but there exists a pair (i, j) such that ∇qi γij 6= 0, then agent i will not be at the desired distance from its neighbors. This type of equilibrium is dependent on the connectivity graph of the system: in fact, we can rewrite the equilibrium condition in terms of the incidence matrix B [22]- [23].
For agent i, the equilibrium condition is: X ∇qi γi = 0 ⇔ ∇qi γij = 0.
(17)
j∈Ni
Now, given the equivalence ∇qi γij = −∇qj γij = ∇rij γij , we can rewrite (17) as: X j∈Ni
∇qi γij = 0 ⇔
N X
bij ∇rij γij = 0,
(18)
j=1
where bij is the entry (i, j) of the matrix B. By writing the equilibrium expression for all the agents, we obtain: ∇r12 γ12 ... ∇r1N γ1N ∇r23 γ23 = 0. B∇ , B (19) ... ∇r2N γ2N ... ∇rN −1,N γN −1,N The undesired equilibrium is a solution of the algebraic system (19) where the unknowns are the gradients ∇rij γij for all the connected pairs of agents (i, j). The number of solutions of the system depends on the rank of B. Now we can show the following Proposition. Proposition 2 If the connectivity graph G is a tree, then at the equilibrium the distances between the connected pairs of agents are the desired values. Proof. If the connectivity graph is a tree, the number of unknown gradients ∇rij γij is N − 1, and B ∈ RN (N −1) . It was shown in [24] that the null space of B coincides with the vector space spanned by the cycle vectors of the graph. In the case of a tree, there are no cycles in the graph; hence, we can say that the null space of B is empty. B is full rank, and the solution of the system (19) is ∇rij γij = 0. This means that in the end if agents are connected and form a tree, then they are at their desired distances. The above proposition suggests that one possible approach to avoid the undesired equilibria is to control the system to always maintain a tree connection. A more general approach should consider the dependence of the desired formation on the connection of the system. Case2) ∇qi γi 6= 0, ∇qi βi 6= 0, and kβi ∇qi γi = γi ∇qi βi The first condition says that the i-th agent does not reach the desired configuration; the second says that the agent is near to one obstacle. The equilibrium reached is not the desired one, but it is possible to tune the parameter k in order to push this undesired equilibrium close to the obstacle. It is also possible to guarantee that this kind of equilibrium is not a local minimum, but only a saddle point. Two Propositions exploit these characteristics of the navigation function. Proposition 3 The critical points analyzed above are in the interior of the workspace F. Proposition 4 It is always possible to find a lower bound for the parameter k, such that:
the collision region near the boundary, but then it enters the same region to reach the desired distances with the others. Fig. 13 shows the distances among the agents. VI. C ONCLUSIONS
Motion of the agents on the plane and convergence to the desired formation. Fig. 6.
the critical points of the navigation function, different from the desired equilibrium, are near to one of the obstacles; • the critical points found near the obstacles are not local minima and not degenerate (ϕi is a Morse function). The propositions can be easily proved for the navigation function introduced here, and they give a lower bound to the parameter k. For proofs, the interested reader is referred to [19]. •
V. S IMULATION RESULTS In this section we numerically verify how the designed navigation function works. The workspace has radius Rw = 60. The multi-agent system considered is composed by three agents, and one obstacle is fixed in the origin of the workspace. We will see how there can be an undesired equilibrium also in this case with few agents. The collision radius is δ = 5, and the connection radius is R = 40. The constants of the control action are α = 1 and k = 1. In the following Figures the initial positions of the agents are represented with ‘∗’ and the final positions with ‘♦’, the regions around the obstacle and the workspace are represented with dashed lines. In all the following examples, the desired configuration is given by distances equal to c = 15 for all the connected pairs of agents. In the first example, shown in Fig. 6, the agents 1 and 3 are initially not connected but during the motion they connect and in the end they reach the desired distance, as shown in Fig. 7. In the second example, shown in Fig. 8, an undesired equilibrium is reached. It is due to the connections graph, that presents a cycle. We see in Fig. 9 that the final distances are different from the desired ones. In the third example, the graph of connections is always a tree. In Fig. 10 we see that agents 1 and 3 are always disconnected over time. Under this condition, the desired distances among the connected agents are reached, as we see in Fig. 11. Fig. 12 shows an example near the boundary of the workspace. Agent 2 exits
A decentralized navigation function is proposed to drive each agent of a group toward a desired final configuration which is expressed in terms of distances between the connected agents. The formation can be reached anywhere in the space and with any orientation. The navigation function assures to avoid collision between the controlled agent and the other agents and obstacles. The connection among the agents is dependent on their mutual distance. The proposed function has all the characteristics of a navigation function, but it introduces some equilibria that cannot be avoided by tuning parameters. These equilibria are related to the connection among the agents. Future work include the study of techniques that allow the system to avoid the undesired equilibria. R EFERENCES [1] J. P. Desai, J. P. Ostrowski, and V. Kumar, “Modeling and control of formations of nonholonomic mobile robots,” IEEE Trans. on Robotics and Automation, vol. 17 no.6, pp. 905908, December 2001. [2] W. Ren and R. Beard, “Trajectory tracking for unmanned air vehicles with velocity and heading rate constraints,” IEEE Trans. on Control System Technology, vol. 12 no.5, September 2004. [3] D. B. Edwards, T. Bean, D. Odell, and M. Anderson, “A leaderfollower algorithm for multiple auv formations,” Proc. of 2004 IEEE/OES Autonomous Underwater Vehicles, Sebasco Estates, Maine, June 2004. [4] Y. Liu and K. M. Passino, “Stable social foraging swarms in a noisy environment,” IEEE Trans. on Automatic Control, vol. 49, no. 4, January 2004. [5] W. M. Spears, D. F. Spears, J. C. Hamann, and R. Heil, “Distributed, physics-based control of swarms of vehicles,” Autonomous Robots 17, 137-162, 2004. [6] A. Jadbabaie, J. Lin, and A. S. Morse, “Coordination of groups of mobile autonomous agents using nearest neighbor rules,” IEEE Trans. on Automatic Control, vol. 48, no. 6, pp. 988–1001, June 2003. [7] N. L. P. Ogren, E. Fiorelli, “Cooperative control of mobile sensing networks: Adaptive gradient climbing in a distributed environment,” IEEE Trans. on Automatic Control, vol. 49(8), pp. 1292–1302, August 2004. [8] J. Cortes, S. Martinez, T. Karatas, and F. Bullo, “Coverage control for mobile sensing networks,” IEEE Trans. on Robotics and Automation, vol. 20, no. 2, pp. 243–255, February 2004. [9] R. O. Saber and R. M. Murray, “Flocking in multiagent dynamic systems: Algorithms and theory,” IEEE Trans. on Automatic Control, vol. 51, March 2006 (in press). [10] E. Justh and P. Krishnaprasad, “Equilibria and steering laws for planar formations,” Systems and Control letters, vol. 52, no. 1, pp. 25–38, May 2004.
Fig. 7. Distances among the agents for the first example: the agents
reach the desired distances, all equal to c.
Motion of the agents on the plane and convergence to a local minimum. Fig. 8.
Fig. 9.
Motion of the agents when the graph of connection is a tree. Agent 2 is connected with 1 and 3. Fig. 10.
Distances among agents in the case of local minimum.
[11] R. O. Saber, W. B. Dunbar, and R. M. Murray, “Cooperative control of multi-vehicle systems using cost graphs and optimization,” Proc. of the American Control Conference, Denver, Colorado, June 2003. [12] T. Balch and R. Arkin, “Behavior-based formation control for multirobot teams,” IEEE Trans. on Robotics and Automation, vol. 14, no. 6. [13] D. E. Chang, S. C. Shadden, J. E. Marsden, and R. O. Saber, “Collision avoidance for multiple agent systems,” Proc. of the 42nd IEEE Conference on Decision and Control, Maui, Hawaii, USA, December 2003. [14] R. W. Beard and T. W. McLain, “Multiple uav cooperative search under collision avoidance and limited range communication constraints,” Proc. of the 42nd IEEE Conference on Decision and Control, Maui, Hawaii, USA, December 2003. [15] E. Rimon and D. E. Koditschek, “Exact robot navigation using artificial potential functions,” IEEE Trans. on Robotics and Automation, vol. 8, no. 5, pp. 501-518, 1992. [16] D. V. Dimarogonas, M. M. Zavlanos, S. G. Loizou, and K. J. Kyriakopoulos, “Decentralized motion control of multiple holonomic agents under input constraints,” Proc. of the 42nd IEEE Conference on Decision and Control, Maui, Hawaii USA, December 2003. [17] D. V. Dimarogonas and K. J. Kyriakopoulos, “Decentralized stabilization and collision avoidance of multiple air vehicles with limited sensing capabilities,” Proc. of the 2005 American Control Conference, Portland, Oregon, USA, 2005. [18] H. G. Tanner and A. Kumar, “Formation stabilization of multiple agents using decentralized navigation functions,” Robotics: Science and Systems, Boston, 2005. [19] ——, “Towards decentralization of multi-robot navigation functions,” Proc. of the 2005 IEEE Conference on Robotics and Automation, April 2005. [20] H. G. Tanner, S. G. Loizou, and K. J. Kyriakopoulos, “Nonholonomic navigation and control of cooperating mobile manipulators,” IEEE Trans. on Robotics and Automation, vol. 19, no. 1, pp.53-64, February 2003. [21] C. Godsil and G. Royle, Algebraic Graph Theory. Springer, 2001. [22] H. G. Tanner, A. Jadbabaie, and G. Pappas, “Flocking in fixed and switching networks,” Submitted to IEEE Trans. on Automatic Control. [23] M. C. D. Gennaro, L. Iannelli, and F. Vasca, “Formation control and collision avoidance in mobile agent systems,” Proc. of the IEEE 13rd Mediterranean Conference on Control and Automation, Lymassol, Cyprus, June 2005. [24] S. Guattery and G. L. Miller, “Graph embeddings and laplacian eigenvalues,” SIAM J. Matrix Anal. Appl., vol. 21, no. 3, pp. 703-723, 2000.
Fig. 11. Distances among the agents when the graph of connection
is a tree. The desired distances among connected agents 1 − 2 and 2 − 3 are reached.
Motion of the agents near the boundary and convergence to the desired configuration. Fig. 12.
Fig. 13.
Distances among the agents near to the workspace.
I. INTRODUCTION Cooperative control and multi-agent robotics is an active research area both in control theory and robotics. Problems such as flocking, consensus, coverage and pattern formation are some of the important problems that have been studied over the past few years. The goal of these problems is to develop distributed protocols and control laws that allow the reaching of complicated global goals [1]–[10]. In this paper, we study the problem of pattern formation among a group of planar agents. The formation is defined as a configuration in a bounded workspace, where each agent is at a desired distance between its neighbors ( i.e. the agents immediately nearest to it). The desired goal is specified in terms of relative distances, so the formation can be achieved in any part of the workspace, and it can have any orientation. The connection among the agents is dependent on their mutual distances, so it evolves over time, affecting the eventual solution of the problem. In the workspace there are also obstacles that have to be avoided by the agents. Formation control for a group of agents is a challenging problem that has been studied in the last few years. The definition of formation is specified in many different ways, as for example the rigid formation [11], or a desired figure in the plane [12]. Also the problem of collision among agents and with some obstacles has been studied; several solutions have been provided in a cooperative framework. In [13] the collision problem is solved for a group of agents moving in the plane, with the use of gyroscopic forces; in [14] an optimization problem is solved for a group of UAVs that search for targets in a common area while avoiding other hazardous areas. Maria Carmela De Gennaro is a PhD Student at the Universit`a del Sannio, Dipartimento di Ingegneria, piazza Roma 21, Benevento, Italy. Email:[email protected] Ali Jadbabaie is with the Department of Electrical and Systems Engineering, University of Pennsylvania, Philadelphia, PA 19104, USA. Email: [email protected]
The strategy implemented in this paper is based on the use of navigation functions. These functions were initially designed [15] in order to steer a single point-mass robot from almost all initial positions toward the desired final position in a bounded workspace, while avoiding obstacles. The navigation function itself is composed of two subfunctions, one that attracts the robot to the goal, the other that repels the robot from obstacles. Moreover, it has some parameters that can be tuned. Early implementations of navigation functions to control groups of robots were centralized. Each robot maintained its own copy of the global navigation function. In this framework each robot required full knowledge of the environment and of the state of the global system. In the recent literature there have been attempts to apply the same idea to “navigate” one or more robots in a workspace, by using a decentralized approach. Each work makes a step toward the final complete decentralization of the problem. In [16] the proposed navigation function is decentralized in the sense that it is different for each agent: it contains only the information about the distances between this agent and all the other agents and obstacles. The goal of each agent in the group is to reach a specific point in the workspace, and the information about the other agents and obstacles is used only for avoiding them; there is not a cooperative task to achieve. In [17] a limited sensing region is considered for each agent, but still without defining any common goal. In [18]- [19] a limited sensing region is defined for the obstacle avoidance, and also a common task is defined for the agents. In fact each agent has to reach a desired formation with the others, and the formation is specified in terms of relative positions and orientations. The connection among the agents is considered fixed and independent of the distances among them. In this paper the last step toward the decentralization is presented. For each agent we propose a navigation function based on the distances with all the other agents and obstacles. Two regions are defined around each agent: the connection region and the collision region, closer to the agent. In the connection region, the goal function drives the agent in order to reach the desired distances with the other connected agents. In the collision region, the collision function avoids the collision between the controlled agent and the other agents and obstacles. We describe the structure of the navigation function, compute the equilibria that can be reached, and analyze the stability properties of these equilibria. We show how the con-
nections among agents influence the equilibria determined by the navigation function. Our analysis assumes that the group remains connected all time. This is not necessarily guaranteed by the navigation function, but it is important to reach the desired final configuration in the workspace. The paper is organized in as follows. In Section II we describe the workspace, agents and obstacles. Also, for each agent we define the connectivity and collision regions. In Section III we describe the expression on the decentralized navigation function. In Section IV we analyze the equilibria of the navigation function. Finally, in Section V we show some simulations to test the effectiveness of the proposed function. II. P ROBLEM F ORMULATION The multi-agent system considered is a group of N mobile agents in R2 , each with a first order dynamics: q˙i = ui , i = 1, ..., N
(1)
where qi and ui are respectively the state and the control input of the agent i. The workspace F is assumed circular and bounded, with radius Rw , and the agents are assumed to be point masses. In the workspace there are M obstacles, modeled as fixed points p1 ,...,pM . The assumption that both agents and obstacles are points is not restrictive, because it was shown in [20] that a large class of shapes can be mapped to single points by suitable coordinate transformations. Each agent has a limited communication capability- that is, it can only communicate with agents within its neighborhood region. The neighborhood region of agent i is a circle of radius Ri (communication radius) around agent i, such that all the other agents inside this region are considered “neighbors”. The set of the i-th agent’s neighbors is given by : Ni , {j 6= i : krij k = kqi − qj k ≤ Ri }. (2) Moreover, for each agent we define a collision region, i.e. a circle of radius δi < Ri around the agent, such that any agent or obstacle inside that radius is considered an obstacle. The set of obstacles of agent i is defined as: Ci , {j 6= i : krij k ≤ δi }
[
{k = 1, .., M : kqi − pk k ≤ δi }. (3)
In the following we assume that all the agents have the same communication radius, Ri = R, and the same collision radius δi = δ, for all i. Fig. 1 shows the i-th agent neighborhood and collision regions. This multi-agent system can be represented by an undirected graph G = (V, E) where V is the set of nodes (agents) and E is the set of edges (connections (i, j)). We call graph G the connectivity graph of the system. Since the connections depend on the distances between agents, the graph G varies over time. The connection between agents can be also represented by an incidence matrix B, whose rows and columns are indexed by the vertices and edges of G respectively. Given an arbitrary orientation for the graph, the incidence matrix B is constructed by assigning the value
Fig. 1.
Neighborhood and collision region for the i-th agent.
1 to the (i, j) element of B if edge j is incoming to vertex i, −1 if edge j is outgoing from vertex i, and 0 otherwise [21]. III. D ECENTRALIZED NAVIGATION F UNCTION The goal for the multi-agent system is to reach a desired formation in the workspace, specified in terms of desired mutual distances among the connected agents. To solve this problem, a local navigation function is defined for each agent i [15]: ϕi (q) =
(γi
(q)k
γi (q) + βi (q))1/k
(4)
where: • γi (q) : F → R+ , the goal function, has only one minimum, reached when the i-th agent is at the desired distances with its neighbors; • βi (q) : F → [0, 1] is the obstacle function, and vanishes when the i-th agent collides with an obstacle or with another agent; • k is a positive tuning parameter. The navigation function is used to control each agent in order to reach the desired final destination and avoid the obstacles. In fact the control action of the i-th agent is: ui = −α∇qi ϕi (q),
(5)
where α is a positive scaling factor. Now we describe the expression of the goal and collision function. A. γi (q): The Goal Function The goal function γi is defined as the sum of pairwise goal functions γij , for all the agents j different from i: γi (q) =
N X
γij (qi , qj ).
(6)
j=1,j6=i
γij (qi , qj ) is dependent on the norm of the distance krij k and on the desired final distance cij : zij 2 ( ) krij k ≤ cij ; cij az2 ij cij < krij k ≤ R; γij = 3 +cz 2 +dz +e bzij ij ij a(R−cij )2 krij k > R, b(R−cij )3 +c(R−cij )2 +d(R−cij )+e (7)
where cij > δ, and zij = krij k−cij . The function γij (qi , qj ) reaches its unique minimum when krij k = cij ; otherwise it is positive and maximum (equal to 1) when the agents are not connected (krij k > R) or when they collide (krij k = 0), as shown in Fig. 2. The constants a, b, c, d, e are chosen to ensure the continuity of γij and its first and second derivatives in the points cij and R. The function γij is twice differentiable ∈ C 2 function on F, as shown in Fig. 3.
(kril k = 0), and its maximum equals to 1 at kril k = δ, as shown in Fig. 4. The coefficients of the polynomials depend on δ, and they are chosen in order to make βil a C 2 function on F.
Fig. 4.
Shape of the collision function βil (kril k), with δ = 5.
The function βiw (qi ) is the obstacle function between the workspace boundary and i, and it is defined in a similar way: Shape of the goal function γij (krij k), with cij = 20 and R = 50. Fig. 2.
1 pI (kqi k) pII (kqi k) βiw (qi ) = pIII (kqi k) 0
kqi k ≥ Rw − δ; Rw − δ < kqi k ≤ Rw − 3δ/4; Rw − 3δ/4 < kqi k ≤ Rw − δ/4; Rw − δ/4 < kqi k ≤ Rw ; kqi k > Rw . (10) Function βiw reaches its unique minimum at the collision with the workspace (kqi k = Rw ), and the maximum equal to 1 at kqi k ≤ Rw − δ, as shown in Fig. 5. The coefficients of the polynomials depend on δ and Rw , and they are chosen in order to make βiw a C 2 function on F.
Fig. 3. First (up) and Second (down) derivative of the goal function
γij respect to krij k.
Shape of the collision function βiw (kqi k), with Rw = 50 and δ = 5. Fig. 5.
B. βi (q): The Obstacle Function The obstacle function βi (q) is the product of pairwise obstacle functions: Y βi (q) = βil (qi , ql )βiw (qi ). (8) l∈Ci
where βil is the obstacle function between agent i and agent or obstacle l, and βiw is the obstacle function between agent i and the boundary. The function βil is given by: p1 (kril k) p2 (kril k) βil (qi , ql ) = p3 (kril k) 1
kril k < δ/4; δ/4 ≤ kril k < 3δ/4; 3δ/4 ≤ kril k < δ; kril k ≥ δ,
(9)
where p1 , p2 and p3 are third order polynomials in kril k. The function βil (qi , ql ) reaches its unique minimum at collision
The function ϕi : F → [0, 1], is a C 2 function on F, and it is admissible on F, i.e. it is uniformly maximum on obstacles and workspace boundary. In order to see if it is a navigation function [15], we have to prove that: • it is polar at the desired formation, i.e. it is minimum only if the agent i is at the desired distance among its neighbors; • and it is a Morse function in F, i.e. its critical points are non-degenerate. We analyze these two properties in the following session. IV. C RITICAL POINTS OF THE NAVIGATION FUNCTION In this paragraph we analyze the critical points of the navigation function previously defined. The navigation function is used to control each agent to reach desired distances with its neighbors, but it does not guarantee the maintenance of
the connection among the controlled agent and its neighbors. For this reason, we assume that a minimum connection level will be always present in the global system, without specifying how it is maintained. Under this assumption, we want to see how the navigation function drives each agent toward its goal. The critical points of the navigation function (4), are studied by analyzing the gradient of the function ϕi : 1 ∇qi ϕi = [kβi ∇qi γi − γi ∇qi βi ], k(γik + βi )1/k
(11)
where the gradients of the functions γi and βi are: ∇qi γi =
N X
∇qi γij ;
(12)
j=1
∇qi βi =
X l∈Ci
S
∇qi βil w
βi . βil
(13)
The critical points of ϕi are obtained when: ∇qi ϕi = 0 ⇔ kβi ∇qi γi = γi ∇qi βi .
(14)
This condition is true in two different cases that we analyze below. Case1) : ∇qi γi = 0 and ∇qi βi = 0 From ∇qi βi = 0 we know that the i-th agent is far from the workspace boundary and the obstacles. If ∇qi γij = 0 for all connected pairs of (i, j), the i-th agent is at the desired distances among the neighbors. This is the desired equilibrium, and we can prove that this equilibrium is a minimum, by evaluating the Hessian of ϕi , as shown in the following proposition. Proposition 1 The navigation function has a nondegenerate minimum at the desired formation. Proof. The Hessian of ϕi is: ∇2qi ϕi = −
1 {(γik + βi )[k∇qi βi (∇qi γi )T k(γik + βi )1/k+2 −∇qi γi (∇qi βi )T + kβi ∇2qi γi − γi ∇2qi βi ]
1 + 1 [kβi ∇qi γi − γi ∇qi βi ][kγik−1 ∇qi γi + ∇qi βi ]T } k
At the desired equilibrium, the Hessian becomes: ∇2qi ϕi = where ∇2qi γi = 2I
kβi ∇2qi γi − γi ∇2qi βi k(γik + βi )1/k+1
(15)
X 1 , c2ij
(16)
∇2qi βi = 0,
j∈Ni
and I is the identity matrix in R2X2 . The Hessian ∇2qi ϕi > 0, so the equilibrium point is a minimum. If ∇qi γi = 0 but there exists a pair (i, j) such that ∇qi γij 6= 0, then agent i will not be at the desired distance from its neighbors. This type of equilibrium is dependent on the connectivity graph of the system: in fact, we can rewrite the equilibrium condition in terms of the incidence matrix B [22]- [23].
For agent i, the equilibrium condition is: X ∇qi γi = 0 ⇔ ∇qi γij = 0.
(17)
j∈Ni
Now, given the equivalence ∇qi γij = −∇qj γij = ∇rij γij , we can rewrite (17) as: X j∈Ni
∇qi γij = 0 ⇔
N X
bij ∇rij γij = 0,
(18)
j=1
where bij is the entry (i, j) of the matrix B. By writing the equilibrium expression for all the agents, we obtain: ∇r12 γ12 ... ∇r1N γ1N ∇r23 γ23 = 0. B∇ , B (19) ... ∇r2N γ2N ... ∇rN −1,N γN −1,N The undesired equilibrium is a solution of the algebraic system (19) where the unknowns are the gradients ∇rij γij for all the connected pairs of agents (i, j). The number of solutions of the system depends on the rank of B. Now we can show the following Proposition. Proposition 2 If the connectivity graph G is a tree, then at the equilibrium the distances between the connected pairs of agents are the desired values. Proof. If the connectivity graph is a tree, the number of unknown gradients ∇rij γij is N − 1, and B ∈ RN (N −1) . It was shown in [24] that the null space of B coincides with the vector space spanned by the cycle vectors of the graph. In the case of a tree, there are no cycles in the graph; hence, we can say that the null space of B is empty. B is full rank, and the solution of the system (19) is ∇rij γij = 0. This means that in the end if agents are connected and form a tree, then they are at their desired distances. The above proposition suggests that one possible approach to avoid the undesired equilibria is to control the system to always maintain a tree connection. A more general approach should consider the dependence of the desired formation on the connection of the system. Case2) ∇qi γi 6= 0, ∇qi βi 6= 0, and kβi ∇qi γi = γi ∇qi βi The first condition says that the i-th agent does not reach the desired configuration; the second says that the agent is near to one obstacle. The equilibrium reached is not the desired one, but it is possible to tune the parameter k in order to push this undesired equilibrium close to the obstacle. It is also possible to guarantee that this kind of equilibrium is not a local minimum, but only a saddle point. Two Propositions exploit these characteristics of the navigation function. Proposition 3 The critical points analyzed above are in the interior of the workspace F. Proposition 4 It is always possible to find a lower bound for the parameter k, such that:
the collision region near the boundary, but then it enters the same region to reach the desired distances with the others. Fig. 13 shows the distances among the agents. VI. C ONCLUSIONS
Motion of the agents on the plane and convergence to the desired formation. Fig. 6.
the critical points of the navigation function, different from the desired equilibrium, are near to one of the obstacles; • the critical points found near the obstacles are not local minima and not degenerate (ϕi is a Morse function). The propositions can be easily proved for the navigation function introduced here, and they give a lower bound to the parameter k. For proofs, the interested reader is referred to [19]. •
V. S IMULATION RESULTS In this section we numerically verify how the designed navigation function works. The workspace has radius Rw = 60. The multi-agent system considered is composed by three agents, and one obstacle is fixed in the origin of the workspace. We will see how there can be an undesired equilibrium also in this case with few agents. The collision radius is δ = 5, and the connection radius is R = 40. The constants of the control action are α = 1 and k = 1. In the following Figures the initial positions of the agents are represented with ‘∗’ and the final positions with ‘♦’, the regions around the obstacle and the workspace are represented with dashed lines. In all the following examples, the desired configuration is given by distances equal to c = 15 for all the connected pairs of agents. In the first example, shown in Fig. 6, the agents 1 and 3 are initially not connected but during the motion they connect and in the end they reach the desired distance, as shown in Fig. 7. In the second example, shown in Fig. 8, an undesired equilibrium is reached. It is due to the connections graph, that presents a cycle. We see in Fig. 9 that the final distances are different from the desired ones. In the third example, the graph of connections is always a tree. In Fig. 10 we see that agents 1 and 3 are always disconnected over time. Under this condition, the desired distances among the connected agents are reached, as we see in Fig. 11. Fig. 12 shows an example near the boundary of the workspace. Agent 2 exits
A decentralized navigation function is proposed to drive each agent of a group toward a desired final configuration which is expressed in terms of distances between the connected agents. The formation can be reached anywhere in the space and with any orientation. The navigation function assures to avoid collision between the controlled agent and the other agents and obstacles. The connection among the agents is dependent on their mutual distance. The proposed function has all the characteristics of a navigation function, but it introduces some equilibria that cannot be avoided by tuning parameters. These equilibria are related to the connection among the agents. Future work include the study of techniques that allow the system to avoid the undesired equilibria. R EFERENCES [1] J. P. Desai, J. P. Ostrowski, and V. Kumar, “Modeling and control of formations of nonholonomic mobile robots,” IEEE Trans. on Robotics and Automation, vol. 17 no.6, pp. 905908, December 2001. [2] W. Ren and R. Beard, “Trajectory tracking for unmanned air vehicles with velocity and heading rate constraints,” IEEE Trans. on Control System Technology, vol. 12 no.5, September 2004. [3] D. B. Edwards, T. Bean, D. Odell, and M. Anderson, “A leaderfollower algorithm for multiple auv formations,” Proc. of 2004 IEEE/OES Autonomous Underwater Vehicles, Sebasco Estates, Maine, June 2004. [4] Y. Liu and K. M. Passino, “Stable social foraging swarms in a noisy environment,” IEEE Trans. on Automatic Control, vol. 49, no. 4, January 2004. [5] W. M. Spears, D. F. Spears, J. C. Hamann, and R. Heil, “Distributed, physics-based control of swarms of vehicles,” Autonomous Robots 17, 137-162, 2004. [6] A. Jadbabaie, J. Lin, and A. S. Morse, “Coordination of groups of mobile autonomous agents using nearest neighbor rules,” IEEE Trans. on Automatic Control, vol. 48, no. 6, pp. 988–1001, June 2003. [7] N. L. P. Ogren, E. Fiorelli, “Cooperative control of mobile sensing networks: Adaptive gradient climbing in a distributed environment,” IEEE Trans. on Automatic Control, vol. 49(8), pp. 1292–1302, August 2004. [8] J. Cortes, S. Martinez, T. Karatas, and F. Bullo, “Coverage control for mobile sensing networks,” IEEE Trans. on Robotics and Automation, vol. 20, no. 2, pp. 243–255, February 2004. [9] R. O. Saber and R. M. Murray, “Flocking in multiagent dynamic systems: Algorithms and theory,” IEEE Trans. on Automatic Control, vol. 51, March 2006 (in press). [10] E. Justh and P. Krishnaprasad, “Equilibria and steering laws for planar formations,” Systems and Control letters, vol. 52, no. 1, pp. 25–38, May 2004.
Fig. 7. Distances among the agents for the first example: the agents
reach the desired distances, all equal to c.
Motion of the agents on the plane and convergence to a local minimum. Fig. 8.
Fig. 9.
Motion of the agents when the graph of connection is a tree. Agent 2 is connected with 1 and 3. Fig. 10.
Distances among agents in the case of local minimum.
[11] R. O. Saber, W. B. Dunbar, and R. M. Murray, “Cooperative control of multi-vehicle systems using cost graphs and optimization,” Proc. of the American Control Conference, Denver, Colorado, June 2003. [12] T. Balch and R. Arkin, “Behavior-based formation control for multirobot teams,” IEEE Trans. on Robotics and Automation, vol. 14, no. 6. [13] D. E. Chang, S. C. Shadden, J. E. Marsden, and R. O. Saber, “Collision avoidance for multiple agent systems,” Proc. of the 42nd IEEE Conference on Decision and Control, Maui, Hawaii, USA, December 2003. [14] R. W. Beard and T. W. McLain, “Multiple uav cooperative search under collision avoidance and limited range communication constraints,” Proc. of the 42nd IEEE Conference on Decision and Control, Maui, Hawaii, USA, December 2003. [15] E. Rimon and D. E. Koditschek, “Exact robot navigation using artificial potential functions,” IEEE Trans. on Robotics and Automation, vol. 8, no. 5, pp. 501-518, 1992. [16] D. V. Dimarogonas, M. M. Zavlanos, S. G. Loizou, and K. J. Kyriakopoulos, “Decentralized motion control of multiple holonomic agents under input constraints,” Proc. of the 42nd IEEE Conference on Decision and Control, Maui, Hawaii USA, December 2003. [17] D. V. Dimarogonas and K. J. Kyriakopoulos, “Decentralized stabilization and collision avoidance of multiple air vehicles with limited sensing capabilities,” Proc. of the 2005 American Control Conference, Portland, Oregon, USA, 2005. [18] H. G. Tanner and A. Kumar, “Formation stabilization of multiple agents using decentralized navigation functions,” Robotics: Science and Systems, Boston, 2005. [19] ——, “Towards decentralization of multi-robot navigation functions,” Proc. of the 2005 IEEE Conference on Robotics and Automation, April 2005. [20] H. G. Tanner, S. G. Loizou, and K. J. Kyriakopoulos, “Nonholonomic navigation and control of cooperating mobile manipulators,” IEEE Trans. on Robotics and Automation, vol. 19, no. 1, pp.53-64, February 2003. [21] C. Godsil and G. Royle, Algebraic Graph Theory. Springer, 2001. [22] H. G. Tanner, A. Jadbabaie, and G. Pappas, “Flocking in fixed and switching networks,” Submitted to IEEE Trans. on Automatic Control. [23] M. C. D. Gennaro, L. Iannelli, and F. Vasca, “Formation control and collision avoidance in mobile agent systems,” Proc. of the IEEE 13rd Mediterranean Conference on Control and Automation, Lymassol, Cyprus, June 2005. [24] S. Guattery and G. L. Miller, “Graph embeddings and laplacian eigenvalues,” SIAM J. Matrix Anal. Appl., vol. 21, no. 3, pp. 703-723, 2000.
Fig. 11. Distances among the agents when the graph of connection
is a tree. The desired distances among connected agents 1 − 2 and 2 − 3 are reached.
Motion of the agents near the boundary and convergence to the desired configuration. Fig. 12.
Fig. 13.
Distances among the agents near to the workspace.
