Crowd Control of Nonlinear Systems

FREDRIK GR ¨ ONBERG

Master’s Degree Project Stockholm, Sweden October 2013

XR-EE-RT 2013:026

We study a multi-agent system in R

varying speed and control inputs corresponding to acceleration and angular velocity.

The system has a dynamic communication topology based on proximity. We propose a novel decentralized control algorithm derived from a double integrator model using a pairwise potential function. By using an energy function we show that a leaderless system converges to a set where connected agents have equal direction and velocity and potential contributions to the control action cancel each other out. The concept of formation density is defined and studied by numerical simulation. We find a relation between parameters of the controller and the system that makes the system converge to a formation with low density, corresponding to agents being at appropriate distances from each other, also when agents are not restricted to communicating only with their closest neighbors. The algorithm is tested for a system with leaders and properties of this system are investigated numerically. The results confirm that the proportion of leaders needed to guide a certain proportion of the agent in average is nonlinear and decreasing with respect to the number of agents.

Vi studerar ett multiagentsystem i R

abel fart och styrsignaler som motsvarar acceleration och vinkelhastighet. Systemet har ett dynamiskt kommunikationsnätverk baserat på agenternas avstånd från varandra.

Vi presenterar en ny styrlag härledd från en vektormodell som använder en potenti- alfunktion för par av agenter. Genom att använda en energifunktion visar vi att ett system utan ledare konvergerar till en mängd där sammankopplade agenter har samma hastighet och riktning och där bidrag från potentialfunktionen till styrsignalerna tar ut varandra. Konceptet formationsdensitet definieras och studeras numeriskt. Vi fin- ner ett förhållande mellan parametrar i styrlagen och systemet som gör att ett system konvergerar till en formation med låg densitet, vilket motsvarar att agenter har ett lämpligt avstånd till varandra, även när agenter inte begränsas till att kommunicera enbart med sina närmaste grannar. Styrlagen prövas för ett system med ledare och systemets egenskaper undersöks numeriskt. Resultaten bekräftar att den proportion av ledare som behövs för att leda en viss proportion av agenterna i genomsnitt är icke-linjär och avtagande med avseende på antalet agenter.

1 Introduction 4

1.1 Background . . . 4

1.1.1 Problem Formulation and Outline . . . 5

1.2 Definitions . . . 6

1.2.1 States and Communication . . . 6

1.2.2 Dynamics . . . 8

1.2.3 Flock Concepts . . . 9

1.2.4 Pairwise Potential . . . 10

1.2.5 Collective Potential Function . . . 11

2 Controllers 14 2.1 A Double Integrator Approach . . . 14

2.1.1 A Double Integrator Controller . . . 14

2.1.2 A Double Integrator Controller with a Common Objective . . . 15

2.2 Transformation to A Unicycle Controller . . . 15

2.2.1 Validation of The Unicycle Controllers . . . 20

2.2.2 A Unicycle Controller with a Common Objective . . . 24

2.3 Stability Analysis . . . 25

2.3.1 Collective Dynamics and Stability of the Double Integrator Systems 25 2.3.2 Stability of the Unicycle Model Controllers . . . 28

3 Density Analysis 34 3.1 A Simple Pairwise Potential . . . 35

3.2 Density Functions . . . 36

3.2.1 Aggregation of Solutions . . . 37

3.2.2 Kernel Density Estimation . . . 39

3.2.3 Packing Density . . . 41

3.2.4 Validation of Density Functions . . . 42

3.3 Simulations . . . 43

3.3.1 Initial Conditions . . . 45

3.4 Results . . . 47

3.4.1 Initial Condition Sensitivity . . . 47

3.4.2 Low Density Formations . . . 47

3.5 Discussion . . . 48

4 Leadership Analysis 50

4.1 A System With Leaders . . . 50

4.2 Measures of Convergence and Accuracy . . . 51

4.3 Simulations . . . 54

4.3.1 Initial Conditions and Parameters . . . 55

4.4 Results . . . 55

4.4.1 Characteristic Time of Alignment . . . 55

4.4.2 Probability of Fragmentation . . . 56

4.4.3 Relationship between pl and N . . . 56

4.5 Discussion . . . 57

5 Conclusion 59 A Apppendix A: Density Analysis Plots 60 A.1 Density Plots for N = 50 and (ρ, r, R) = (3, 3, 5) . . . 60

A.1.1 Density Plots for a_max= 1 . . . 60

A.1.2 Density Plots for a_max= 2 . . . 63

A.2 Density Plots for N = 50 and (ρ, r, R) = (3, 4, 5) . . . 65

A.2.1 Density Plots for a_max= 1 . . . 65

A.2.2 Density Plots for a_max= 2 . . . 68

A.3 Density Plots for N = 50 and (ρ, r, R) = (3, 6, 10) . . . 70

A.3.1 Density Plots for a_max= 1 . . . 70

B Appendix B: Leadership Analysis Plots 74 B.1 Alignment Plots . . . 74

B.2 Fragmentation Plots . . . 75

B.3 Guidance Plots . . . 76

Introduction

1.1 Background

A group of individuals, each of them known as an agent or a particle, under the assumption that they can somehow interact or communicate can be modeled as a switched system

˙x(t) = f_σ(t)(t, x)

where σ(t) is a switching sequence that corresponds to the communication topology.

The problem has been studied in many different settings regarding agent dynamics, communication topologies and control sets. An important question is under what conditions such a system reaches consensus, or state agreement, which is when the system converges a point or a subspace of the state space, corresponding to all state variables or some state variables being constant and equal for each agent.

Early studies of flock behavior were done by Viczek et al in [5], [6], [14], and Reynolds in [11]. These were in the setting of particles and human or animal flocks. Larger reviews of theory have been done by Murray et al in [3], [9] and Olfati-Saber in [10]

in the setting of self organizing networks and vehicle formations. Applications include control of multiple robots or vehicles in scenarios where the task is beyond the scope of what can be achieved with a single vehicle, such as search and rescue operations.

Other applications are e.g. control of satellite clusters and vehicle formations.

Two important features distinguish the study of of flocks from pure consensus prob- lems: the concepts of cohesion and collision avoidance. When implemented as a control system these correspond to attractive and repulsive control action between agents. The presence of repulsive control action implies that consensus regarding position (conver- gence to a point in space) is not a suitable objective for flocks. A flock might instead converge to a formation. Consensus regarding direction is the third essential feature of a flock, commonly referred to as alignment, flocking is therefore still a consensus problem.

