2. Mathematical background

This section briefly describes the mathematical basis of consensus theory.

#### 2.1. Basic graph theory

We define a graph G as a pair ð Þ V; E consisting of two finite sets having elements; a set of points called vertices V ¼ f g 1; 2; ⋯; n , and a set of connecting lines called edges, E⊆ vi; vj � � : � vi; vj ∈ V; j 6¼ ig or endpoints, Eð Þ i; j or vi; vj � �, of the vertices [15]. Thus, an edge is incident with vertices vi and vj. Graph G is said to be undirected if for every edge connecting two vertices, communication between the vertices is possible in both directions across the edge, i.e. vi; vj � �∈E implies vj; vi � �∈ E; otherwise it is called a directed graph (digraph), and it is symmetric. The quantity j j V is called the order, and j j E the size, respectively, of G. The set of neighbors of node vi is denoted by N<sup>i</sup> ¼ vj ∈V : vi; vj � �∈ E � �. The number of edges incident with vertex v is called the degree or valence of v. Furthermore, the number of directed edges incident into v is called the In-degree of v, while the Out-degree is similarly defined as the number of edges incident out of the v.

We define the adjacency matrix <sup>A</sup><sup>G</sup> <sup>¼</sup> aij � � of <sup>G</sup> of order <sup>n</sup> as the <sup>n</sup> � <sup>n</sup> matrix

$$a\_{\vec{\eta}} = \begin{cases} 1 & \text{if } e(\mathfrak{i}, \mathfrak{j}) \in \mathcal{E} \\ 0 & \text{otherwise} \end{cases} \tag{1}$$

by first-order dynamics, the following first-order consensus protocol (or its variants) has been

more comprehensive presentation of the necessary mathematical tools for this work (including

We state the problem of constrained motion on a unit sphere as follows: given a set of communicating vehicles randomly positioned on a unit sphere, with initial positions

ð Þ <sup>t</sup><sup>0</sup> <sup>∈</sup> <sup>R</sup>3, <sup>i</sup> <sup>¼</sup> <sup>1</sup>, <sup>⋯</sup>, n (referenced to a coordinate frame centered on the centroid of the sphere),

L, find a sequence of collision-free consensus trajectories along the surface of the unit sphere. In

The problem is illustrated in Figure 1; the unit sphere is centered on 0 which implies that

trajectory vectors. The angle between the position vectors of vehicles i and j is θij, while wik is

We determine that consensus has been achieved when xi � xj

graph theory and consensus theory), can be found in [18].

this development, a vehicle is modeled as a point mass.

<sup>x</sup>\_ðÞ¼� <sup>t</sup> L xð Þ� <sup>t</sup> <sup>x</sup>off : (4)

Multi-Path Planning on a Sphere with LMI-Based Collision Avoidance

! xij off as <sup>t</sup> ! <sup>∞</sup>, <sup>∀</sup> <sup>i</sup> 6¼ <sup>j</sup>. A

http://dx.doi.org/10.5772/intechopen.71216

29

, j ¼ 1, ⋯, m, and the Laplacian matrix of their communication graph

obs are unit vectors and must be kept so throughout the evolution of the

proposed, e.g. [16, 17]

3. Problem statement

j obs <sup>∈</sup> <sup>R</sup><sup>3</sup>

j

Figure 1. Constrained position control on a unit sphere.

a set of obstacles x

vectors xi and x

xi

For the undirected graph A<sup>G</sup> is always symmetric, while A<sup>G</sup> of a digraph G is symmetric if and only if <sup>G</sup> is symmetric. The out-degree matrix <sup>D</sup><sup>G</sup> <sup>¼</sup> dij � � of <sup>G</sup> of order <sup>n</sup>, is an <sup>n</sup> � <sup>n</sup> matrix

$$d\_{\vec{\imath}\vec{\imath}} = \sum\_{i \neq \vec{\jmath}} a\_{\vec{\imath}\vec{\jmath}} \tag{2}$$

which is simply the diagonal matrix with each diagonal element equal to the out-degree of the corresponding vertex. The in-degree matrix of G is similarly defined.

The Laplacian matrix <sup>L</sup> <sup>¼</sup> lij � � of digraph <sup>G</sup> of order <sup>n</sup>, is the <sup>n</sup> � <sup>n</sup> matrix

$$\mathbf{L} = \mathbf{D}\_{\mathcal{G}} - \mathbf{A}\_{\mathcal{G}} \tag{3}$$

An important property of any Laplacian L is that its rows and columns, sum to zero.

#### 2.2. Basic consensus theory

