4. Solutions

We develop a solution that incorporates four steps: (i) synthesis of position consensus on the unit sphere; (ii) formulation of CAC based collision avoidance on the unit sphere; (iii) formulation of formation control on the unit sphere; (iv) consensus-based collision-free arbitrary reconfigurations on the unit sphere.

#### 4.1. Synthesis of position consensus on the unit sphere

The basic consensus protocol Eq. (4) on its own does not solve the consensus problem on a sphere; neither does it solve the collision avoidance problem in adversarial situations (when there is opposing motion and static obstacles). To incorporate consensus on a unit sphere, we follow an optimization approach, by coding requirements as a set of linear matrix inequalities (LMI) and solving for consensus trajectories on the sphere. The main problem at this stage is to find a feasible sequence of consensus trajectories for each vehicle on the sphere, which satisfies norm and avoidance constraints. For this purpose, rather than state the objective function as a minimization or maximization problem (as usual in optimization problems), we state the objective function as the discrete time version of a semidefinite consensus dynamics, which will be augmented with an arbitrary number of constraints.

A basic requirement is that any vehicle i can communicate with at least one other neighboring vehicle. Given that τ is the number of vehicles in the neighborhood of i that it can communicate with, then i, ið Þ <sup>¼</sup> <sup>1</sup>; <sup>⋯</sup>; <sup>n</sup> individually synthesizes a Laplacian-like stochastic matrix <sup>L</sup><sup>i</sup> so that all xi are driven to consensus on the unit sphere. The synthesis of L<sup>i</sup> is as follows. A semidefinite matrix variable, Λ<sup>i</sup> ∈ S<sup>3</sup> for each i is generated. Then

$$\begin{aligned} \mathcal{L}^i(t) &= \begin{bmatrix} \tau \Lambda\_1^i(t) - \Lambda\_2^i(t) \ \cdots \ \Lambda\_\tau^i(t) \end{bmatrix}\_\prime \\ \dot{\mathbf{x}}^i(t) &= \begin{bmatrix} \tau \Lambda\_1^i(t) - \Lambda\_2^i(t) \ \cdots \ \Lambda\_\tau^i(t) \end{bmatrix} \begin{bmatrix} \mathbf{x}\_1^T(t) \ \mathbf{x}\_2^T(t) \ \cdots \ \mathbf{x}\_\tau^T(t) \end{bmatrix}^T \\ &= \begin{bmatrix} -\mathcal{L}^i(t) \begin{bmatrix} \mathbf{x}\_1^T(t) \ \mathbf{x}\_2^T(t) \ \cdots \ \mathbf{x}\_\tau^T(t) \end{bmatrix}^T \end{aligned} \tag{5}$$

where x<sup>T</sup> <sup>i</sup> ð Þt , i ¼ 1, ⋯, τ are the position vectors of vehicles that i is communicating with at time t. For the purpose of analysis, the collective description for n vehicles is given as

$$\mathcal{L}(t) = \underbrace{\begin{bmatrix} \Lambda^1(t) & \cdots & 0\\ \vdots & \ddots & \vdots\\ 0 & \cdots & \Lambda^n(t) \end{bmatrix}}\_{\Lambda(t)} \underbrace{\begin{bmatrix} l\_{11}\mathbf{I}\_3 & \cdots & l\_{1n}\mathbf{I}\_3\\ \vdots & \ddots & \vdots\\ l\_{n1}\mathbf{I}\_3 & \cdots & l\_{nn}\mathbf{I}\_3 \end{bmatrix}}\_{\Gamma = \mathbf{L} \otimes \mathbf{I}\_3},\tag{6}$$

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, 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

$$
\dot{\mathbf{x}}(t) = -\mathcal{L}(t)\mathbf{x}(t). \tag{7}
$$

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

the angle between vehicle i and obstacle k. The control problem is to drive all xi to a consensus

sphere. From the solution trajectories, obtained as unit vectors, the actual desired vehicle

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).

We develop a solution that incorporates four steps: (i) synthesis of position consensus on the unit sphere; (ii) formulation of CAC based collision avoidance on the unit sphere; (iii) formulation of formation control on the unit sphere; (iv) consensus-based collision-free arbitrary

The basic consensus protocol Eq. (4) on its own does not solve the consensus problem on a sphere; neither does it solve the collision avoidance problem in adversarial situations (when there is opposing motion and static obstacles). To incorporate consensus on a unit sphere, we follow an optimization approach, by coding requirements as a set of linear matrix inequalities (LMI) and solving for consensus trajectories on the sphere. The main problem at this stage is to find a feasible sequence of consensus trajectories for each vehicle on the sphere, which satisfies norm and avoidance constraints. For this purpose, rather than state the objective function as a minimization or maximization problem (as usual in optimization problems), we state the objective function as the discrete time version of a semidefinite consensus dynamics, which will

A basic requirement is that any vehicle i can communicate with at least one other neighboring vehicle. Given that τ is the number of vehicles in the neighborhood of i that it can communicate with, then i, ið Þ <sup>¼</sup> <sup>1</sup>; <sup>⋯</sup>; <sup>n</sup> individually synthesizes a Laplacian-like stochastic matrix <sup>L</sup><sup>i</sup> so that all xi are driven to consensus on the unit sphere. The synthesis of L<sup>i</sup> is as follows. A semidefinite