1.1.1 Problem Formulation and Outline

In [1] the effects of leadership in a flock of agents was studied for agents with a dynamic communication topology and a discrete time unicycle model with constant speed. It was found that the relationship between the number of leaders that are necessary to guide a flock with a certain accuracy and the number of agents in the flock was nonlinear, such that larger flocks need a smaller proportion of leaders to be guided accurately (to reach consensus) in average. The question of how many leaders that are needed for consensus was further studied in [8] for a similar discrete time unicycle model with a constant speed and a dynamic communication topology.

The consensus problem of position and direction for a system of agents using a con- tinuous time unicycle model with time varying speed and a dynamic communication topology was studied in [2]. Consensus and flocking for a similar agent model but a static communication topology was studied in [7]. Algorithms that result in flocking for a system of agents using a double integrator model and a dynamic communication topology was studied in [10].

The aim of this thesis has been threefold; first to develop a decentralized controller for a system of agents using a continuous time unicycle model with time varying speed for a dynamic communication topology that results in flocking; second to study properties of the resulting agent formations; third to study the effects of leadership and the number of leaders needed to reach consensus. This work thus investigates similar questions as in [1] and [8] but for an extended model. It also relates to the work done in [2], [7] and [10] but for differences in objective, communication topology and agent model respectively.

This report is structured as follows. Next in Chapter 1 follows definitions and notation that will be used in the rest of the report. The pairwise potential function and collec- tive potential function are defined and some of their general properties are derived in Section 1.2.4 and 1.2.5. Chapter 2 presents the derivation of multiple control laws and a theoretical analysis of their behavior. In particular we show that the closed loop system with unicycle dynamics and time varying velocity converges to a set where connected agents have equal velocity for a controller with no objective velocity. In Chapter 3 we define the concept of formation density and study the density of formations in steady state given different initial conditions. Chapter 4 introduces leaders to the system and studies the proportion of leaders needed to achieve a certain objective and its relation to the number of agents. Also, the probability of fragmentation is studied. Chapter 5 concludes and discusses further topics.

Good read!

1.2 Definitions

1.2.1 States and Communication

LetV = {1, . . . , N} denote a set of N agents or particles in R². The position of agent i is denoted q_i ∈ R² and its velocity by p_i ∈ R².

Definition 1.2.1. The collective position vector is defined as q = [q₁, . . . , q_N]^T ∈ R^2N.

Definition 1.2.2. The collective velocity vector is defined as p = [p1, . . . , pN]^T ∈ R^2N.

Definition 1.2.3. Let q_i := (x_i, y_i)^T and p_i := (v_icos θ_i, v_isin θ_i)^T, i∈ V, where vi ∈ R denotes an agent’s speed and θi ∈ [−π, π) its direction. In a unicycle model the direction is always defined. We define

x = [x₁, . . . , x_N]^T ∈ R^N, y = [y₁, . . . , y_N]^T ∈ R^N, v = [v1, . . . , vN]^T ∈ R^N, θ = [θ1, . . . , θN]^T ∈ [−π, π)^N

(1.1)

We will occasionally abuse the notation by not referring to (1.1) when using the variable x, the intended usage will be clear from the context or explicitly stated.

Definition 1.2.4. The unit tangent vector and unit normal vector of agent i are always defined in a unicycle model. We denote them

ˆt_i = (cos θ_i, sin θ_i)^T, i∈ V ˆ

n_i = (− sin θⁱ, cos θ_i)^T, i∈ V

We assume that agents can see or communicate with each other according to a prox- imity criterion, which is by definition time varying.

Definition 1.2.5. The neighbors of agent i is the set

Nⁱ(q) ={j ∈ V \ {i} : kqⁱ− q^jk < R}, (1.2) where R > 0 is the sensing radius and k · k denotes the Euclidian norm.

Definition 1.2.6. The communication topology induces a communication graph G(q) with verticesV and the set of edges

E(q) = {(i, j) ∈ V × V : j ∈ Nⁱ(q)}.

By the definition of the proximity criterion (1.2), communication is symmetric,

(i, j)∈ E(q) ⇔ (j, i) ∈ E(q). (1.3) The number edges or communication links is denoted |E(q)|. We define G(q) as a directed graph with edges going back and forth between connected vertices. When we want to count single edges between vertices we use the condition i < j. The following relations between enumerations of E(q) will be useful. Let a^ij denote a function of the states of any two agents, then

X

i∈V

X

j∈Ni(q)

a_ij =X

(i,j)∈E(q)

a_ij =X

(i,j)∈E(q) i<j

a_ij + a_ji

Definition 1.2.7. If G(q) has K connected components G^k(q) ⊂ G(q) with the corre- sponding subsets Vk⊂ V, k = 1, . . . , K, we denote these subsets by

comp(V) = {V¹, . . . ,V^k}.

It holds that Vⁱ ∩ V^j = ∅ for Vⁱ,V^j ∈ comp(V) and that S comp(V) = V. Moreover it holds that @(i, j)∈ E(q) such that i ∈ Vk ∈ comp(V) and j ∈ Vl ∈ comp(V) for k 6= l.

Definition 1.2.8. IfG(q) is initially connected but loses connectivity for some time t, we say that the set of agents V undergoes fragmentation.

Remark. When G(q) is not connected we would like to compute comp(V). A method for doing is Tarjan’s Algorithm [13] and has been implemented in MATLAB as graph- conncomp.

Definition 1.2.9. The adjacency matrix A(q) ∈ N^{N ×N} of G(q) is a matrix represen- tation of E(q)

[A(q)]_ij =

(1, (i, j)∈ E(q) 0, otherwise

By (1.3), the adjacency matrix is symmetric, A = A^T, and by (1.2) its diagonal elements are equal to zero, since an agent is not a neighbor to itself.

Definition 1.2.10. The degree matrix ∆(q) ∈ N^{N ×N} of G(q) is the diagonal matrix

∆(q) = diag(N¹(q), . . . ,N^N(q)).

Definition 1.2.11. The Laplacian L(q)∈ N^{N ×N} of G(q) is the matrix defined by L(q) = ∆(q)− A(q).

The Laplacian has many important properties, among them that i) by (1.3), L(q) is symmetric.

