Formation control with collision avoidance for first-order multi-agent systems: Experimental results

8th IFAC Symposium on Mechatronic Systems 8th IFAC Symposium on Mechatronic Systems Vienna, Sept. on 4-6, 2019 8th IFACAustria, Symposium Systems Vienna, Austria, Sept. 4-6,Mechatronic 2019 Available 8th IFACAustria, Symposium Systems online at www.sciencedirect.com Vienna, Sept. on 4-6,Mechatronic 2019 Vienna, Austria, Sept. 4-6, 2019

ScienceDirect

IFAC PapersOnLine 52-15 (2019) 127–132

Formation control with collision avoidance Formation control with collision avoidance Formation control with collision avoidance for first-order multi-agent systems: Formation control multi-agent with collision avoidance for first-order systems: for first-order multi-agent systems: Experimental results for first-order multi-agent systems: Experimental Experimental results results Experimental results J. F. Flores-Resendiz ∗∗ J. Meza-Herrera ∗∗ ∗∗

J. F. Flores-Resendiz ∗ J. Meza-Herrera ∗∗ ∗∗ E. J. F. Flores-Resendiz J. Meza-Herrera ∗∗ E. Aranda-Bricaire Aranda-Bricaire ∗ ∗∗ ∗∗ J. F. Flores-Resendiz J. Meza-Herrera E. Aranda-Bricaire ∗∗ ∗ E. Aranda-Bricaire o ∗ Facultad Facultad de de Ingenier´ Ingenier´ııa a yy Negocios, Negocios, Tecate, Tecate, Universidad Universidad Aut´ Aut´ onoma noma de de ∗ Baja California, Mexico (e-mail:[email protected]) Facultad de Ingenier´ ı a y Negocios, Tecate, Universidad Aut´ o noma de Baja California, Mexico (e-mail:[email protected]) ∗ ∗∗ Facultad de Ingenier´ ı a y Negocios, Tecate, Universidad Aut´ o noma de Engineering Department, Baja California, Mexico (e-mail:[email protected]) ∗∗ Electrical Electrical Engineering Department, Mechatronics Mechatronics Section, Section, ∗∗ Baja California, Mexico (e-mail:[email protected]) CINVESTAV, City, Mexico Electrical Mexico Engineering Mechatronics Section, CINVESTAV, City, Department, Mexico (e-mail:[email protected], (e-mail:[email protected], ∗∗ Electrical Mexico Engineering Department, Mechatronics Section, CINVESTAV, Mexico City, Mexico (e-mail:[email protected], [email protected]) CINVESTAV, [email protected]) City, Mexico (e-mail:[email protected], [email protected]) [email protected]) Abstract: Abstract: This This paper paper deals deals with with the the general general formation formation control control problem problem for for first first order order multimultiagent systems. We investigate the behaviour of an arbitrary number of mobile agents moving on aa Abstract: This paper deals with the general formation control problem for first order multiagent systems. We investigate the behaviour of an arbitrary number of mobile agents moving on Abstract: This paper deals velocity with thewhen general formation control problem foragents first order multiplane with constrained input they are required to reach a specific spatial pattern. agent systems. We investigate the behaviour of an arbitrary number of mobile moving on a plane with constrained input velocity when they required to reach a specific spatial pattern. agent systems. We investigate the behaviour of anare arbitrary number of mobile agents moving on a A general communication graph among agents is considered, relaxing conditions to the only plane with constrained input velocity when they are required to reach a specific spatial pattern. A general graph among agents is considered, conditions to the only plane with communication constrained input velocity when theytree. are required torelaxing reach athis specific spatial pattern. requirement of containing a directed spanning The solution to problem is provided A general communication graph among agents is considered, relaxing conditions to the only requirement of containing a directed spanning tree. The solution to this problem is provided A general two communication graph among agents is considered, relaxing conditions to the only attending issues. First, the asymptotic convergence to the desired formation pattern is requirement of containing a directed spanning tree. The solution to this problem is provided attending two First,a directed the asymptotic convergence to the desired is requirement of issues. containing spanning tree. The solution tomost this formation problem ispattern provided ensured. Then a non-collision analysis is presented regarding the general geometrical attending two issues. First, the asymptotic convergence to the desired formation pattern is ensured. Then a non-collision is presented regarding the most formation general geometrical attending two can issues. theanalysis asymptotic convergence to the desired pattern is ensured. Then a non-collision analysis is presented regarding the general geometrical scenario which which leadFirst, to aa collision collision among agents. This later issue ismost solved by using using repulsive scenario can lead to among agents. This later issue is solved by repulsive ensured. Then a unstable non-collision analysis is Numerical presented regarding the most general geometrical vector fields with focus behaviour. simulations as well as experimental results scenario which can lead to a collision among agents. This later issue is solved by using repulsive vector fields with unstable focus behaviour. Numerical simulations as well as experimental results scenario which lead a collision agents.ofThis issue is solved by using repulsive are provided provided incan order to to illustrate theamong effectiveness the later proposed control law. vector fields with unstable focus behaviour. Numerical simulations as control well as experimental results are in order to illustrate the effectiveness of the proposed law. vector fields with unstable focus behaviour. Numerical as control well as experimental results are provided in order to illustrate the effectiveness of simulations the proposed law. © 2019, IFAC (International Federationthe of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. are provided in order to illustrate effectiveness of the proposed control law. Keywords: Mobile Mobile robots, robots, Discontinuous Discontinuous control, control, Formation Formation control, control, Collision Collision avoidance, avoidance, Keywords: First-order systems. Keywords: Mobile robots, Discontinuous control, Formation control, Collision avoidance, First-order systems. Keywords: Mobile robots, Discontinuous control, Formation control, Collision avoidance, First-order systems. First-order systems. 1. INTRODUCTION INTRODUCTION Martinez and and Aranda-Bricaire Aranda-Bricaire (2013), (2013), Flores-Resendiz Flores-Resendiz and and 1. Martinez 1. INTRODUCTION Aranda-Bricaire (2014), Flores-Resendiz et al. (2015)). As Martinez and Aranda-Bricaire (2013), Flores-Resendiz and Aranda-Bricaire (2014), Flores-Resendiz et al. (2015)).and As 1. INTRODUCTION Martinez andundesired Aranda-Bricaire (2013), Flores-Resendiz a result, no equilibrium points appear and the Aranda-Bricaire (2014), Flores-Resendiz et al. (2015)). As a result, no undesired equilibrium pointsetappear and the In the the last last decade, decade, multi-agent multi-agent systems systems (MAS) (MAS) has has been been Aranda-Bricaire (2014),equilibrium Flores-Resendiz al. (2015)). As agents avoid collisions by staying points at aa distance distance greater aagents result,avoid no undesired appear and the In collisions by staying at greater In the last decade, multi-agent systems (MAS) has been an area of intensive research because of its wide range a result, no undesired equilibrium points appear and the than or equal to a predefined bound. In Flores-Resendiz agents avoid collisions by staying at a distance greater an the area of decade, intensivemulti-agent research because of its wide range than oravoid equal collisions to a predefined bound. In Flores-Resendiz In systems has been agents of applications, applications, such research as surveillance, surveillance, seeking and rescue, an arealast of intensive because of(MAS) its and wide range by staying at In a distance greater et al. al. or (2015), the particular casebound. of communication communication graphs than equalthe to particular a predefined Flores-Resendiz of such as seeking rescue, et (2015), case of graphs an area of and intensive because of(2004), its and wide range than or equal to a predefined bound. In Flores-Resendiz etc., (Kim Bang (2016), Lin et al. al. Marshall of applications, such research as surveillance, seeking rescue, composed exclusively of cyclic paths was considered. et al. (2015), the particular case of communication graphs etc., (Kim and Bang (2016), Lin et (2004), Marshall composed exclusively of cyclic paths was considered. of applications, such as seeking and rescue, et et al. al. (2004), Mousavi etsurveillance, al. (2010)). (2010)). Problems asMarshall motion etc., (Kim and Bang (2016), Lin et al. (2004), al. (2015), the particular casepaths of communication graphs composed exclusively of cyclic was considered. et (2004), Mousavi et al. Problems as motion etc., (Kim and Bang (2016), Lin et al. (2004), Marshall In this paper,the problem is solved in itsconsidered. full generality; generality; et al. (2004), Mousavi et al. (2010)). Problems as motion without collisions have attracted to much attention in composed exclusively of cyclic paths was In this paper,the problem is solved in its full without collisions haveet attracted to much attention in namely, no assumption on the type of communication et al. (2004), al. ascollisions motion this paper,the problemonis the solved in of its communication full generality; order to collisions be Mousavi applied in attracted real(2010)). scenarios. Both, without have to Problems much attention in In namely, no assumption type order to be applied in real scenarios. Both, collisions this ispaper,the problem is the solved in of itsa communication full generality; namely, no assumption on type without collisions have attracted to much attention in In graph made, other than containing spanning tree among agents as well as with environmental obstacles have order to be applied in real scenarios. Both, collisions graph is no made, other than containing a communication spanning tree amongto agents as well asinwith environmental obstacles have namely, assumption on the type of order be applied real scenarios. Both, collisions (Lafferriere et al. (2005), Ren and Beard (2008)). When graph is made, other than containing a spanning tree been regarded (Ahmadi Barogh et al. (2015), Kostic et al. among agents as well as with environmental obstacles have (Lafferriere et al. (2005), Ren and Beard (2008)). When been regarded (Ahmadi Barogh et al. (2015), Kostic et al. graph is made, other spanning tree among agents as well(2017)). as with environmental obstacles have the agents agents areal.not not in than risk ofand collision, they move on et (2005), Rencontaining Bearda (2008)). When been regarded (Ahmadi Barogh et al. (2015), Kostic et al. (Lafferriere (2010), Phan et et al. the are in risk of collision, they move on (2010), Phan al. (2017)). et al.not (2005), Renbounded and Beard (2008)). When been regarded Barogh et al. (2015), Kostic et al. (Lafferriere plane at some known velocity by using the agents are in risk of collision, they move on (2010), Phan et(Ahmadi al. (2017)). the agents plane at known bounded velocity by using In aa first first approach, collisions can can be be predicted, predicted, but but not not the aresome not while in risk of collision, they move on plane functions at some known bounded velocity by using (2010), Phan et al. (2017)). saturation functions in Flores-Resendiz Flores-Resendiz et al. al. (2015) the In approach, collisions saturation while in et (2015) In a first approach, collisions can be predicted, but not avoided, from initial conditions (Hernandez-Martinez and the plane at some known bounded velocity by using the agents move at some constant velocity because the use saturation functions while in Flores-Resendiz et al. (2015) avoided, from initial conditions (Hernandez-Martinez and the agents move at some constant velocity because the use In a firstfrom approach, collisions can be predicted, but and not saturation Aranda-Bricaire (2011), Hernandez-Martinez and Bricaire Bricaire avoided, initial conditions (Hernandez-Martinez functions whilevector in Flores-Resendiz et to al.the (2015) of normalized normalized attractive fields. Similarly Floresthe agents move at some constant velocity because use Aranda-Bricaire (2011), Hernandez-Martinez and of attractive vector fields. Similarly to Floresavoided, from initial conditions (Hernandez-Martinez and (2012)). If If the the initial initial conditions can be be known, known, or even even the Aranda-Bricaire (2011),conditions Hernandez-Martinez and Bricaire agentsetmove at somethe constant velocity because use Resendiz al.attractive (2015), collision avoidance problem is of normalized vector fields. Similarly to the Flores(2012)). can or Resendiz et al. (2015), the collision avoidance problem is Aranda-Bricaire (2011),conditions Hernandez-Martinez and Bricaire selected, this approach could be enough. The most known (2012)). If the initial can be known, or even of normalized attractive vector fields. Similarly to FloresResendiz et al. (2015), the collision avoidance problem is attended by using repulsive vector fields in such a way selected, this approach could be enough. The most known attended et by al. using repulsive vector avoidance fields in such a way (2012)). the initialin conditions canRepulsive be The known, orknown even Resendiz selected, If this approach could be enough. most approach, consists the use use of Potential (2015), theacollision problem is that every everybyagent agent regards scaled unstable focus vector attended using repulsive vector unstable fields in focus such avector way approach, consists in could the of Repulsive Potential that regards a scaled selected, this approach be enough. The most known attended Functions (RPF) in combination with Attractive Potenapproach, consists in the use of Repulsive Potential by using repulsive vector fields in such a way field centred at the position of the rest of them. that every agent regards a scaled unstable focus vector Functions (RPF) in combination with Attractive Potenfield centred at the position of the rest of them. approach, consists the use of Repulsive Potential tial Functions Functions (APF) (Dimarogonas and Kyriakopoulos Functions (RPF) in in combination with Attractive Poten- that every agent a of scaled unstable focus vector field centred at theregards position the rest of them. tial (APF) (Dimarogonas and Kyriakopoulos Functions (RPF) in combination with Attractive PotenThis paper is organized as follows. In Section II we we inin(2006), Do (2006)). This approach could lead to high magtial Functions (APF) (Dimarogonas and Kyriakopoulos field centred at the position of the rest of them. This paper is organized as follows. In Section II (2006), Do (2006)). This(Dimarogonas approach could lead to high mag- troduce some preliminary definitions and present tial Functions (APF) and Kyriakopoulos an imThis paper is organized as follows. In Section II we in(2006), Do (2006)). This approach could lead to high magnitude control signals while the distance between agents troduce some preliminary definitions and present an imnitude control signals while the distance between agents paper is preliminary organized follows. Inand Section II an weconin(2006), Do (2006)). This approach could to high magportant Lemma which is is as useful to prove prove asymptotic troduce some definitions present imcould become become arbitrarily small. In orderlead to take into ac- This nitude control signals while the In distance between agents portant Lemma which useful to asymptotic concould arbitrarily small. order to take into actroduce some preliminary definitions and present an important Lemma which is useful to prove asymptotic connitude control signals while the distance between agents vergence. In Section III, we state the problem and procount become the physical physical dimensions of agents and guarantee could arbitrarily small. of In agents order toand takeguarantee into ac- vergence. In Section III, we state the problem and procount the dimensions Lemma whichIII, is both, useful to prove asymptotic concould become arbitrarily small. of In agents order to takeguarantee into ac- portant pose aa solution solution regarding convergence to the theand desired vergence. In Section we state the problem procollision-free tracking, in Mastellone et al. (2008), Mondal count the physical dimensions and pose regarding both, convergence to desired collision-free tracking, in Mastellone et al. (2008), Mondal vergence. In Section III, avoidance we state the problem and procount the physical dimensions of agents and guarantee formation and collision problems. Numerical pose a solution regarding both, convergence to the desired et al. (2017), Rodr´ ıguez-Seda et al. (2014), a modification collision-free tracking, in Mastellone et al. (2008), Mondal formation and regarding collision avoidance problems. Numerical et al. (2017), Rodr´ ıguez-Seda et al. (2014), a modification pose a solution both, convergence to the desired collision-free tracking, in Mastellone et al. (2008), Mondal formation simulation and and real real time experiments experiments were carried carried out and and collision avoidance problems. Numerical to al. this(2017), last approach approach has beenetproposed. proposed. et Rodr´ıguez-Seda al. (2014), a modification simulation time were out to this last has been formation and collision avoidance problems. Numerical et Rodr´ıguez-Seda al. (2014), a modification simulation the results results and are presented presented in Section IV. IV. Finally, in out Section real time experiments were carried and to al. this(2017), last approach has beenetproposed. the are in Section Finally, in Section Finally, theapproach design of ofhas thebeen repulsive component as as been been the simulation and real timeconclusions experiments were carried and to this last proposed. V we we summarize some about this work. results are presented in Sectionabout IV. Finally, in out Section Finally, the design the repulsive component V summarize some conclusions this work. modified by using unstable focus structures, (HernandezFinally, the design of the repulsive component as been the results are presented in Sectionabout IV. Finally, in Section V we summarize some conclusions this work. modified by using unstable focus structures, (HernandezFinally, designunstable of the repulsive component as been V we summarize some conclusions about this work. modifiedthe by using focus structures, (Hernandezmodified by using unstable focus structures, (Hernandez2405-8963 © 2019, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved.

Copyright © 2019 IFAC 408 Copyright 2019 responsibility IFAC 408Control. Peer review©under of International Federation of Automatic Copyright © 2019 IFAC 408 10.1016/j.ifacol.2019.11.662 Copyright © 2019 IFAC 408

2019 IFAC MECHATRONICS 128 Vienna, Austria, Sept. 4-6, 2019

J.F. Flores-Resendiz et al. / IFAC PapersOnLine 52-15 (2019) 127–132

2. PRELIMINARIES A formation graph G = {V, E, C} can be used to describe the communication among a group of agents. It consists of a set of vertices V = {R1 , . . . , Rn }, corresponding to each agent and a set of edges E = {(Rj Ri ) ∈ V × V, i �= j}, which provide information about the information exchange between a pair of agents. If (Rj Ri ) ∈ E, the agent Ri receives information from Rj , but not necessarily vice versa. Finally, the set C = {cji ∈ R2 | (Rj Ri ) ∈ E, i �= j} of constant vectors represents the relative desired position of agent Ri with respect to Rj . For the edge (Rj Ri ), Rj is called the parent node and Ri is the child node and it is said Rj is neighbour of Ri . The set of all neighbours of Ri is denoted as Ni . A directed tree is a directed graph in which every node has exactly one parent, except for one single node called the root. A directed spanning tree of a directed graph G is a directed tree involving every node in G. Associated to a formation graph G, the Laplacian matrix is given by L (G) = ∆ − Ad , (1) where ∆ is the in-degree matrix defined as ∆ = diag{n1 , . . . , nn }, where ni ∈ R is the cardinality of Ni , i = 1, . . . , n, and Ad = [aij ] ∈ Rn×n is the adjacency matrix of G defined as  1, if (Rj Ri ) ∈ E (2) aij = 0, otherwise. As mentioned before, we consider physical constraints in the input signals. Definition 1. Let φ : R → R be a saturation function, then it is indexed by a parameter r > 0 in such a way that −r ≤ φ(x) ≤ r,

∀x ∈ R,

xφ(x) > 0,

∀x �= 0.

Now, we recall a useful Lemma. Lemma 2. Consider the dynamical system x˙ = Ax, where T x = [x1 , . . . , xn ] ∈ Rn and the matrix A ∈ Rn×n is Hurwitz. Then, the saturated system x˙ = Aφ(x) with T φ(x) = [φ(x1 ), . . . , φ(xn )] ∈ Rn , is globally asymptotically stable. Proof. See the detailed proof in Flores-Resendiz and Aranda-Bricaire (2019). 3. CONTROL DESIGN Consider a group of N mobile agents on a plane. Each of this robots are numbered by R1 , . . . , Rn . The position coordinates of agent Ri are specified by zi (t) = [xi (t), yi (t)]T ∈ R2 , i = 1, . . . , n. Every robot is modelled as a single integrator, z˙i = ui , i = 1, . . . , n, (3) T

where ui = [ui1 , ui2 ] ∈ R2 are the velocities along the X and Y axes. We assume that robot Ri can determine the position of a specific set of neighbours, say Ni ⊂ N , at any time instant. This set is defined by the corresponding communication graph G. The desired position zi∗ of robot Ri is defined according to its set of neighbours as 1  (zj + cji ), (4) zi∗ = ni j∈Ni

409

where ni is defined as above and cji = [hji , vji ]T ∈ C, ∀j ∈ Ni define the desired geometrical distribution of agents. Moreover, inside a region around, the agent Ri can sense and determine any other agent’s position. This region is assumed to be a circle of radius d. The subset of agents inside the detection region of robot Ri is denoted as Mi (t) = {Rj ∈ N | �zi (t) − zj (t)� ≤ d}. The goal is to design distributed control laws ui = α(zi , zi∗ , Mi ∪ Ni ), i = 1, . . . , n such that: i) The agents reach a desired formation, that is, lim (zi (t) − zi∗ (t)) = 0, i = 1, . . . , n, and t→∞

ii) the agents avoid collisions in finite-time and, after that, they remain at some distance greater than or equal to a predefined bound d from each other, i.e., �zi (t) − zj (t)� ≥ d, ∀t ≥ t1 ≥ 0, i �= j. A solution to this problem can be found attending the two objectives separately. The convergence issue is treated as in Flores-Resendiz and Aranda-Bricaire (2014) and Flores-Resendiz et al. (2015), with a relaxation in the conditions imposed to the communication graph. The only requirement is to contain a directed spanning tree. Once this objective is ensured, a complementary control law to avoid collisions is designed. The proposed control law to reach the desired formation is given by γi = −µφ(˜ zi ),

i = 1, . . . , n,

(5)

zi∗

is the position error, µ > 0 ∈ R and where z˜i = zi − φ(·) is a saturation function. Without loss of generality, we assume φ(·) is parametrised by unity gain, this implies that agents can move at a maximum velocity µ. Theorem 3. Consider a group of n first-order agents, described by (3) along with the desired positions (4) and the control law (5). Consider also that the communication graph contains a directed spanning tree and �zi (0) − zj (0)� ≥ d, ∀i �= j. Then, in the closed-loop system (3)-(5) the agents converge asymptotically to the desired formation. Proof. See the detailed proof in Flores-Resendiz and Aranda-Bricaire (2019). In order to avoid collisions among agents while they reach the desired configuration a complementary reactive control law is needed. This new component was presented firstly in Flores-Resendiz et al. (2015). A novelty in our current approach is to find design parameters that minimize the control input signal. This issue is solved by using repulsive vector fields, based on the relative distance between any pair of agents. The relative position coordinates are defined as pij = xj − xi ,

(6)

qij = yj − yi ,

(7)

for i, j = 1, . . . , n, i �= j. The repulsive vector fields are such that for robot Ri there exists an unstable counterclockwise focus, centred at the position of any other agent in risk of collision. The general expression of the repulsive vector fields is   n  pij − qij βi = −ε , (8) δij pij + qij j=1,j�=i

2019 IFAC MECHATRONICS Vienna, Austria, Sept. 4-6, 2019

J.F. Flores-Resendiz et al. / IFAC PapersOnLine 52-15 (2019) 127–132

Fig. 1. P-Q Plane where ε > 0 and the functions δij depend on the distance between Ri and Rj , in the following way � 1, if �zi − zj � ≤ d, (9) δij = 0, if �zi − zj � > d, where d is the minimum allowed distance between any pair of agents. If the sensed area is the same for all agents, it is clear that δij = δji , i �= j. In Figure ??, an auxiliary plane Pij − Qij is shown. Outside the circumference of radius d, only attractive vector fields exists (δij = 0) while in the inner region the repulsive vector fields appear (δij = 1). Then, the composite control law for each agent is ui = γi + βi , i = 1, . . . , n, (10) where γi and βi are given by (5) and (8), respectively. Theorem 4. Consider the system (3) and the control law (10) along with definitions (4)-(9). Suppose at the time instant t0 there exists a pair of agents in risk of collision, i. e. �zi (t0 ) − zj (t0 )� < d, and ε > 2 µd . Then, in the closedloop system (3)-(10) the mobile robots avoid the collision in finite-time, t1 > t0 end they reach their desired position asymptotically while staying at a distance greater than or equal to d for all t ≥ t1 ≥ t0 . Sketch of proof For the sake of space, here we start by proving that no collision occur for the most simple case which consist in two agents in risk of collision, while the rest of them are not involved. Under this same approach, more complex interactions among agents could be treated. Suppose the agents Ra and Rb get closer than the minimum allowed distance, that is, their relative position trajectory gets into the circumference of radius d, where �[pab , qab ]� ≤ d. Then, the relative position is inside the region 2 σab = p2ab + qab − d2 ≤ 0 (11) and it is of interest because in the boundary of this region the control law becomes discontinuous. Once the repulsive vector fields appear, we need to verify that the surface σab = 0 is attractive from the inner region which means the conflict is solved. The relative position dynamics in the closed-loop system (3)-(10) is � � � � p˙ ab pab − qab = −µ (φ (˜ zb ) − φ (˜ . (12) za )) + 2ε q˙ab pab + qab Therefore, in order to prove convergence to the discontinuous surface σab , we take the Lyapunov function candidate 1 2 V = σab , (13) 4 which evaluated along the relative position trajectories becomes � � 1 1 p˙ ab ˙ V = σab σ˙ ab = σab [ pab qab ] . (14) q˙ab 2 2 410

129

Evaluating in the dynamics of the relative position variables � �� σab � 2 − µ [pab , qab ] (φ (˜ V˙ = zb ) − φ (˜ za )) + 2ε p2ab + qab . 2 (15) Now, the first term inside the parenthesis depends on the desired position of agents, then it is bounded by the worst case. Regarding that functions φ(˜ zi ) are parameterized by unity gain and the trajectories are inside σab where 2 p2ab + qab ≤ d2 , we have � � 1 (16) V˙ ≤ σab −2µd + 2εd2 . 2 Regarding σab < 0, it can be written σab = −|σab | (17) V˙ ≤ −d|σab | (−µ + εd) . Thus, the parameter ε should be selected in such a way that ε > 2 µd . Assume ε = 2k µd with k > 1, then V˙ ≤ −dµ(2k − 1)|σab |, (18) which can be written as 1 V˙ < −c|σab | < −cV 2 , (19) for some c > 0 and |σab | > 0. The last expression, according to Bhat and Bernstein (2000), implies not only that the surface σab is attractive from inside but also that the surface is reached in finite-time. Once the conflict between the agents has been solved, the direct application of Theorem 1 ensures the agents reach the desired formation asymptotically. This concludes the proof. Remark 5. The generalization of the previous Theorem for any number of agents could be obtained by adding agents in risk of collision with an arbitrary agent Rr . 4. SIMULATION AND EXPERIMENTAL RESULTS In this Section, we present the results of a numerical simulation as well as its experimental validation. The control law previously presented needs to be modified for the case of this unicycle-type mobile robots. The kinematic model for each robot Ri , according to Figure 2, is given by

Fig. 2. Kinematic model of the uniclycle-type mobile robot.   � � x˙ i cos θi 0 � � vi  y˙ i  = sin θi 0 , i = 1, . . . , n (20) wi ˙θi 0 1 where vi is the longitudinal velocity of the middle point of wheels axis of the i-th robot, wi its angular velocity and θi the orientation with respect to the X axis. According to Brockett (1983), mobile agents modelled by (20) cannot be stabilized by any continuous and time-invariant control law . Then, to avoid singularities in the control law, we

2019 IFAC MECHATRONICS 130 Vienna, Austria, Sept. 4-6, 2019

J.F. Flores-Resendiz et al. / IFAC PapersOnLine 52-15 (2019) 127–132

study the kinematics of a point αi off the wheels axis, Hernandez-Martinez and Aranda-Bricaire (2013). The coordinates of point αi are given by     xi + ℓ cos θi αxi αi = = , (21) αyi yi + ℓ sin θi and its dynamics is given by     α˙ xi vi α˙ i = = Ai (θi ) (22) α˙ yi wi where   cos θi −ℓ sin θi Ai (θi ) = (23) sin θi ℓ cos θi is the so-called decoupling matrix for each robot Ri . The decoupling matrix is non-singular since det (Ai (θi )) = ℓ �= T 0. By defining auxiliary control inputs ui = [uix , uiy ] we can establish a strategy for controlling the position of the point αi by     vi uix −1 = Ai (θi ) (24) wi uiy where A−1 i (θi ) is the inverse of the decoupling matrix. The closed-loop system (22)-(24) becomes α˙ i = ui (25) The desired position of robot Ri , related to the coordinates αi , is given by 1  (αj + cji ) (26) α∗i = ni j∈Ni

Then, a formation control with collision avoidance, similar to the presented previously is written as ui = γ˜i + β˜i , i = 1, 2, . . . , n (27) ˜ where γ˜i and βi are defined in a similar way than in (5) and (8), respectively, but in terms of αi . Thus, the system is reduced to the case presented in previous Sections. It is important to remark that (24) steers the coordinates of the points αi to a desired position while the angles θi remain uncontrolled.

Fig. 3. AmigoBot Robots. • A positioning system. The position and orientation of each robot is measured through a vision system composed of 12 cameras Flex 13 manufactured by Natural Point (Figure 4) which are located at a height of 4 meters. These cameras have a resolution of 1280× 1024 pixels with at a frequency of 120 frames per second. To detect an object, it must have a minimum of 3 markers and at least 3 cameras must locate the object within their range of vision.

Fig. 4. Cameras Flex 13. • One Intel core i3-based computer. The software Motive calculates the position of the centroid of the geometric figure formed by the markers and its orientation. The control law is calculated in Visual C++ using Aria libraries which are also used to communicate with the robots. The protocol VRPN is used to share information between Motive and Visual C++. Finally, the velocities of each wheel are sent through Wi-Fi to the robots.

4.1 Experimental setup

4.2 Experiment results

The experiments were developed by using an experimental platform composed of the following elements:

The experiment consists of four agents R1 , ..., R4 and the goal is to reach the square formation shown in Figure 5 where √the relative position vectors are defined as c31 = √ 1 1 1 3 T 3 T ℓ[ 2 , − 2 ] , c13 = ℓ[− 2 , 2 ] , c12 = ℓ[− 21 , 2√ ], c32 = 3

• Four differential-drive mobile robots, model AmigoBot manufactured by MobileRobots Inc (Figure 3). Each one is furnished on their top with infrared markers which form a geometric pattern such that the centroid of this figure coincides with the middle point of the wheels’ axis of each robot for identification. The parameters of AmigoBot robots are: wheel radius r = 0.06 meters, length of wheels axis is L = 0.28 meters, distance to the front point of the robot ℓ = 0.15 meters, 8 sonar sensors to avoid collisions or locate obstacles and the maximum longitudinal velocity of 1 m/s. They feature wireless serial ethernet for remote operations, two position encoders and built-in velocity controllers. The workspace measures are 3.6 × 4.8 meters. The linear and angular velocities vi and wi obtained from the control law developed in this paper are transformed into linear velocities of the right and left wheel vri , vℓi through the isomorphism:      1 2 L vri vi = , i = 1, ..., n. (28) vℓi wi 2 2 −L 411

√

1 T ℓ[0, − √13 ]T , c43 = ℓ[ 21 , 23 ]T and c24 = ℓ[− 21 , − 2√ ] . In 3 this case, we take ℓ = 1.5 m, the velocity bound when no collision risk exists is µ = 0.5 m/s, the minimum allowed distance is d = 0.65 m and ε = 2.5. The initial conditions for the agents are [x10 , y10 ]T = [−1, 1]T , [x20 , y20 ]T = [1, −1]T , [x30 , y30 ]T = [−1, −1]T and [x40 , y40 ]T = [1, 1]T . Figure 5 illustrate the desired formation for this experiment as well as the communication links among agents. The results of numerical simulation are shown in Figures 6 and 7 while the experimental results are depicted in Figures 8 and 9. The trajectories followed by agents are shown in Figures 6 and 8 while Figures 7 and 9 show the distances between any pair of agents. As can be seen, numerical simulation and real-time experiment exhibit some differences. The most relevant reason to explain this differences is that the theoretical results are valid for firstorder systems, while the real robots are modelled by second order differential equations. Despite these differences,

2019 IFAC MECHATRONICS Vienna, Austria, Sept. 4-6, 2019

J.F. Flores-Resendiz et al. / IFAC PapersOnLine 52-15 (2019) 127–132

the multi-agent system converge to the desired formation and the agents exhibit a behaviour closely related to the simulation. The minimum distance between any pair of robots is always around the predefined bound. Finally, in

131

1.5

1

Y[m]

0.5 R1 R2 R3 R4 Initial position Final position

0

−0.5

−1

−1.5 −1.5

−1

−0.5

0

0.5

1

1.5

X [m]

Fig. 5. Desired formation.

Fig. 8. Real-time experiment: Trajectories of the four agents at the plane.

1.5

3 1

αi − αj  [m]

2.5

Y[m]

0.5

R1 R2 R3 R4 Initial position Final position

0

−0.5

1 0.5 0 0

−1

−1.5 −1.5

2 1.5

−1

−0.5

0

0.5

1

10

20

time [s]

30

40

50

Fig. 9. Real-time experiment: Distances among any pair of agents.

1.5

X [m]

Fig. 6. Simulation result: Trajectories of the four agents at the plane. 3

solve the issue but also it is done in finite-time. Simulation and real time experiment were performed to verify the effectiveness of the proposed algorithm. As future work, the design of formation control strategies with no collisions for second order agents is considered.

zi − zj  [m]

2.5

REFERENCES

2 1.5 1 0.5 0

5

10

15

time [s]

20

25

30

Fig. 7. Simulation result: Distances among any pair of agents. Figure 10 are shown the positions of robots at some specific time instants in order to illustrate the convergence to the desired formation. 5. CONCLUSIONS A general solution for the formation control problem without collisions has been presented. A control law is designed for each of the objectives, formation control and collision avoidance. A saturated error-based control law is used to achieve the desired formation, while a distancebased complement is designed for each agent to avoid collision with another agents. The convergence to the desired formation agents is proved to be asymptotically stable. On the other hand, when two or more agents are in risk of collision, the algorithm proposed is able not only to 412

Ahmadi Barogh, S., Rosero, E., and Werner, H. (2015). Formation control of non-holonomic agents with collision avoidance. Proceedings of the American Control Conference, 2015-July, 757–762. doi:10.1109/ACC.2015. 7170825. Bhat, S. and Bernstein, D. (2000). Finite-time stability of continuous autonomous systems. SIAM Journal on Control and Optimization, 38(3), 751–766. doi:10.1137/ S0363012997321358. URL https://doi.org/10.1137/ S0363012997321358. Brockett, R.W. (1983). Asymptotic stability and feedback stabilization. In R.S. Millman, R.W. Brockett, and H.J. Sussmann (eds.), Differential Geometric Control Theory, 181 – 191. Birkhauser, Boston, Massachusetts, USA. Dimarogonas, D.V. and Kyriakopoulos, K.J. (2006). Distributed cooperative control and collision avoidance for multiple kinematic agents. Proceedings of the 45th IEEE Conference on Decision and Control, 721–726. doi:10.1109/CDC.2006.376884. URL http://ieeexplore.ieee.org/lpdocs/epic03/ wrapper.htm?arnumber=4177320. Do, K.D. (2006). Formation control of mobile agents using local potential functions. In 2006 American Control Conference, 6 pp.–. doi:10.1109/ACC.2006.1656537.

2019 IFAC MECHATRONICS 132 Vienna, Austria, Sept. 4-6, 2019

J.F. Flores-Resendiz et al. / IFAC PapersOnLine 52-15 (2019) 127–132

Fig. 10. Real-time experiment: Agents’ positions in the plane at different times. Flores-Resendiz, J.F. and Aranda-Bricaire, E. (2014). Cyclic pursuit formation control without collisions in multi-agent systems using discontinuous vector fields. Congreso Latinoamericano de Control Autom´ atico, Canc´ un, M´exico. Flores-Resendiz, J.F. and Aranda-Bricaire, E. (2019). A general solution to the formation control problem without collisions for first order multi-agent systems. ROBOTICA, Cambridge University Press. Flores-Resendiz, J.F., Aranda-Bricaire, E., GonzalezSierra, J., and Santiaguillo-Salinas, J. (2015). Finitetime formation control without collisions for multiagent systems with communication graphs composed of cyclic paths. Mathematical Problems in Engineering, 17 pp. doi:10.1155/2015/948086. Hernandez-Martinez, E.G. and Aranda-Bricaire, E. (2011). Convergence and collision avoidance in formation control: A survey of the artificial potential functions approach. In Multi-agent Systems- Modeling, Control, Programming, Simulations and Applications. INTECH, Austria, 105–126. Hernandez-Martinez, E.G. and Aranda-Bricaire, E. (2013). Collision avoidance in formation control using discontinuous vector fields. 9th IFAC Symposium on Nonlinear Control Systems. Toulouse, France. Hernandez-Martinez, E. and Bricaire, E.A. (2012). Noncollision conditions in multi-agent virtual leader-based formation control. International Journal of Advanced 413

Robotic Systems, 9(4), 100. doi:10.5772/50722. URL https://doi.org/10.5772/50722. Jiang, Z.P., Lefeber, E., and Nijmeijer, H. (2001). Saturated stabilization and tracking of a nonholonomic mobile robot. Systems & Control Letters, 42(5), 327 – 332. doi:https://doi.org/10.1016/S0167-6911(00) 00104-3. URL http://www.sciencedirect.com/ science/article/pii/S0167691100001043. Kim, Y. and Bang, H. (2016). Decentralized control of multiple unmanned aircraft for target tracking and obstacle avoidance. 2016 International Conference on Unmanned Aircraft Systems, ICUAS 2016, 327–331. doi:10.1109/ICUAS.2016.7502547. Kostic, D., Adinandra, S., Caarls, J., and Nijmeijer, H. (2010). Collision-free motion coordination of unicycle multi-agent systems. In Proceedings of the 2010 American Control Conference (ACC 2010), 30 June 30 - 2 July 2010, Baltimore, Maryland, USA, 3186–3191. Institute of Electrical and Electronics Engineers (IEEE), United States. doi:10.1109/ACC.2010.5530852. Lafferriere, G., Williams, A., JohnS.Caughman, I.S.C., and Veerman, J.J.P. (2005). Decentralized control of vehicle formations. Systems & Control Letters, 54, 899– 910. Lin, Z., Broucke, M., and Francis, B. (2004). Local control strategies for groups of mobile autonomous agents. IEEE Transactions on Automatic Control, 49(4), 622– 629. Marshall, J.A., Broucke, M.E., and Francis, B.A. (2004). Formations of Vehicles in Cyclic Pursuit. IEEE Transactions on Automatic Control, 49(11), 1963–1974. Mastellone, S., Stipanovi´c, D.M., Graunke, C.R., Intlekofer, K.A., and Spong, M.W. (2008). Formation control and collision avoidance for multi-agent nonholonomic systems: Theory and experiments. The International Journal of Robotics Research, 27(1), 107–126. doi:10.1177/0278364907084441. URL https://doi.org/10.1177/0278364907084441. Mondal, A., Behera, L., Sahoo, S.R., and Shukla, A. (2017). A novel multi-agent formation control law with collision avoidance. IEEE/CAA Journal of Automatica Sinica, 4(3), 558–568. doi:10.1109/JAS.2017.7510565. Mousavi, M.S.R., Khaghani, M., and Vossoughi, G. (2010). Collision avoidance with obstacles in flocking for multi agent systems. 2010 International Conference on Industrial Electronics, Control and Robotics, IECR 2010, 1–5. doi:10.1109/IECR.2010.5720122. Phan, D., Yang, J., Grosu, R., Smolka, S.A., and Stoller, S.D. (2017). Collision avoidance for mobile robots with limited sensing and limited information about moving obstacles. Formal Methods in System Design, 51(1), 62– 86. doi:10.1007/s10703-016-0265-4. Ren, W. and Beard, R. (2008). Distributed consensus in multi-vehicle cooperative control: theory and applications. Communications and Control engineering series. Springer-Verlag, London. Rodr´ıguez-Seda, E.J., Tang, C., Spong, M.W., and Stipanovi´c, D.M. (2014). Trajectory tracking with collision avoidance for nonholonomic vehicles with acceleration constraints and limited sensing. The International Journal of Robotics Research, 33(12), 1569–1592. doi:10. 1177/0278364914537130. URL https://doi.org/10. 1177/0278364914537130.