Distributed coordination control for multi-robot networks using Lyapunov-like barrier functions

Transcription

1 IEEE TRANSACTIONS ON 1 Distribute coorination control for multi-robot networks using Lyapunov-like barrier functions Dimitra Panagou, Dušan M. Stipanović an Petros G. Voulgaris Abstract This paper aresses the problem of multi-agent coorination an control uner multiple objectives, an presents a set-theoretic formulation amenable to Lyapunov-base analysis an control esign. A novel class of Lyapunovlike barrier functions is introuce an use to encoe multiple, non-trivial control objectives, such as collision avoiance, proximity maintenance an convergence to esire estinations. The construction is base on recentere barrier functions an on maximum approximation functions. Thus, a single Lyapunov-like function is use to encoe the constraine set of each agent, yieling simple, graient-base control solutions. The erive control strategies are istribute, i.e., base on information locally available to each agent, which is ictate by sensing an communication limitations. Furthermore, the propose coorination protocol ictates semi-cooperative conflict resolution among agents, which can be also thought as prioritization, as well as conflict resolution with respect to an agent (the leaer) which is not actively participating in collision avoiance, except when necessary. The consiere scenario is pertinent to surveillance tasks an involves nonholonomic vehicles. The efficacy of the approach is emonstrate through simulation results. I. INTRODUCTION Multi-agent systems have seen increase interest uring the past ecae, in part ue to their relevance to many research omains an real worl applications. Depening on the global an local/iniviual objectives, various istribute coorination an control problems have been introuce, namely consensus (also seen as agreement/synchronization/renezvous), formation, istribute optimization an istribute estimation; for a recent survey on the relate topics the reaer is referre to [1]. When it comes to multi-vehicle systems in particular, a common groun may be that multiple agents nee to work together in a collaborative fashion in orer to achieve one or multiple common goals. Coorination an control in such cases is naturally ictate by the available patterns on sensing an information sharing, as well as by physical/environmental constraints an inherent limitations (e.g., motion constraints, obstacles, unmoele This work has been supporte by Qatar National Research Fun uner NPRP Grant an AFOSR grant FA Dimitra Panagou is with the Department of Aerospace Engineering, University of Michigan, Ann Arbor; panagou@umich.eu. Dušan M. Stipanović is with the Coorinate Science Laboratory an the Inustrial an Enterprise Systems Engineering Department, University of Illinois at Urbana-Champaign; usan@illinois.eu. Petros G. Voulgaris is with the Coorinate Science Laboratory an the Aerospace Engineering Department, University of Illinois at Urbana- Champaign; voulgari@illinois.eu.

2 IEEE TRANSACTIONS ON 2 isturbances, input saturations etc). Therefore, the problem of motion planning, coorination an control has been an still remains an active topic of research within both the robotics an control communities. While it is out of the scope of this paper to provie an overview of the existing methoologies on these topics, the intereste reaer is referre to [1], [2] an the references therein for more information. The main concerns when coorinating the motions of multi-vehicle or multi-robot (the terms are use interchangeably) teams inclue inter-agent collision avoiance, convergence to spatial estinations/regions or tracking of reference signals/trajectories, maintenance of information exchange among agents an avoiance of physical obstacles. Such objectives are encountere in flocking [3] [8], an in consensus, renezvous an/or formation control [9] [14]. Collision avoiance is an unnegotiable requirement in such problems, an is often aresse with potential function methos an Lyapunov-base analysis. For a recent survey on potential function methos in formation control an similar problems see [15]. It is worth mentioning that these contributions o not consier all the aforementione control objectives. In fact, the algorithmic planning an control esign in such cases is, to the best of our knowlege, a very challenging, often intractable problem, an still remains an open issue in many respects. Recently there has been significant interest in the eployment of robotic networks (or teams) for exploration, surveillance an patrolling of inaccessible, angerous or even hostile (inoor an outoor) environments, such as oil rilling platforms, nuclear reactors etc, see [16] [22] an the references therein. Solutions to the relevant problems range from combinatorial motion planning to optimization-base an Lyapunov-base methos. A. Overview This paper is motivate in part by surveillance applications which bring in the nee for the evelopment of multirobot coorination an control algorithms uner multiple objectives. We consier a network of mobile agents which are assigne with the task to converge an remain close to preefine goal estinations, while avoiing collisions an while maintaining connectivity, realize as preserving upper boune istances with respect to (w.r.t.) one agent calle the leaer of the network. The leaer is not actively trying to avoi the remaining agents (followers), an is only responsible for communicating goal estinations to them. Note that the one leaer - multiple followers terminology aopte here oes not follow the usual sense in the relate literature; the agents are not assigne with the task to keep fixe istances w.r.t. a physical or virtual leaer, or move in a rigi geometric formation. We propose a motion coorination approach relying on a set-theoretic formulation [23] which is amenable to Lyapunov-like analysis an istribute control esign. More specifically: 1) We introuce a class of Lyapunov-like barrier functions in orer to encoe multiple, non-trivial control objectives, such as collision avoiance, connectivity (interprete as proximity) maintenance, an convergence to esire estinations. The construction is base on the concept of recentere barrier functions [24] an on the approximation functions introuce in the authors earlier work [25]. Therefore, a single Lyapunov-like function is use to encoe the constraine set of each agent, yieling simple, graient-base control solutions. In this respect, one of the merits of the propose Lyapunov-like barrier functions is the flexibility they offer