ii) L(q) is positive semi definite with at least one eigenvalue equal to zero, corre- sponding to the eigenvector

1_N = (1, . . . , 1)^T ∈ R^N

iii) if G(q) is not connected, but has K connected components G^k(q) ⊂ G(q) with the corresponding subsets Vk ∈ comp(V), k = 1, . . . , K, then L(q) has an eigen- value equal to zero for each connected component G^k(q) with the corresponding eigenvector

[v_k]_i =1Vk(i), i = 1, . . . , N (1.4) where 1V_k is the indicator function of the setVk.

iv) the number of connected components ofG(q) is equal to

N − rank L(q) (1.5)

v) L(q) has the sum of squares property x^TL(q)x =X

(i,j)∈E(q) i<j

(x_i− x^j)². (1.6)

vi) For a stack of N n-vectors, x = [x₁, . . . , x_N]^T ∈ R^nN, where x_i ∈ Rⁿ for i ∈ V, L(q) has the sum of squared norms property

x^T(L(q)⊗ Iⁿ)x = X

(i,j)∈E(q) i<j

kxⁱ− x^jk², (1.7)

where ⊗ denotes the Kronecker product.

These results are well known and proofs may be found in [4].

1.2.2 Dynamics

Definition 1.2.12. Let qi = (xi, yi)^T ∈ R² and pi = (vicos θi, visin θi)^T ∈ R² as stated above. The standard unicycle model is given by the following kinematic equations

˙x_i = v_icos θ_i

˙

y_i = v_isin θ_i θ˙_i = ω_i

(1.8)

and so has three states xi, yi and θi (position and direction) and two control inputs vi

and ω_i (speed and angular velocity).

Definition 1.2.13. The dynamic extension of (1.8) with respect to v_i is given by

˙x_i = v_icos θ_i

˙

y_i = v_isin θ_i

˙v_i = a_i θ˙_i = ω_i

(1.9)

and so has four states xi, yi, vi and θi (position, speed and direction), and two control inputs a_i and ω_i (acceleration and angular velocity). This extension implies that v_i(·) will be Lipschitz continuous.

Remark. There exists no physical system that generates force according to the kine- matic equations of the extended unicycle model. This is because of the sideslip angle being omitted.

We use (1.9) for agent dynamics because of the intuitiveness of continuous speed as well as the need of taking the derivative of v_i in the coming derivation of control laws.

1.2.3 Flock Concepts

By Reynolds, [11], flocking involves three principles, cohesion, alignment and collision avoidance.

Definition 1.2.14. For any subset of agents V⁰ ⊂ V let qc⁰ and p⁰_c denote the centroid and the mean velocity of the agents in V⁰,

q_c⁰ = 1 N

X

i∈V⁰

qi,

p⁰_c= 1 N

X

i∈V⁰

p_i.

Definition 1.2.15. A subset of agents V⁰ ⊂ V is cohesive if there is a fixed radius r and a time t⁰ such that

kqⁱ(t)− qc⁰(t)k < r, ∀ i ∈ V⁰, ∀ t ≥ t⁰.

In other words, V⁰ is cohesive if the corresponding subgraph ofG(q) is connected for all times greater than t⁰ and V is cohesive if G(q) is connected for all times greater than t⁰.

Definition 1.2.16. A subset of agents V⁰ ⊂ V displays alignment if

t→∞lim kvⁱ(t)− v^j(t)k = 0, ∀ i, j ∈ V⁰ and

t→∞lim kθⁱ(t)− θ^j(t)k = 0, ∀ i, j ∈ V⁰.

IfV is cohesive this is equivalent to

t→∞lim p_i(t) = lim

t→∞p_c(t), ∀i ∈ V.

Definition 1.2.17. We say that a set of agentsV displays collision avoidance if there is no time t such that

∃ (i, j) ∈ E(q(t)) : qⁱ(t) = q_j(t), i.e. no collisions between agents.

We see that cohesion and collision avoidance both have to do with agents staying at appropriate distances from one another; agents should repel if they are too close together and attract if they are too far apart. We introduce the two following quantities to describe these characteristics.

Definition 1.2.18. The repulsion radius ρ is the distance within two agents repel.

The attraction radius r is the distance outside two agents attract if they can see each other. We require

0 < ρ ≤ r < R,

where R is the sensing radius. If ρ < r there is not a unique distance at which two agents neither repel nor attract.

We go on to define a pairwise potential to describe this behavior of repulsion and attraction.

1.2.4 Pairwise Potential

We introduce the following shorthand notation.

Definition 1.2.19. The vector going from q_j to q_i is denoted q_ij and its norm, the distance between agent i and agent j, is denoted β_ij,

q_ij ,= qi− q^j, β_ij ,kq^j − qⁱk.

We go on to define the following functions.

Definition 1.2.20. Consider aC⁰ function f : R→ R satisfying f (x) < 0, x∈ [0, ρ)

f (x) = 0, x∈ [ρ, r]

f (x) > 0, x∈ (r, R) f (x) = 0, x∈ [R, ∞)

(1.10)

where ρ, r and R are as in Definition 1.2.18. Consider its primitive function γ(x) =

Z x ρ

f (u)du. (1.11)

We define the pairwise potential function V_ij(q) as

V_ij(q) = γ(β_ij), i, j ∈ V. (1.12) It follows that the following properties hold for V_ij(q),

i) Vij(q)∈ C¹, (1.13)

ii) V_ij(q)≥ 0, with equality iff ρ ≤ β^ij ≤ r, (1.14)

iii) Vij(q)≤ max(γ(0), γ(R)), (1.15)

iv) V_ij(q) = γ(R) if j /∈ Nⁱ(q). (1.16)

Definition 1.2.21. We define the following shorthand notaion, ρ_ij , f (β_ij)

β_ij . Since β_ij is symmetric in i and j, so is ρ_ij.

Proposition 1.2.1. It holds that

∂Vij(q)

∂q_i = ρ_ijq_ij

∂V_ij(q)

∂q_j =−ρ^ijq_ij Proof.

∂V_ij(q)

∂qi

= ∂

∂qi

γ(β_ij) = f (β_ij) ∂

∂qi

β_ij = f (β_ij) ∂

∂qi

q

(q_i− q^j)^T(q_i− q^j)