> <sup>2</sup>ð Þ<sup>t</sup> <sup>⋯</sup> <sup>Λ</sup><sup>i</sup> <sup>τ</sup>ð Þ<sup>t</sup> ,

<sup>2</sup>ð Þ<sup>t</sup> <sup>⋯</sup> <sup>Λ</sup><sup>i</sup> <sup>τ</sup>ð Þ<sup>t</sup> xT

> <sup>2</sup> ð Þ<sup>t</sup> <sup>⋯</sup> <sup>x</sup><sup>T</sup> <sup>τ</sup> ð Þ<sup>t</sup> <sup>T</sup>

<sup>i</sup> ð Þt , i ¼ 1, ⋯, τ are the position vectors of vehicles that i is communicating with at time

<sup>1</sup> ð Þ<sup>t</sup> xT

,

<sup>2</sup> ð Þ<sup>t</sup> <sup>⋯</sup> <sup>x</sup><sup>T</sup> <sup>τ</sup> ð Þ<sup>t</sup> <sup>T</sup>

(5)

trajectories are recovered via scalar multiplication and coordinate transformation.

j

obs along the way on the unit

position or to a formation while avoiding each other and the x

The solutions are presented in the next section.

4.1. Synthesis of position consensus on the unit sphere

be augmented with an arbitrary number of constraints.

ðÞ ¼ <sup>t</sup> <sup>τ</sup>Λ<sup>i</sup>

ðÞ ¼ <sup>t</sup> <sup>τ</sup>Λ<sup>i</sup>

¼ �L<sup>i</sup>

∈ S<sup>3</sup> for each i is generated. Then

<sup>1</sup>ðÞ� <sup>t</sup> <sup>Λ</sup><sup>i</sup>

<sup>1</sup>ðÞ� <sup>t</sup> <sup>Λ</sup><sup>i</sup>

<sup>1</sup> ð Þ<sup>t</sup> xT

t. For the purpose of analysis, the collective description for n vehicles is given as

ð Þ<sup>t</sup> <sup>x</sup><sup>T</sup>

reconfigurations on the unit sphere.

30 Advanced Path Planning for Mobile Entities

4. Solutions

matrix variable, Λ<sup>i</sup>

where x<sup>T</sup>

Li

x\_i