3 IEEE TRANSACTIONS ON 3 regaring to the composition of multiple control objectives. 2) We evelop istribute coorination an control strategies for each agent, which are characterize by specific levels of ecentralization in terms of information sharing. The consiere ecentralization levels are ictate by the type of information require for an agent to accomplish the esire objectives. B. Contributions an Organization The contributions of the current paper lie in: 1) The introuction of the novel class of Lyapunov-like barrier functions, which offer the flexibility to compose multiple control objectives for the multi-agent system into a single one for each agent, an thus provie a starting point for the control esign an analysis. 2) The aopte set-theoretic formulation for the control esign an analysis. More specifically, Nagumo s Theorem [23] provies the necessary an sufficient conitions for ensuring system safety (in terms of avoiing collisions an maintaining connectivity), an therefore alleviates the stanar control esign practice on forcing a common (or multiple) Lyapunov function(s) to always ecrease along the system trajectories, as it is often one in the relate literature. 3) The consieration of nonuniform agents in terms of assigne objectives an sensing/ communication capabilities. In particular, we consier a single heterogeneous agent (the leaer) who is not involve in ensuring collision avoiance (except when necessary), in contrast to the remaining homogeneous agents (followers). 4) The propose istribute coorination an control protocol. More specifically: on one han, the aopte assumptions on the available sensing an communication are relevant to realistic applications such as surveillance missions, where multiple robotic agents nee to collaborate towars the accomplishment of a common task uner limite information. On the other han, by aopting the necessary an sufficient conitions of Nagumo s Theorem on renering a given set (weakly) positively invariant, we immeiately have the necessary an sufficient conitions on preserving safety an connectivity w.r.t. the aopte safety an connectivity sets. Base on these conitions we provie characterizations on conflict resolution among agents (see the analysis in Appenix B), an base on these characterizations we buil the propose coorination an control protocol. In that sense, our control strategies employ levels of available information only when necessary. Furthermore, they ictate semi-cooperative conflict resolution among homogeneous agents (which can be also thought as prioritization), as well as conflict resolution w.r.t. an agent (the leaer) which is not participating in collision avoiance. A preliminary version of the current paper aressing mostly the objectives encoing via our novel Lyapunovlike barrier functions appeare in [26]. Compare to the conference version, the current paper aitionally inclues: (i) The etaile efinition of our istribute conflict resolution an motion coorination protocol, (ii) the control esign for the perturbe multi-agent system, i.e., for the case when the goal estinations an connectivity region are ynamic, as well as (iii) the etaile proofs verifying the correctness of the propose control algorithms, which where omitte in the conference submission in the interest of space.