The basic consensus problem is that of driving the states of a team of communicating agents to an agreed state, using distributed protocols based on their communication graph. In this framework, the agents (or vehicles) i ið Þ ¼ 1; ⋯; n are represented by vertices of the graph, while the edges of the graph represent communication links between them. Let xi denote the state of a vehicle i and x is the stacked vector of the states all vehicles in the team. For systems modeled by first-order dynamics, the following first-order consensus protocol (or its variants) has been proposed, e.g. [16, 17]

$$
\dot{\mathbf{x}}(t) = -\mathbf{L}\left(\mathbf{x}(t) - \mathbf{x}^{\text{off}}\right). \tag{4}
$$

We determine that consensus has been achieved when xi � xj ! xij off as <sup>t</sup> ! <sup>∞</sup>, <sup>∀</sup> <sup>i</sup> 6¼ <sup>j</sup>. A more comprehensive presentation of the necessary mathematical tools for this work (including graph theory and consensus theory), can be found in [18].

## 3. Problem statement

2. Mathematical background

28 Advanced Path Planning for Mobile Entities

vi; vj ∈ V; j 6¼ ig or endpoints, Eð Þ i; j or vi; vj

node vi is denoted by N<sup>i</sup> ¼ vj ∈V : vi; vj

We define the adjacency matrix A<sup>G</sup> ¼ aij

only if G is symmetric. The out-degree matrix D<sup>G</sup> ¼ dij

corresponding vertex. The in-degree matrix of G is similarly defined.

2.1. Basic graph theory

� �∈E implies vj; vi

incident out of the v.

The Laplacian matrix L ¼ lij

2.2. Basic consensus theory

vi; vj

This section briefly describes the mathematical basis of consensus theory.

We define a graph G as a pair ð Þ V; E consisting of two finite sets having elements; a set of points called vertices V ¼ f g 1; 2; ⋯; n , and a set of connecting lines called edges, E⊆ vi; vj

vertices vi and vj. Graph G is said to be undirected if for every edge connecting two vertices, communication between the vertices is possible in both directions across the edge, i.e.

The quantity j j V is called the order, and j j E the size, respectively, of G. The set of neighbors of

is called the degree or valence of v. Furthermore, the number of directed edges incident into v is called the In-degree of v, while the Out-degree is similarly defined as the number of edges

> aij <sup>¼</sup> 1 if e ið Þ ; <sup>j</sup> <sup>∈</sup> <sup>E</sup> 0 otherwise

For the undirected graph A<sup>G</sup> is always symmetric, while A<sup>G</sup> of a digraph G is symmetric if and

dii <sup>¼</sup> <sup>X</sup> i6¼j

which is simply the diagonal matrix with each diagonal element equal to the out-degree of the

� � of digraph <sup>G</sup> of order <sup>n</sup>, is the <sup>n</sup> � <sup>n</sup> matrix

The basic consensus problem is that of driving the states of a team of communicating agents to an agreed state, using distributed protocols based on their communication graph. In this framework, the agents (or vehicles) i ið Þ ¼ 1; ⋯; n are represented by vertices of the graph, while the edges of the graph represent communication links between them. Let xi denote the state of a vehicle i and x is the stacked vector of the states all vehicles in the team. For systems modeled

An important property of any Laplacian L is that its rows and columns, sum to zero.

�

� �∈ E; otherwise it is called a directed graph (digraph), and it is symmetric.

� � of <sup>G</sup> of order <sup>n</sup> as the <sup>n</sup> � <sup>n</sup> matrix

� �∈ E � �. The number of edges incident with vertex v

� �, of the vertices [15]. Thus, an edge is incident with

� � of <sup>G</sup> of order <sup>n</sup>, is an <sup>n</sup> � <sup>n</sup> matrix

L ¼ D<sup>G</sup> � A<sup>G</sup> (3)

aij, (2)

� � : �

(1)

We state the problem of constrained motion on a unit sphere as follows: given a set of communicating vehicles randomly positioned on a unit sphere, with initial positions xi ð Þ <sup>t</sup><sup>0</sup> <sup>∈</sup> <sup>R</sup>3, <sup>i</sup> <sup>¼</sup> <sup>1</sup>, <sup>⋯</sup>, n (referenced to a coordinate frame centered on the centroid of the sphere), a set of obstacles x j obs <sup>∈</sup> <sup>R</sup><sup>3</sup> , j ¼ 1, ⋯, m, and the Laplacian matrix of their communication graph L, find a sequence of collision-free consensus trajectories along the surface of the unit sphere. In this development, a vehicle is modeled as a point mass.

The problem is illustrated in Figure 1; the unit sphere is centered on 0 which implies that vectors xi and x j obs are unit vectors and must be kept so throughout the evolution of the trajectory vectors. The angle between the position vectors of vehicles i and j is θij, while wik is

Figure 1. Constrained position control on a unit sphere.