= f (βij) 1 2β_ij

∂

∂q_i(qi· qⁱ− 2qⁱ· q^j + qj · q^j) = f (βij)

β_ij (qi− q^j) = ρijqij. The second identity is proven similarly.

1.2.5 Collective Potential Function

Definition 1.2.22. The graph induced potential function VG(q) is defined as VG(q) = 1

2 X

i∈V

X

j∈Ni(q)

Vij(q).

Definition 1.2.23. The collective potential function V (q) is defined as V (q) = 1

2 X

i∈V

X

j6=i

V_ij(q).

Proposition 1.2.2. It holds that V (q) = VG(q) + 1

2(N (N− 1) − |E(q)|)γ(R) Proof. We have

1 2

X

i∈V

X

j6=i

V_ij(q) = 1 2

X

i∈V

X

j∈Ni(q)

V_ij(q) +1 2

X

i∈V

X

j /∈N_i

V_ij(q).

The number of terms on the left hand side is N (N − 1), which is also the number of edges in a complete graph with N vertices. The first sum on the right hand side has

|E(q)| terms since it sums over all edges in G(q), also, this sum is equal to V^G(q). The second sum must therefore have N (N − 1) − |E(q)| terms that by (iv)) are equal to γ(R). Hence the proposition holds.

Definition 1.2.24. By (1.13), (1.14) and (1.15), Vij(q) is C¹ and bounded from below and above for every i, j ∈ V. Since V (q) is a sum of these function that does not depend on the time discontinuous setsNi(q) and E(q) therefore so is V (q). Since V (q) is continuous it attains a minimum for some q in any closed set of R²ⁿ. A suitable choice of this set is the largest closed set in which the graph G(q) can be connected,

ΩG ={q : qⁱ − q^j ≤ (N − 1)R ∀ i, j ∈ V}

Let min_qV (q) for q∈ Ω^G be denoted V_min. It follows from (1.14) that V_min ≥ 0,

whether it attains the value of zero or not depends on the communication parameters ρ, r and R.

Proposition 1.2.3. It holds that i) _∂q^∂

iV (q) =P

j∈Niρ_ijq_ij ii) ∇qV (q) = (P (q)⊗ I2)q,

where [P (q)]_ij =





 X

k6=i

ρik, i = j

− ρ^ij i6= j Proof. We have that

∂

∂qi

V (q) = ∂

∂qi

1 2

X

k∈V

X

j6=k

V_kj(q) = 1 2

X

k∈V

X

j6=k

∂V_kj(q)

∂qi

.

We get contributions from the summation when k = i or j = i and k6= i, thus

∂

∂qi

V (q) = 1 2

X

j6=i

∂V_ij(q)

∂qi

+X

k6=i

∂V_ki(q)

∂qi

!

By changing the summation index in the second sum to j and using Proposition 1.2.1 we get

∂

∂q_iV (q) = 1 2

X

j6=i

ρ_ijq_ij − ρ^jiq_ji = 1 2

X

j6=i

(ρ_ij+ ρ_ji)q_ij =X

j6=i

ρ_ijq_ij =X

j∈Ni(q)

ρ_ijq_ij,

where we finally use that ρ_ij = ρ_ji, that q_ji =−qij and that ρ_ji = 0 if j /∈ Ni(q). Thus part i) holds.

For part ii) we have by part i)

∇^qV (q) =







∂V

∂q1

...

∂V

∂q_N





(q) =





 P

j6=1ρ_1jq_1j ... P

j6=Nρ_{N j}q_{N j}





=







P

j6=1ρ_1jq₁− ρ¹²q₂− · · · − ρ^1Nq_N ...

−ρN 1q₁− · · · − ρN,N −1q_{N −1}+P

j6=Nρ_{N j}q_N





 Writing the right hand side vector as a matrix product with q we get

∇^qV (q) = (P (q)⊗ I²)q, where P (q) is as proposed.

Remark. Note the similarity in structure between the graph Laplacian L(q) and P (q), it follows that P (q) is also positive semi definite and has the eigenvector v that satisfies

v_i = v_j, (i, j)∈ E(q) corresponding to the zero eigenvalue.

Corollary 1.2.1. It follows that

V (q) =˙ ∇^qV (q) ˙q = ( ˆP (q)q)^Tq =˙ X

i∈V

X

j6=i

ρ_ijq_ij · ˙qⁱ (1.17)

We are now ready to study a control algorithm for (1.9)

Controllers

We want to design a decentralized controller that makes a set of agents display flocking.

The main idea behind the derivation is that the competing objectives of alignment, cohesion and collision avoidance are easily incorporated in a vector controller by the means of vector addition. We then use a suitable transformation to get a controller for the extended unicycle model (1.9).

2.1 A Double Integrator Approach

2.1.1 A Double Integrator Controller

Proposition 2.1.1. Consider a double integrator model

˙ q_i = p_i,

˙

p_i = u_i, (2.1)

where i∈ V. We propose that the following vector controller incorporates the objectives of alignment, cohesion and collision avoidance. We call the first sum the alignment contribution and the second sum the potential contribution.

u_i =−X

j∈Ni(q)

(p_i− p^j)−X

j∈Ni(q)

∂

∂q_iV_ij(q) (2.2)

We start with alignment, disregard the second sum of (2.2) and assume

˙

p_i =−X

j∈Ni(q)

(p_i− pj),

Globally this becomes for the collective velocity vector

˙

p =−ˆL(q)p, (2.3)

where ˆL(q) = L(q)⊗ I² and L(q) is the graph Laplacian. Since L(q) is positive semi definite, it follows that −ˆL(q) is negative semi definite. By (1.4) and (1.5) the kernel of L(q) is spanned by the set of vectors

{1^V1(i), . . . ,1V_K(i)}, i ∈ V,

where eachV^kcorresponds to a connected component ofG(q). Thus for any equilibrium of (2.3) the agents in each connected component of G(q) have equal velocity and are thus aligned. Now disregard the first sum of (2.2) and assume

˙

p_i =− X

j∈Ni(q)

∂

∂q_iV_ij(q).

By Proposition 1.2.1 we have

˙

pi =−X

j∈Ni(q)

ρijqij =X

j∈Ni(q)

f (βij)ˆqji.