4 IEEE TRANSACTIONS ON 4 The paper is organize as follows: Section II escribes the mathematical moeling, consiere assumptions an problem formulation, while Section III presents the objectives encoing via our novel Lyapunov-like barrier functions. The motion coorination an control esign is aresse in Section IV an simulation results emonstrate the efficacy of our approach in Section V. Conclusions an ieas on current an future research are summarize in Section VI. Finally, the Appenices A-D inclue the proofs of the Theorems 2-5. II. MODELING AND PROBLEM STATEMENT Consier a network of N mobile agents with unicycle kinematics, which is eploye in a known workspace (environment) W with static obstacles. Each agent i {1,..., N} is moele as a circular isk of raius r a, an its motion w.r.t. a global Cartesian coorinate frame G is escribe by: ẋ i cos θ i 0 q i = G i (θ i )ν i ẏ i = sin θ i 0 u i, (1) ω i θ i 0 1 [ ] T [ ] T where q i = x i y i θ i Qi is the configuration vector of agent i, comprising the position r i = x i y i ] T R i an the orientation θ i S of agent i, Q i = R i S is the configuration space of agent i, an ν i = [u i ω i is the vector of control inputs, comprising the linear velocity u i an the angular velocity ω i, expresse in the boy-fixe frame B i. The environment W is populate with M static circular obstacles of known raii, which are centere at known positions p m, m {1,..., M}. The consieration of polygonal obstacle environments is beyon the length an the scope of the current paper, while a relevant iscussion is inclue later on in the Conclusions section. Remark 1: The subsystem escribing the evolution of the orientation trajectories θ i (t) is linear an can be controlle using a PD controller. In this respect, the orientation θ i can be regulate to any esire value θ i, as long as the resulting angular velocity ω i respects realistic saturation bouns (these are not explicitly consiere in this paper). Such assumption is plausible for ifferentially-riven mobile robots, an is a key characteristic which allows to perform the time scale ecomposition of the multi-agent system escribe later on. We enote the leaer agent as agent i = 1. The leaer is not actively avoiing collisions with followers, an furthermore oes not eviate from its nominal motion plan except if necessary, i.e., except for avoiing static obstacles. On the other han, the followers are assigne to move towars esire estinations (convergence) while avoiing inter-agent collisions an static obstacles (safety), an while staying close enough to the leaer, so that they can reliably receive information on their goal positions (connectivity maintenance). The leaer has access only to its own state, i.e., oes not sense or receive any information on the states of the remaining agents an communicates information to them regaring their goal estinations. Each follower has access to its own state, measures the position of agents lying in its sensing area (realize as a circular isk of raius R s ), exchanges information on pose an velocity with agents lying in its safety area (realize as a circular isk of raius R c < R s ), an receives information from the leaer regaring to its goal estination as long as the leaer

5 IEEE TRANSACTIONS ON 5 σ i j Ο j i j i R z R 0 i j R c R s Fig. 1. The leaer agent can reliably broacast information to any follower agent lying within istance less than 2R 0 ; this is realize by forcing all agents to move in a circular connectivity region O of raius R 0, centere at some point r 0. Each follower agent i can measure the position r j of any agent j lying within istance ij R s, i.e., within its sensing region. Furthermore, each follower agent i receives the orientation θ j an linear velocity u j of any agent j lying within istance ij R c, i.e., within its safety region. Finally, the circular region of raius R z centere at each follower agent i enotes the region in which the collision avoiance objective is active for agent i, see the etaile analysis in Section III-B. lies within a known upper boune istance w.r.t. the follower. This requirement is ensure if all N agents remain [ ] T within a circular region O centere at r 0 = x 0 y 0 an of raius R0, see also Fig. 1. The transmission of goal estinations from the leaer to the followers oes not nee to be in a synchronous moe, i.e., the followers goal estinations are not necessarily upate at the same time. Finally, all agents move in a cooperative fashion, i.e., they are rational an o not behave in a malicious way. In the next section, we escribe how these iverse objectives an sensing/communication patterns can be encoe

6 IEEE TRANSACTIONS ON 6 via single Lyapunov-like barrier functions for each one of the agents. III. ENCODING OBJECTIVES VIA LYAPUNOV-LIKE BARRIER FUNCTIONS In constraine optimization, a barrier function is a continuous function whose value on a point increases to infinity as the point approaches the bounary of the feasible region; therefore, a barrier function is use as a penalizing term for violations of constraints. The concept of recentere barrier functions in particular was introuce in [24] in orer to not only regulate the solution to lie in the interior of the constraine set, but also to ensure that, if the system converges, then it converges to a esire point. A. Connectivity maintenance Any follower can reliably receive information from the leaer as long as the istance between them remains less or equal than a maximum istance 2R 0. This requirement can be ensure if all N agents remain within a circular [ ] T region O centere at r 0 = x 0 y 0 an of raius R0 as shown in Fig. 1, or in other wors, if the istance i0 = r i r 0 remains always less or equal than R = R 0 r a, which reas: c i0 = R r i r 0 0. (2) The inequality (2) is a nonlinear inequality constraint which shoul never be violate. In the sequel, for all i {1,..., N} we refer to the N constraints (2) as to proximity constraints. The constraine set encoing the circular region O, or the connectivity region, for each agent i is enote as K i0 = {r i R i c i0 (r i ) 0}. 1 Connectivity for the robotic network is then maintaine as long as the constraints (2) hol for all i {1,..., N}. Inspire in part by interior point methos [27], we efine the logarithmic barrier function 2 b i0 (r i ) : R i R of the constraint c i0 (r i ) as: b i0 (r i ) = ln (c i0 (r i )), which tens to + as c i0 (r i ) 0. Then, the graient recentere barrier function for the constraint c i0 (r i ) is efine as [24]: ( ) T r i0 (r i ) = b i0 (r i ) b i0 (r i ) b ri i0 (ri r i ), (3) [ ] T [ ] T where r i = x i y i is the goal position of agent i, bi0 = b i0 b i0 x i y is the graient (column) vector ( i ) T of the function b i0 (r i ), an b ri i0 is the transpose of this graient vector (i.e., is a row vector, for the imensions to match) evaluate at the goal position r i. Note that the analytical expression of the graient vector 1 Note that requiring all agents to remain within a region may also be esirable for ensuring that the group avois (static) physical obstacles. 2 The choice of the logarithmic barrier function is not restrictive; one may use other types of barriers, e.g., the inverse barrier function b ij = 1 c ij.

7 IEEE TRANSACTIONS ON 7 is b i0 = 1 r i r 0 (R r i r 0 ) (r i r 0 ) an therefore we take r i r 0 an r i r 0 = R, so that b i0 ri always well-efine. 3 The recentere barrier function (3): (i) is zero at the goal position r i of agent i, an (ii) tens to + as c i0 (r i ) 0, i.e., as the position r i of agent i approaches the bounary of the constraine set K i0. Motivate by these characteristics, the main iea here is to employ (3) in orer to encoe both connectivity maintenance (i.e., staying within the connectivity region) an convergence to a goal position r i for each agent i {1,..., N}. In orer to ensure that we have an everywhere nonnegative function encoing these objectives so that it can be use in Lyapunov-like control esign an analysis as in [25], we efine the function V i0 : R i R + as: V i0 (r i ) = (r i0 (r i )) 2. (4) At this point let us state the following lemma, which is useful for the analysis later on. Lemma 1: The recentere barrier function (3) vanishes only at the goal position r i. Consequently, the function V i0 given by (4) is positive efinite w.r.t. the goal position r i. where: Proof: Assume the existence of a point ri r i which is a solution of (3). Then: ( ) T b i0 (ri) b i0 (r i ) b ri i0 (r i r i ) = 0, (5) is b i0 (r i) = log(r r i r 0 ), b i0 (r i ) = log(r r i r 0 ), b i0 ri = 1 r i r 0 (R r i r 0 ) (r i r 0 ), an by efinition: r i r 0 0, r i r 0 R. To further simplify the notation, let us enote: r i = r 0 + x x = r i r 0, r i = r 0 + y y = r i r 0, r i = r i + z z = r i r i. (6a) (6b) (6c) (7a) (7b) (7c) 3 This physically means that the goal position r i can lie neither on the center, nor on the bounary of the connectivity region O, an assumption which is not restrictive from a practical point of view.

8 IEEE TRANSACTIONS ON 8 Substituting (6) an (7) into (5) yiels: log(r y ) + log(r x ) x T z x (R x ) = 0 x T z log(r y ) log(r x ) + x (R x ) = 0 ( ) R y log + xt (y x) R x x (R x ) = 0 ( ) R y x y cos β x 2 log + = 0 x 0 R x x (R x ) ( ) R y y cos β x log + = 0 R x R x ( ) R y log = R x x y cos β, (8) R x where β is the angle between vectors x an y. We procee with consiering the following two cases: 1) Assume y x. We can write: y = λ (R x ) + x, where λ [0, 1]. Then (8) reas: ( ) R λ (R x ) x log = R x Denote log(1 λ) = x R x = γ to further write: Since cos β 1, we have: x (λ (R x ) + x ) cos β = R x ( ) λ + cos β. x R x x R x log(1 λ) = γ (λ + γ) cos β cos β = γ log(1 λ). λ + γ γ log(1 λ) λ + γ log(1 λ) + λ 0. (9) }{{} g(λ) Consier the equality g(λ) = 0 an note that λ = 0 is a solution, g(0) = 0. Furthermore, one has g (λ) = λ 1 λ < 0 for λ (0, 1]. This implies that g(λ) is ecreasing for λ (0, 1], i.e., g(λ) 0 for λ [0, 1]. Consequently, (9) is true only as an equality, an furthermore the solution λ = 0 of this equality is unique. Recall that λ = 0 correspons to y = x. Then out of (8): x (1 cos β) x 0 = 0 cos β = 1. Therefore one has: y = x an cos β = 1, which implies that y = x r i = r i. 2) Assume that y < x. We can write: y = λ x, where λ (0, 1). Then (8) reas: ( ) R λ x x (1 λ cos β) log = R x R x ( ) R x + (1 λ) x log = x x λ cos β R x R x R x.

