Decentralized Formation Control and Connectivity Maintenance of Multi-Agent Systems using Navigation Functions

Transcription

1 Decentralized Formation Control and Connectivity Maintenance of Multi-Agent Systems using Navigation Functions Alireza Ghaffarkhah, Yasamin Mostofi and Chaouki T. Abdallah Abstract In this paper we consider a team of mobile robots (agents, with limited sensing capabilities, that are tasked with forming a pre-specified assembly in a cluttered environment occupied with several obstacles. The goal is to design decentralized navigation functions which can navigate the robots to a set that contains the desired assembly (formation while avoiding collisions. We prove the convergence of our proposed navigation function framework using a hybrid system approach. We also show that the proposed framework can preserve the connectivity of the underlying sensing graph. Finally, our simulation results show the performance of the proposed framework. I. INTRODUCTION Coordination and control of cooperative multi-agent systems has attracted a considerable amount of attention over the last few years. Consensus, coverage control, flocking and formation control are among the important problems that have been studied in this field. The goal of this paper is to propose a distributed control scheme for formation control of multi-agent systems with limited sensing capabilities. Applications include the coordination of multiple mobile robots, unmanned air vehicles (UAVs and aircrafts [1]-[10]. So far, several methods have been proposed for formation control of multi-agent systems. In the leader-follower approach [4]-[7], a few agents (leaders track their predefined traectories while other robots (followers move based on the states of their nearest neighbors. A disadvantage is that the leaders receive no feedback from the followers, making the maintenance of desired formation difficult. Also, the obstacle avoidance is rarely addressed in this approach. In the behavioral approach [8], the idea is to define several behaviors such as obstacle avoidance, inter-robot collision avoidance and target tracking. The relative importance of each behavior then specifies the movement of each agent. However, the mathematical analysis of the behavioral approaches is difficult and the convergence cannot be proved analytically. In the optimization-based approach [9], [10], the formation is controlled by solving online optimization problems. In [9], the authors formulate an optimization problem over cost graphs to reach the formation in an obstacle-free environment. In [10], an optimization problem This work is ported in part by ARO CTA MAST proect # W911NF A. Ghaffarkhah and Y. Mostofi are with the Cooperative Network Lab, Department of Electrical and Computer Engineering, University of New Mexico, Albuquerque, NM 87113, USA {alinem, ymostofi}@ece.unm.edu C. T. Abdallah is with the Department of Electrical and Computer Engineering, University of New Mexico, Albuquerque, NM 87113, USA chaouki@ece.unm.edu is solved for a group of UAVs in order to track a target while avoiding other hazardous areas. Navigation functions are special types of artificial potential fields and have been extensively used in motion planning literature for many years [11]-[14]. However, formation control of multi-agent systems via navigation functions is a new topic. In [13], the authors propose a decentralized navigation function framework for formation control in case of perfect sensing. Another attempt to use navigation functions for formation stabilization is reflected in [14]. But, the authors in [14] assume that the group remains connected all the time. This is not necessarily guaranteed by the navigation functions, but it is crucial in order to guarantee the convergence. In this paper, we propose a novel extension of the navigation functions for decentralized formation control of multi-agent systems which ensures convergence, collision avoidance and connectivity maintenance at the same time. To make our scenario more realistic, we assume limited sensing capabilities, meaning that each robot only senses the robots and obstacles that are within its sensing radius. This way, the overall closed-loop system will be a nonlinear switching one with state dependent switchings [15], since each robot updates its navigation function when it senses a new robot or obstacle. The authors have previously used the concept of switching navigation functions for communication-aware motion planning of robotic networks [16]. This paper shows another application of switching navigation functions while providing some proofs that were missing in [16]. In this paper, the formation is defined as a configuration in the collision-free space each agent is at prespecified relative distances from other robots. As a result, the desired positions of the agents can be any in the space as long as the formation is achieved. This makes the use of the navigation functions more challenging, as the classical navigation functions only consider one set of desired positions for the agents. The rest of this paper is organized as follows: In Section II, we present a mathematical framework for a general formation control problem. In Section III, we show how to build our novel decentralized navigation functions and provide the correctness analysis. Then, the simulation results of Section IV show the performance of our navigation framework when applied to a simple robotic network. II. PROBLEM FORMULATION Consider a team of mobile robots that are posed to form a predefined assembly. They start from an initial configuration in a cluttered environment occupied with several

2 obstacles. The robots are equipped with sensing devices to measure the positions of the obstacles and other robots. However, their local capabilities are limited, meaning that each robot can only sense the robots and obstacle that are within a certain range around it. We consider a spherical workspace W = { q R 2 q R } R 2 punctured by m disoint disc-shaped obstacles and n disc-shaped robots. The assumption of spherical workspace is not limiting as long as we can find a diffeomorphism that translates the given workspace into a spherical one [11], [17]. The free configuration space as we will define later, is a connected manifold in R 2n. Thus, the diffeomorphism applicable to this multi-robotic scenario will be a 2n-dimensional extension of what proposed in [17] considering inter-robot collisions as well. The robots and the obstacles are specified by the following sets R = { q R 2 q q r }, 1 m + n, (1 q R 2 is position of the th robot (or the th obstacle and r is its radius. The first n sets specify the robots and the rest specify the obstacles which are considered stationary robots. The overall state of the system is denoted by q =[q1 T qn T ] T. We assume holonomic robots with the following dynamics q = u, 1 n, (2 u R 2 is the control input to be designed. Each robot measures its own position as well as the positions of the robots and the obstacle which are within its sensing range. In this paper, we assume that the robots use homogeneous sensors with the same sensing range, denoted by r s max 1 i n+m, 1 n (r i + r. By the Sensing Graph G =(V, E, we mean an undirected graph the node set V = {v 1,,v n } is correspondent to the set of mobile robots and the edge set E is a set of unordered pairs of nodes, with {v i,v } Eif and only if robots i and can sense each other (i.e. q i q r s. This graph is dynamic and depends on the positions if the robots. We assign two different sets N and O to the th robot N { i qi q r s, 1 i n, i }, O { i qi q r s,n+1 i m + n }. (3 We refer to N and O as the neighbor set and the obstacle set of the th robot respectively. The positions of the obstacles and robots in these two sets form the local information available to the th robot 1. Assuming that the initial sensing graph is connected, the goal is to synthesize decentralized control laws based solely on local information, to navigate the robots to a set that contains the desired configuration (assembly or formation, while preserving the connectivity of the sensing graph and avoiding collisions. 1 Without loss of generality, we assume that the workspace is large enough that does not constraint the movement of the robots. Thus, there is no need to consider the boundary of the workspace in O. Note that our decentralized controllers introduced in the next section, enforce any two connected robots (which can sense each other to remain connected. As a result, the connectivity of the sensing graph would be nondecreasing in time. A formation pattern is defined by a set of all desired distances between the robots. The desired distance between the ith and the th robot is denoted by d i,. In this paper, we assume that (r i + r <d i, <r s for all i and. This means that the final sensing graph is a fully connected one. III. DECENTRALIZED FORMATION CONTROL USING NAVIGATION FUNCTIONS In this section, we propose decentralized controllers based on navigation functions. We show how the proposed control scheme has all the required properties (convergence to a set that contains the desired formation, connectivity maintenance and collision avoidance. Note that the decentralized navigation functions that we introduce in this section, do not have all the properties of the traditional navigation functions as studied by Rimon and Koditschek [11]. For instance, they have multiple minima in the free configuration space and experience different structures as the robots enter the sensing regions of each other. Still, it can be shown that these navigation functions have good properties that can result in stability and collision avoidance. In the rest of the paper, when introducing our navigation functions, we agree the following assumptions for the sake of mathematical proofs: The probability that a robot collides with more that one robot (or obstacle at the same time is consistently low. At any time, only one robot can enter the sensing range of another one. As discussed before, the nodes i and are adacent in the sensing graph if their relative distance is less than r s.in order to enforce the connected nodes to stay connected as they move, we modify the definition of the obstacle function in [11]. We introduce the extended obstacle function for the th robot as β (q = β i, (q i,q λ i, (q i,q, 1 n, O and (4 β i, (q i,q = q i q 2 (r i + r 2, (5 λ i, (q i,q =r 2 s q i q 2. (6 When a node enters the sensing region of another node, both of them switch to other obstacle functions to maintain their connectivity. Hence, the cardinality of N is a nondecreasing function of time. At any time, the collision-free space (in which the sensing graph is connected is a compact connected analytic manifold F R 2n with F denoting its boundary. We have ( n F = F i, =1 O ( n S i, =1, (7

3 F i, { q βi, (q i,q 0 }, (8 S i, { q λi, (q i,q 0 }. It is important to note that the collision-free space is a dynamic time-varying set and changes when the robots enter the sensing regions of each other. As long as the robots are within the interior of the current F, the control obective is to minimize a scalar function whose minima occur when the robots are in their desired formation considering only their current neighbors. We propose a dynamic energy-type obective function of the following form for the th robot: J (q = γ i, (r i,, (9 r i, = q i q and γ i, : [0, [0, is a positive scalar function which is differentiable every but the origin. We require that for all : γ i, (d i, =0, γ i, (d i, =0, γ i, (d i, > 0, γ i,(r i, r s =0, (10 d i, is the desired distance between the ith and the th robots. One possible choice for γ i, could be γ i, (r i, = ax 3 i, r i, d i,, bx 3 i, b(r s d i, x 2 i, d i, <r i, r s, 1 2 b(r s d i, 3 r i, >r s, (11 x i, = r i, d i, and the constants a and b are positive constants. Fig. 1 shows a sample plot of γ i,. Clearly, J (q gamma i, r i, Fig. 1. A sample plot of the γ i, for d i, =3and r s =10. has several minima inside the collision-free space and any of these minima can be a potential desired configuration for the th robot. Since (r i + r <d i, <r s, at any time the desired configurations are within the interior of F We now propose the following decentralized navigation function for the th robot: J (q ϕ (q = ( 1/, (12 J (q+β (q >0 is a tuning parameter. The control signals are then calculated as u = μ q ϕ (q μ is a positive gain. One can easily find the following statements true about the proposed navigation function: The function ϕ has a time-varying structure as the robots enter the sensing region of each other. The ϕ has more than one local minima in the collisionfree space. Next, we will show how the proposed navigation function enforces collision avoidance and connectivity maintenance while converging to a set that contains the desired formation. A. Proof of Correctness We show the correctness of our proposed control scheme by proving the following lemmas. In the proofs we make use of the following sets: The set near collision or switching : ( n ( n F 0 (δ, ε B i, (ε L i, (ε F d (δ, =1 O =1 (13 The set away from collision and switching : F 1 (δ, ε F { F d (δ F 0 (δ, ε F }, (14 F d (δ { q 0 q J (q <δ, 1 n }, B i, (ε { q 0 <β i, (q i,q <ε }, L i, (ε { q 0 <λ i, (q i,q <ε }. (15 Note that all these sets are time variant. In the following lemmas, we consider a snapshot of these sets at a particular time. As long as the robots are within F 1 (δ, ε for small δ>0and ε>0, there will be no collision or switching and q J (q 0at least for one. We assume that the workspace is large enough and there exist δ 0 > 0 and ε 0 > 0 such that for δ<δ 0 and ε<ε 0, F 1 (δ, ε is a compact connected manifold in which infinite number of movements can be considered for the team. In the rest of this paper, we mean by valid workspace, a workspace for which δ 0 and ε 0 can be found. In the following lemma, we prove that any desired configuration of the team is indeed a valid local minimum of the ϕ for all. Lemma 1: If the work space is valid, any desired configuration q d { q qi q = d i,, 1 i< n } is a valid local minimum of ϕ (q for all. Proof: For all, wehave q ϕ (q d = β q J J q β ( J + β 1+1/ =0, (16 qd since J (q d = γ i, (d i, =0and q J (q d = γ i,(d i, q q i =0. (17 d i,

4 Also, since 2 q J (q d = we get γ i, (d i, (q q i (q q i T d 2 i, 0, (18 2 q ϕ (q d =β 1/ 2 q J qd > 0. (19 In the next lemma, we prove that there exists no critical point of ϕ at the boundary of F. We furthermore show how the proposed navigation function can guarantee the collision avoidance and connectivity maintenance. Lemma 2: If the workspace is valid, all the critical points of ϕ (q for all are within the interior of F. Proof: Let q b F. Based on our earlier assumptions, for any robot the probability of more than one collision or switching is negligible. This implies that for the th robot, not more than one term can be zero in Eq. 4. Consider a pair {i, } such that β i, =0or λ i, =0.Ifβ i, =0, similar to proposition 3.3 of [12] we have q ϕ (q b = J ( β k, k N O k i k N λ k,. q β i, qb 0, (20 since q β i, = 2(q q i. This also implies that q ϕ (q b will point toward the interior of the workspace (collision avoidance. Similarly, if λ i, =0we have q ϕ (q b = J ( λ k, k N k i k N O β k,. q λ i, qb 0, (21 for q λ i, =2(q i q. Furthermore, this shows that when q i q = r s, the navigation function forces the ith and th robot to get closer to each other (connectivity maintenance. Next, we show that if the workspace is valid and is large enough, there exists no common critical point of the navigation functions in F 1 (δ, ε. Lemma 3: Assume that the workspace is valid and select δ<δ 0 and ε<ε 0. Then there exists a positive N(δ, ε such that the configurations, for all robots q ϕ =0, are not inside F 1 (δ, ε as long as >N(δ, ε. Proof: The critical points of ϕ (q are obtained when β q J = J q β. (22 Since β > 0 inside F 1 (δ, ε, three different cases may arise: 1 q J = 0 and J = 0 for all : This case only happens if the robots are at the desired configuration (corresponding to the current sensing graph. The critical points of this case are in F d (δ and not F 1 (δ, ε. 2 q J =0for all but for a set of robots, J 0 and q β =0:If q β =0, we conclude that the th node is far from collisions. In this case, the points for all robots q ϕ =0, can be obtained by solving q J =0and J q β =0for all. These points reside in F d (δ too. From the definition of J we have q J =0 q q i γ i, r (r i, =0. (23 i, By defining the stacked vector of the derivatives: γ 1,2 (r 1,2.. g γ 1,n(r 1,n γ 2,3 (r 2,3, (24. γ n 1,n (r n 1,n we can rewrite (23 as A ( q g =0, A ( q is matrix which depends on the current configuration of the system. The feasibility of this set of equations depends on the rank of A ( q which itself is a direct function of the adacency matrix of the sensing graph. Such system has 2n equations and a number of unknown variables which are the derivatives γ i, (r i, when nodes i and are connected. The number of unknown variables is at most n(n 1/2 (when the graph is fully connected and at least n 1 (when the graph is a tree. In case of a fully connected graph, if n 5, the number of equations are greater or equal to the number of unknowns variables. The only solution is then proved to be g =0, which is the case when the robots are at the desired formation. If n>5 for a fully connected graph, there may exist multiple nonzero solutions. Similar conditions can be found for other types of sensing graphs. For example it can be proved that if the sensing graph is a tree, the solution of A ( q g =0is still unique and is given by g =0 [18]. But practically, the situations for all robots A ( q g =0and J q β =0simultaneously, are rare and can be ignored in most cases. 3 For a set of robots q J 0 and q β 0 but β q J = J q β : If the robots are within F 1 (δ, ε, we know that at least for one robot q J δ. We show that it is possible to select large enough to push such undesired equilibrium points out of F 1 (δ, ε. Let us define l F 1(δ,ε q J δ J q J. (25 Then, the sufficient condition for the whole team not to get stuck inside F 1 (δ, ε ( q ϕ 0for at least one robot is [ > max F 1(δ,ε We have q β = q β i, β β i, O ] q β β + max l. (26 q λ i, λ i,, (27

5 q β i, =2(q q i and q λ i, =2(q i q. This implies that q β 4R(2 N + O, (28 F 1(δ,ε β ε R is the dimension of the workspace, N is the cardinality of N and O is the cardinality of O. Thus, in order to push all the undesired critical points of this kind out of F 1 (δ, ε, it is sufficient to choose N(δ, ε = 4R(2n + m 2 ε max l. (29 Note that following a similar approach to Proposition 3.6 and 3.7 of [12], it can be shown that no local minimum of the navigation functions can exist in F 0 (δ, ε as long as >N(δ, ε. We omit the details here for the sake of the space. While the aforementioned lemmas prove collision avoidance, connectivity maintenance and other required properties for the proposed navigation functions, they cannot guarantee the convergence of the overall system. The main challenge is that each node switches its navigation function whenever another node enters its sensing region. In order to address this, we model the overall system as a hybrid nonlinear system with state-dependent switching [15]. Many stability analysis tools for hybrid and switching systems have been proposed [15]. In this paper our goal is to find a Common Lyapunov Function [15] for the whole system. First, we define the extended obstacle function and total obective function (total energy of the whole system in case the sensing graph is fully connected: β(q = J(q = n n+m =1 i=+1 n n =1 i=+1 β i, (q i,q γ i, (r i,, n n =1 i=+1 λ i, (q i,q, (30 β i,, λ i, and γ i, have been defined in Eq. 4 and Eq. 9. The centralized navigation function is then proposed as J(q ϕ(q = ( 1/. (31 J (q+β(q One can easily prove the centralized version of the Lemma 1 to 3 for this function [19]. We choose ϕ(q as a common Lyapunov function candidate for the whole system. Then, we use the extension of LaSalle s invariant principle for the switching systems [15] to prove the following lemma. Lemma 4: Define D(δ { q, q J <δ }. Assume that the robots start from F 1 (δ, ε D(δ. Then, a positive number M(δ, ε can be found such that for >M(δ, ε the control signals calculated as u = μ q ϕ (q will navigate the whole system to the largest invariant subset of D(δ X, without any collision, X { } q i, q i q = r s. Proof: We have n ( ϕ = q ϕ T n ( q = μ q ϕ T q ϕ, (32 =1 =1 q ϕ = β q J J q β ( J + β 1+1/, q ϕ = β q J J q β ( J + β 1+1/. (33 Due to the definition of the obective function n q J = q γ i, = q γ i, = q J, (34 =1 since q γ i, =0if nodes i and are not connected. Lets define a q J, b J β q β, b J q β. (35 β The sufficient condition for ϕ <0 in F 1 (δ, ε D(δ is a 2 > 1 ( a T (b + b + bt b,. (36 We have a T (b + b < a 2 + b 2 + b 2, (37 2 which implies a T (b + b + bt b < a 2 + b + b 2, (38 2 for sufficient large. Thus, its sufficient to have a 2 > a 2 + ( b + b 2,. (39 2 These conditions can be translated to a single condition for exponent > [ ] 2 b + b, (40 2 F 1(δ,ε D(δ a Following a procedure similar to Lemma 3 [ ] b 4R(2n + m 2 J F 1(δ,ε D(δ a ε F 1(δ,ε D(δ q J and F 1(δ,ε D(δ [ b 4Rn(n + m 1 a ε F 1(δ,ε D(δ J (41 q J (42 which can be used to find M(δ, ε, a lower bound for. By deploying the extension of LaSalle s invariant principle for the switching systems [15], the team will converge to the largest invariant subset of D(δ X. Analyzing the unwanted situations the system traps insides D(δ X is left as a future work. Note that maintaining the connectivity of the sensing graph (as we proved before is a necessary condition for the team to be able to reach the desired configuration as long as the sensing radius is large enough (conservatively, one can select r s (n 1 max i, d i,. ],

6 IV. SIMULATION RESULTS In this section, we present the simulation results for two different cases with different number of robots. In the first case, three mobile robots with radius 2.0, form a predefined triangular assembly in the free space. In the second case, we repeat the simulations for a team of four mobile robots with radius 2.0 and a rectangular assembly. Fig. 2 and 3 show the traectory of the robots from the beginning to the end for the first and second case respectively. In both cases, the robots start from connected sensing graphs (shown by dashed lines. In the simulations, we set r s =40.0. The desired configuration in the first case is specified by { d 1,2 = d 1,3 = d 2,3 =10.0 }. Similarly, the second desired configuration is specified by { d 1,2 = d 1,3 = d 2,4 = d 3,4 =10.0, d 1,4 = d 2,3 = }. In the figures, the empty boxes and the filled ones denote the initial and final positions respectively. Fig. 2. Traectories of the robots in case 1. Fig. 3. Traectories of the robots in case 2. V. CONCLUSIONS AND FURTHER EXTENSIONS In this paper we considered a team of mobile agents, with limited sensing capabilities, that are tasked with forming a prescribed assembly in a cluttered environment occupied by several obstacles. We designed decentralized navigation functions that could navigate the robots to a set containing the desired assembly or formation, while avoiding collisions. We also showed that the proposed decentralized navigation functions preserve the connectivity of the sensing graph. At the end, we provided simulation results to show the performance of the proposed framework. The proposed formation control technique can also be used for formation changing and reconfiguration. The team can change its current formation by switching to another navigation function corresponding to the new formation. REFERENCES [1] 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 , June [2] R. Olfati-Saber and R. M. Murray, Distibuted cooperative control of multiple vehicle formations using structural potential functions, The 15th IFAC World Congress, June [3] K. D. Do, Formation control of mobile agents using local potential functions, Proc. of the American Control Conference, Minneapolis, Minnesota, USA, June [4] 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 [5] M. Mesbahi and F. Y. Hadaegh, Formation flying control of multiple spacecraft via graphs, matrix inequalities and switching, AIAA J. Guidance, Control and Dynam., vol. 24, no. 2, pp , [6] 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 , December [7] P. K. C. Wang, Navigation strategies for multiple autonomous mobile robots moving in formation, J. Robot. Syst., vol. 8, no. 2, pp , [8] T. Balch and R. C. Arkin, Behavior-based formation control for multirobot teams, IEEE Trans. Robot. Automat., vol. 14, pp , [9] 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 [10] 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 [11] E. Rimon and D. Koditschek, Exact robot navigation using artificial potential functions, IEEE Transactions on Robotics and Automation, vol. 8, no. 5, pp , October [12] D. E. Koditschek and E. Rimon, Robot navigation functions on manifolds with boundary, Advances in Applied Mathematics, Vol. 11, Issue 4, pp , December [13] H. G. Tanner and A. Kumar, Formation stabilization of multiple agents using decentralized navigation functions, Robotics: Science and Systems, Boston, [14] M. C. Gennaro and A. Jadbabaie, Formation Control for a Cooperative Multi-Agent System using Decentralized Navigation Functions, Proc. of the American Control Conference, Minneapolis, Minnesota, USA, June [15] D. Liberzon, Switching in Systems and Control, Birkhauser, Boston, [16] A. Ghaffarkhah and Y. Mostofi, Communication-Aware Navigation Functions for Robotic Networks, to appear, American Control Conference, St. Louis, Missouri, June [17] E. Rimon and D. Koditschek, The construction of analytic diffeomorphisms for exact robot navigation on star worlds, Trans. Amer. Math. Soc., vol. 327, no. 1, pp , Sept [18] C.D. Godsil, G. Royle, Algebric Graph Theory, Springer-Verlag, [19] A. Ghaffarkhah and Y. Mostofi, Communication-Aware Target Tracking using Navigation Functions - Centralized Case, to appear, International Conference on Robot Communication and Coordination (RoboComm, Odense, Denmark, 2009.