The vector ˆq_ji is the unit vector that points from agent i to agent j. If agents i and j are further apart than the attraction radius r then f (β_ij) > 0 and the control action will point in the direction towards agent j. For agent j there will an equal contribution in the control action toward agent i, and they will tend to attract. If they are closer to each other than the repulsion radius ρ then f (β_ij) < 0 and the control action will point in the direction away from agent j. For agent j there will an equal contribution in the control action away from agent i, and they will tend to repel. Thus the controller (2.2) incorporates the objectives of cohesion and collision avoidance.

2.1.2 A Double Integrator Controller with a Common Objec- tive

Of interest is also the following controller, a modification of (2.2) that adds a term with a common objective velocity pd to each agent.

u_i =−X

j∈Ni(q)

(p_i− pj)−X

j∈Ni(q)

∂

∂q_iV_ij(q)− c(pi− pd), (2.4) where c > 0 is a constant. Clearly the control action from this term is convergence towards the common objective velocity pd.

2.2 Transformation to A Unicycle Controller

By Definition 1.2.3 and 1.2.4 we have

pi = vitˆi, i∈ V. (2.5)

However, it should be noted that there are two solutions (v_i, θ_i) for any velocity p_i, namely

v_itˆ_i(θ_i) =−viˆt_i(θ_i+ π). (2.6) Taking the derivative of ˆt_i with respect to θ_i yields

d

dθ_iˆt_i(θ_i) = ˆn_i(θ_i) (2.7) Thus taking the time derivative of (2.5) we get by using (2.7) that

˙

p_i = ˙v_itˆ_i+ v_i˙ˆt= ˙v_iˆt_i+ v_iθ˙_inˆ_i. Since ˆti and ˆni are orthonormal we obtain

˙

pi· ˆtⁱ = ˙vi,

˙

p_i· ˆni = v_iθ˙_i, (2.8)

where· denotes the scalar product.

We are now ready to propose a transformation from a vector control law to a control law for the extended unicycle model.

Definition 2.2.1. Let u ∈ R², v ∈ R and θ ∈ [−π, π). Moreover let t : θ 7→

(cos θ, sin θ)^T and n : θ7→ (− sin θ, cos θ)^T. We define the map ϕ : R²×R×[−π, π) → R² as

ϕ(u, v, θ) = (u· t(θ),1

vu· n(θ))^T,

where· denotes the scalar product. We also define the map ϕ for an  > 0 as ϕ_(u, v, θ) = (u· t(θ),sgn(v)

|v| + u· n(θ))^T,

where· denotes the scalar product and sgn(·) denotes the sign function.

Consider a vector control law ui for the double integrator model (2.1), i.e. ˙pi = ui and the dynamics of the extended unicycle model (1.9). From (2.8) we have

 ˙vi

θ˙_i



= ϕ( ˙p_i, v_i, θ_i), i∈ V in accordance we propose the following control law for (1.9).

 a_i ω_i



= ϕ_(u_i, v_i, θ_i), i∈ V. (2.9) Note that we use the map ϕ_ instead of ϕ because it is not singular for v equal to zero as is the case for ϕ.

Definition 2.2.2. Let φ_ij ∈ [−π, π) denote the angle between the direction of agent i, θ_i, and the position of agent j relative to agent i, q_j− qⁱ. This angle is known as the bearing angle and denotes the polar coordinate angle of agent j in the local frame of agent i. In the global frame this implies

qj = qi+ βij(cos(θi+ φij), sin(θi+ φij))^T, which in turn implies

ˆ

q_ji = (cos(θ_i+ φ_ij), sin(θ_i+ φ_ij))^T, (2.10) where the hat denotes unit length.

Proposition 2.2.1. Let θ_ij = θ_i− θ^j and let φ_ij be as defined above. By applying ϕ to the vector controller (2.2) we get the following control laws for the extended unicycle model (1.9)

a_i =−X

j∈Ni(q)

(v_i− v^jcos θ_ij) +X

j∈Ni(q)

f (β_ij) cos φ_ij

ω_i =−X

j∈Ni(q)

v_j

v_i sin θ_ij + 1 v_i

X

j∈Ni(q)

f (β_ij) sin φ_ij

(2.11)

Proof. We have according to the proposed transformation that

a_i =



−X

j∈Ni(q)

(p_i − pj)−X

j∈Ni(q)

∂

∂q_iV_ij(q)



· ˆti.

By Proposition 1.2.1 and Equation (2.5) this becomes

ai =



−X

j∈Ni(q)

(viˆti− v^jtˆj)−X

j∈Ni(q)

ρijqij



· ˆtⁱ. (2.12)

Recall that

ρ_ij = f (βij) β_ij , hence

− ρ^ijq_ij = f (β_ij)ˆq_ji, (2.13) where the hat denotes unit length. Using trigonometric identities we obtain the fol- lowing relations

ˆti· ˆtⁱ = cos²θi+ sin²θi = 1,

ˆt_j · ˆti = cos θ_jcos θ_i+ sin θ_jsin θ_i = cos(θ_i− θj) = cos θ_ij, ˆ

q_ji· ˆtⁱ = cos(θ_i+ φ_ij) cos θ_i+ sin(θ_i+ φ_ij) sin θ_i = cos(θ_i+ φ_ij − θⁱ) = cos φ_ij,

(2.14)

where the last relation follows from Equation (2.10). By expanding (2.12) and using (2.13) and (2.14) we get

a_i =−X

j∈Ni(q)

(v_i− vjcos θ_ij) +X

j∈Ni(q)

f (β_ij) cos φ_ij

which proves the first part of the proposition. Similarly we have

ω_i = 1 v_i



−X

j∈Ni(q)

(p_i− pj)−X

j∈Ni(q)

∂

∂q_iV_ij(q)



· ˆni

which by Proposition 1.2.1 and Equation (2.5) becomes

ω_i = 1 vi



−X

j∈Ni(q)

(v_iˆt_i− v^jtˆ_j)−X

j∈Ni(q)

ρ_ijq_ij



· ˆnⁱ. (2.15) Using trigonometric identities we obtain

tˆi· ˆnⁱ =− cos θⁱsin θi+ sin θicos θi = 0

ˆt_j · ˆni =− cos θjsin θ_i+ sin θ_jcos θ_i = sin(θ_j− θi) = − sin θij, ˆ