9 IEEE TRANSACTIONS ON 9 Denote x R x = γ to further write: log (1 + (1 λ)γ) = γ γλ cos β Since cos β 1, we further have: cos β = γ log (1 + (1 λ)γ). λγ γ log(1 + (1 λ)γ) λγ log(1 + (1 λ)γ) (1 λ)γ 0. (10) }{{} g(λ) Consier the equality g(λ) = 0 an note that λ = 1 is a solution, g(1) = 0. Furthermore, one has g (λ) = γ 2 (1 λ) 1+γ(1 λ) > 0 for λ (0, 1). This implies that g(λ) is increasing for λ (0, 1), i.e., g(λ) < 0 for λ (0, 1). Consequently, the inequality (10) oes not have any solutions for λ (0, 1). In summary, one conclues that the solutions of (8) reuce to r i = r i, i.e., the goal position r i is the unique solution of the recentere barrier function (3). It then trivially follows that the function (4) is positive efinite w.r.t. r i. This completes the proof. B. Collision avoiance Agent i {2,..., N} realizes agent j {1,..., N}, j i, as a physical obstacle. Therefore, agent i avois collision with agent j as long as the istance ij = r i r j remains greater or equal than a minimum separation istance s 2r a, i.e.,: c ij = (x i x j ) 2 + (y i y j ) 2 s 2 0. (11) In the sequel, for all i {2,..., N} an all j {1,..., N}, with j i, we refer to the resulting (N 1) (N 2) constraints as to collision avoiance constraints, while the constraine set encoing the collision-free space of agent i w.r.t. agent j is enote with K ij = {r i R i, r j R j c ij (r i, r j ) 0}. The logarithmic barrier function b ij (r i, r j ) : R i R j R for the constraint c ij (r i, r j ) is efine as: We consier the barrier function q ij : R i R j R as: which is zero at the goal position r i ij b ij (r i, r j ) = ln (c ij (r i, r j )). q ij (r i, r j ) = b ij (r i, r j ) b ij (r i, r j ), (12) of agent i an tens to + as c ij (r i, r j ) 0, i.e., as the istance tens to the minimum separation s. To have an everywhere nonnegative function, we efine the function V ij : R i R j R + as: V ij(r i, r j ) = (q ij (r i, r j )) 2. (13)

10 IEEE TRANSACTIONS ON 10 Let us now recall that agent i senses agent j only when the istance ij between them is less than the sensing raius R s > s. In other wors, agent i nees only locally to avoi collision with agent j. To encoe this requirement, we efine the bump function: 1, if s ij R z ; σ ij = A 3 ij + B 2 ij + C ij + D, if R z < ij < R s ; 0, if ij R s, 2 where the coefficients: A = (R z R s) B = 3(Rz+Rs) 3 (R z R s), C = 6RzRs 3 (R z R s), D = Rs 2 (3R z R s) 3 (R z R s) have been compute 3 such that σ ij ( ) is a C 2 function w.r.t. the istance ij. We then efine: with σ ij given by (14) an V ij (14) V ij (r i, r j ) = σ ij V ij, (15) given by (13). In this way, collision avoiance w.r.t. for agent i w.r.t. agent j is encoe only within a finite zone of raius R z < R s aroun agent i. C. Barrier functions for collision avoiance, proximity an convergence objectives The analytical construction an properties of the functions (13), (15), (4) allow for hanling the collision avoiance, proximity an convergence objectives via a single Lyapunov-like function V i for each agent i. A follower agent i 1 has N 1 avoiance objectives encoe via N 1 functions V ij, an one proximity objective encoe via function V i0. Agent i nees finally to converge to goal estination r i ; to this en, note that by construction all functions V in, where n N = {0, 1,..., N} an n i, are zero at the goal estination r i. Thus, following our previous work [25], we encoe the accomplishment of all objectives for agent i by an approximation of the maximum function (which also is a δ-norm when δ takes integer values), of the form: 1 δ ( ) 1 N δ v i = (V i0 ) δ + (V ij ) δ = (V in ) δ, (16) j=1,j i where δ [1, + ). The function v i : R 2N R + is nonnegative everywhere in the constraine set K i = n N K in n N of agent i an tens to + as at least one of the terms V in tens to +, i.e., as the position r i of agent i approaches the bounary of the constraine set K i. Furthermore, v i is zero when all functions V in are zero. Thus, v i is by efinition zero at the goal position r i. Finally, for ensuring that all objectives are encoe by a single function which uniformly attains its maximum value on the bounary of the constraine set K i, 4 we efine: ( V i = v i n N (V in) δ) 1 δ = 1 + v ( i 1 + n N (V in) δ), (17) 1 δ which is zero for v i = 0, i.e., at the goal position r i of agent i, an equal to 1 as v i +, i.e., on the bounary of the constraine set K i. We can now state the following Theorem. 4 The reason for requiring this property is justifie in Lemma 3.

11 IEEE TRANSACTIONS ON 11 Theorem 1: The Lyapunov-like function (17) is positive efinite w.r.t. the goal position r i. Proof: Since the function f(κ) = κ 1+κ is monotonically increasing for κ [0, + ), it suffices to prove that (16) is positive efinite w.r.t. the goal position r i. Out of Lemma 1 one has that the function V i0 given by (4) is positive efinite w.r.t. r i. This trivially implies that (16) is positive efinite w.r.t. r i as well; to see why, consier that if ri r i were a solution of (16), then it woul hol that V in (ri ) = 0, n {0, 1,..., N} with n i, i.e., it woul hol that V i0 (r i ) = 0, which is a contraiction since (4) is positive efinite w.r.t. r i. Remark 2: One of the merits of the analytical construction of (17) is that it may easily incorporate collision avoiance of agent i w.r.t. all or just a subset of agents j i, i.e., one may take into account the neighbor agents j in (17) accoring to given communication topologies. IV. MOTION COORDINATION All agents initiate in the region O, so that reliable wireless communication links can be establishe. The leaer j = 1 communicates a goal position r j to each follower j 1 an moves towars its goal estination r 1 ; however, the leaer is not actively trying to avoi any follower throughout its motion towars r 1. Each follower j 1 nees to navigate towars, an remain close to, its goal estination r j, while avoiing the leaer an other followers, an while remaining connecte with the leaer. The pairwise istance between goal estinations is assume to be r j r k > 2 R s, (j, k) so that each agent j oes not sense any other goal estination r k when alreay at r j. We assume for now that: 1) The goal positions r j, j {1,..., N} an the center r 0 of the region O are static, which yiels: t r j = 0 an t r 0 = 0, respectively. In this case we refer to the multi-robot system as to the nominal system. 2) There are no physical obstacles in the region O. A. Motion Coorination for the Nominal System 1) Control laws for agent j = 1: Theorem 2: The position trajectories r 1 (t) of the leaer starting in the interior of the connectivity region are asymptotically stable to the goal estination r 1 an always remain in the connectivity region uner: u 1 = k 1 tanh( r 1 r 1 ), (18a) ω 1 = λ 1 (θ 1 φ 1 ) + φ 1, (18b) ( where V 1 is the Lyapunov-like function (17) for n = 0, δ 1, φ 1 atan2 V1 y 1, V1 x 1 ), an k 1, λ 1 > 0. The proof is given in the Appenix A. 2) Control laws for agents j {2,..., N}: Theorem 3: Each agent j {2,..., N} converges almost surely to its goal estination while avoiing collisions