$$\mathbf{x}\_{k+1}^{i} = \mathbf{x}\_{k}^{i} - \Delta t \mathcal{L}^{i}(t) \mathbf{x}\_{k'}^{i} \tag{8}$$

$$\mathbf{x}\_{k+1} = \mathbf{x}\_k - \Delta t \dot{\mathbf{x}}\_k = \mathbf{x}\_k - \Delta t \mathcal{L}(t) \mathbf{x}\_k \tag{9}$$

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 on the unit sphere, norm constraints will be defined for each i as

$$\left(\mathbf{x}^{i}\right)\_{k}^{T}\left(\mathbf{x}\_{k+1}^{i} - \mathbf{x}\_{k}^{i}\right) = \mathbf{0}.\tag{10}$$

Eq. (10) is the discrete time version of xi ð Þt T x\_i ðÞ¼ <sup>t</sup> 0 or <sup>x</sup>ð Þ<sup>t</sup> <sup>T</sup> x\_ðÞ¼ t 0, which guarantees that xi ð Þt T xi ðÞ¼ <sup>t</sup> 1 or <sup>x</sup>ð Þ<sup>t</sup> <sup>T</sup> <sup>x</sup>ðÞ¼ <sup>t</sup> <sup>n</sup> for <sup>n</sup> vehicles, iff <sup>x</sup><sup>i</sup> ð Þ<sup>0</sup> � � � � <sup>¼</sup> <sup>1</sup><sup>∀</sup> <sup>i</sup>. Eq. (8) drives the positions xi ð Þ0 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, the strategy x\_ðÞ¼� t Lxð Þt achieves global consensus asymptotically for L [19].

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

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

Theorem 2: The time varying system Eq. (7) achieves consensus if L is connected. Note that this proof had already been presented in [20].

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 x∈ C. Then,

$$\begin{aligned} \dot{V} &= -\mathbf{x}^T \Gamma \dot{\mathbf{x}} + \dot{\mathbf{x}}^T \Gamma \mathbf{x} \\ &= -\mathbf{x}^T \Gamma \mathcal{L}(t) \mathbf{x} - \mathbf{x}^T \mathcal{L}(t)^T \Gamma \mathbf{x} \\ &= -\mathbf{x}^T \Gamma \Lambda(t) \Gamma \mathbf{x} - \mathbf{x}^T \Gamma \Lambda(t) \Gamma \mathbf{x} \\ &= -2\mathbf{x}^T \Gamma \Lambda(t) \Gamma \mathbf{x} \\ &= -2\mathbf{z}^T \Lambda(t) \mathbf{z} \end{aligned} \tag{11}$$

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

Multiply Eq. (20) by 2 and we have

absolute value of the eigenvalues of G, then

0<sup>3</sup> I<sup>3</sup> I<sup>3</sup> 0<sup>3</sup> � � � � �<sup>1</sup>

Let M ¼ μI<sup>6</sup> þ

x1 ð Þt T

plement formula, the LMI equivalent of Eq. (12) becomes

x1 obs T 03

1 2 I<sup>3</sup> 0<sup>3</sup>

2 6 4

ð Þ<sup>t</sup> <sup>T</sup> <sup>0</sup><sup>3</sup> <sup>I</sup><sup>3</sup> I<sup>3</sup> 0<sup>3</sup> � �

> |fflfflfflfflfflffl{zfflfflfflfflfflffl} G

0<sup>3</sup> I<sup>3</sup> I<sup>3</sup> 0<sup>3</sup> � � � � x1

2 cos <sup>α</sup><sup>1</sup> <sup>þ</sup> <sup>μ</sup> � �

<sup>x</sup><sup>1</sup>ð Þ<sup>t</sup> x1 obs

2 cos <sup>α</sup><sup>1</sup> <sup>þ</sup> <sup>μ</sup> � �

<sup>x</sup><sup>1</sup>ð Þ <sup>k</sup> <sup>þ</sup> <sup>1</sup>


2 cos <sup>α</sup><sup>1</sup> <sup>þ</sup> <sup>μ</sup> � �

<sup>x</sup><sup>2</sup>ð Þ <sup>k</sup> <sup>þ</sup> <sup>1</sup>


x1 obs " #

x1 obs " #

The LMI equivalents of Eqs. (12) to (17) in discrete time can now be written as follows

" #

1 2 I3

x1

We desire a positive definite G, i.e. G > 0, or whose eigenvalues are all nonnegative (this is synonymous with R in the Schur inequality). To make G positive definite, one only needs to shift the eigenvalues by choosing a positive real number μ which is larger than the largest

3 7 5

<sup>x</sup><sup>1</sup>ð Þ<sup>t</sup> x1 obs " #

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

≤ cos α<sup>1</sup> (20)

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

33

: (21)

ð Þ<sup>t</sup> <sup>≤</sup> 2 cos <sup>α</sup><sup>1</sup> <sup>þ</sup> <sup>μ</sup> � �: (22)

≥ 0, (23)

≥ 0, (24)

≥ 0, (25)

, then M is positive definite. Thus, following the Schur's com-

<sup>x</sup><sup>1</sup>ð Þ<sup>t</sup> x1 obs


M

<sup>x</sup><sup>1</sup>ð Þ <sup>k</sup> <sup>þ</sup> <sup>1</sup>


M

<sup>x</sup><sup>2</sup>ð Þ <sup>k</sup> <sup>þ</sup> <sup>1</sup>


M

x1 obs " #

x1 obs " #

" #

T

T

T


ð Þ<sup>t</sup> <sup>≤</sup> 2 cos <sup>α</sup><sup>1</sup>

h i


x1

μI<sup>6</sup> þ

where z ¼ Γx 6¼ 0 for x∉C. This implies that x approaches a point in C as t ! ∞, which proves the claim. Eq. (11) is true for as long as L is nonempty, i.e., if some vehicles can sense, see or communicate with each other at all times.

#### 4.2. Formulation of CAC based collision avoidance on the unit sphere

To incorporate collision avoidance, we apply the concept of constrained attitude control (CAC), as illustrated in Figure 1. We want the time evolution of the position vectors <sup>x</sup><sup>1</sup>ð Þ<sup>t</sup> , <sup>x</sup><sup>2</sup>ð Þ<sup>t</sup> and <sup>x</sup><sup>3</sup>ð Þ<sup>t</sup> to avoid two constraint regions around <sup>x</sup><sup>1</sup> obs and x<sup>2</sup> obs. The obstacle regions are defined by cones, whose base radii are r<sup>1</sup> and r2, respectively. Let the angle between vehicles i and j be θij, and that between vehicle i and obstacle k be wik. Then the requirements for collision avoidance are: w<sup>11</sup> ≥ α1, w<sup>21</sup> ≥ α1, w<sup>31</sup> ≥ α1, and w<sup>12</sup> ≥ α2, w<sup>22</sup> ≥ α2, w<sup>32</sup> ≥ α2, ∀t∈ t0; tf . They have the following equivalent quadratic constraints:

$$\mathbf{x}^1(t)^T \mathbf{x}\_{obs}^1 \le \cos \alpha^1,\tag{12}$$

$$\mathbf{x}^2(t)^T \mathbf{x}\_{\text{obs}}^1 \le \cos \alpha^1,\tag{13}$$

$$\alpha^3(t)^T x\_{obs}^1 \le \cos \alpha^1,\tag{14}$$

$$\mathbf{x}^1(t)^T \mathbf{x}\_{\text{obs}}^2 \le \cos \alpha^2 \tag{15}$$

$$\text{tr}^2(t)^T \mathbf{x}\_{obs}^2 \le \cos \alpha^2,\tag{16}$$

$$\left(\boldsymbol{x}^{3}(t)\right)^{\top}\boldsymbol{x}\_{obs}^{2} \leq \cos\alpha^{2}.\tag{17}$$

By using the Schur's complement formula [21], the above constraints will be converted to the form of linear matrix inequalities (LMI) in order to include them into the respective SDPs. The Schur's complement formula states that the inequality

$$\mathbf{S} \mathbf{R}^{-1} \mathbf{S}^{\mathsf{T}} - \mathbf{Q} \le 0 \tag{18}$$

where <sup>Q</sup> <sup>¼</sup> <sup>Q</sup>T, <sup>R</sup> <sup>¼</sup> <sup>R</sup>T, and <sup>R</sup> <sup>&</sup>gt; 0, is equivalent to, and can be represented by the linear matrix inequality

$$
\begin{bmatrix}
\mathbf{Q} & \mathbf{S} \\
\mathbf{S}^T & \mathbf{R}
\end{bmatrix} \succeq 0. \tag{19}
$$

Next, we attempt to make our quadratic constraints to look like the Schur's inequality. Observe that Eq. (12) is equivalent to

#### Multi-Path Planning on a Sphere with LMI-Based Collision Avoidance http://dx.doi.org/10.5772/intechopen.71216 33

$$
\underbrace{\begin{bmatrix} \mathbf{x}^1(t)^T & \mathbf{x}^1\_{obs} \end{bmatrix}}\_{\mathbf{x}^1(t)^T} \begin{bmatrix} \mathbf{0}\_3 & \frac{1}{2}\mathbf{I}\_3 \\\\ \frac{1}{2}\mathbf{I}\_3 & \mathbf{0}\_3 \end{bmatrix} \underbrace{\begin{bmatrix} \mathbf{x}^1(t) \\\\ \mathbf{x}^1\_{obs} \end{bmatrix}}\_{\mathbf{x}^1(t)} \le \cos a^1 \tag{20}
$$

Multiply Eq. (20) by 2 and we have

<sup>V</sup>\_ <sup>¼</sup> <sup>x</sup><sup>T</sup>Γx\_ <sup>þ</sup> <sup>x</sup>\_<sup>T</sup>Γx,

4.2. Formulation of CAC based collision avoidance on the unit sphere

<sup>x</sup><sup>2</sup>ð Þ<sup>t</sup> and <sup>x</sup><sup>3</sup>ð Þ<sup>t</sup> to avoid two constraint regions around <sup>x</sup><sup>1</sup>

the following equivalent quadratic constraints:

Schur's complement formula states that the inequality

matrix inequality

that Eq. (12) is equivalent to

communicate with each other at all times.

32 Advanced Path Planning for Mobile Entities

¼ �2x<sup>T</sup>ΓΛð Þ<sup>t</sup> <sup>Γ</sup>x, ¼ �2zTΛð Þ<sup>t</sup> z,

¼ �x<sup>T</sup>ΓLð Þ<sup>t</sup> <sup>x</sup> � <sup>x</sup><sup>T</sup>Lð Þ<sup>t</sup> <sup>T</sup>Γx, ¼ �x<sup>T</sup>ΓΛð Þ<sup>t</sup> <sup>Γ</sup><sup>x</sup> � <sup>x</sup><sup>T</sup>ΓΛð Þ<sup>t</sup> <sup>Γ</sup>x,

where z ¼ Γx 6¼ 0 for x∉C. This implies that x approaches a point in C as t ! ∞, which proves the claim. Eq. (11) is true for as long as L is nonempty, i.e., if some vehicles can sense, see or

To incorporate collision avoidance, we apply the concept of constrained attitude control (CAC), as illustrated in Figure 1. We want the time evolution of the position vectors <sup>x</sup><sup>1</sup>ð Þ<sup>t</sup> ,

defined by cones, whose base radii are r<sup>1</sup> and r2, respectively. Let the angle between vehicles i and j be θij, and that between vehicle i and obstacle k be wik. Then the requirements for collision

obs ≤ cos α<sup>1</sup>

obs ≤ cos α<sup>1</sup>

obs ≤ cos α<sup>1</sup>

obs ≤ cos α<sup>2</sup>

obs ≤ cos α<sup>2</sup>

obs ≤ cos α<sup>2</sup>

By using the Schur's complement formula [21], the above constraints will be converted to the form of linear matrix inequalities (LMI) in order to include them into the respective SDPs. The

where <sup>Q</sup> <sup>¼</sup> <sup>Q</sup>T, <sup>R</sup> <sup>¼</sup> <sup>R</sup>T, and <sup>R</sup> <sup>&</sup>gt; 0, is equivalent to, and can be represented by the linear

Next, we attempt to make our quadratic constraints to look like the Schur's inequality. Observe

Q S S<sup>T</sup> R 

avoidance are: w<sup>11</sup> ≥ α1, w<sup>21</sup> ≥ α1, w<sup>31</sup> ≥ α1, and w<sup>12</sup> ≥ α2, w<sup>22</sup> ≥ α2, w<sup>32</sup> ≥ α2, ∀t∈ t0; tf

x1 ð Þt T x1

x2 ð Þt T x1

x3 ð Þt T x1

x1 ð Þt T x2

x2 ð Þt T x2

x3 ð Þt T x2

SR�<sup>1</sup>

obs and x<sup>2</sup>

obs. The obstacle regions are

, (12)

, (13)

, (14)

, (15)

, (16)

: (17)

<sup>S</sup><sup>T</sup> � <sup>Q</sup> <sup>≤</sup> <sup>0</sup> (18)

≥ 0: (19)

. They have

(11)

$$\mathbf{x}^{\mathbf{1}}(t)^{T} \underbrace{\begin{bmatrix} \mathbf{0}\_{3} & \mathbf{I}\_{3} \\ \mathbf{I}\_{3} & \mathbf{0}\_{3} \end{bmatrix}}\_{\mathbf{G}} \mathbf{x}^{\mathbf{1}}(t) \le 2 \cos \alpha^{1}. \tag{21}$$

We desire a positive definite G, i.e. G > 0, or whose eigenvalues are all nonnegative (this is synonymous with R in the Schur inequality). To make G positive definite, one only needs to shift the eigenvalues by choosing a positive real number μ which is larger than the largest absolute value of the eigenvalues of G, then

$$\mathbf{x}^1(t)^T \left(\mu \mathbf{I}\_6 + \begin{bmatrix} \mathbf{0}\_3 & \mathbf{I}\_3 \\ \mathbf{I}\_3 & \mathbf{0}\_3 \end{bmatrix} \right) \mathbf{x}^1(t) \le 2 \left(\cos a^1 + \mu\right). \tag{22}$$

Let M ¼ μI<sup>6</sup> þ 0<sup>3</sup> I<sup>3</sup> I<sup>3</sup> 0<sup>3</sup> � � � � �<sup>1</sup> , then M is positive definite. Thus, following the Schur's complement formula, the LMI equivalent of Eq. (12) becomes

$$\begin{bmatrix} 2\left(\cos\alpha^1 + \mu\right) & \underbrace{\begin{bmatrix} \mathbf{x}^1(t) \\ \mathbf{x}\_{obs}^1 \end{bmatrix}^T}\_{\mathbf{M}} \\\\ \begin{bmatrix} \mathbf{x}^1(t) \\ \mathbf{x}\_{obs}^1 \end{bmatrix} & \mathbf{M} \end{bmatrix} \succeq \mathbf{0}, \tag{23}$$

The LMI equivalents of Eqs. (12) to (17) in discrete time can now be written as follows

$$\begin{bmatrix} 2(\cos \alpha^1 + \mu) & \underbrace{\begin{bmatrix} \mathbf{x}^1(k+1) \\ \mathbf{x}\_{\text{obs}}^1 \end{bmatrix}^T}\_{\mathbf{M}} \\\\ \underbrace{\begin{bmatrix} \mathbf{x}^1(k+1) \\ \mathbf{x}\_{\text{obs}}^1 \end{bmatrix}}\_{\mathbf{M}} \end{bmatrix} \ge 0,\tag{24}$$
 
$$\begin{bmatrix} 2(\cos \alpha^1 + \mu) & \underbrace{\begin{bmatrix} \mathbf{x}^2(k+1) \\ \mathbf{x}\_{\text{obs}}^1 \end{bmatrix}^T}\_{\mathbf{M}} \\\\ \underbrace{\begin{bmatrix} \mathbf{x}^2(k+1) \\ \mathbf{x}\_{\text{obs}}^1 \end{bmatrix}}\_{\mathbf{M}} & \mathbf{M} \end{bmatrix} \ge 0,\tag{25}$$

$$\begin{bmatrix} 2(\cos \alpha^1 + \mu) & \underbrace{\begin{bmatrix} \mathbf{x}^3(k+1) \\ \mathbf{x}\_{\text{abs}}^1 \end{bmatrix}^T}\_{\mathbf{x}\_{\text{abs}}^1} \\\\ \underbrace{\begin{bmatrix} \mathbf{x}^3(k+1) \\ \mathbf{x}\_{\text{abs}}^1 \end{bmatrix}}\_{\mathbf{M}} & \mathbf{M} \end{bmatrix} \ge 0,\tag{26}$$
 
$$\begin{bmatrix} 2(\cos \alpha^2 + \mu) & \underbrace{\begin{bmatrix} \mathbf{x}^3(k+1) \\ \mathbf{x}\_{\text{abs}}^2 \end{bmatrix}^T}\_{\mathbf{x}\_{\text{abs}}^2} \\\\ \underbrace{\begin{bmatrix} \mathbf{x}^1(k+1) \\ \mathbf{x}\_{\text{abs}}^2 \end{bmatrix}}\_{\mathbf{x}\_{\text{abs}}^2} & \mathbf{M} \end{bmatrix} \ge 0,\tag{27}$$
 
$$\underbrace{\begin{bmatrix} 2(\cos \alpha^2 + \mu) & \underbrace{\begin{bmatrix} \mathbf{x}^2(k+1) \\ \mathbf{x}\_{\text{abs}}^2 \end{bmatrix}}\_{\mathbf{M}} & \mathbf{M} \end{bmatrix}}\_{\mathbf{M}} \ge 0,\tag{28}$$
 
$$\underbrace{\begin{bmatrix} 2(\cos \alpha^2 + \mu) & \underbrace{\begin{bmatrix} \mathbf{x}^3(k+1)}\_{\mathbf{x}\_{\text{abs}}^2} \end{bmatrix}^T}\_{\mathbf{M}} \end{bmatrix}\_{\mathbf{M}} \ge 0.\tag{29}$$

Figure 2 shows the result for applying the above strategy to the rendezvous of 4 vehicles on a sphere, with avoidance of a static obstacle xobs, with <sup>α</sup> <sup>¼</sup> 30o.

#### 4.3. Formulation of formation control on the unit sphere

Formation patterns are obtained by specifying a minimum angular separation of <sup>β</sup>ijð Þ<sup>t</sup> between any two vehicles i and j thereby defining relative spacing between individual vehicles. Using the avoidance strategy formerly described, the constraint θij ≥ βij∀ i, j is used to define the set of avoidance constraints that will result in the desired formation pattern. The relative spacing results in intervehicle collision avoidance. For n vehicles, the avoidance requirements result in extra P nð Þ¼ � <sup>2</sup> <sup>n</sup>! ð Þ <sup>n</sup>�<sup>2</sup> ! constraints, which are included along with the static obstacle avoidance constraints such as Eqs. (24) to (29). Figure 3 shows the result for applying the above strategy to the rendezvous with inter-vehicle avoidance and static obstacle avoidance, of four vehicles, using a fully connected graph Topology 1 in Figure 4. In this experiment <sup>α</sup> <sup>¼</sup> 30o and the

minimum angular separations between the vehicles is set at a constant value <sup>β</sup>ij <sup>¼</sup> 20o<sup>∀</sup> i, j. Therefore, in addition to the four static obstacle avoidance constraints (such as Eqs. (24) to (29), with <sup>α</sup><sup>1</sup> <sup>¼</sup> <sup>α</sup>), each vehicle has three more intervehicle collision avoidance constraints

Figure 3. Four-vehicle formation acquisition on a unit sphere with collision avoidance of a static obstacle, and with intervehicle collision avoidance. The figure shows the evolution of the x, y, z positions of the four vehicles x<sup>1</sup>, x<sup>1</sup>, x<sup>3</sup>, x<sup>4</sup> from

Figure 2. Four-vehicle rendezvous on a unit sphere with collision avoidance of a static obstacle. The figure shows the

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

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

35

evolution of the x, y, z positions of the four vehicles x<sup>1</sup>, x<sup>1</sup>, x<sup>3</sup>, x<sup>4</sup> from initial to final positions.

such as

initial to final positions.

Multi-Path Planning on a Sphere with LMI-Based Collision Avoidance http://dx.doi.org/10.5772/intechopen.71216 35

2 cos <sup>α</sup><sup>1</sup> <sup>þ</sup> <sup>μ</sup> � �

<sup>x</sup><sup>3</sup>ð Þ <sup>k</sup> <sup>þ</sup> <sup>1</sup>


2 cos <sup>α</sup><sup>2</sup> <sup>þ</sup> <sup>μ</sup> � �

<sup>x</sup><sup>1</sup>ð Þ <sup>k</sup> <sup>þ</sup> <sup>1</sup>


2 cos <sup>α</sup><sup>2</sup> <sup>þ</sup> <sup>μ</sup> � �

<sup>x</sup><sup>2</sup>ð Þ <sup>k</sup> <sup>þ</sup> <sup>1</sup>


2 cos <sup>α</sup><sup>2</sup> <sup>þ</sup> <sup>μ</sup> � �

<sup>x</sup><sup>3</sup>ð Þ <sup>k</sup> <sup>þ</sup> <sup>1</sup>


" #

x2 obs

sphere, with avoidance of a static obstacle xobs, with <sup>α</sup> <sup>¼</sup> 30o.

4.3. Formulation of formation control on the unit sphere

extra P nð Þ¼ � <sup>2</sup> <sup>n</sup>!

" #

x2 obs

" #

x2 obs

" #

x1 obs

34 Advanced Path Planning for Mobile Entities

<sup>x</sup><sup>3</sup>ð Þ <sup>k</sup> <sup>þ</sup> <sup>1</sup>


M

<sup>x</sup><sup>1</sup>ð Þ <sup>k</sup> <sup>þ</sup> <sup>1</sup>


M

<sup>x</sup><sup>2</sup>ð Þ <sup>k</sup> <sup>þ</sup> <sup>1</sup>


M

<sup>x</sup><sup>3</sup>ð Þ <sup>k</sup> <sup>þ</sup> <sup>1</sup>


M

ð Þ <sup>n</sup>�<sup>2</sup> ! constraints, which are included along with the static obstacle avoidance

" #

x2 obs

Figure 2 shows the result for applying the above strategy to the rendezvous of 4 vehicles on a

Formation patterns are obtained by specifying a minimum angular separation of <sup>β</sup>ijð Þ<sup>t</sup> between any two vehicles i and j thereby defining relative spacing between individual vehicles. Using the avoidance strategy formerly described, the constraint θij ≥ βij∀ i, j is used to define the set of avoidance constraints that will result in the desired formation pattern. The relative spacing results in intervehicle collision avoidance. For n vehicles, the avoidance requirements result in

constraints such as Eqs. (24) to (29). Figure 3 shows the result for applying the above strategy to the rendezvous with inter-vehicle avoidance and static obstacle avoidance, of four vehicles, using a fully connected graph Topology 1 in Figure 4. In this experiment <sup>α</sup> <sup>¼</sup> 30o and the

" #

x2 obs

" #

x2 obs

" #

T

T

T

T

≥ 0, (26)

≥ 0, (27)

≥ 0, (28)

≥ 0: (29)

x1 obs

Figure 2. Four-vehicle rendezvous on a unit sphere with collision avoidance of a static obstacle. The figure shows the evolution of the x, y, z positions of the four vehicles x<sup>1</sup>, x<sup>1</sup>, x<sup>3</sup>, x<sup>4</sup> from initial to final positions.

Figure 3. Four-vehicle formation acquisition on a unit sphere with collision avoidance of a static obstacle, and with intervehicle collision avoidance. The figure shows the evolution of the x, y, z positions of the four vehicles x<sup>1</sup>, x<sup>1</sup>, x<sup>3</sup>, x<sup>4</sup> from initial to final positions.

minimum angular separations between the vehicles is set at a constant value <sup>β</sup>ij <sup>¼</sup> 20o<sup>∀</sup> i, j. Therefore, in addition to the four static obstacle avoidance constraints (such as Eqs. (24) to (29), with <sup>α</sup><sup>1</sup> <sup>¼</sup> <sup>α</sup>), each vehicle has three more intervehicle collision avoidance constraints such as

Figure 4. Topology 1 (left) is a fully connected communication graph with no leader, topology 2 (center) is a cyclic communication graph with one leader, node 1, and topology 3 (right) is a cyclic communication graph with no leader.

$$\begin{bmatrix} 2\left(\cos\beta^{\vec{\eta}} + \mu\right) & \underbrace{\begin{bmatrix} \mathbf{x}^{i}(k+1) \\ \mathbf{x}^{i}(k+1) \end{bmatrix}^{T}}\_{\mathbf{M}} \\\\ \underbrace{\begin{bmatrix} \mathbf{x}^{i}(k+1) \\ \mathbf{x}^{i}(k+1) \end{bmatrix}}\_{\mathbf{M}} \end{bmatrix} \ge 0,$$

where xi

constraints.

If x<sup>i</sup>

vehicle pair x<sup>i</sup>

<sup>k</sup>þ<sup>1</sup> and <sup>Λ</sup><sup>i</sup>

declared as SDP variables where Λ<sup>i</sup>

data for inter-vehicle collision avoidance.

<sup>v</sup>ð Þ<sup>t</sup> <sup>T</sup> xi ð Þ<sup>t</sup> <sup>T</sup> h i<sup>T</sup>

ðÞ¼ <sup>t</sup> xi

<sup>k</sup> (which are components of L<sup>i</sup>

Figure 5. Multiple virtual leaders graph topology with an undirected topology.

<sup>k</sup> shapes the trajectories xi

Consider a more traditional reconfiguration problem that may not require formation control. For example, in a tracking problem, several vehicles are required to change their positions by tracking that of a set of virtual leaders, whose positions may be static or time-varying. For this to be possible, each vehicle must be connected to its corresponding virtual leader via a leaderfollower digraph, see Figure 5 for an example topology for three vehicles. In Figure 5, the vertices in dashed circles are the states of the virtual leaders, while those with solid circles correspond to the states of the real vehicles. There are three unconnected separate leader follower digraphs (edges indicated with arrows). In addition, there is an undirected graph (edges without arrows) which enables the vehicles to communicate bidirectionally to provide

<sup>v</sup>ð Þt is the state of a virtual leader corresponding to vehicle i, then for each leader-follower

Lt ðÞ¼ t

the corresponding leader-follower Laplacian matrix is

� �. (31)

4.4. Consensus-based collision-free arbitrary reconfigurations on the unit sphere

) are the optimization variables. They are

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

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

37

<sup>k</sup>þ<sup>1</sup> to satisfy norm and avoidance

∀ i, j ið Þ 6¼ j .

Putting it all together, the optimization problem of finding a feasible sequence of consensus trajectories with collision avoidance on a unit sphere may be posed as a semidefinite program (SDP) as follows. Given the set of initial positions xi ð Þ t<sup>0</sup> , ið Þ ¼ 1⋯n and the plant Eq. (5) for each vehicle, find a feasible sequence of trajectories that satisfies the following constraints:

$$\begin{cases} \mathbf{x}\_{k+1}^{\hat{\mathbf{x}}} = \mathbf{x}\_{k}^{\hat{\mathbf{x}}} - \Delta t \underbrace{\mathbf{x}^{\prime}}\_{\mathbf{x}^{\prime}} (\mathbf{x}\_{k}^{\prime}) \mathbf{x}\_{k^{\prime}}^{\prime} & \text{dynamic constraint} \\ \mathbf{x}^{\prime} \mathbf{x}^{\prime}\_{k} (\mathbf{x}\_{k+1}^{\prime} - \mathbf{x}\_{k}^{\prime}) = 0, & \text{norm constraint} \\ \begin{bmatrix} 2 \left(\cos a^{\hat{\mathbf{y}}} + \mu\right) & \underbrace{\mathbf{x}^{\prime} (k+1)}\_{\mathbf{x}^{\prime}\_{\mathrm{abs}}} \\ \underbrace{\mathbf{x}^{\prime}\_{\mathrm{abs}}}\_{\mathbf{x}^{\prime}\_{\mathrm{abs}}} & \mathbf{M} \\ \underbrace{\mathbf{x}^{\prime}\_{\mathrm{abs}}}\_{\mathbf{x}^{\prime}\_{\mathrm{abs}}} & \mathbf{M} \end{bmatrix} \succeq 0, & \text{static obstacle} \text{ covariance constraint} \\ \begin{bmatrix} 2 \left(\cos \beta^{\hat{\mathbf{y}}} + \mu\right) & \underbrace{\left[\mathbf{x}^{\prime} (k+1)\right]}\_{\mathbf{x}^{\prime} \left(k+1\right)} \\ \mathbf{x}^{\prime} (k+1) & \mathbf{M} \end{bmatrix}^{T} \\ \boldsymbol{\nu} \succeq 0, & \text{intervehicle covariance constraint} \end{cases}$$

Multi-Path Planning on a Sphere with LMI-Based Collision Avoidance http://dx.doi.org/10.5772/intechopen.71216 37

Figure 5. Multiple virtual leaders graph topology with an undirected topology.

2 cos <sup>β</sup>ij <sup>þ</sup> <sup>μ</sup> � � xi

xi ð Þ k þ 1

(SDP) as follows. Given the set of initial positions xi

ð Þ<sup>t</sup> xi

xi ð Þ k þ 1

2 4

x j obs

xi ð Þ k þ 1

xj ð Þ k þ 1


M

" #


M

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

3 5

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

2 cos <sup>α</sup>ij <sup>þ</sup> <sup>μ</sup> � �


2 cos <sup>β</sup>ij <sup>þ</sup> <sup>μ</sup> � �

" #


xi ð Þ k þ 1

2 4

x j obs

xi ð Þ k þ 1

xj ð Þ k þ 1

∀ i, j ið Þ 6¼ j .

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

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

36 Advanced Path Planning for Mobile Entities

" #


xj ð Þ k þ 1 ð Þ k þ 1

T

≥ 0, (30)

ð Þ t<sup>0</sup> , ið Þ ¼ 1⋯n and the plant Eq. (5) for each

≥ 0, static obstacle avoidance constraint

≥ 0, intervehicle avoidance constraint

" #


M

xj ð Þ k þ 1

Figure 4. Topology 1 (left) is a fully connected communication graph with no leader, topology 2 (center) is a cyclic communication graph with one leader, node 1, and topology 3 (right) is a cyclic communication graph with no leader.

Putting it all together, the optimization problem of finding a feasible sequence of consensus trajectories with collision avoidance on a unit sphere may be posed as a semidefinite program

<sup>k</sup>, dynamics constraint

vehicle, find a feasible sequence of trajectories that satisfies the following constraints:

� � <sup>¼</sup> <sup>0</sup>, norm constraint

T

T

3 5 where xi <sup>k</sup>þ<sup>1</sup> and <sup>Λ</sup><sup>i</sup> <sup>k</sup> (which are components of L<sup>i</sup> ) are the optimization variables. They are declared as SDP variables where Λ<sup>i</sup> <sup>k</sup> shapes the trajectories xi <sup>k</sup>þ<sup>1</sup> to satisfy norm and avoidance constraints.

#### 4.4. Consensus-based collision-free arbitrary reconfigurations on the unit sphere

Consider a more traditional reconfiguration problem that may not require formation control. For example, in a tracking problem, several vehicles are required to change their positions by tracking that of a set of virtual leaders, whose positions may be static or time-varying. For this to be possible, each vehicle must be connected to its corresponding virtual leader via a leaderfollower digraph, see Figure 5 for an example topology for three vehicles. In Figure 5, the vertices in dashed circles are the states of the virtual leaders, while those with solid circles correspond to the states of the real vehicles. There are three unconnected separate leader follower digraphs (edges indicated with arrows). In addition, there is an undirected graph (edges without arrows) which enables the vehicles to communicate bidirectionally to provide data for inter-vehicle collision avoidance.

If x<sup>i</sup> <sup>v</sup>ð Þt is the state of a virtual leader corresponding to vehicle i, then for each leader-follower vehicle pair x<sup>i</sup> ðÞ¼ <sup>t</sup> xi <sup>v</sup>ð Þ<sup>t</sup> <sup>T</sup> xi ð Þ<sup>t</sup> <sup>T</sup> h i<sup>T</sup> the corresponding leader-follower Laplacian matrix is

$$\mathbf{L}^{t}(t) = \begin{bmatrix} \mathbf{0} & \mathbf{0} \\ \mathbf{0} & -1 \end{bmatrix}. \tag{31}$$

The corresponding collective dynamics of x<sup>i</sup> ð Þt is

$$\dot{\mathbf{x}}^i(t) = -\begin{bmatrix} \Lambda^i(t) & \mathbf{0} \\ \mathbf{0} & \Lambda^i(t) \end{bmatrix} \{ \mathbf{L}^t(t) \otimes \mathbf{I}\_3 \} \mathbf{x}^i(t). \tag{32}$$

This configuration was applied in the reconfiguration experiment in Section 5.2. Practical application of this strategy to the problem of separation in air traffic control is presented in [18, 20].