q_ji· ˆnⁱ =− cos(θⁱ+ φ_ij) sin θ_i+ sin(θ_i+ φ_ij) cos θ_i = sin(θ_i+ φ_ij − θⁱ) = sin φ_ij. (2.16) By expanding (2.15) and using (2.13) and (2.16) we get

ω_i =−X

j∈Ni(q)

v_j

v_i sin θ_ij + 1 v_i

X

j∈Ni(q)

f (β_ij) sin φ_ij

which proves the second part of the proposition.

Proposition 2.2.2. Let θ_ij = θ_i− θ^j and let φ_ij be as defined above. By applying ϕ_ to the vector controller (2.2) we get the following control laws for the extended unicycle model (1.9)

a_i =−X

j∈Ni(q)

(v_i− v^jcos θ_ij) +X

j∈Ni(q)

f (β_ij) cos φ_ij

ω_i =−X

j∈Ni(q)

v_jsgn(v_i)

|vⁱ| +  sin θ_ij+ sgn(v_i)

|vⁱ| +  X

j∈Ni(q)

f (β_ij) sin φ_ij

(2.17)

Proof. The proof is the same as for Proposition 2.2.1, using the map ϕ_ instead of ϕ.

Finally, there is a subtlety caused by the relation given in (2.6); the velocity of an agent moving backwards has the same velocity as an agent moving forwards in the opposite

direction. If both of these situations are seen as equal (as in equally attractive, or stable) by the angular velocity controller it would be in conflict with the objective of alignment. The remedy to this issue is to take the absolute values of the speeds in the alignment contribution, i.e.

v_j

v_i 7→ |vj|

|vⁱ|.

Similarly for the non-singular controller we propose the following modification, which also has the benefit that the alignment contribution does not vanish for velocities equal to zero,

v_jsgn(v_i)

|vⁱ| +  7→ |vj| + 

|vⁱ| + .

Applying these modifications to the control laws (2.11) and (2.17), they become a_i =−X

j∈Ni(q)

(v_i− v^jcos θ_ij) +X

j∈Ni(q)

f (β_ij) cos φ_ij

ω_i =−X

j∈Ni(q)

|v^j|

|vⁱ| sin θ_ij+ 1 vi

X

j∈Ni(q)

f (β_ij) sin φ_ij

(2.18)

and

a_i =−X

j∈Ni(q)

(v_i− vjcos θ_ij) +X

j∈Ni(q)

f (β_ij) cos φ_ij

ω_i =−X

j∈Ni(q)

|vj| + 

|vⁱ| +  sin θ_ij + sgn(v_i)

|vⁱ| +  X

j∈Ni(q)

f (β_ij) sin φ_ij

(2.19)

Proposition 2.2.3. The controller (2.19) is decentralized provided that the quantities v_i, β_ij, ˙β_ij, φ_ij and ˙φ_ij are available to each agent i ∈ V for corresponding neighbors j ∈ Ni(q) by measurement or estimation.

Proof. In the local frame of agent i, the position of agent j is q_j = β_ij(cos φ_ij, sin φ_ij)^T

and taking its derivative yields

˙

q_j = ˙β_ij(cos φ_ij, sin φ_ij)^T + β_ijφ˙_ij(− sin φij, cos φ_ij)^T.

We can also express ˙q_j in the local frame of agent i by subtracting ˙q_i and rotating the frame by θ_i, which yields

˙

q_j = (v_jcos(θ_j − θⁱ)− vⁱ, v_jsin(θ_j− θⁱ))^T. Thus

v_jcos(θ_j− θi) = v_i + ˙β_ijcos φ_ij − βijφ˙_ijsin φ_ij, v_jsin(θ_j− θⁱ) = ˙β_ijsin φ_ij + β_ijφ˙_ijcos φ_ij.

The quantities on the right hand side are assumed to be known and it is clear that the controller (2.19) may be computed using the quantities on the left hand side along with vi, βij and φij, which were assumed to be known. Thus the controller (2.19) is decentralized.

Remark. The quantities v_i, β_ij and φ_ij correspond to speed, inter agent distance and bearing angle. In applications it is reasonable and essential to assume that these quantities can be measured if any kind of flocking algorithm is to be implemented.

The corresponding derivatives would have to be estimated by an observer and would be sensitive to measurement noise.

Next we do a validation of these controllers using simple examples to argue.

2.2.1 Validation of The Unicycle Controllers

We do a simple analysis of the derived controller (2.18) to check that it is reasonable.

Note that the direction of an agent may be equal or opposite to the direction it travels in. The alignment contribution of the acceleration control a_i consists of the sum

−X

j∈Ni(q)

(vi− v^jcos θij).

Geometrically each term of the sum can be interpreted as the difference between the velocity of agent i and the projection of the velocity of agent j along the direction of agent i. The control action tries to reduce these differences to zero. To illustrate, take the case of two agents , i and j, having the same direction, implying that θij = 0. The alignment contribution then becomes

−(vⁱ− v^j), agent i

−(v^j − vⁱ), agent j.

Thus if v_i < v_j we have that ˙v_i > 0 and ˙v_j < 0 and the opposite if v_j < v_i. Hence the control action tries to achieve equal speeds (v_i = v_j) if the agents have the the same direction.

If they have the opposite direction, θ_ij = π and the alignment contribution becomes

−(vⁱ + v_j), agent i

−(v^j + v_i), agent j.

Thus if v_i <−v^j we have that ˙v_i > 0 and ˙v_j > 0 and the opposite if v_i >−v^j. Hence the control action tries to achieve opposite speeds (v_i =−vj) if the agents have opposite direction. Both cases imply that the agents try to travel in the same direction, which is reasonable.

The potential contribution of the acceleration control ai consists of the sum X

j∈Ni(q)

f (β_ij) cos φ_ij

The term f (β_ij) is negative if agents i and j are too close and positive if they are too far apart. The term cos φ_ij is positive if agent j lies in the half circle in front of agent i, (|φ^ij| < π/2), and negative if agent j lies in the half circle behind agent i, (|φ^ij| > π/2).

Hence the control action speeds up to catch an agent too far ahead or get away from an agent too close behind and slows down to catch an agent too far behind or get away from an agent too close ahead, which is reasonable.

The alignment contribution of the angular velocity control ω_i consists of the sum

−X

j∈Ni(q)

|v^j|

|vi|sin θ_ij.