12 IEEE TRANSACTIONS ON 12 an while remaining in the connectivity region uner: min i I Ji<0 u j i, s ji R c, u j = u jc, R c < ji ; ω j = λ j (θ j φ j ) + φ j,, (19a) (19b) where: I {k, l, m,... } the set of agents in the safety region of agent j, J i = r ij T ij = r ij, η j = u j i = u jc ij s R c s + u js i R c ij R c s, [ cos φj sin φ j ], r ij = r j r i, r T ij η i u jc = k j tanh ( r j r j ), u js i = u i r T, ij η j [ ( cos φj sin φ j ], φ j atan2 Vj y j, Vj x j ), δ 1 an k j, λ j > 0. The proof is given in the Appenix B. Remark 3: Let us now assume that the connectivity region O is populate with M static circular obstacles centere at positions p m, m {1,..., M}. Obstacle avoiance for agent i {1,..., N} can then be encoe by incorporating M barrier functions of the form (15) into the Lyapunov-like function (16). In this case, the analysis regaring to the collision-free an connectivity preserving motion of the followers is similar with the one in Theorem 3. The only ifference lies in the analysis regaring to the convergence to their goal estinations, since the square recentere barrier functions introuce aitional critical points in the Lyapunov-like function V j. Similarly, the convergence of the leaer becomes almost sure, since at least M sale points are introuce in the Lyapunov-like function V 1. B. Motion Coorination for the Perturbe System So far we assume that t r 0 = 0 an t r j = 0, i.e., that the agents operate in a static connectivity region an nee to converge to static goal estinations. Let us now consier the perturbe system, i.e., the case when the leaer moves with some linear velocity k 1 0 an upates the estinations r j an the center r 0 so that t r 0 k, t r j k. Theorem 4: The position trajectories r 1 (t) of the leaer never escape the connectivity region O an are locally asymptotically stable w.r.t. the ynamic goal estination r 1, varying so that t r 1 k, uner the control law: u 1 = k 1, ω 1 = λ 1 (θ 1 φ 1 ) + φ 1, (20a) (20b) where φ 1 is efine as in Theorem 2, V 1 is the Lyapunov-like function (17) for j = 0, δ 1 an k 1 > 2k > 0, λ 1 > 0. The proof is given in the Appenix C. Finally, let us consier the motion of the followers when the goal estinations r j an the center r 0 of the connectivity region are upate by the leaer so that t r 0 k, t r j k.

13 IEEE TRANSACTIONS ON 13 Theorem 5: Each agent j {2,..., N} converges almost surely to its ynamic goal estination r j, while avoiing collisions w.r.t. agents k j an while remaining in the connectivity region uner the control law as efine in Theorem 3. The proof is given in the Appenix D. V. SIMULATIONS The efficacy of the propose control algorithms is emonstrate through some representative computer simulations. We consier a scenario of N = 14 agents in the operating environment. All agents are of raius r a = 0.5 m an initiate in the connectivity region O, which is of raius R 0 = 12 m an initially centere at r 0 = [ 3 0] T. Each agent nees to move to its estination r i O, epicte in the same color as shown in Fig. 2. Agent i = 1 is the leaer, i.e., oes not receive any information on the followers states, nor takes into account their motion. The sensing raius an the safety raius for each follower are set equal to R s = 1.9 m an R c = m, respectively, while the parameter δ is set equal to 1. The linear velocity gains k i are ranomly selecte for each agent such that k i [2.25, 4.8], while the angular velocity gain λ i is set the same for all agents, λ i = 2. This choice is to emonstrate that the algorithm works efficiently espite the fact that k i > λ i, i.e., that the time-scale ecomposition aopte in the analysis is not restrictive from a practical point of view. We consier the perturbe system, i.e., the case when the connectivity region an the goal estinations are moving with k = m/sec, while the leaer moves with constant linear velocity k 1 = 2.25 m/sec an upates the goal positions of the followers. All agents converge to their (ynamic) goal estinations, while always remaining in the connectivity region, epicte as the external black circle. Snapshots of the agents positions throughout the uration of the simulation are provie in Fig. 2, 3. It is noteworthy that the propose coorination protocol manages to prouce collision-free trajectories even when the agents get congeste, see for instance Fig. 2(c) through Fig. 2(h), an at the same time allows agents to move in very close proximity. This is also emonstrate through the evolution of the pairwise inter-agent istances over time, which is illustrate in Fig. 4: none of the pairs of agents approaches closer than the minimum istance s, illustrate as the re line. Furthermore, note that followers in conflict with the leaer move asie so that the leaer goes through an oes not eviate from its nominal trajectory. At simulation time t = 15 sec the leaer upates some of the followers goal estinations; this coul be, for instance, so that the group avois newly etecte static obstacles, or passes through narrow corriors. The motion of the followers towars their new estinations is epicte in Fig. 3. Finally, the evolution of the inter-agent istances ij (t) uring the entire simulation time interval [0, 20] sec is epicte in Fig. 4. The inter-agent istances remain strictly greater than 2r a, which verifies that collisions are avoie. VI. CONCLUSIONS This paper presente a set-theoretic formulation for multi-objective control problems encountere in the motion coorination of multiple agents an introuce a novel class of Lyapunov-like barrier functions to encoe the agents constraine sets. The propose Lyapunov-like functions encoe objectives such as collision avoiance, proximity