the angle between vehicle i and obstacle k. The control problem is to drive all xi to a consensus position or to a formation while avoiding each other and the x j obs along the way on the unit sphere. From the solution trajectories, obtained as unit vectors, the actual desired vehicle trajectories are recovered via scalar multiplication and coordinate transformation.

LðÞ¼ t

Λ1

ð Þt ⋯ 0 ⋮⋱⋮ <sup>0</sup> <sup>⋯</sup> <sup>Λ</sup><sup>n</sup>ð Þ<sup>t</sup>


we only want it to be positive semidefinite, therefore it is an optimization variable.

We can now define a collective semidefinite consensus protocol on a sphere as

The Euler's first-order discrete time equivalents of Eqs. (5) and (7) are

on the unit sphere, norm constraints will be defined for each i as

Eq. (10) is the discrete time version of xi

this proof had already been presented in [20].

ðÞ¼ <sup>t</sup> 1 or <sup>x</sup>ð Þ<sup>t</sup> <sup>T</sup>

xi ð Þt T xi

x∈ C. Then,

xi <sup>k</sup>þ<sup>1</sup> <sup>¼</sup> xi

x<sup>i</sup> � �<sup>T</sup> <sup>k</sup> xi

the strategy x\_ðÞ¼� t Lxð Þt achieves global consensus asymptotically for L [19].

<sup>x</sup>ðÞ¼ <sup>t</sup> <sup>n</sup> for <sup>n</sup> vehicles, iff <sup>x</sup><sup>i</sup>

ð Þt T x\_i

to consensus while the norm constraint Eq. (10) keeps the trajectories on the unit sphere.

Theorem 1: As long as the associated (static) communication graph of L has a spanning tree,

Proof: The proof [19], is essentially that of convergence of the first-order consensus dynamics.

Theorem 2: The time varying system Eq. (7) achieves consensus if L is connected. Note that

Proof: Note that if x belongs to the consensus space <sup>C</sup> <sup>¼</sup> <sup>x</sup>jx<sup>1</sup> <sup>¼</sup> <sup>x</sup><sup>2</sup> <sup>¼</sup>; <sup>⋯</sup>; <sup>¼</sup> xn � �, then <sup>x</sup>\_ <sup>¼</sup> 0, (i.e. all vehicles have stopped moving). Because C is the nullspace of Lð Þt , where Lð Þt x ¼ 0 ∀x. Meaning that once x enters C it stays there since there is no more motion. If consensus has not been achieved then <sup>x</sup>∉C, consider a Lyapunov candidate function V <sup>¼</sup> <sup>x</sup><sup>T</sup>Γx; V <sup>&</sup>gt; 0 unless

Next, we use the proof of Theorem 1 as a basis to develop the proof convergence of Eq. (7).

where, <sup>L</sup> <sup>¼</sup> lij � �, ið Þ ; <sup>j</sup> <sup>¼</sup> <sup>1</sup>; <sup>⋯</sup>; <sup>n</sup> is the collective Laplacian matrix. Note that any <sup>Λ</sup><sup>i</sup> is unknown,

<sup>k</sup> � <sup>Δ</sup>tL<sup>i</sup>

Each vehicle builds a SDP in which Eq. (8) is included as the dynamics constraint, augmented with several required convex constraints. For example, for the solution trajectories to remain

> <sup>k</sup>þ<sup>1</sup> � xi k

ð Þ<sup>t</sup> xi

ðÞ¼ <sup>t</sup> 0 or <sup>x</sup>ð Þ<sup>t</sup> <sup>T</sup>

ð Þ<sup>0</sup> � � �

l11I<sup>3</sup> ⋯ l1nI<sup>3</sup> ⋮⋱⋮ ln1I<sup>3</sup> ⋯ lnnI<sup>3</sup>


http://dx.doi.org/10.5772/intechopen.71216

Multi-Path Planning on a Sphere with LMI-Based Collision Avoidance

x\_ðÞ¼� t Lð Þt xð Þt : (7)

� � <sup>¼</sup> <sup>0</sup>: (10)

x<sup>k</sup>þ<sup>1</sup> ¼ x<sup>k</sup> � Δtx\_ <sup>k</sup> ¼ x<sup>k</sup> � ΔtLð Þt x<sup>k</sup> (9)

<sup>k</sup>, (8)

x\_ðÞ¼ t 0, which guarantees that

ð Þ0

� <sup>¼</sup> <sup>1</sup><sup>∀</sup> <sup>i</sup>. Eq. (8) drives the positions xi

, (6)

31

There are two parts to the problem: consensus and collision avoidance. The consensus part is that of incorporating consensus behavior into the team on the unit sphere, which can lead to collective motion such as rendezvous, platooning, swarming and other formations. The second part which is collision avoidance, is resolved by applying constrained attitude control (CAC). The solutions are presented in the next section.