Recall that θ_i ∈ [−π, π), i ∈ V, thus θⁱ− θ^j ∈ [−2π, 2π). For non zero speeds vⁱ and v_j the control action has the same sign as sin θ_ij. Thus if θ_i > θ_j we have that the control action is positive if π < θ_i− θ^j < 2π and negative if 0 < θ_i− θ^j < π. Geometrically this can be interpreted as that the control action of agent i is to turn towards the direction of agent j, taking the shortest arc length. It is also clear from the stated inequalities that the equilibrium θ_i− θ^j = π is unstable. The case when θ_i < θ_j is treated similarly and the conclusion is that the control action is reasonable with respect to the objective of alignment.

Finally we have the potential contribution of the angular velocity control, which consists of the sum

1 v_i

X

j∈Ni(q)

f (β_ij) sin φ_ij.

Assume that v_i > 0. The term f (β_ij) is negative if agents i and j are too close and positive if they are too far apart. The term sin φij is positive if agent j lies in the half circle to the left of agent i, (0 < φ_ij < π), and negative if agent j lies in the half circle to the right of agent i, (−π < φ^ij < 0). Hence the control action is to turn to the left if there is an agent too close on the right side or too far away on the left side and to turn right if there is an agent too close on the left side or too far away on the right side. In the case v_i < 0 the motion is reversed but the geometric interpretation is the same.

The conclusion is that the controller (2.18) has reasonable control action with respect to the objectives of alignment, cohesion and collision avoidance. It is clear that the same analysis is valid for the non-singular controller (2.19).

Remark. The analysis above might be useful in the design of other controllers for unicycle models.

Definition 2.2.3. We define the controlled system as the system (1.9) using the con- troller (2.19)

˙xi = vicos θi

˙

y_i = v_isin θ_i

˙v_i =−X

j∈Ni(q)

(v_i− v^jcos θ_ij) +X

j∈Ni(q)

f (β_ij) cos φ_ij

θ˙_i =−X

j∈Ni(q)

|v^j| + 

|vⁱ| + sin θ_ij+ sgn(v_i)

|vⁱ| +  X

j∈Ni(q)

f (β_ij) sin φ_ij

(2.20)

where i∈ V. An equivalent formulation, evident from the proof of Proposition 2.2.1 is given by

˙x_i = v_icos θ_i

˙

y_i = v_isin θ_i

˙v_i =−X

j∈Ni(q)

(v_i− v^jcos θ_ij) +X

j∈Ni(q)

ρ_ijq_ij · ˆtⁱ

θ˙_i =−X

j∈Ni(q)

|v^j| + 

|vi| + sin θ_ij +sgn(v_i)

|vi| +  X

j∈Ni(q)

ρ_ijq_ij · ˆnⁱ

(2.21)

Simulations of a controlled system with two agents initially facing the same direction are shown in Figures 2.1 to 2.4, illustrating different kinds of rendezvous.

Figure 2.1: Rendezvous of two agents initially facing the same direction and traveling in the same direction. Red circles mark half of the repulsion radius. Blue curves mark the traveled paths.

Figure 2.2: Rendezvous of two agents initially facing the same direction and traveling in the same direction. Red circles mark half of the repulsion radius. Blue curves mark the traveled paths.

Figure 2.3: Rendezvous of two agents initially facing the same direction and traveling in opposite and converging directions. Red circles mark half of the repulsion radius.

Blue curves mark the traveled paths.

Figure 2.4: Rendezvous of two agents initially facing the same direction and traveling in opposite and diverging directions. Red circles mark half of the repulsion radius. Blue curves mark the traveled paths.

Simulations of a controlled system with two agents initially facing each other are shown in Figures 2.5 to 2.8, illustrating different kinds of rendezvous. The case with two agents facing away from each other initially is similar. These maneuvers are necessarily complicated because one of the agents has to perform a sharp turn or a reverse in speed.

As such they are sensitive to the initial conditions and we only show a few of the possible behaviors to give a sense of how the system works.

Figure 2.5: Rendezvous of two agents initially facing each other and traveling in con- verging directions. Red circles mark half of the repulsion radius. Blue curves mark the traveled paths.

Figure 2.6: Rendezvous of two agents initially facing each other and traveling in di- verging directions. Red circles mark half of the repulsion radius. Blue curves mark the traveled paths.

Figure 2.7: Rendezvous of two agents initially facing each other and traveling in the same direction. The lower agent initially travels with a greater speed than the upper agent. Red circles mark half of the repulsion radius. Blue curves mark the traveled paths.

Figure 2.8: Rendezvous of two agents initially facing each other and traveling in the same direction. The upper agent initially travels with a greater speed than the lower agent. Red circles mark half of the repulsion radius. Blue curves mark the traveled paths.

2.2.2 A Unicycle Controller with a Common Objective

Adding a global alignment contribution term to the vector controller (2.2) yielded the controller (2.4) that made the system converge to a common objective velocity (Proposition 2.3.4). Similarly we can add alignment contribution terms to an objective speed v_d and an objective direction θ_d to the controller (2.19) as follows

a_i =−X

j∈Ni(q)

(v_i− vjcos θ_ij) +X

j∈Ni(q)

f (β_ij) cos φ_ij − c(vi− vdcos θ_id)

ω_i =−X

j∈Ni(q)

|vj| + 

|vⁱ| + sin θ_ij +sgn(v_i)

|vⁱ| +  X

j∈Ni(q)

f (β_ij) sin φ_ij − c|vd| + 

|vⁱ| +  sin θ_id

(2.22)

where c > 0 is a constant and θ_id = θ_i − θ^d. If only an objective speed is desired, this is equivalent to having θ_id= 0 for every i ∈ V, yielding

a_i =−X

j∈Ni(q)

(v_i− vjcos θ_ij) +X

j∈Ni(q)

f (β_ij) cos φ_ij − c(vi− vd)

ω_i =−X

j∈Ni(q)

|vj| + 

|vⁱ| + sin θ_ij +sgn(v_i)

|vⁱ| +  X

j∈Ni(q)

f (β_ij) sin φ_ij

(2.23)

and similarly if only an objective direction is desired, this is equivalent to having v_d= v_i for all i∈ V, yielding

a_i =−X

j∈Ni(q)

(v_i− vjcos θ_ij) +X

j∈Ni(q)