14 IEEE TRANSACTIONS ON 14 (a) t = 0 sec (b) t = 1 sec (c) t = 1.5 sec () t = 2 sec (e) t = 2.5 sec (f) t = 3 sec (g) t = 4 sec (h) t = 5 sec (i) t = 7 sec (j) t = 9 sec (k) t = 11 sec (l) t = 13 sec Fig. 2. The motion of the robotic group in the case that the goal estinations an the connectivity region are ynamic.

15 IEEE TRANSACTIONS ON 15 (a) t = 15 sec (b) t = 17.5 sec (c) t = 20 sec Fig. 3. The motion of the robotic group in the case that the goal estinations an the connectivity region are ynamic. Fig. 4. The evolution of the inter-agent istances ij (t).

16 IEEE TRANSACTIONS ON 16 an convergence to esire estinations, an are amenable to istribute control formulations an Lyapunov-base control esign an analysis. As a consequence, one may erive graient-base control solutions using information locally available to each agent. The aopte assumptions on the agents moeling are implementable on realistic robotic setups such as ifferentiallyriven mobile robots, since the only requirement implie by the aopte time scale ecomposition is to be able to control the orientation trajectories to esire values via PD controllers. The assumptions on limite sensing an communication for each agent are also relevant to stanar setups such as sonars, omniirectional cameras, laser scanners an wireless networks. Furthermore, the analysis on circular obstacles irectly extens to ellipsoial obstacles as well. When polygonal obstacles are of interest, one may efine recentere barrier functions for the linear constraint functions encoing the faces of obstacles. Proviing a rigorous analysis an guarantees for this case is beyon the scope an the length of the current paper. For a relevant treatment see for instance [28]. Another way of using the propose approach in a polygonal obstacle environment is to combine the current results with those in [22]. To see how, the iea is that any follower robot of raius R 0 in the sense efine in [22] can be substitute by a group of (smaller) robots confine within a circular region of raius R 0, in the spirit presente here. This furthermore justifies the use of the circular connectivity region as a means of not only establishing reliable communications, but also of avoiing static obstacles in complex environments. The efficacy of the approach an its relevance to surveillance missions using multiple nonholonomic vehicles is illustrate via simulations. Our current work focuses on the use of Lyapunov-like barrier functions to encoe vision-base sensing constraints which are pertinent to surveillance an coverage problems with robotic vehicles, an also on the consieration of agents of more complex ynamics an input constraints, such as aerial an marine robotic vehicles. APPENDIX A PROOF OF THEOREM 2 Proof: The require objectives for agent 1 are encoe via the Lyapunov-like function V 1 taken out of (17) for δ = 1 an n = 0. The graient V 1 is efine everywhere in the constraine set K 1 except for on the bounary K 1 an on the singleton {r 0 }. Denote Γ = K 1 {r0 }. Away from Γ, the time erivative V 1 along the trajectories of agent 1 reas: [ ] ( V 1 = V 1 V 1 x 1 y ẋ1 V1 = cos θ 1 + V ) 1 sin θ 1 u 1, 1 x 1 y 1 since by construction one has V1 θ 1 = 0. Substituting the control law (18a) yiels: ( V1 V 1 = k 1 cos θ 1 + V ) 1 sin θ 1 tanh( r 1 r 1 ). x 1 y 1 To keep notation compact, enote the graient vector ζ 1 as the orientation of the negate graient ζ 1 away from Γ. Then: ẏ 1 [ ] T ( ) V 1 V 1 x 1 y an efine φ1 atan2 V1 1 y 1, V1 x 1 V 1 x 1 = ζ 1 cos φ 1, V 1 y 1 = ζ 1 sin φ 1. (21)

17 IEEE TRANSACTIONS ON 17 Let us at this point assume that the orientation trajectories θ 1 (t) are controlle at a much faster time-scale compare to the position trajectories r 1 (t). This can be realize by ecomposing the agent ynamics into the bounary-layer (fast) subsystem, which escribes the evolution of the orientation trajectories θ 1 (t), an the reucelayer (slow) subsystem, which escribes the evolution of the position trajectories r 1 (t). One may easily verify that uner the control law (18b) the roots θ 1 = φ 1 are globally exponentially stable equilibria for the bounary-layer subsystem. 5 Thus, the time erivative of the Lyapunov-like function V 1 for the reuce system, evaluate at the roots of the bounary-layer subsystem reas: V 1 (21) = k 1 ( ζ1 cos 2 φ 1 + ζ 1 sin 2 φ 1 ) tanh( r1 r 1 ) = k 1 ζ 1 tanh( r 1 r 1 ). (22) Thus, away from Γ, V1 vanishes at the goal estination r 1 as well as at the points where ζ 1 = 0. The analytical expression for V 1 yiels that the graient vector ζ 1 vanishes at the critical points of the function V 10, i.e., at the positions where the graient vector V 10 vanishes. The graient of the function V 10 is written analytically as: V 10 = 2r 10 r 10, which implies that V 10 = 0 at the goal position r 1 an at the critical points of the recentere [ ] T barrier function r 10. One has that r 10 = 0 at the points r 1 = x 1 ȳ 1, where: x 1 x 0 r 1 r 0 (R r 1 r 0 ) = x 1 x 0 r 1 r 0 (R r 1 r 0 ), ȳ 1 y 0 r 1 r 0 (R r 1 r 0 ) = y 1 y 0 r 1 r 0 (R r 1 r 0 ). The graient vector r 10 is not efine on Γ. After some algebraic manipulations an away from Γ we obtain: ( x 1 x 0 ) 2 + (ȳ 1 y 0 ) 2 (23) = r 1 r 0 2 (R r 1 r 0 ) 2 (R r 1 r 0 ) 2 r 1 r 0 2 = r 1 r 0 2 (R r 1 r 0 ) 2 (R r 1 r 0 ) 2, which is true for r 1 = r 1, i.e., the unique critical point of V 1 in K 1 \ Γ reuces to the goal position r 1. Note also that V 1 has compact level sets, an that V1 0 everywhere on K 1 \ Γ. To be able to examine the behavior of the system solutions on Γ we employ the notion of the Clarke generalize graient. In general, the Clarke generalize graient of a function f(p) at a point p ψ Ψ is efine as: f(p ψ ) = co{ lim χ + f(p χ) : p χ p ψ, p χ / Ψ}, where Ψ is any set of measure zero where the graient f is not efine. In our case, the efinition of Clarke generalize graient of the function V 1 at a point r γ Γ, reuces to: V 1 (r γ ) = co{ lim V 1(r β ) : r β β + r γ, r β / Γ}, where Γ = K 1 {r0 }. Let us first consier the set Γ 1 = K 1, that is, the circle O efining the connectivity region. Γ 1 can be seen as a switching surface with the graient being equal to V 1 on the interior of the set K 1 an zero on the exterior of K 1. Thus the generalize graient V 1 (r γ1 ) on the points r γ1 Γ 1 reuces to the (23a) (23b) 5 The analysis follows the pattern use in Lemma 2 an is omitte here in the interest of space.

