Safe Coverage of Moving Domains for Vehicles with Second Order Dynamics
aa r X i v : . [ c s . M A ] S e p Safe Coverage of Moving Domains for Vehicleswith Second Order Dynamics
Juan Chacon, Mo Chen, and Razvan C. Fetecau
Abstract —Autonomous coverage of a specified area by robotsoperating in close proximity with each other has many potentialapplications such as real-time monitoring of rapidly changingenvironments, and search and rescue; however, coordination andsafety are two fundamental challenges. For coordination, wepropose a distributed controller for covering moving, compactdomains for two types of vehicles with second order dynamics(double integrator and fixed-wing aircraft) with bounded inputforces. This control policy is based on artificial potentials andalignment forces designed to promote desired vehicle-domainand inter-vehicle separations and relative velocities. We provethat certain coverage configurations are locally asymptoticallystable. For safety, we establish energy conditions for collisionfree motion and utilize Hamilton-Jacobi (HJ) reachability theoryfor last-resort pairwise collision avoidance. We derive an ana-lytical solution to the associated HJ partial differential equationcorresponding to the collision avoidance problem between twodouble integrator vehicles. We demonstrate our approach inseveral numerical simulations involving the two types of vehiclescovering convex and non-convex moving domains.
Index Terms —Artificial potentials, autonomous robots, cover-age control, decentralized control, Hamilton-Jacobi reachability,swarm intelligence.
I. I
NTRODUCTION
Autonomous systems have many potential applications inalmost every part of society; however, these systems stilltypically operate in controlled environments in the absenceof other agents. Two major challenges – coordination andsafety – arise when autonomous systems cooperate in closeproximity with each other. In this paper, we consider specifi-cally the problem of controlling multiple autonomous systemsto cover a desired possibly moving area in a decentralizedand safe manner. Applications of this problem include real-time surveillance of dynamic environments, efficient searchand rescue, and multi-agent aerobatics.The objective of coverage control problems is to deployagents to a possibly moving target area such that they canachieve an optimal sensing of the domain of interest. Acommon solution is through minimizing a coverage functionalinvolving a Voronoi tessellation and the locations of vehicleswithin the tessellation [1], [2]. This is a high-dimensionaloptimization problem which needs to be solved in real time.In our approach we achieve coverage through swarming byartificial potentials [3]–[5]. In a related problem, artificial J. Chacon is with the Department of Mathematics, Simon Fraser University,Burnaby, BC V5A 1S6, CANADA (e-mail: [email protected]).M. Chen is with the School of Computing Science, Simon Fraser University,Burnaby, BC V5A 1S6, CANADA (e-mail: [email protected]).R.C. Fetecau is with the Department of Mathematics, Simon Fraser Uni-versity, Burnaby, BC V5A 1S6, CANADA (e-mail: [email protected]). Alternative terminologies are balanced or anti-consensus configurations (a) t = 0( s ) (b) t = 9( s ) (c) t = 24( s ) (d) t = 60( s ) Fig. 1: Vehicles covering and following a moving triangulardomain, when N = 10 , c r = 2 (m), v max = 10 (m/s), u max =3 (m/s ) , t safety = 5 (s), a I = 1 (m/s ) , a h = 2 (m/s ) , a v = 0 . (m/s ) , C al = 0 . (m/s ) , l al = 7 . (m), v d = (cid:16) √ , √ (cid:17) (m/s), A = 292 . (m ) and r d = q AN = 5 . (m).Vehicles start in linear formation.potentials have been used for containment of follower agentswithin the convex hull of leaders [6], [7].Reachability analysis has been studied and used extensivelyin the past several decades as a tool for providing guaranteeson performance and safety of dynamical systems [8]–[10],as well as controller synthesis in many cases. In particular,Hamilton-Jacobi (HJ) reachability [11], [12] has seen successin collision avoidance [13], [14], air traffic management [15],[16], and emergency landing [17]. HJ reachability analysisis based on dynamic programming, and involves solving anHJ partial differential equation (PDE) to compute a backwardreachable set (BRS) representing states from which dangeris inevitable. By using the derived optimal controller on theboundary of the BRS, safety can be guaranteed despite theworst-case actions of another agent.In this paper we develop a new approach to self-collectivecoordination of autonomous agents that aim to reach and cover a moving target domain. We consider two types ofplanar vehicle dynamics which differ in nature by the allowedcontrol actions. The first, double integrator dynamics, allowsthe controller to specify the x and y accelerations at anytime; among others, it is a simplified model for quadrotors.The second, planar fixed-wing aircraft dynamics, in which thevehicle controls the acceleration and turn rate, is a naturalmodel for cars, bicycles or planes.Our approach aims to enable the following: i) reaching andspreading over a target domain without having set a priori the coverage configuration and the final state of each vehicle,ii) the use of a distributed control policy from which self-organization and intelligence emerges at the group level, and(iii) guarantee of collision-avoidance throughout the coordina-tion process.In this aim, we consider a control policy that includes botha coverage and a safety controller. The coverage controllerbrings vehicles inside a target domain, spreads them over thetarget domain, and aligns vehicle and domain velocities witheach other. On the other hand, the safety controller guaranteescollision avoidance of vehicles. A simulation of the emergentbehaviour resulting from our controller is shown in Figure 1,where N = 10 vehicles move to cover a moving triangulardomain.The proposed coverage controller uses two types of ar-tificial potentials and velocity alignment terms resemblingthe Cucker-Smale model with single leader [18], [19]. Oneartificial potential is for inter-individual forces which aredesigned to achieve a certain desired inter-vehicle spacingas in [3]. Such controller enables emergent self-collectivebehaviour of the vehicles, similar to the highly coordinatedmotions observed in biological groups such as flocks of birdsand schools of fish [20]. The other artificial potential is usedfor vehicle-target forces by which vehicles reach the targetand cover it. The Cucker-Smale terms promote the vehiclesto match the velocity of the target domain, which acts as aleader.We emphasize that the proposed coverage controller, whichalso drives vehicles inside the target domain, is done throughagent swarming; there is no leader and no order among theagents. This means that the controller does not rely on thewell functioning of each individual agent. Such self-collectiveand cooperative behaviour is present in systems of interactingagents in the physics and biology literature [21]–[25]. An agentsearch and target-locating algorithm based on a swarmingmodel was studied in [26].Unlike first-order models, where agents directly control theirvelocities, our vehicle models are second-order: agents areimplicitly or explicitly controlled through their acceleration.In addition, We set a priori bounds on the control forces,making our controller more realistic than previous approaches,in which infinite forces may be needed to guarantee collisionavoidance [3], [27].The safety controller for vehicles with double integratordynamics is derived from HJ reachability analysis. Insteadof numerically solving an associated Hamilton-Jacobi-Isaacs(HJI) PDE, we derive the analytical solution to the PDE toeliminate numerical errors and the need to specify compu- tation bounds. While multi-vehicle collision avoidance is ingeneral intractable, incorporating pairwise collision avoidancedrastically reduced the collision rate.The paper is organized as follows. Section II presents somebackground on Hamilton-Jacobi reachability. In Section III westudy the safe coverage problem for static domains with doubleintegrator dynamics. In Section IV we generalize the studyto moving domains following inertial trajectories. In SectionV we formulate a control algorithm for coverage of movingdomains with fixed-wing aircraft dynamics. Finally, we makeconcluding remarks and discuss open problems and potentialfuture directions of research.II. B ACKGROUND : H
AMILTON -J ACOBI R EACHABILITY
We review here some basic Hamilton-Jacobi reachabilitytheory, which will be used in the paper to address pairwisecollision avoidance.Consider the two-player differential game described by thejoint system ˙ z ( t ) = f ( z ( t ) , u ( t ) , d ( t )) ,z (0) = x, (1)where z ∈ R n is the joint state of the players, u ∈ U is thecontrol input of Player 1 (hereafter referred to as “control”)and d ∈ D is the control input of Player 2 (hereafter referredto as “disturbance”) .We assume f : R n × U × D → R n is uniformly continuous,bounded, and Lipschitz continuous in z for fixed u and d , and u ( · ) ∈ U , d ( · ) ∈ D are measurable functions. Under theseassumptions we can guarantee the dynamical system (1) hasa unique solution.In this differential game, the goal of player 2 (the distur-bance) is to drive the system into some target set using onlynon-anticipative strategies [11], while player 1 (the control)aims to drive the system away from it.We introduce the time-to-reach problem as follows. (Time-to-reach) Find the time to reach a target set Γ D while avoiding the obstacle Γ S from any initial state x , ina scenario where player 1 maximizes the time, while player2 minimizes the time. Player 2 is restricted to using non-anticipative strategies, with knowledge of player 1’s currentand past decisions. Such a time is denoted by φ ( x ) . Following [11], given u ( · ) and d ( · ) , the time to reach aclosed target set Γ D with compact boundary, while avoidingthe obstacle Γ S , is defined as T x [ u, d ] = min { t | z ( t ) ∈ Γ D and z ( s ) / ∈ Γ S , ∀ s ∈ [0 , t ] } . Then, the
Time-to-reach problem reduces to finding: φ ( x ) = min θ ∈ Θ max u ∈U T x [ u, θ [ u ]] , where Θ represents the set of non-anticipative strategies. Thecollection of all the states that are reachable in a finite timeis the capturability set R ∗ = { x ∈ R n | φ ( x ) < + ∞} . Applying the dynamic programming principle, as done in[28], one can obtain φ as the viscosity solution of the followingstationary HJ PDE: min u ∈U max d ∈D {−∇ φ ( z ) · f ( z, u, d ) − } = 0 in R ∗ \ (Γ D ∪ Γ S ) ,φ ( z ) = 0 on Γ D , φ ( z ) = ∞ on Γ S . (2)In applications, this PDE is typically solved using finitedifference methods such as the Lax-Friedrichs method [11].Also, from the solution φ ( x ) one can obtain the control inputfor optimal avoidance as: u ∗ ( z ) = arg min u ∈U max d ∈D {−∇ φ ( z ) · f ( z, u, d ) − } . (3)III. C OVERAGE OF A STATIC DOMAIN
A. Problem formulation
We consider a group of N vehicles, denoted by Q i , i =1 , . . . , N , with the double integrator dynamics given by ˙ p i = v i , ˙ v i = u i ; k v i k ≤ v max , k u i k ≤ u max . (4)Here, p i = ( p i,x , p i,y ) and v i = ( v i,x , v i,y ) are the positionand velocity of Q i respectively, and u i = ( u i,x , u i,y ) is thecontrol force applied to this vehicle.Given a predefined collision radius c r , a vehicle is consid-ered safe if there is no other vehicle within distance c r to it,i.e., if k p i − p j k > c r , for any j = i. (5)In this paper we are interested in certain configurations ofthe agents in the domain Ω . Specifically, we set the followingdefinitions. Definition III.1 ( r -Subcover) . A group of agents is an r -subcover for a compact domain Ω ⊆ R if: The distance between any two vehicles is at least r . The signed distance from any vehicle to Ω is less thanequal to − r . Definition III.2 ( r -Cover) . An r -subcover for Ω is an r -cover for Ω if its size is maximal (i.e., no larger number of agentscan be an r -subcover for Ω ). The r -subcover definition is closely related to the packingproblem for circular objects of radius r in a container withshape Ω . Having an r -cover implies the container is full andthere is no room for more of such objects.The following safe domain coverage problem is of maininterest to our work in this chapter. Safe-domain-coverage by vehicles with double integratordynamics:
Consider a compact domain Ω in the plane and N vehicles each with dynamics described by (4) , starting fromsafe initial conditions. Find the maximal r > and a controlpolicy that leads to a stable steady state which is an r -coverfor Ω , while satisfying the safety condition (5) at any time. The controller we design and present below has two compo-nents: a coverage controller and a safety controller, the latterbeing based on Hamilton-Jacobi reachability. We will presentthem separately. Fig. 2: Illustration of control forces acting on two vehicleslocated at p i and p j . B. Coverage controller
Define p ij := p i − p j , and denote by P ∂ Ω ( p i ) the closestpoint of ∂ Ω to p i (i.e., the projection of p i on ∂ Ω ). Also,define h i := p i − P ∂ Ω ( p i ) , and denote by [[ h i ]] the signeddistance of p i from ∂ Ω – see Figure 2.The proposed control force is given by: u i = − N X j = i f I ( k p ij k ) p ij k p ij k − f h ([[ h i ]]) h i [[ h i ]] − a v v i , (6)where the three terms in the right-hand-side represent inter-vehicle, vehicle-domain, and braking forces, respectively. Weassume each vehicle is able to measure its distance to thetarget domain, its speed, as well as its position relative to theother vehicles. In (6), a v is a fixed positive constant.Figure 2 illustrates the control forces for two genericvehicles located at p i and p j . Shown there are the unit vectorsin the directions of the inter-vehicle and vehicle-domain forces(yellow and blue arrows, respectively), along with the resultantthat gives the overall control force (red arrows). Note that dueto the nonsmoothness of the boundary, different points mayhave different types of projections: p i projects on the foot ofthe perpendicular to ∂ Ω , while p j projects on a corner pointof ∂ Ω .Figure 3 shows the specific forms of the functions f I and f h that we consider in this paper. Note that f I ( r ) is negative for r < r d , and zero otherwise. This means that for two vehicleswithin distance < r < r d from each other, their inter-vehicleinteractions are repulsive, while two vehicles at distance largerthan r d apart do not interact at all. The vehicle-domain force f h ( r ) is zero for r < − r d , and positive for r > − r d . For avehicle i outside the target domain, i.e., with [[ h i ]] > , thisresults in an attractive interaction force toward ∂ Ω . On theother hand, for a vehicle inside the domain, where [[ h i ]] < ,one distinguishes two cases: i) the vehicle is within distance r d to the boundary, in which case it experiences a repulsive forcefrom it, or ii) the vehicle is more than distance r d from theboundary, in which case it does not interact with the boundaryat all. Lemma III.3.
The inter-vehicle and vehicle-domain forces areconservative.
Fig. 3: Inter-vehicle and vehicle-domain control forces.Fig. 4: Inter-vehicle and vehicle-domain potentials.
Proof.
Define the potentials: V I ( p ij ) = Z k p ij k r d f I ( s ) ds, V h ( p i ) = Z [[ h i ]] − rd f h ( s ) ds. Then, ∇ i V h ( p i ) = f h ([[ h i ]]) ∇ ([[ h i ]]) = f h ([[ h i ]]) h i [[ h i ]] , where we have used the identity ∇ ([[ h i ]]) = h i [[ h i ]] (seeTheorem 5.1(iii) in [29]). Similarly, the inter-vehicle force isthe negative gradient of the potential V I .The potentials V I and V h are shown in Figure 4. Theirexplicit expressions are given by: V I ( x ) = ( a I ( k x k − r d ) for k x k < r d , for k x k ≥ r d , (7)and V h ( x ) = ( for [[ x − P ∂ Ω x ]] ≤ − r d , a h ([[ x − P ∂ Ω x ]] + r d ) for [[ x − P ∂ Ω x ]] > − r d , (8) where a I > is the slope of the function f I on [0 , r d ] and a h > is the slope of the function f h on (cid:2) − r d , ∞ (cid:1) . Notethat as a I and a h are positive, both potentials V I and V h arenon-negative.By Lemma III.3, the control given in Equation (6) becomes u i = N X j = i − ∇ i V I ( p ij ) − ∇ i V h ( p i ) − a v v i . (9) Asymptotic behaviour of the controlled system.
Considerthe following candidate for a Lyapunov function, consisting inkinetic plus (artificial) potential energy:
Φ = 12 N X i =1 (cid:16) ˙ p i · ˙ p i + N X j = i V I ( p ij ) + 2 V h ( p i ) (cid:17) . (10) Note that each term in Φ is non-negative, and Φ reachesits absolute minimum value when the vehicles are totallystopped. Also, at the global minimum Φ = 0 , the equilibriumconfiguration is an r d -subcover of Ω ; in particular, all vehiclesare inside the target domain.The time derivative of Φ can be calculated as: ˙Φ = N X i =1 ˙ p i · (cid:16) u i + N X j = i ∇ i V I ( p ij ) + ∇ i V h ( p i ) (cid:17) = − N X i =1 a v k v i k , where we used the dynamics (4) and equation (9). Note that ˙Φ is negative semidefinite and equal to zero if and only if v i = 0 for all i (i.e., all vehicles are at equilibrium).We first show that the group of vehicles remains withina compact set through time evolution. The key idea is thatthe vehicle-domain potential V h is confining the vehicles, andkeeps them as a group [30]. Proposition III.4.
Solutions of (4) , with control law given by (9) remain cohesive through time, i.e., there exists an
R > such that k p i ( t ) k ≤ R , for all i and t ≥ .Proof. Using that the kinetic energy and the potential V I arenon-negative, and Φ given by (10) is non-increasing, we have: N X i =1 V h ( p i ( t )) ≤ Φ( t ) ≤ Φ(0) . To show the boundedness of p i we only need to consider thecase p i / ∈ Ω , as otherwise the vehicles are inside the compactset Ω . Using the expression (8) for V h , we then find: a h N X i =1 (cid:16) k p i ( t ) − P ∂ Ω p i ( t ) k + r d (cid:17) ≤ Φ(0) . This shows that the distances from p i ( t ) to the domain Ω remain bounded for all t ≥ by q a h when p i / ∈ Ω . Remark III.5.
From LaSalle Invariance Principle we canconclude that the controlled system approaches asymptoticallyan equilibrium configuration. By the expressions (9) of thecontrol force and (10) of the Lyapunov function, these areequilibria that are critical points of the artificial potentialenergy P Ni =1 (cid:16)P Nj = i V I ( p ij )+ 2 V h ( p i ) (cid:17) . We expect that anycritical point other than the local minima (e.g., saddles orlocal maxima) are unstable [30], and hence, almost everysolution of the system will approach asymptotically a localminimum of the potential energy. For certain simple setups (e.g., a square number of vehiclesin a square domain – see Figure 6f, or a triangular numberof vehicles in a triangular domain), the r d -covers are isolatedequilibria. Hence, together with the fact that such equilibriaare global minimizers for Φ , their local asymptotic stabilitycan be inferred. The formal result is given by the followingproposition. Proposition III.6.
Consider a group of N vehicles withdynamics defined by (4) , and the control law given by (9) . Letthe equilibrium of interest be of the form ˙ p i = 0 , k p ij k ≥ r d and [[ h i ]] ≤ − r d for i, j = 1 , · · · , N (see Definitions III.1and III.2), and assume that this equilibrium configuration isisolated. Also assume that there is a neighborhood about theequilibrium in which the control law remains smooth. Then,the equilibrium is a global minimum of the sum of the artificialpotentials and is locally asymptotically stable.Proof. The proof follows from LaSalle invariance principleand the arguments made above.Choosing an adequate r d when solving the safe-domain-coverage problem leads to a nonlinear optimization problem(see [31]), which in general can be quite difficult. We setthe value of this parameter based on the assumption that anyvehicle is covering roughly the same square area, i.e., r d = r Area (Ω)
N . (11)Note that (11) gives the exact maximal radius when both thenumber of vehicles and the domain are square. The numericalexperiments presented in this paper, which also involve targetdomains in the shape of a triangle or an arrowhead, show that(11) leads indeed to the desired covers.
C. Collision avoidance
An important component of our study is the guarantee thatvehicles do not collide through the time evolution. For smallinitial energies, collision avoidance can be shown directly. Forgeneral cases, we introduce a safety controller based on HJreachability analysis.
Small initial energy.
The following results hold for initialdata with small energy Φ . Proposition III.7.
Consider a target domain Ω and a groupof N vehicles with dynamics defined by (4) and (9) . Assumethe energy Φ(0) of the initial configuration satisfies
Φ(0) < Z c r r d f I ( s ) ds = a I c r − r d ) . Then, no vehicle collision can occur for all t ≥ .Proof. Suppose by contradiction that there is a time t ∗ atwhich vehicles k and l are at collision radius from each other,that is, k p k ( t ∗ ) − p l ( t ∗ ) k = c r . Given that V I is non-negative,the inter-vehicle potential energy at the collision time can bebounded below as: N X i =1 N X j = i V I ( p ij ( t ∗ )) ≥ V I ( p k ( t ∗ ) − p l ( t ∗ )) = Z c r r d f I ( s ) ds. On the other hand, using that the kinetic energy and thepotential V h are non-negative, we have: N X i =1 N X j = i V I ( p ij ( t ∗ )) ≤ Φ( t ∗ ) ≤ Φ(0) . By combining the two sets of inequalities above one finds
Φ(0) ≥ R c r r d f I ( s ) ds , which contradicts the assumption on theinitial energy Φ(0) .The result above can be generalized as follows.
Proposition III.8.
Consider a target domain Ω and a groupof N vehicles with dynamics defined by (4) and (9) . Assumethe energy Φ(0) of the initial configuration satisfies
Φ(0) < ( k + 1) Z c r r d f I ( s ) ds, for some k ∈ Z + . Then, at most k distinct pairs of vehiclescould be possibly unsafe ( k = 0 guarantees a safe motion) forall t ≥ .Proof. Assume by contradiction that k +1 pairs of vehicles areunsafe at time t ∗ , i.e., their relative distances are less than orequal to c r at t ∗ . Then, on one hand, following the argumentin Proposition III.7, we have: N X i =1 N X j = i V I ( p ij ( t ∗ )) ≥ ( k + 1) Z c r r d f I ( s ) ds, where we use the fact that V I ( p ij ) is non-negative and non-increasing.On the other hand, N X i =1 N X j = i V I ( p ij ( t ∗ )) ≤ Φ( t ∗ ) ≤ Φ(0) , leading to a contradiction.Note that the two last results assume that the control law (9)is applied as it is, that is, it does not take into account the inputforce constrains in (4). The following HJ reachability analysisdeals with the input force bounds to guarantee pairwise safety. Collision avoidance via Hamilton-Jacobi theory.
To guar-antee pairwise collision avoidance for general configurations,we design a safety controller based on HJ reachability analysis.Consider the dynamics between two vehicles Q i , Q j definedin terms of their relative states p r,x = p i,x − p j,x , v r,x = v i,x − v j,x ,p r,y = p i,y − p j,y , v r,x = v i,x − v j,x , where the vehicle Q i is the evader, located at the origin, and Q j is the pursuer, the latter being considered as the modeldisturbance. The relative dynamical system can be written as: ˙ p r,x = v r,x , ˙ v r,x = u i,x − u j,x , ˙ p r,y = v r,y , ˙ v r,y = u i,y − u j,y , (12)with k u i k , k u j k ≤ u max , where u i = ( u i,x , u i,y ) and u j =( u j,x , u j,y ) are the control inputs of the agents Q i and Q j ,respectively. From the perspective of agent Q i , the controlinputs of Q j are treated as worst-case disturbance.System (12) can be put in the general form (1) fromSection II, with z = ( p r,x , p r,y , v r,x , v r,y ) , u = ( u x , u y ) :=( u i,x , u i,y ) , d = ( d x , d y ) := ( u j,x , u j,y ) , and f ( z, u, d ) beingthe right-hand-side of (12). Fig. 5: Geometric illustration for solving HJI PDE (2). Here c p represents the collision point.According to (5), the unsafe states are described by thetarget set Γ D = (cid:8) z : p r,x + p r,y ≤ c r (cid:9) . For now, the obstacleset Γ S is the empty set as it is not needed until Section V-B.Consider ψ ( z ) as the time it takes for the solution of thedynamical system (12), with starting point z in R ∗ \ Γ D , toreach Γ D when the disturbance and control inputs are optimal.As the two vehicles have the same capabilities we make theeducated guess that the optimal non-anticipative strategy forthe pursuer is to copy the evader accelerations, having so a zerorelative acceleration. This implies that the relative velocity v r will remain constant through time.If p r and v r are such that a collision can occur, thereexist a collision point c p , see Figure 5. This will be oneof the intersection points of the line crossing through p r with direction parallel to v r and the circle of radius c r centered at the origin. To get the collision time we replacethe coordinates of c p = ( p r,x + ψ ( z ) v r,x , p r,y + ψ ( z ) v r,y ) into the canonical equation of the circle. Using this geometricargument one can show that this time is the minimum of thetwo solutions of the quadratic equation: (cid:0) v r,x + v r,y (cid:1) ψ ( z ) + 2 ( p r,x v r,x + p r,y v r,y ) ψ ( z )+ (cid:0) p r,x + p r,y − c r (cid:1) = 0 . (13)It was shown in [32] that the collision time computed asabove satisfies indeed the HJI PDE (2). The formal result isthe following. Proposition III.9.
Consider the function ψ ( z ) defined as ψ ( z ) := − ( p r,x v r,x + p r,y v r,y ) − √ ∆ v r,x + v r,y in R ∗ \ Γ D , where ∆ = ( p r,x v r,x + p r,y v r,y ) − (cid:0) v r,x + v r,y (cid:1) (cid:0) p r,x + p r,y − c r (cid:1) . Also define ψ ( z ) to be on Γ D . Then ψ ( z ) satisfies equation (2) .Proof. We refer to [32, Prop. 5] for the proof of this result. Weonly note that the proof there is based on an explicit calculationof the arg min max of the expression in equation (2), whichwas found to be: u ∗ = d ∗ = u max (cid:16) ∂ψ ( z ) ∂v r,x , ∂ψ ( z ) ∂v r,y (cid:17)(cid:13)(cid:13)(cid:13) ∂ψ ( z ) ∂v r,x , ∂ψ ( z ) ∂v r,y (cid:13)(cid:13)(cid:13) . (14) By implicit differentiation of (13) we find: ∂ψ∂v r,x = − v r,x ψ ( z ) − p r,x ψ ( z ) (cid:0) v r,x + v r,y (cid:1) ψ ( z ) + ( p r,x v r,x + p r,y v r,y ) (15a) ∂ψ∂v r,y = − v r,y ψ ( z ) − p r,y ψ ( z ) (cid:0) v r,x + v r,y (cid:1) ψ ( z ) + ( p r,x v r,x + p r,y v r,y ) , (15b) and hence, from (14) we can derive a closed expression forthe optimal avoidance controller. Note that to use this pairwiseavoidance strategy we require each vehicle to know its speedand position relative to the other vehicles.The static HJI PDE (2) is typically approximated by finitedifference methods such as the one presented in [11]. Ourapproach, using an analytic solution, leads to two main ad-vantages. First, we do not have to deal with large amounts ofmemory and long computational times involved in refinementsof the numerical resolution. Second, while numerical methodscan only compute the solution in a bounded domain, an ana-lytical solution allows us to have the best possible resolutionin unbounded domains. This allows us to predict and react topossible collisions arbitrarily far into the future. D. Overall control logic
In this subsection we describe how to switch between thetwo controllers presented above.We will consider that vehicle Q i is in potential conflict withvehicle Q j if the time to collision ψ ( z i ) (here z i denotes therelative current state of the two vehicles), is less than or equalto a specified time horizon t safety . In such a case Q i mustuse the safety controller, otherwise, the coverage controller isused.In the case that a vehicle detects more than one conflict, itwill apply the control policy of the first conflict detected atthat particular time. Algorithm 1 describes the overall controllogic for a generic vehicle Q i .In Algorithm 1, lines 6 and 7 can be obtained from equations(14), (15a) and (15b) (also note the normalization step in line14), while line 12 comes from the explicit coverage control(6). Remark III.10.
By thresholding the force (Algorithm 1 line14), the theoretical results may not necessary hold anymore.However, when close to the desired operation point, the cov-erage forces are small enough to not need to be thresholded,in which case the theoretical results are indeed valid.E. Numerical simulations
Square domain.
We consider the coverage problem fora square domain. We present two strategies: both use thecoverage controller described in Section III-B, but only onestrategy switches to the safety controller when necessary,according to Section III-D. In both cases 16 vehicles start froma horizontal line setup outside of the target square domain; seethe starting locations of the trajectories in Figures 6a and 6b.The simulations from the left and right columns in Figure 6do not include, and respectively include, the safety controller.
Algorithm 1
Overall control logic for a generic vehicle Q i . IN : State x i of a vehicle Q i ; states { x j } j = i of other vehicles { Q j } j = i ; a domain Ω to cover. PARAMETER:
A time horizon for safety check t safety ; OUT : A control u i for Q i . saf e ← True; for j = i do z ← x i − x j ; if ψ ( z ) ≤ t safety then saf e ← False; U ix = − v r,x ψ ( z )+ p r,x ψ ( z ) ( v r,x + v r,y ) ψ ( z )+( p r,x v r,x + p r,y v r,y ) ; U iy = − v r,y ψ ( z )+ p r,y ψ ( z ) ( v r,x + v r,y ) ψ ( z )+( p r,x v r,x + p r,y v r,y ) ; break for; end if end for if safe then ( U ix , U iy ) =- P Nj = i f I ( k p ij k ) p ij k p ij k - f h ([[ h i ]]) h i [[ h i ]] − a v v i ; end if u i = u max ( U ix ,U iy ) k ( U ix ,U iy ) k ; RETURN : u i The large coloured dots represent the position of the vehicles,the dashed tails are past trajectories (shown for the previous5 seconds), and the arrows indicate the movement direction.Note that we do not show the arrows when the velocities aretoo small.At t = 0 (s) the only contributions come from the vehicle-domain forces, which pull the mobile agents toward theinterior of the square; see initial trajectory tails in Figures6a and 6b. At t = 5 (s) the vehicles without safety controllerare more prone to collisions due to the symmetry of the initialcondition. The safety controller breaks down the symmetryand enables the vehicles to enter the crowded area withoutcollisions.The presence of overshoots at later times (Figures 6c and6d) is expected, being due to the piece-wise linear vehicle-domain forces (i.e., spring-like forces). However, Figure 6dindicates that in addition to preventing collisions, the use of thesafety controller also reduces the overshoots. After t = 50 (s)both control strategies reach a steady state which is an r d -cover for the square . We note that the system with collisionavoidance reaches the equilibrium faster; compare Figures 6eand 6f.A collision event starts when the distance between twovehicles is less than or equal to the collision radius c r , andends when the distance becomes greater than c r . The collisionevent count for the square domain coverage with and withoutthe safety controller, for various number of vehicles, is shownin Table I. We point out that in the absence of the safetycontroller, the collision count increases significantly with thenumber of vehicles, while it remains zero or very low whenthe safety controller is used. (a) t = 5( s ) (b) t = 5( s ) (c) t = 10( s ) (d) t = 10( s ) (e) t = 50( s ) (f) t = 50( s ) Fig. 6: Square domain coverage at different time instants,without (left) and with (right) safety controller, when N = 16 , c r = 2 (m), v max = 10 (m/s), u max = 3 (m/s ) , t safety =5 (s), side length l = 20 (m), domain area A = l = 400 (m )and r d = q AN = 5 (m). Vehicles start in a horizontal lineconfiguration and reach a square grid steady state which isan r d -cover of the domain (see Definition III.2). The use ofthe safety controller reduces both the collision count and theovershoot, and helps reach the steady state faster.TABLE I: Square coverage collision count. number of vehicles without avoidance with avoidance9 8 016 51 025 146 2 Safety issues may also arise when a vehicle needs toavoid two or more vehicles at the same time. Our safetycontroller does not guarantee collision avoidance in such cases.Guaranteed collision avoidance for more than two vehicles isan unsolved problem, as explored for example in [14].
IV. C
OVERAGE OF A MOVING DOMAIN
A. Problem formulation
We consider now the coverage problem when the target do-main moves with prescribed constant velocity v d . Specifically,let Ω ⊆ R be a compact domain and define Ω t = Ω + tv d ,representing the moving domain at time t . Alternatively, if onesets an arbitrary marker point p d (e.g., the centre of mass) in Ω , its motion is given by p d ( t ) = p d + tv d .We are interested in covering the domain Ω t (see DefinitionsIII.1 and III.2), which changes through time. For this reasonwe want the vehicles to reach asymptotically, as t → ∞ , thevelocity of the target domain, while maintaining a cohesivegroup through dynamics. This is expressed by the concept offlocking [24], [30], [33].Consider a group of N vehicles, each of them governed bythe double integrator dynamics, i.e., ˙ p i = v i , ˙ v i = u i , i = 1 , . . . , N, (16)with control u i to be specified later. We adapt below thedefinition of flocking from [34] to the problem of movingtarget. Definition IV.1 (Flocking with a moving target) . A group ofvehicles has a time-asymptotic flocking with a target domainmoving with constant velocity v d if its positions and velocities { p i , v i } , i = 1 , · · · , N satisfy the following two conditions: The relative positions with respect to the marker pointin the domain are uniformly bounded in time (forminga group): sup ≤ t< ∞ N X i =1 k p i ( t ) − p d ( t ) k < ∞ . The relative velocities with respect to the moving domaingo to zero asymptotically in time (velocity alignment): lim t → + ∞ N X i =1 k v i ( t ) − v d k = 0 . In this case, our safe domain coverage problem of interestis the following:
Safe-domain-coverage by vehicles with double integratordynamics for moving domains:
Consider a compact domain Ω t that moves with constant velocity v d in the plane, and N vehicles with dynamics described by (16) , starting from safeinitial conditions. Find the maximal r > and a control policythat leads to an r -cover for Ω t that flocks with the movingtarget, while satisfying the safety condition (5) at any time.B. Coverage controller with alignment Using the same coverage controller (6) for this problemmakes the vehicles lag behind the domain, reacting only whenthey are outside of it. This suggests that the vehicles requirea mechanism to align their velocities with that of the targetdomain, as well as with the velocities of their neighbors. Inspired by the Cucker-Smale model with rooted leadership(see [18], [19]), we propose a control force with inclusion ofinter-vehicle and vehicle-domain alignment forces, given by: u i = − N X j = i f I ( k p ij k ) p ij k p ij k − f h ([[ h i ]]) h i [[ h i ]] − N X j = i f al ( k p ij k ) v ij | {z } inter-vehicle − a v ( v i − v d ) | {z } vehicle-domain . (17)Here, p ij := p i − p j , v ij := v i − v j , and P ∂ Ω t ( p i ) denotes theprojection of p i on ∂ Ω t . Also, h i := p i − P ∂ Ω t ( p i ) , and [[ h i ]] denotes the signed distance of p i from ∂ Ω t . In addition, f al is a non-negative communication function and a v is a positiveconstant.The inter-vehicle alignment force, which controls the align-ment of vehicle i ’s velocity with the velocities of the rest ofthe vehicles, depends on the relative distance k p ij k betweenthe interacting vehicles. For a communication function f al thatis non-increasing (this is a typical assumption in the literature[33], [34]), vehicles align stronger with their neighbours, andless with vehicles that are further apart. The results presentedin this paper correspond to a communication function in theform: f al ( k p ij k ) = C al e − k pij k lal , where C al and l al are constants associated to the alignmentstrength and alignment range, respectively. This function wasconsidered in [25] in the context of honeybee swarms.The vehicle-domain alignment force drives the velocity ofthe vehicles to the domain’s velocity v d . In this regard, thebraking force in the static domain model (see (6)) can also beinterpreted as an alignment force that brings the vehicles toa stop. Also, while for simplicity we have taken a commonconstant a v for all vehicles, the considerations that followapply to the more general alignment forces a v,i ( v i − v d ) , with a v,i > .By changing to relative coordinates with respect to the frameof the moving domain, one can recover the case of a stationarydomain ( v d = 0 ). Indeed, change variables to: ˜ p i := p i − tv d , ˜ v i := v i − v d , (18)and note that the inter-vehicle positions and velocities areinvariant to this change of coordinates, i.e., ˜ p ij := ˜ p i − ˜ p j = p ij , ˜ v ij := ˜ v i − ˜ v j = v ij . Also, by translation, the distance to the target domain satisfies h i = ( p i − tv d ) − P ∂ Ω t − tv d ( p i − tv d )= ˜ p i − P ∂ Ω (˜ p i ) . Hence, in the new variables, the signed distances [[˜ h i ]] , where ˜ h i := ˜ p i − P ∂ Ω (˜ p i ) , (19)are with respect to the initial (fixed) domain Ω . The observations above allow us to rewrite the control (17)in the new variables. We find that in the moving coordinateframe the dynamics of the N vehicles is given by: ˙˜ p i = ˜ v i , ˙˜ v i = ˜ u i , i = 1 , . . . , N, where ˜ u i = − N X j = i f I ( k ˜ p ij k ) ˜ p ij k ˜ p ij k − f h ([[˜ h i ]]) ˜ h i [[˜ h i ]] − N X j = i f al ( k ˜ p ij k ) ˜ v ij − a v ˜ v i . Note that this corresponds to the dynamics in the originalvariables for a stationary domain.
C. Asymptotic behaviour
We first investigate the dynamics with control (17) for astationary target ( v d = 0 ), using the same interaction functions f I and f h from Section III, corresponding to potentials (7) and(8). Consider the same candidate for a Lyapunov function,consisting in kinetic plus (artificial) potential energy: Φ = 12 N X i =1 (cid:16) ˙ p i · ˙ p i + N X j = i V I ( p ij ) + 2 V h ( p i ) (cid:17) . Note that each term in Φ is non-negative, and Φ reaches itsabsolute minimum value when the vehicles are totally stopped.The time derivative of Φ can be calculated as: ˙Φ = N X i =1 ˙ p i · (cid:16) u i + N X j = i ∇ i V I ( p ij ) + ∇ i V h ( p i ) (cid:17) = N X i =1 v i · (cid:18) − N X j = i f al ( k p ij k )( v i − v j ) − a v v i (cid:19) . (20)For the inter-vehicle alignment term, write N X i =1 v i · N X j = i f al ( k p ij k )( v i − v j ) =12 N X i =1 v i · N X j = i f al ( k p ij k )( v i − v j )+ 12 N X j =1 v j · N X i = j f al ( k p ji k )( v j − v i ) , where in the second term in the right-hand-side we simplyrenamed i ↔ j as indices of summation. From there, use that k p ij k = k p ji k to get: N X i =1 v i · N X j = i f al ( k p ij k )( v i − v j ) = 12 N X i =1 N X j = i f al ( k p ij k ) k v i − v j k . Hence, from (20), we find: ˙Φ = − N X i =1 N X j = i f al ( k p ij k ) k v i − v j k − a v N X i =1 k v i k . In the case of a target domain moving with velocity v d , onecan change to relative coordinates (18) as explained in SectionIV-B, and set: Φ = 12 N X i =1 (cid:16) ˙˜ p i · ˙˜ p i + N X j = i V I (˜ p ij ) + 2 V h (˜ p i ) (cid:17) . (21)Then, by the calculations for the stationary target above, ˙Φ = − N X i =1 N X j = i f al ( k ˜ p ij k ) k ˜ v i − ˜ v j k − a v N X i =1 k ˜ v i k = − N X i =1 N X j = i f al ( k p ij k ) k v i − v j k − a v N X i =1 k v i − v d k . (22)Note that ˙Φ is negative semidefinite and equal to zero ifand only if ˜ v i = 0 (or equivalently v i = v d ) for all i , i.e.,when vehicles’ velocities are aligned with the velocity of thedomain. The construction of this Lyapunov function leads tothe following flocking result. Theorem IV.2 (Flocking with the moving target) . Consider atarget domain Ω t that moves with constant velocity v d , and agroup of N vehicles with smooth dynamics governed by (16) ,with the control law given by (17) . Then, the group of agentshas a time-asymptotic flocking with the moving target Ω t .Proof. We have to check the conditions in Definition IV.1.Group cohesiveness (condition 1) can be shown exactly asfor Proposition III.4, by using relative coordinates. Indeed,since in relative coordinates the distances to the target are withrespect to the fixed domain Ω (see (19)), a similar argumentshows that the distances from ˜ p i ( t ) to the domain Ω remainbounded by p /a h when ˜ p i / ∈ Ω . Restoring the originalvariables, we can then conclude that there exists R > suchthat k p i ( t ) − p d ( t ) k ≤ R , for all i and t ≥ .To show velocity alignment (condition 2), we first note thatthe velocities are also uniformly bounded in time. Indeed,since the potentials V I and V h are non-negative and Φ is non-increasing, we have: N X i =1 k ˜ v i ( t ) k ≤ t ) ≤ . Hence, the solutions (˜ p i ( t ) , ˜ v i ( t )) of the relative system areconfined within a compact set through dynamics. By LaSalleInvariance Principle we conclude that the solutions approachasymptotically the largest invariant set in { ˙Φ = 0 } . Con-sequently, we infer by (22) that as t → ∞ , the vehicles’velocities approach the velocity of the target domain. Remark IV.3.
The asymptotic states are critical points of Φ that satisfy ˜ v i = 0 for all i . Alternatively, these equi-libria are critical points of the artificial potential energy P Ni =1 (cid:16)P Nj = i V I (˜ p ij )+2 V h (˜ p i ) (cid:17) . We expect that almost everysolution of the relative system will approach asymptotically alocal minimum of this potential energy. Most relevant to our study are the r d -covers discussed inSection III, now in the context of these configurations being equilibria in the moving frame of the target. Since the potentialenergy vanishes at such configurations, these relative equilibriaare global minimizers. As discussed in the stationary case,in some certain simple geometries, such equilibria are alsoisolated. For such states, by similar arguments to those usedfor Proposition III.6, the following local asymptotic result canbe established. Proposition IV.4.
Consider a target domain Ω t that moveswith constant velocity v d , and a group of N vehicles withdynamics defined by (16) and (17) . Let the relative equilibriumof interest be of the form ˙˜ p i = 0 , k ˜ p ij k ≥ r d and [[˜ h i ]] ≤− r d for i, j = 1 , · · · , N (see Definitions III.1 and III.2), andassume that this equilibrium configuration is isolated. Alsoassume that there is a neighborhood about the equilibriumin which the control law remains smooth. Then, the relativeequilibrium is a global minimum of the sum of all the artificialpotentials and is locally asymptotically stable. Remark IV.5.
All considerations in this subsection apply tothe case of zero inter-individual alignment forces ( f al = 0 ).In such case, by working in the moving frame of the domain,the problem reduces in fact to the one studied in Section III. As mentioned in Remark III.10, the previous theoreticalresults can only be guaranteed if the control force remainssufficiently small, in other words, if it is threshold free.
D. Numerical simulations
In this subsection, we show three numerical simulationscenarios for vehicles using the coverage controller (17).While the first two scenarios are covered by the theory, the lastone illustrates how our strategy still leads to appropriate finalconfigurations even when the domain follows non-inertial tra-jectories. The possible safety issues are addressed as describedin Section III-C.
Triangular domain.
We consider the scenario in which anequilateral triangular domain moving with constant velocity, v d = (cid:16) √ , √ (cid:17) , is covered by a triangular number of vehicles,i.e. N = n ( n +1)2 , n ∈ N . At the start of the simulation thevehicles lie on a line outside the domain (see Figure 1a).The evolution for a group of N = 10 agents, each of themusing the coverage with velocity alignment and pairwise safetystrategies discussed above, is illustrated in Figures 1b-1d. Thetails represent the 15-second history of the vehicle positions.Some of the effects of strong alignments, that is, large a v or C al values, include vehicles spreading slowly inside thedomains or in some cases not reaching the target formation,as pointed out in [35].On the other hand, weak alignments, i.e. small a v and C al values, cause undesired overshoots, and slower asymptoticflocking. Therefore, it is important to maintain a good balancebetween the strength of the alignment and coverage forces. Non-convex domain.
We now study the scenario in whichvehicles cover and follow a moving non-convex domain inthe shape of an arrowhead. While the domain preserves itsshape, it moves with a constant velocity v d = (cid:16) √ , √ (cid:17) . (a) t = 18( s ) (b) t = 60( s ) Fig. 7: Vehicles covering and following a moving, non-convexdomain, when N = 9 , c r = 2 (m), v max = 10 (m/s), u max =3 (m/s ) , t safety = 5 (s), a I = 1 (m/s ) , a h = 2 (m/s ) , a v =0 . (m/s ) , C al = 0 . (m/s ) , l al = 7 . (m), domain area A = 225 (m ) and r d = q AN = 5 (m). The vehicles startin linear formation, approach and cover the domain, whilefollowing it. The vehicles lagging behind exhibit oscillationsdue to a bouncing effect in the narrow corners.Different time instants of the simulation are shown in Figure7, where the tails represent the vehicle positions during thelast 20 seconds of the simulation. Initially, all the 9 vehicleslie on a line perpendicular to the movement direction of thetarget domain, as shown by the tails of the vehicles in Figure7a.We distinguish two main behaviours: during a first phaseof the simulation (Figure 7a) the vehicles cover the domainapproximately evenly, adopting the arrow shape, while in asecond phase (Figure 7b), a clearer domain-following be-haviour is observed. The oscillations of the two vehicles thatare lagging behind are the effect of their proximity to thecorners. Indeed, as one of the line segments of the boundarywedge gets closer to the vehicle near the corner, it pushesit towards the other segment of the wedge, a back-and-forthmotion that causes the zigzagging. These oscillations can bereduced by reinforcing the velocity alignment.Unlike the convex case, in non-convex domains the projec-tion on the boundary for points outside of the domain may notbe unique; this is the case for instance of the green vehiclein the middle of the initial setup – see start of the tails inFigure 7a. Although the chance for a vehicle to lie in one ofthese states is extremely unlikely (the set of points where thishappens has zero measure), this fact may yield ambiguity inthe definition of the domain-vehicle force. We mitigate thisissue by considering the contribution from only one of themultiple projection points; consequently, the numerical timeevolution may depend on the chosen projection method. Domain moving in a circle.
Finally, we include the caseof a target domain moving with non-zero acceleration, morespecifically, an equilateral triangular domain moving on acircular path. The triangular domain moves so that its centreof mass describes a circular motion of radius 30 with constantangular velocity π , while aligning its heading to be tangent (a) t = 9( s ) (b) t = 24( s ) (c) t = 48( s ) (d) t = 70( s ) Fig. 8: Vehicles covering and following a non-zeroacceleration triangular domain moving over the path (cid:0)
30 cos (cid:0) π t (cid:1) ,
30 sin (cid:0) π t (cid:1)(cid:1) . Here, N = 6 , c r = 2 (m), v max = 10 (m/s), u max = 3 (m/s ) , t safety = 5 (s), a I =10 (m/s ) , a h = 10 (m/s ) , a v = 1 (m/s ) , C al = 1 . (m/s ) , l al = 10 . (m), domain area A = 292 . (m ) and r d = q AN = 6 . (m). The vehicles start in linear formation,approach and cover the domain, while following it.to this circle (see Figure 8). Note that this non-inertial path isnot covered by our previous theoretical results (Theorem IV.2and Proposition IV.4).The vehicles’ time evolution is illustrated in Figure 8, wherethe tails represent the vehicle positions during the last 20seconds of the simulation. At the beginning, the N = 6 vehicles are in the line formation as shown by the beginning ofthe tails in Figure 8a. As in previous simulations, the vehiclestry to reach the moving domain, this time rotating around thedomain’s circular path (Figures 8a and 8b). Then, the vehiclesreach coverage of the domain (Figure 8c) which is maintainedby each vehicle by remaining in a circular movement ofconstant radius (Figure 8d).When a vehicle describes a uniform circular movementwith angular velocity ω and radius r , its speed remainsconstant over time and is given by rω . As the vehicles moveasymptotically along circles with different radii, they havedifferent velocities, and hence, this type of ”flock” does notsatisfy Definition IV.1. In contrast to the case when the domainis moving along inertial paths, in this case each vehicle’scontrol force magnitude does not go asymptotically to zero,but it approaches its centripetal acceleration rω instead. V. P LANAR F IXED -W ING A IRCRAFT
A. Problem formulation
In this section, we consider the flocking coverage problemfor N vehicles governed by the planar fixed-wing aircraftdynamics, given by: ˙ p i = s i (cos ( θ i ) , sin ( θ i )) , (cid:16) ˙ θ i , ˙ s i (cid:17) = ( u i,θ , u i,s ) ;0 < s min ≤ k ˙ p i k ≤ s max , | u i,θ | ≤ u θ max , | u i,s | ≤ u s max . (23)Here, θ i is the heading angle, s i is the vehicle speed and p i = ( p i,x , p i,y ) is the position of the i -th agent. The variables u i,s and u i,θ are the acceleration and turn rate applied tothis vehicle respectively; these are the control inputs to bespecified later. In addition to the bounds for the controls, wealso impose maximum and minimum speed limits, the latterbeing particularly relevant for aerial vehicles.In this case, our safe domain coverage problem of interest isthe same as for the double integrator dynamics, but consideringfixed-wing vehicles. B. Coverage controller and safety controller
The goal is to find expressions for each agent’s controllaw based on the proposed coverage policy with inter-vehicleand vehicle-domain alignment forces (17), while satisfying theconstraints given in (23).By differentiation of the vehicle dynamics (23) with respectto time, one can find that the acceleration in Cartesian coor-dinates of the i -th agent in terms of the control inputs are asfollows: (cid:18) ¨ p i,x ¨ p i,y (cid:19) = R ( θ i , s i ) (cid:18) u i,θ u i,s (cid:19) , (24)where R ( θ i , s i ) := (cid:18) − s i sin ( θ i ) cos ( θ i ) s i cos ( θ i ) sin ( θ i ) (cid:19) . This relation allows us to compute an expression for thevehicle control inputs in terms of its acceleration in Cartesiancoordinates whenever s i = 0 : (cid:18) u i,θ u i,s (cid:19) = ( R ( θ i , s i )) − (cid:18) ¨ p i,x ¨ p i,y (cid:19) . (25)Using this correspondence, one can obtain the necessary con-trols ( u i,θ , u i,s ) to achieve the same acceleration in Cartesiancoordinates produced by the proposed control force (17) as: (cid:18) u i,θ u i,s (cid:19) = ( R ( θ i , s i )) − − N X j = i f I ( k p ij k ) p ij k p ij k− f h ([[ h i ]]) h i [[ h i ]] − N X j = i f al ( k p ij k ) v ij − a ( v i − v d ) . (26) The changes of coordinates (24) and (25) guarantee that allthe stability results in Chapter IV are still valid in the caseof the planar fixed-wing aircraft model when no constraintsare applied, as long as none of the vehicles stop along theirtrajectories. This seems to be a very plausible assumption inpractice, as the minimum speed is supposed to be greater thanzero. Fig. 9: Thresholded fixed-wing aircraft control input ˆ u i forvehicle with speed s i and heading θ i computed from its setof admissible accelerations in Cartesian coordinates S ( θ i , s i ) and a reference acceleration dv i dt . Thresholding the control force.
While finding an admissi-ble force satisfying the constraints for the double integratormodel (16) is done by simply normalizing the vehicles’control input (17), obtaining a suitable fixed-wing control inputsatisfying the constraints (23) is not as straightforward.In order to obtain the appropriate fixed-wing aircraft controlinputs we use relation (24), which allows us to represent theset of admissible accelerations from the Cartesian perspective: S ( θ i , s i ) = (cid:26) R ( θ i , s i ) (cid:18) u θ u s (cid:19) : (cid:18) u θ u s (cid:19) ∈ [ − u θ max , u θ max ] × [ − u s max , u s max ] (cid:27) . This set can be understood as a stretch and rotation of therectangle containing the admissible vehicle control inputs.The input constraints affect the magnitude of the vehicle’sacceleration, however, we intend to preserve its direction. Letus define the Cartesian admissible force associated to thecontrol (26) as (cid:18) ˆ u i,x ˆ u i,y (cid:19) = τ ( θ i , s i ) R ( θ i , s i ) (cid:18) u i,θ u i,s (cid:19) , where τ ( θ i , s i ) = sup (cid:26) t ∈ R : tR ( θ i , s i ) (cid:18) u i,θ u i,s (cid:19) ∈ S ( θ i , s i ) (cid:27) . In other words, ˆ u i us the largest acceleration in the setof admissible accelerations in Cartesian coordinates that isparallel to the desired acceleration – see Figure 9.Finally, the thresholded fixed-wing aircraft control inputscan be obtained by using (24) as (cid:18) ˆ u i,θ ˆ u i,s (cid:19) = ( R ( θ i , s i )) − (cid:18) ˆ u i,x ˆ u i,y (cid:19) = τ ( θ i , s i ) (cid:18) u i,θ u i,s (cid:19) . Collision avoidance.
As mentioned above, the changes ofcoordinates (24) and (25) guarantee that the unconstrainedfixed-wing aircraft dynamics satisfies the vehicles safety con-ditions when the initial energy is small enough, as establishedin Proposition III.7.As the set of control inputs and non-anticipative distur-bances differ from those in the double integrator dynamics,the collision avoidance via Hamilton-Jacobi reachability for Fig. 10: Relative coordinate system for pairwise collisionavoidance in the fixed-wing model. Figure adapted from [36]© [2005] IEEE with authors’ permission.this type of vehicles requires a different study than the onecarried out in Section III-C.Similarly to the double integrator case, we consider therelative dynamics between a pair of vehicles, where one acts asevader and the other as pursuer. Using as reference Figure 10,to obtain the relative dynamics we consider the evader fixedat the origin, and facing the positive x axis. In the figure, x is the projection of the vector connecting the vehicles’positions on the axis parallel to the evader’s heading, and x isits projection on the orthogonal direction. Also, x representsthe difference of the two agents’ headings, while x and x represent the evader and pursuer speeds, respectively.The pursuer’s speed, relative location and heading, alongwith the evader’s speed, are described by the following dy-namical system: ˙ x = f ( x, u, d ) = x cos ( x ) − x + u θ x x sin ( x ) − u θ x d θ − u θ u s d s . Here u := ( u θ , u s ) and d := ( d θ , d s ) , where u θ , u s and d θ , d s are the evader’s, respectively the pursuer’s, turn rate andacceleration, with the latter being treated as disturbances.By the dynamic programming principle, the time φ to reachcollision is the viscosity solution for the stationary HJ PDE(2) where Γ D = (cid:8) z : z + z ≤ c r or z < s min or z > s max (cid:9) is the union of a five dimensional cylinder of radius c r on thefirst two dimensions, with two half-spaces, and Γ S = { z : z < s min or z > s max } is the union of two half-spaces.Note that unlike the typical time-to-reach setting, we con-sider Γ D to be the set of dangerous states, and Γ S to be theset of unsafe states that “invalidates” danger. Specifically, Γ S ensures that the pursuer does not violate its speed constraintswhile trying to cause a collision.Obtaining an analytical solution for the HJ PDE associatedto this problem seems to be much more difficult than in thedouble integrator case, and we opt for solving it numerically. The challenges of solving numerically this HJ PDE aretwo-fold. First, the memory requirements to store the solu-tion are large even for coarse resolutions, and second, thecomputational time scales poorly as the grid size grows.This is particularly problematic when the algorithm specifi-cations are not optimized for the hardware architecture. Wealleviate the second issue by using the new python toolboxhttps://github.com/SFU-MARS/optimized dp for solving HJPDEs, which yields faster executions by decoupling the al-gorithm from the hardware specifications.
C. Numerical simulations
In this subsection we consider two simulation scenariosrelated to those investigated in Section IV-D, and illustrate howour control strategy leads to similar coverage configurations.We do not intend to compare the systems’ evolution, as theyare two distinct types of vehicles with different capabilities.
Triangular domain.
In the first scenario, we consider anequilateral triangular domain moving with constant velocity v d = (cid:16) √ , √ (cid:17) , which is covered by a triangular number ofvehicles. The minimum for vehicles’ speed is set at . (m/s),while the maximum is (m/s). Each of the fixed-wing agentsuses the coverage controller with velocity alignment (26)discussed in Subsection V-B.Figure 11 shows four different time steps of the evolutionof the vehicles. The tails represent the last 15 seconds of thevehicles’ position history. At the start of the simulation the N = 10 vehicles lie on a line outside the domain, moving withthe minimum allowed speed and random headings (see Figure11a). As time evolves, the vehicles approach the domain(Figure 11b), and then cover it by taking a triangular formationmoving with constant velocity as expected, see Figures 11cand 11d.In this particular case, the same coverage controller param-eters used for the double integrator vehicles seem to workwell. However, it is not a rule of thumb, as the thresholdingstrategies are very different. We also note that the collisionscount goes from 2, when no collision avoidance is included,to 0, when the safety controller is used. Domain moving in a circle.
The vehicles start in a lineformation as shown in Figure 12a. They reach the targetdomain and spread inside it (Figures 12b and 12c). Once theycover the domain, each of the fixed-wing agents follows acircular path with constant angular velocity ω = π (Figure12d). Under this configuration the vehicles have reached theirterminal speed and do not require extra acceleration, i.e. u s = 0 , however they should maintain a turn rate of u θ = ω .The collision count goes from 7 to 1 by including collisionavoidance. Similar to previous sections, we note that ourapproach based on pairwise collision avoidance does notguarantee safety when a vehicle has to avoid two or morevehicles at the same time.VI. C ONCLUSION
A. Summary of results
Our proposed controller for multi-vehicle coordination al-lows a swarm of vehicles to cover moving planar shapes. (a) t = 0( s ) (b) t = 2 . s ) (c) t = 12( s ) (d) t = 48( s ) Fig. 11: Vehicles with planar fixed-wing aircraft dynamicscovering and following a moving equilateral triangular do-main, when N = 10 , c r = 2 (m), s max = 10 (m/s), s min = 0 . (m/s), u θ max = π/ (rad/s), u s max = 3 (m/s ) , a I = 1 (m/s ) , a h = 2 (m/s ) , a v = 0 . (m/s ) , C al =0 . (m/s ) , l al = 7 . (m), v d = (cid:16) √ , √ (cid:17) (m/s), domainarea A = 292 . (m ) and r d = q AN = 5 . (m). Collisionavoidance controller is included. The vehicles start in linearformation.Unlike previous coverage controllers that assumed first-ordervehicle models, our coverage controllers use more realisticsecond-order models – double integrator and fixed-wing air-craft. We prove that our coverage controller achieves coverageand flocking with moving planar domains, and that the coverconfigurations of interest are locally asymptotically stable.Using HJ reachability analysis, we guarantee pairwise collisionavoidance while accounting for bounded control inputs. In ad-dition, we also derive the analytical solution to the associatedHJ PDE for the double integrator model.Our numerical simulations illustrate successful coverage ofstatic and moving domains on four representative scenarios:static square, non-accelerated moving triangular and arrow-head (non-convex) domains, and a triangular domain followinga circular path. While the first three scenarios are covered byour theoretical results, the last is not. Nevertheless, we findsatisfactory numerical results in this case as well, suggestingsome generality of the proposed technique. For simulationsinvolving the double integrator, we observe drastic reductionof collisions when using the HJ-based collision avoidancecontroller. (a) t = 0( s ) (b) t = 12( s ) (c) t = 20( s ) (d) t = 40( s ) Fig. 12: Vehicles with planar fixed-wing aircraft dynamicscovering and following a non-zero acceleration triangulardomain moving over the path (cid:0)
30 cos (cid:0) π t (cid:1) ,
30 sin (cid:0) π t (cid:1)(cid:1) .Here, N = 6 , c r = 2 (m), s min = 0 . (m/s), s max = 5 (m/s), u θ max = π/ (rad/s), u s max = 3 (m/s ), a v = 1 . , l al = 3 . , C al = 1 . , a I = 5 , a h = 2 . , domain area A = 73 . (m )and r d = q AN = 3 . (m). The vehicles start in a linearformation, approach and cover the domain, while following it.The collision avoidance controller is included. B. Future work
Immediate future work includes parameter tuning to re-duce oscillations in the vehicles’ movement, studying three-dimensional coverage, investigating geometrical properties ofsteady states, investigating scenarios involving partial infor-mation, and implementing our approach on robotic platforms.R
EFERENCES[1] J. Cort´es, S. Mart´ınez, T. Karatas, and F. Bullo, “Coverage controlfor mobile sensing networks,”
IEEE Trans. Robotics and Automation ,vol. 20, pp. 243–255, 2004.[2] C. Gao, J. Cort´es, and F. Bullo, “Notes on averaging over acyclic di-graphs and discrete coverage control,”
Automatica , vol. 44, p. 21202127,2012.[3] N. Leonard and E. Fiorelli, “Virtual Leaders, Artificial Potentials andCoordinated Ccontrol of Groups,”
Proc. IEEE Conf. Decision andControl , 2001.[4] R. Sepulchre, D. Paley, and N. Leonard, “Stabilization of PlanarCollective Motion: All-to-All Communication,”
IEEE Trans. AutomaticControl , vol. 52, no. 5, pp. 1–14, 2007.[5] S.-J. Chung, A. Paranjape, P. Dames, S. Shen, and V. Kumar, “A surveyon aerial swarm robotics,”
IEEE Trans. Robotics , vol. 34, no. 4, pp. 837–855, 2018.[6] W. Ren and Y. Cao,
Distributed Coordination of Multi-agent Networks:Emergent Problems, Models, and Issues , vol. 83 of
Communications andControl Engineering . London: Springer-Verlag, 2011. [7] Y. Cao, W. Ren, and M. Egerstedt, “Distributed containment control withmultiple stationary or dynamic leaders in fixed and switching directednetworks,”
Automatica , vol. 48, p. 15861597, 2012.[8] M. Althoff and J. Dolan, “Set-based computation of vehicle behaviorsfor the online verification of autonomous vehicles,” in
Proc. IEEE Int.Conf. on Intelligent Transportation Systems , 2011.[9] G. Frehse, C. Le Guernic, A. Donz´e, S. Cotton, R. Ray, O. Lebeltel,R. Ripado, A. Girard, T. Dang, and O. Maler, “SpaceEx: ScalableVerification of Hybrid Systems,” in
Proc. Int. Conf. Computer AidedVerification , 2011.[10] X. Chen, E. ´Abrah´am, and S. Sankaranarayanan, “Flow*: An Analyzerfor Non-linear Hybrid Systems,” in
Proc. Int. Conf. Computer AidedVerification , 2013.[11] I. Yang, S. Becker-Weimann, M. Bissell, and C. Tomlin, “One-shotcomputation of reachable sets for differential games,” in
Proc. Int. Conf.Hybrid Systems: Computation and Control , 2013.[12] M. Chen and C. Tomlin, “HamiltonJacobi Reachability: Some RecentTheoretical Advances and Applications in Unmanned Airspace Manage-ment,”
Annual Review of Control, Robotics, and Autonomous Systems ,vol. 1, pp. 333–358, May 2018.[13] A. Gattami, A. Al Alam, K. Johansson, and C. Tomlin, “EstablishingSafety for Heavy Duty Vehicle Platooning: A Game Theoretical Ap-proach,”
IFAC Proceedings Volumes , vol. 44, pp. 3818–3823, Jan. 2011.[14] M. Chen, J. Shih, and C. Tomlin, “Multi-vehicle collision avoidance viaHamilton-Jacobi reachability and mixed integer programming,” in
Proc.IEEE Conf. Decision and Control , Dec. 2016.[15] K. Margellos and J. Lygeros, “Toward 4-D trajectory management in airtraffic control: A study based on monte carlo simulation and reachabilityanalysis,”
IEEE Trans. Control Systems Technology , vol. 21, pp. 1820–1833, Sep. 2013.[16] M. Chen, S. Bansal, J. Fisac, and C. Tomlin, “Robust SequentialTrajectory Planning Under Disturbances and Adversarial Intruder,”
IEEETrans. Control Systems Technology , pp. 1–17, Nov. 2018.[17] A. Akametalu, C. Tomlin, and M. Chen, “Reachability-Based ForcedLanding System,”
Journal of Guidance, Control, and Dynamics , vol. 41,pp. 2529–2542, Dec. 2018.[18] Z. Li and X. Xue, “Cucker–Smale flocking under rooted leadership withfixed and switching topologies,”
SIAM Journal on Applied Mathematics ,vol. 70, no. 8, pp. 3156–3174, 2010.[19] S.-Y. Ha, Z. Li, M. Slemrod, and X. Xue, “Flocking behavior of theCucker-Smale model under rooted leadership in a large coupling limit,”
Quarterly of Applied Mathematics , pp. 689–701, 2014.[20] S. Camazine, J.-L. Deneubourg, N. Franks, J. Sneyd, G. Theraulaz, andE. Bonabeau,
Self-organization in biological systems . Princeton Studiesin Complexity, Princeton, NJ: Princeton University Press, 2003. Reprintof the 2001 original.[21] T. Vicsek, A. Czir´ok, E. Ben-Jacob, I. Cohen, and O. Shochet, “Noveltype of phase transition in a system of self-driven particles,”
Phys. Rev.Lett. , vol. 75, no. 6, pp. 1226–1229, 1995.[22] I. Couzin, J. Krause, R. James, G. Ruxton, and N. Franks, “Collectivememory and spatial sorting in animal groups,”
J. Theor. Biol. , vol. 218,pp. 1–11, 2002.[23] M. D’Orsogna, Y.-L. Chuang, A. Bertozzi, and L. Chayes, “Self-propelled particles with soft-core interactions: patterns, stability, andcollapse,”
Phys. Rev. Lett. , vol. 96, no. 10, p. 104302, 2006.[24] F. Cucker and S. Smale, “Emergent behavior in flocks,”
IEEE Trans.Automat. Control , vol. 52, no. 5, pp. 852–862, 2007.[25] R. Fetecau and A. Guo, “A mathematical model for flight guidance inhoneybee swarms,”
Bull. Math. Biol. , vol. 74, no. 11, pp. 2600–2621,2012.[26] W. Liu, M. Short, Y. Taima, and A. Bertozzi, “Multi-scale collaborativesearching through swarming,”
Proc. Int. Conf. Informatics in Control,Automation, and Robotics , 2010.[27] I. Hussein and D. Stipanovi´c, “Effective coverage control for mobilesensor networks with guaranteed collision avoidance,”
IEEE Trans.Control Systems Technology , vol. 15, no. 4, pp. 642–657, 2007.[28] M. Bardi, “A boundary value problem for the minimum-time function,”
SIAM journal on control and optimization , vol. 27, no. 4, pp. 776–785,1989.[29] M. Delfour and J.-P. Zol´esio,
Shapes and geometries: analysis, differ-ential calculus, and optimization . Advances in Design and Control,Philadelphia, PA, USA: SIAM, 2001.[30] R. Olfati-Saber, “Flocking for Multi-Agent Dynamic Systems: Algo-rithms and Theory,”
IEEE Trans. Autom. Control , vol. 51, no. 3, pp. 401–421, 2006. [31] C. L´opez and J. Beasley, “A heuristic for the circle packing problemwith a variety of containers,” European Journal of Operational Research ,vol. 214, no. 3, pp. 512–525, 2011.[32] J. Chacon, M. Chen, and R. Fetecau, “Safe coverage of compact domainsfor second order dynamical systems,”
To appear in Proceedings of theIFAC World Congress, Berlin , 2020. preprint arXiv:1911.06519.[33] S.-Y. Ha and E. Tadmor, “From particle to kinetic and hydrodynamicdescriptions of flocking,”
Kinet. Relat. Models , vol. 1, no. 3, pp. 415–435, 2008.[34] S.-Y. Ha and J.-G. Liu, “A simple proof of the Cucker-Smale flock-ing dynamics and mean-field limit,”
Communications in MathematicalSciences , vol. 7, no. 2, pp. 297–325, 2009.[35] J. Chacon, “Safe coverage of compact domains for second order dynam-ical systems,” Master’s thesis, Simon Fraser University, 2020.[36] I. Mitchell, A. Bayen, and C. Tomlin, “A time-dependent Hamilton-Jacobi formulation of reachable sets for continuous dynamic games,”
IEEE Transactions on automatic control , vol. 50, no. 7, pp. 947–957,2005.
Juan Chacon is a recently graduated master’s stu-dent from the department of mathematics at SimonFraser University, Burnaby, BC, Canada, member ofthe Multi-Agent Robotic Systems Lab. He completedhis MSc and BASc in the Mathematics Departmentat Universidad Nacional de Colombia, Bogota in2013 and 2015 respectively. He received his BEng inElectronic Engineering from the Universidad Distri-tal Francisco Jose de Caldas in 2016. His researchinterests include multi-agent systems and aggrega-tion models.
Mo Chen is an Assistant Professor in the Schoolof Computing Science at Simon Fraser University,Burnaby, BC, Canada, where he directs the Multi-Agent Robotic Systems Lab. He completed his PhDin the Electrical Engineering and Computer SciencesDepartment at the University of California, Berkeleyin 2017, and received his BASc in EngineeringPhysics from the University of British Columbiain 2011. From 2017 to 2018, Mo was a postdoc-toral researcher in the Aeronautics and AstronauticsDepartment in Stanford University. His researchinterests include multi-agent systems, safety-critical systems, and practicalrobotics.