f (β_ij) cos φ_ij − cvi(1− cos θid)

ω_i =−X

j∈Ni(q)

|vj| + 

|vⁱ| + sin θ_ij + sgn(v_i)

|vⁱ| +  X

j∈Ni(q)

f (β_ij) sin φ_ij − c sin θ^id

(2.24)

2.3 Stability Analysis

2.3.1 Collective Dynamics and Stability of the Double Integra- tor Systems

Proposition 2.3.1. The collective dynamics of the double integrator model (2.1) using the controller (2.2) become

˙ q = p

˙

p = −ˆL(q)p− ∇^qV (q) (2.25)

Proof. We have that

[−ˆL(q)p]_i =−|Ni|pi+X

j∈Ni(q)

p_j =−X

j∈Ni(q)

(p_i− pj)

which is exactly the alignment contribution in (2.2). Furthermore we have by Proposi- tion 1.2.3 that the ith element of ∇^qV (q) is exactly the potential contribution in (2.2).

Thus the proposition holds.

Corollary 2.3.1. Using the result of Proposition 1.2.3 we get that the collective dy- namics (2.25) become

˙ q = p

˙

p = −ˆL(q)p− ˆP (q)q (2.26)

where ˆP (q) = (P (q)⊗ I²) and P (q) is as shown in Proposition 1.2.3.

Remark. By separating q into x and y and p into p_x = vcos θ and p^y = vsin θ, where  denotes the Hadamard or element wise product and the trigonometric functions are applied element wise, we get

˙x = p_x

˙ y = p_y

˙

p_x =−L(q)p^x− P (q)q^x

˙

p_y =−L(q)p^y− P (q)q^y

This formulation is advantageous for fast simulation in MATLAB.

Proposition 2.3.2. The collective dynamics of the double integrator model (2.1) using the controller (2.4) become

˙ q = p

˙

p = −ˆL(q)p− ∇qV (q)− c(p − ¯pd), (2.27) or equivalently

˙ q = p

˙

p =−ˆL(q)p− ˆP (q)q− c(p − ¯p^d), (2.28) where c > 0 is a constant, ¯pd is the vector (p^T_d, . . . , p^T_d) ∈ R^2N and pd ∈ R² is the common objective velocity.

Proof. The proposition is proven similarly to Proposition 2.3.1.

Proposition 2.3.3. The solutions of the system (2.26) converge to a set which is aligned, i.e. a set in the state space where connected agents have equal velocities, and where potential forces between agents cancel each other out (an extremal of V (q)).

Proof. Take the following function which is positive definite and radially unbounded in p

H(q, p) = 1

2p^Tp + V (q), and define the set

Ω_c={(q, p) : H(q, p) ≤ c}

Since V (q) ∈ C¹ and p ∈ C¹ almost everywhere we can take the derivative of H. By (1.17) and (2.26) we get

H(q, p) = p˙ ^T(−ˆL(q)p− ˆP (q)q) + ( ˆP q)^Tq˙

=−p^TL(q)pˆ − p^TP (q)q + qˆ ^TP (q)pˆ

=−p^TL(q)p =ˆ −X

(i,j)∈E(q) i<j

kpⁱ− p^jk² ≤ 0,

where we use the sum of squared norms property of ˆL(q). Thus Ω_c is an invariant set under (2.26) and by the LaSalle invariance principle the solution of (2.26) will converge to the greatest invariant subset contained in

D = {(q, p) ∈ Ω^c : p_i = p_j ∀ (i, j) ∈ E(q)},

This immediately implies that the alignment contribution in ˙p vanishes in D, thus

˙ q = p

˙

p =− ˆP (q)q (q, p)∈ D.

Furthermore, in any invariant subset of D it must hold that

˙

p_i = ˙p_j ∀ (i, j) ∈ E(q). (2.29) as the solution would otherwise leave D. Therefore q must satisfy the equation

− ˆP (q)q = ˜p,

for some ˜p that satisfies (2.29). However, such a ˜p is in the kernel of ˆP , this follows from that P (q) and L(q) have the same structure. Thus ˆP (q)q ∈ ker( ˆP (q)), but of course ˆP (q)q ∈ Im( ˆP (q)). Since Im( ˆP (q)) = (ker( ˆP^T(q))^⊥ and ˆP^T(q) = ˆP (q) we end up with

P (q)qˆ ∈ ker( ˆP (q))∩ (ker( ˆP (q)))^⊥= 0.

Thus q ∈ ker( ˆP (q)) for any invariant subset in D, which implies X

j6=i

ρ_ijq_ij = 0, ∀ i ∈ V or equivalently

∇^qV (q) = 0

which exactly implies that the potential forces cancel each other out.

Proposition 2.3.4. The solutions of the system (2.28) converge to a set which is aligned, i.e. a set in the state space where connected agents have equal velocities, and where potential forces between agents cancel each other out (an extremal of V (q)).

Moreover the velocity of each agent is equal to the common objective velocity p_d. Proof. Take the following function which is positive definite and radially unbounded in p

H(q, p) = 1

2(p− ¯pd)^T(p− ¯pd) + V (q), and define the set

Ω_c ={(q, p) : H(q, p) ≤ c}

Since V (q) ∈ C¹ and p ∈ C¹ almost everywhere we can take the derivative of H. By (1.17) and (2.28) we get

H(q, p) = (p˙ − ¯pd)^T(−ˆL(q)p− ˆP (q)q− c(p − ¯pd)) + ( ˆP q)^Tq˙

=−p^TL(q)pˆ − p^TP (q)q + ¯ˆ p^T_d( ˆL(q)p + ˆP (q)q)− c(p − ¯p^d)^T(p− ¯p^d) + q^TP (q)p.ˆ Now

−p^TP (q)q + qˆ ^TP (q)p = 0ˆ

and ¯p_d is in the kernel of both ˆL(q) and ˆP (q), since it is on the form 1n ⊗ a, where a ∈ R² is constant and 1n = (1, . . . , 1)^T ∈ Rⁿ is in the kernel of both L(q) and P (q).

Thus

H(q, p) =˙ −p^TL(q)pˆ − c(p − ¯p^d)^T(p− ¯p^d) = −X

(i,j)∈E(q) i<j

kpⁱ− p^jk²− cX

i∈V

kpⁱ− p^dk² ≤ 0,