18 IEEE TRANSACTIONS ON 18 convex combination of the graients on both sies of surface Γ 1, that is, V 1 (r γ1 ) = α V 1 + (1 α) 0 = α V 1, where α [0, 1]. The generalize erivative on Γ 1 for the close-loop system ynamics is consequently efine as V 1 (r γ1 ) = α V 1 β u 1 cos φ 1 + (1 β) 0, u 1 sin φ 1 where b [0, 1] an u 1 0 by efinition of the linear control law (18a), an further reas: ( ( V1 ) V T ) 1 (r γ1 ) = αβu 1 V 1 = αβu 1 V 1, V 1 with the norm V 1 an the linear velocity u 1 vanishing only at r 1. Let us know consier the set Γ 2 = {r 0 }. The generalize erivative V (r0 ) is then equal to { } { ( ( V1 (r 1 )) T )} co lim V 1 (r 1 ) co lim u 1 = r 1 r 0 r 1 r 0 V 1 (r 1 ) ( ) = u 1 V 1 V 1 T = u 1 V 1 0, V 1 with u 1, V 1 vanishing at r 1 only. Thus, one has that the generalize erivative V (r1 (t)) along the system trajectories is non-positive everywhere on K 1 an furthermore that it vanishes either at the goal position {r 1 }, an on the bounary K 1 (for α = 0 or β = 0). Setting α = 0 implies that the generalize graient vector is zero; then the orientation φ 1 can be efine to point to the interior of K i, an also one has u 1 0. This implies that system trajectories o not escape the connectivity region. Setting β = 0 implies that u 1 = ω 1 = 0, which means that system trajectories stay on K 1. This implies that the position trajectories r 1 (t) starting in K 1 never cross the bounary of K 1, i.e., never escape the connectivity region O, an also that they are asymptotically stable to r 1, except for the set of initial conitions of measure zero corresponing to β = 0, i.e., except for initial positions on the bounary K 1. This completes the proof on the connectivity maintenance an on the convergence to the goal estination for agent 1. APPENDIX B PROOF OF THEOREM 3 Proof: In orer to esign control strategies for the followers j 1 one may employ the function V j given by (17) as a caniate Lyapunov-like function for each agent j. A. Non-overlapping sensing regions Let us first consier the case when none of the remaining N 1 agents ever lies in the sensing region of agent j, i.e., that jk (t) > R s, t [0, ), k {1,..., N}, k j. The Lyapunov-like function V j is then taken out of (17) with V jk = 0 k {1,..., N}, k j, since one has σ jk = 0 out of (14). No collisions occur, apparently, while the analysis on proximity maintenance an convergence to esire estination r j follows the same pattern as in Theorem 2.

19 IEEE TRANSACTIONS ON 19 B. Overlapping sensing regions Let us now consier the worst-case when all the remaining N 1 agents initiate or happen to lie in the sensing area of agent j. The Lyapunov-like function (17) for agent j is: ( k j V δ) 1 δ jk V j (r j, r j, r k ) = ( 1 + k j V δ), (24) 1 δ jk where k {0, 1,..., N}, k j. The function V j encoes proximity (or connectivity maintenance) for k = 0, collision avoiance w.r.t. the leaer for k = 1, an collision avoiance w.r.t. the remaining followers for k j. Its time erivative along the trajectories of agent j is: T V j = ζ j cos θ j u j + sin θ j N k=1,k j ζ jk T cos θ k u k, (25) sin θ k [ ] T [ ] T V where ζ j j V j x j y, V ζjk j V j j x k y. Denote Γj the set where the graients ζ j, ζ jk, an consequently k the graient V j, are not efine. More specifically, Γ j is given as Γ j = { K j {r0 } {r k r j r k = s }}, where the bounary K j of the constraine set K j comprises the bounary of the connectivity region an the bounary of the circular isk efining each agent k j. Thus, efining the generalize graient V j ( ) at the points on the set K j {r0 } follows the same proceure as in Theorem A. Furthermore, at the points on the surface S jk = {r k r j r k = s }, one has that the generalize graient is efine as the convex combination of the graients on both sies of S jk, which eventually reas: V j ( ) = V j, since the graient vectors on both sies of S jk coincie an are equal to V j. In the sequel, with some abuse of notation, when referring to a graient vector of the function V j at a point p, we will be implying the stanar graient vector of V j if p / Γ j, an the Clarke generalize graient vector if p Γ j. As expecte, the evolution of the time erivative V j along the trajectories of agent j epens on the motion of the agents k j through their linear velocities u k. The proofs for the satisfaction of the esire objectives are given sequentially in the following three Lemmas. Briefly: first, we prove that the position trajectories an orientation trajectories of each agent j can be ecompose into slow an fast ynamics in a singular perturbations sense. Secon, we prove that the position trajectories of each agent are collision-free an remain in the connectivity region. Thir, we prove that the position trajectories of each agent are almost globally convergent to its esire estination. We first perform a time-scale ecomposition of the agent s trajectories q j (t) into fast an slow ynamics, in orer to employ arguments relate to singular perturbations [29]. We assume that the orientation trajectories θ j (t) are controlle at a much faster time scale via the control law (19b) compare to the position trajectories r j (t). 6 The system ynamics of agent j can then be ecompose into two subsystems, with the ynamics along the position trajectories r j (t) serving as the reuce (slow) subsystem, an the orientation ynamics serving as the bounarylayer (fast) subsystem. Let us now consier the sufficiently small parameter ε j 1 λ j, an rewrite the close-loop 6 This is plausible for the unicycle-type vehicles that are consiere here, such as for ifferentially-riven mobile robots.

20 IEEE TRANSACTIONS ON 20 bounary-layer (fast) ynamics as: 1 λ j θj = (θ j φ j ) + 1 λ j φj 1 λ j θj = (θ j φ j ) + 1 λ j φj ε j θj = (θ j φ j ) + ε j φj. (26) Lemma 2: The orientation θ j of agent j is globally exponentially stable to φ j. Proof: The roots of the bounary-layer subsystem are given for ε j = 0; out of (26) one has that the roots of the fast subsystem lie on the manifol θ j = φ j. Denote η j = θ j φ j an take the ynamics: where τ t η j t = t (θ j φ j ) (26) = 1 ε j (θ j φ j ) = 1 ε j η j ε j η j t = η j, which further reas: η j τ = η j, 1 ε j. The origin η j = 0 of the bounary-layer subsystem is thus globally exponentially stable, which implies that θ j is globally exponentially stable to φ j. Lemma 3: The control law (19) reners the constraine set K j a positively invariant set for the reuce (slow) ynamics of agent j. This in turn implies that collision avoiance an proximity for agent j are guarantee. Proof: Avoiing collisions an maintaining proximity for agent j are encoe via ensuring that the constraint functions c jk (r j (t), r k (t)) 0, t, (j, k). To prove this, we resort to set-theoretic analysis [23]. More specifically, the necessary an sufficient conitions for ensuring that the position trajectories r j (t) starting in the set K j always remain in K j are given by Nagumo s theorem an rea: t c jk(r j, r k ) 0, r j K j, (j, k). This conition essentially states that the vector fiel of the system ynamics of agent j shoul always point into the interior of the constraine set K j. 1) Inter-agent Collision Avoiance: Let us consier the time erivative of the collision avoiance constraint c jk = (x j x k ) 2 + (y j y k ) 2 s 2, k {1, 2,..., N}, on the bounary of the constraine set K j, an evaluate at the equilibria θ j = φ j, θ k = φ k of the bounary-layer (fast) subsystems of agents j, k, respectively, which reas: t c T jk = 2u j r kj cos φ j T 2u k r kj cos φ k, sin φ j sin φ k (27) } {{ }} {{ } J K [ where r kj r j r k an the velocities u j, u k are positive by construction. Denote η j cos φ j [ ] T η k cos φ k sin φ k an let us provie the following efinitions: ] T sin φ j, Definition 1: Assume that r kj T η j 0 an r kj T η k 0: Then J 0 an K 0, which implies that both agents j, k contribute in satisfying the collision avoiance conition. We say that collision avoiance is fully cooperative. Definition 2: Assume that r kj T η j 0 an r kj T η k 0: Then J 0 an K 0, which implies that agent j contributes towars avoiing collision, whereas agent k oes not. If J + K 0, we say that collision avoiance is semi-cooperative by agent j ; otherwise, collision occurs.

21 IEEE TRANSACTIONS ON 21 Definition 3: Assume that r T kj η j 0 an r T kj η k 0: Then J 0 an K 0, which implies that agent k contributes towars avoiing collision, whereas agent j oes not. If J + K 0, we say that collision avoiance is semi-cooperative by agent k ; otherwise, collision occurs. Definition 4: Assume that r T kj η j 0 an r T kj η k 0: Then J 0 an K 0, which implies that collision occurs. Recall our assumption that agent j may measure only the position vector r k of agents k lying in its sensing region of raius R s, i.e., can not measure the full state vectors q k, ν k. In this respect, agent j may only etermine the term J in (27), i.e., whether he/she contributes towars avoiing collision with agent k. Yet, this information is not enough to guarantee inter-agent collision avoiance, as illustrate in the efinitions above. In aition, etermining worst-case conitions on J an K is in general intractable, since: the vectors η j, η k enote the irections of the negate graient vector fiels ζ j, ζ k evaluate on the bounary of the constraine sets K j, K k, respectively, an thus they epen on the number an the relative positions of the agents in conflict. Furthermore, the linear velocities u j, u k are epenent on the current positions an goal estinations. In view of these, we assume that each agent j oes exchange information on linear velocities an orientations, yet only with agents lying within a circular safety region of raius R c, where s < R c < R z, see Fig. 1. Within the safety region we efine the following conflict resolution an coorination protocol: Case 1: Agent j is in conflict with an agent k 1. J 0: Agent j moves away from or maintains fixe istance w.r.t., agent k. Collision avoiance may then be either fully cooperative or semi-cooperative by agent j, epening on the effect of agent k in (27) via the term K. Collision occurs if an only if K is negative enough to rener the conition (27) negative. We let agent j move with linear velocity: u jc = k j tanh( r j r j ). (28) The case of agent k moving so that J + K < 0 is exclue through the coorination impose below. J < 0: Agent j moves towars agent k. Collision is avoie if an only if the term K reners the conition (27) positive, i.e., avoiing collision is, at best, semi-cooperative by agent k. Nevertheless, agent j ignores the intentions of agent k. Thus, a way to ensure that J + K 0 is to have agent j suitably ajust its linear velocity u j. To this en we assume that, within the safety region, agent j communicates with agent k, acquires its linear velocity u k an orientation φ k, an moves accoring to: where: u j k = u jc jk s R c s + u js k R c jk R c s, (29) u js k u k r kj T η k r kj T η j (30) is the safe (i.e., collision avoiing) velocity for agent j w.r.t. agent k ictate by the conition (27), an jk is the istance between j an k. A straightforwar option is to set u js k satisfying the equality in (30). The velocity profile u j k in (29) is epicte in Fig. 5.