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

Abstract

The problem of path planning with collision avoidance for autonomous flying vehicles will become more critical as the density of such vehicles increase in the skies. Global aerial navigation paths can be modeled as a path-planning problem on a unit sphere. In this work, we apply consensus theory and semidefinite programming to constrained multi-path planning with collision avoidance for a team of communicating vehicles navigating on a sphere. Based on their communication graph, each vehicle individually synthesizes a time-varying Laplacian-like matrix which drives each of them from their initial positions to consensus positions on the surface of the sphere. The solution trajectories obtained on the unit sphere are transformed back to actual vehicle coordinates. Formation configurations are realized via consensus theory, while collision avoidance is realized via semidefinite programming. A Lyapunov-based stability analysis is also provided, together with simulation results to demonstrate the effectiveness of the approach.

1. Introduction

In this chapter, we present an approach to constrained multi-agent control on the unit sphere; by applying consensus theory and constrained attitude control (CAC) via semidefinite programming. Global navigation can be modeled by control on the unit sphere and such algorithms have applications in: aerial navigation [1]; sea navigation and ocean sampling [2]; space navigation and satellite cluster positioning [3, 4]. For example, the algorithm presented in this chapter will find practical application in aircraft horizontal separation.

Most path-planning work generally focus on two-dimensional (2D) [5, 6], and three- dimensional motion planning (3D) [7, 8, 9, 10]. However, both path planning models are limited when the motion is constrained to evolve on a sphere.

Looking at the main works that have been done on control on a sphere, [11] applied Lie algebra to develop a model of self-propelled particles (as point masses) which move on the surface of a unit sphere at constant speed. Circular formations of steady motions of the particles around a fixed small circle on the sphere were identified as relative equilibria of the model using a Lie group representation. The paper also provided mathematically justified shape control laws that stabilize the set of circular formations. They also proposed a shape control to isolate circular formations of particles with symmetric spacing by using Laplacian control. Further work on this is presented in [12].

The works [11, 12] are based on [5, 13], where a geometric approach to the gyroscopic control of vehicle motion in planar and three-dimensional particle models was developed for formation acquisition and control with collision avoidance in free space. They discovered three possible types of relative equilibria for their unconstrained gyroscopic control system on $\mathit{SE}\left(3\right)$: (i) parallel particle motion with arbitrary spacing; (ii) circular particle motion that has a common radius, axis and direction of rotation, and arbitrary along-axis spacing; (iii) helical particle motion that has a common radius, axis and direction of rotation, along-axis speed (pitch) and arbitrary along-axis spacing. This approach is effective in formation control of multiple systems in unconstrained spaces and for formations that conform to the three types of relative equilibria described above.

Therefore, it is necessary to consider consensus on a sphere, which can be applied to the more general motion control problem involving: (i) constrained spaces which contain static obstacles such as clutter; (ii) speed constrained vehicles; (iii) other arbitrary formations which are different from the relative equilibria described above. We apply consensus theory to collective motion of a team of communicating vehicles on the sphere and the concept of constrained attitude control (CAC) to generate collision avoidance behavior among the vehicles as they navigate to arbitrary formations [14].

We assume that each individual vehicle can communicate with neighbors within its sensor view. Each vehicle can therefore use the Laplacian matrix of the communication graph L in a semidefinite program to plan consensus trajectories on the sphere. Then the concept of CAC is used to incorporate collision avoidance, by maintaining specified minimum angles between vectors of vehicle positions. The algorithm presented here is applicable to motion control in both constrained and unconstrained spaces on the sphere, e.g. for planning consensus trajectories around static obstacles or adversarial non-cooperative obstacles on the sphere. The approach can also be applied to constrained vehicle motion of non-constant velocities. It is also possible to generate formations on the sphere that are different from circular motion.

The rest of this chapter is organized as follows. In Section 2, we present the mathematical basis of consensus theory, while the problem statement is presented Section 3. In Section 4, the solution and convergence analysis are presented. This is followed by simulation results in Section 5 and references in Section 6. Table 1 lists frequently used notation in this chapter.

Notation	Meaning
$n$	Number of vehicles
$i$	Vehicle number $i$
$x^i$	Position vector of vehicle $i$
$\widehat{x}$	Unit vector corresponding to vector $x$
$u^i,{\dot{x}}^i$	Control input of vehicle $i$
$x_{\mathit{obs}}^j$	Obstacle vector number $j$
${\left(x^{\mathit{ij}}\right)}^{\mathit{off}}$	Offset vector between vehicles $i$ and $j$
${\phi }^{\mathit{ij}}$	Angle between vehicle $i$ and obstacle $j$
${\theta }^{\mathit{ij}}$	Angle between vehicles $i$ and $j$
${\alpha }^i$	Minimum angular separation from obstacle number $i$
${\beta }^{\mathit{ij}}$	Minimum angular separation between vehicles $i$ and $j$
$\mathbf{x}$	Stacked vector of $n$ position vectors
$\mathbf{u}\mathbf{,}\dot{\mathbf{x}}$	Stacked vector of $n$ control inputs
$\mathbf{L}$	Laplacian matrix, $\mathbf{L}={\mathbf{D}}_{\mathcal{G}}-{\mathbf{A}}_{\mathcal{G}}$
$\mathcal{L},{\mathcal{L}}^i$	Laplacian-like stochastic matrix
$\mathbf{0}$	A vector consisting of all zeros
$\otimes$	Kronecker multiplication operator
$\mathit{SE}\left(3\right)$	Special Euclidean group
${\mathcal{S}}^m$	The set of $m\times m$ positive definite matrices
${\mathbf{I}}_n$	The $n\times n$ identity matrix
$\mathrm{\Lambda }$	A positive definite matrix variable, $\mathrm{\Lambda }\in S^m$
$\mathcal{C}$	The consensus space for $\mathbf{x}\mathbf{,}\mathcal{C}=\left\{\mathbf{x}|x^1=x^2=,\cdots ,=x^n\right\}$
$\mathcal{G}$	Graph
$\mathcal{V}$	Set of vertices of $\mathcal{G}$
$\mathcal{E}$	Set of edges of $\mathcal{G}$
$v_i$	Vertex $v_i\in \mathcal{V}$
$\left(v_i,v_j\right)$	Endpoint or edge $\left(v_i,v_j\right)\in \mathcal{E}$
${\mathcal{N}}_i$	Neighbors of $v_i$; ${\mathcal{N}}_i=\left\{v_j\in \mathcal{V}:\left(v_i,v_j\right)\in \mathcal{E}\right\}$
${\mathbf{A}}_{\mathcal{G}}$	Adjacency matrix of $\mathcal{G}$; ${\mathbf{A}}_{\mathcal{G}}=\left[a_{\mathit{ij}}\right]$
${\mathbf{D}}_{\mathcal{G}}$	Out-degree matrix of $\mathcal{G}$; ${\mathbf{D}}_{\mathcal{G}}=\left[d_{\mathit{ij}}\right]$
$\mathbf{S}$	A vector or matrix in the Schur’s inequality
$\mathbf{R}$	A positive definite matrix in the Schur’s inequality
$\mathbf{Q}$	A symmetric matrix in the Schur’s inequality
$\mathbf{M}$	A positive definite matrix variable
$\mathbf{G}$	A positive semidefinite matrix

Table 1.

Frequently used notation in this chapter.

2. Mathematical background

This section briefly describes the mathematical basis of consensus theory.

2.1. Basic graph theory

We define a graph $\mathcal{G}$ as a pair $\left(\mathcal{V},\mathcal{E}\right)$ consisting of two finite sets having elements; a set of points called vertices $\mathcal{V}=\left\{1,2,\cdots ,n\right\}$, and a set of connecting lines called edges, $\mathcal{E}\subseteq \left\{\left(v_i,v_j\right):v_i,v_j\in \mathcal{V},j\ne i\right\}$ or endpoints, $\mathcal{E}\left(i,j\right)$ or $\left(v_i,v_j\right)$, of the vertices [15]. Thus, an edge is incident with vertices $v_i$ and $v_j$. Graph $\mathcal{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. $\left(v_i,v_j\right)\in \mathcal{E}$ implies $\left(v_j,v_i\right)\in \mathcal{E}$; otherwise it is called a directed graph (digraph), and it is symmetric. The quantity $\left|\mathcal{V}\right|$ is called the order, and $\left|\mathcal{E}\right|$ the size, respectively, of $\mathcal{G}$. The set of neighbors of node $v_i$ is denoted by ${\mathcal{N}}_i=\left\{v_j\in \mathcal{V}:\left(v_i,v_j\right)\in \mathcal{E}\right\}$. 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 ${\mathbf{A}}_{\mathcal{G}}=\left[a_{\mathit{ij}}\right]$ of $\mathcal{G}$ of order $n$ as the $n\times n$ matrix

$$a_{\mathit{ij}}=\left\{\begin{array}{cc}1& \text{if}e\left(i,j\right)\in \mathcal{E}\\ 0& \text{otherwise}\end{array}\right.$$E1

For the undirected graph ${\mathbf{A}}_{\mathcal{G}}$ is always symmetric, while ${\mathbf{A}}_{\mathcal{G}}$ of a digraph G is symmetric if and only if $\mathcal{G}$ is symmetric. The out-degree matrix ${\mathbf{D}}_{\mathcal{G}}=\left[d_{\mathit{ij}}\right]$ of $\mathcal{G}$ of order $n$, is an $n\times n$ matrix

$$d_{\mathit{ii}}=\sum _{i\ne j}{a}_{\mathit{ij}},$$E2

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

The Laplacian matrix $\mathbf{L}=\left[l_{\mathit{ij}}\right]$ of digraph $\mathcal{G}$ of order $n$, is the $n\times n$ matrix

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

An important property of any Laplacian $\mathbf{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\left(i=1,\cdots ,n\right)$ are represented by vertices of the graph, while the edges of the graph represent communication links between them. Let $x^i$ denote the state of a vehicle $i$ and $\mathbf{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}}\left(t\right)=-\mathbf{L}\left(\mathbf{x}\left(t\right)-{\mathbf{x}}^{\mathit{off}}\right)\mathit{.}$$E4

We determine that consensus has been achieved when $‖x^i-x^j‖\to {\left(x^{\mathit{ij}}\right)}^{\mathit{off}}$ as $t\to \infty$, $\forall \hspace{0.17em}i\ne j$. 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

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 $x^i\left(t_0\right)\in {\mathbb{R}}^3$, $i=1,\cdots ,n$ (referenced to a coordinate frame centered on the centroid of the sphere), a set of obstacles $x_{\mathit{obs}}^j\in {\mathbb{R}}^3$, $j=1,\cdots ,m$, and the Laplacian matrix of their communication graph $\mathbf{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 $\mathbf{0}$ which implies that vectors $x^i$ and $x_{\mathit{obs}}^j$ 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 ${\theta }^{\mathit{ij}}$, while ${\phi }^{\mathit{ik}}$ is the angle between vehicle $i$ and obstacle $k$. The control problem is to drive all $x^i$ to a consensus position or to a formation while avoiding each other and the $x_{\mathit{obs}}^j$ 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.

Figure 1.

Constrained position control on a unit sphere.

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.

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 $\tau$ is the number of vehicles in the neighborhood of $i$ that it can communicate with, then $i,\left(i=1,\cdots ,n\right)$ individually synthesizes a Laplacian-like stochastic matrix ${\mathcal{L}}^i$ so that all $x^i$ are driven to consensus on the unit sphere. The synthesis of ${\mathcal{L}}^i$ is as follows. A semidefinite matrix variable, ${\mathrm{\Lambda }}^i\in {\mathcal{S}}^3$ for each $i$ is generated. Then

$$\begin{array}{ccc}{\mathcal{L}}^i\left(t\right)& =& \left[\tau {\mathrm{\Lambda }}_1^i\left(t\right)-{\mathrm{\Lambda }}_2^i\left(t\right)\cdots {\mathrm{\Lambda }}_{\tau }^i\left(t\right)\right],\\ {\dot{x}}^i\left(t\right)& =& \left[\begin{array}{cccc}\tau {\mathrm{\Lambda }}_1^i\left(t\right)& -{\mathrm{\Lambda }}_2^i\left(t\right)& \cdots & {\mathrm{\Lambda }}_{\tau }^i\left(t\right)\end{array}\right]{\left[x_1^T\left(t\right) x_2^T\left(t\right)\cdots x_{\tau }^T\left(t\right)\right]}^T\\ & =& -{\mathcal{L}}^i\left(t\right){\left[x_1^T\left(t\right) x_2^T\left(t\right)\cdots x_{\tau }^T\left(t\right)\right]}^T,\end{array}$$E5

where $x_i^T\left(t\right),i=1,\cdots ,\tau$ 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}\left(t\right)=\underset{\mathrm{\Lambda }\left(t\right)}{\underbrace{\left[\begin{array}{ccc}{\mathrm{\Lambda }}^1\left(t\right)& \cdots & 0\\ ⋮& \ddots & ⋮\\ 0& \cdots & {\mathrm{\Lambda }}^n\left(t\right)\end{array}\right]}}\underset{\mathrm{\Gamma }=\mathbf{L}\otimes {\mathbf{I}}_3}{\underbrace{\left[\begin{array}{ccc}{l}_{11}{\mathbf{I}}_3& \cdots & l_{1n}{\mathbf{I}}_3\\ ⋮& \ddots & ⋮\\ l_{n1}{\mathbf{I}}_3& \cdots & l_{\mathit{nn}}{\mathbf{I}}_3\end{array}\right]}},$$E6

where, $\mathbf{L}=\left[l_{\mathit{ij}}\right],\left(i,j=1,\cdots ,n\right)$ is the collective Laplacian matrix. Note that any ${\mathrm{\Lambda }}^i$ 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}}\left(t\right)=-\mathcal{L}\left(t\right)\mathbf{x}\left(t\right).$$E7

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

$$x_{k+1}^i=x_k^i-\mathrm{∆}t{\mathcal{L}}^i\left(t\right)x_k^i,$$E8

$${\mathbf{x}}_{k+1}={\mathbf{x}}_k-\mathrm{∆}t{\dot{\mathbf{x}}}_k={\mathbf{x}}_k-\mathrm{∆}t\mathcal{L}\left(t\right){\mathbf{x}}_k$$E9

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(x^i\right)}_k^T\left(x_{k+1}^i-x_k^i\right)=0.$$E10

Eq. (10) is the discrete time version of $x^i{\left(t\right)}^T{\dot{x}}^i\left(t\right)=0$ or $\mathbf{x}{\left(t\right)}^T\dot{\mathbf{x}}\left(t\right)=0$, which guarantees that $x^i{\left(t\right)}^T{x}^i\left(t\right)=1$ or $\mathbf{x}{\left(t\right)}^T\mathbf{x}\left(t\right)=n$ for $n$ vehicles, iff $‖x^i\left(0\right)‖=1\forall \hspace{0.17em}i$. Eq. (8) drives the positions $x^i\left(0\right)$ 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 $\dot{\mathbf{x}}\left(t\right)=-\mathbf{Lx}\left(t\right)$ 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 $\mathbf{L}$ is connected. Note that this proof had already been presented in [20].

Proof: Note that if x belongs to the consensus space $\mathcal{C}=\left\{\mathbf{x}|x^1=x^2=,\cdots ,=x^n\right\}$, then $\dot{\mathbf{x}}=\mathbf{0}$, (i.e. all vehicles have stopped moving). Because $\mathcal{C}$ is the nullspace of $\mathcal{L}\left(t\right)$, where $\mathcal{L}\left(t\right)\mathbf{x}=0$ $\forall \mathbf{x}$. Meaning that once $\mathbf{x}$ enters $\mathcal{C}$ it stays there since there is no more motion. If consensus has not been achieved then $\mathbf{x}\notin \mathcal{C}$, consider a Lyapunov candidate function $\mathrm{V}={\mathbf{x}}^T\mathrm{\Gamma }\mathbf{x}$; $\mathrm{V}>0$ unless $\mathbf{x}\in \mathcal{C}$. Then,

$$\begin{array}{ccc}\dot{V}& =& {\mathbf{x}}^T\mathrm{\Gamma }\dot{\mathbf{x}}+{\dot{\mathbf{x}}}^T\mathrm{\Gamma }\mathbf{x}\mathit{,}\\ & =& -{\mathbf{x}}^T\mathrm{\Gamma }\mathcal{L}\left(t\right)\mathbf{x}-{\mathbf{x}}^T\mathcal{L}{\left(t\right)}^T\mathrm{\Gamma }\mathbf{x}\mathit{,}\\ & =& -{\mathbf{x}}^T\mathit{\Gamma \Lambda }\left(t\right)\mathrm{\Gamma }\mathbf{x}-{\mathbf{x}}^T\mathrm{\Gamma \Lambda }\left(t\right)\mathrm{\Gamma }\mathbf{x}\mathit{,}\\ & =& -2{\mathbf{x}}^T\mathrm{\Gamma \Lambda }\left(t\right)\mathrm{\Gamma }\mathbf{x}\mathbf{,}\\ & =& -2z^T\mathit{\Lambda }\left(t\right)z,\end{array}$$E11

where $z=\mathrm{\Gamma }\mathbf{x}\ne 0$ for $\mathbf{x}\notin \mathcal{C}$. This implies that $\mathbf{x}$ approaches a point in $\mathcal{C}$ as $t\to \infty$, which proves the claim. Eq. (11) is true for as long as $\mathbf{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 $x^1\left(t\right)$, $x^2\left(t\right)$ and $x^3\left(t\right)$ to avoid two constraint regions around $x_{\mathit{obs}}^1$ and $x_{\mathit{obs}}^2$. The obstacle regions are defined by cones, whose base radii are $r^1$ and $r^2$, respectively. Let the angle between vehicles $i$ and $j$ be ${\theta }^{\mathit{ij}}$, and that between vehicle $i$ and obstacle $k$ be ${\phi }^{\mathit{ik}}$. Then the requirements for collision avoidance are: ${\phi }^{11}\ge {\alpha }^1$, ${\phi }^{21}\ge {\alpha }^1$, ${\phi }^{31}\ge {\alpha }^1$, and ${\phi }^{12}\ge {\alpha }^2$, ${\phi }^{22}\ge {\alpha }^2$, ${\phi }^{32}\ge {\alpha }^2$, $\forall t\in \left[t_0,t_f\right]$. They have the following equivalent quadratic constraints:

$$x^1{\left(t\right)}^T{x}_{\mathit{obs}}^1\le \text{cos}{\alpha }^1,$$E12

$$x^2{\left(t\right)}^T{x}_{\mathit{obs}}^1\le \text{cos}{\alpha }^1,$$E13

$$x^3{\left(t\right)}^T{x}_{\mathit{obs}}^1\le \text{cos}{\alpha }^1,$$E14

$$x^1{\left(t\right)}^T{x}_{\mathit{obs}}^2\le \text{cos}{\alpha }^2,$$E15

$$x^2{\left(t\right)}^T{x}_{\mathit{obs}}^2\le \text{cos}{\alpha }^2,$$E16

$$x^3{\left(t\right)}^T{x}_{\mathit{obs}}^2\le \text{cos}{\alpha }^2.$$E17

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}}^{\mathrm{T}}-\mathbf{Q}\le 0$$E18

where $\mathbf{Q}={\mathbf{Q}}^{\mathrm{T}}$, $\mathbf{R}={\mathbf{R}}^{\mathrm{T}}$, and $\mathbf{R}>0$, is equivalent to, and can be represented by the linear matrix inequality

$$\left[\begin{array}{cc}\mathbf{Q}& \mathbf{S}\\ {\mathbf{S}}^{\mathrm{T}}& \mathbf{R}\end{array}\right]\ge 0.$$E19

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

$$\underset{{\mathbf{x}}^{\mathbf{1}}{\left(t\right)}^{\mathit{T}}}{\underbrace{\left[\begin{array}{cc}{x}^1{\left(t\right)}^T& x_{\mathit{obs}}^1{}^T\end{array}\right]}}\left[\begin{array}{cc}{\mathbf{0}}_3& \frac{1}{2}{\mathbf{I}}_3\\ \frac{1}{2}{\mathbf{I}}_3& {\mathbf{0}}_3\end{array}\right]\underset{{\mathbf{x}}^{\mathbf{1}}\left(t\right)}{\underbrace{\left[\begin{array}{c}{x}^1\left(t\right)\\ x_{\mathit{obs}}^1\end{array}\right]}}\le \mathit{cos}{\alpha }^1$$E20

Multiply Eq. (20) by 2 and we have

$${\mathbf{x}}^{\mathbf{1}}{\left(t\right)}^{\mathit{T}}\underset{\mathbf{G}}{\underbrace{\left[\begin{array}{cc}{\mathbf{0}}_3& {\mathbf{I}}_3\\ {\mathbf{I}}_3& {\mathbf{0}}_3\end{array}\right]}}{\mathbf{x}}^{\mathbf{1}}\left(t\right)\le 2\mathit{cos}{\alpha }^1.$$E21

We desire a positive definite $\mathbf{G}\mathbf{,}$ i.e. $\mathbf{G}>\mathbf{0}\mathbf{,}$ or whose eigenvalues are all nonnegative (this is synonymous with $\mathbf{R}$ in the Schur inequality). To make $\mathbf{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 $\mathbf{G}$, then

$${\mathbf{x}}^{\mathbf{1}}{\left(t\right)}^{\mathit{T}}\left(\mu {\mathbf{I}}_6+\left[\begin{array}{cc}{\mathbf{0}}_3& {\mathbf{I}}_3\\ {\mathbf{I}}_3& {\mathbf{0}}_3\end{array}\right]\right){\mathbf{x}}^{\mathbf{1}}\left(t\right)\le 2\left(\mathit{cos}{\alpha }^1+\mu \right).$$E22

Let $\mathbf{M}={\left(\mu {\mathbf{I}}_6+\left[\begin{array}{cc}{\mathbf{0}}_3& {\mathbf{I}}_3\\ {\mathbf{I}}_3& {\mathbf{0}}_3\end{array}\right]\right)}^{-1}$, then M is positive definite. Thus, following the Schur’s complement formula, the LMI equivalent of Eq. (12) becomes

$$\left[\begin{array}{ll}2\left(\mathit{cos}{\alpha }^1+\mu \right)& {\underbrace{\left[\begin{array}{l}{x}^1\left(t\right)\\ x_{\mathit{obs}}^1\end{array}\right]}}^T\\ \left[\begin{array}{l}{x}^1\left(t\right)\\ x_{\mathit{obs}}^1\end{array}\right]& \mathbf{M}\end{array}\right]\ge 0,$$E23

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

$$\left[\begin{array}{ll}2\left(\mathit{cos}{\alpha }^1+\mu \right)& {\underbrace{\left[\begin{array}{l}{x}^1\left(k+1\right)\\ x_{\mathit{obs}}^1\end{array}\right]}}^T\\ \underbrace{\left[\begin{array}{l}{x}^1\left(k+1\right)\\ x_{\mathit{obs}}^1\end{array}\right]}& \mathbf{M}\end{array}\right]\ge 0,$$E24

$$\left[\begin{array}{ll}2\left(\mathit{cos}{\alpha }^1+\mu \right)& {\underbrace{\left[\begin{array}{l}{x}^2\left(k+1\right)\\ x_{\mathit{obs}}^1\end{array}\right]}}^T\\ \underbrace{\left[\begin{array}{l}{x}^2\left(k+1\right)\\ x_{\mathit{obs}}^1\end{array}\right]}& \mathbf{M}\end{array}\right]\ge 0,$$E25

$$\left[\begin{array}{ll}2\left(\mathit{cos}{\alpha }^1+\mu \right)& {\underbrace{\left[\begin{array}{l}{x}^3\left(k+1\right)\\ x_{\mathit{obs}}^1\end{array}\right]}}^T\\ \underbrace{\left[\begin{array}{l}{x}^3\left(k+1\right)\\ x_{\mathit{obs}}^1\end{array}\right]}& \mathbf{M}\end{array}\right]\ge 0,$$E26

$$\left[\begin{array}{ll}2\left(\mathit{cos}{\alpha }^2+\mu \right)& {\underbrace{\left[\begin{array}{l}{x}^1\left(k+1\right)\\ x_{\mathit{obs}}^2\end{array}\right]}}^T\\ \underbrace{\left[\begin{array}{l}{x}^1\left(k+1\right)\\ x_{\mathit{obs}}^2\end{array}\right]}& \mathbf{M}\end{array}\right]\ge 0,$$E27

$$\left[\begin{array}{ll}2\left(\mathit{cos}{\alpha }^2+\mu \right)& {\underbrace{\left[\begin{array}{l}{x}^2\left(k+1\right)\\ x_{\mathit{obs}}^2\end{array}\right]}}^T\\ \underbrace{\left[\begin{array}{l}{x}^2\left(k+1\right)\\ x_{\mathit{obs}}^2\end{array}\right]}& \mathbf{M}\end{array}\right]\ge 0,$$E28

$$\left[\begin{array}{ll}2\left(\mathit{cos}{\alpha }^2+\mu \right)& {\underbrace{\left[\begin{array}{l}{x}^3\left(k+1\right)\\ x_{\mathit{obs}}^2\end{array}\right]}}^T\\ \underbrace{\left[\begin{array}{l}{x}^3\left(k+1\right)\\ x_{\mathit{obs}}^2\end{array}\right]}& \mathbf{M}\end{array}\right]\ge 0.$$E29

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 $x_{\mathit{obs}}$, with $\alpha ={30}^{\mathrm{o}}$.

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^1,x^1,x^3,x^4$ from initial to final positions.

4.3. Formulation of formation control on the unit sphere

Formation patterns are obtained by specifying a minimum angular separation of ${\beta }^{\mathit{ij}}\left(t\right)$ between any two vehicles $i$ and $j$ thereby defining relative spacing between individual vehicles. Using the avoidance strategy formerly described, the constraint ${\theta }^{\mathit{ij}}\ge {\beta }^{\mathit{ij}}\forall \hspace{0.17em}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\left(n-2\right)=\frac{n!}{\left(n-2\right)!}$ 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 $\alpha ={30}^{\mathrm{o}}$ and the minimum angular separations between the vehicles is set at a constant value ${\beta }^{\mathit{ij}}={20}^{\mathrm{o}}\forall \hspace{0.17em}i,j$. Therefore, in addition to the four static obstacle avoidance constraints (such as Eqs. (24) to (29), with ${\alpha }^1=\alpha$), each vehicle has three more intervehicle collision avoidance constraints such as

$$\left[\begin{array}{ll}2\left(\mathit{cos}{\beta }^{\mathit{ij}}+\mu \right)& {\underbrace{\left[\begin{array}{l}{x}^i\left(k+1\right)\\ x^j\left(k+1\right)\end{array}\right]}}^T\\ \underbrace{\left[\begin{array}{l}{x}^i\left(k+1\right)\\ x^j\left(k+1\right)\end{array}\right]}& \mathbf{M}\end{array}\right]\ge 0,$$E30

Figure 3.

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

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.

$\forall \hspace{0.17em}i,j\left(i\ne j\right)$.

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 $x^i\left(t_0\right),\left(i=1\cdots n\right)$ and the plant Eq. (5) for each vehicle, find a feasible sequence of trajectories that satisfies the following constraints:

$$\begin{array}{ll}{x}_{k+1}^i=x_k^i-\mathrm{∆}t{\mathcal{L}}^i\left(t\right)x_k^i,& \text{dynamics constraint}\\ {\left(x^i\right)}_k^T\left(x_{k+1}^i-x_k^i\right)=0,& \text{norm}\text{constraint}\\ \left[\begin{array}{ll}2\left(\mathit{cos}{\alpha }^{\mathit{ij}}+\mu \right)& {\underbrace{\left[\begin{array}{l}{x}^i\left(k+1\right)\\ x_{\mathit{obs}}^j\end{array}\right]}}^T\\ \underbrace{\left[\begin{array}{l}{x}^i\left(k+1\right)\\ x_{\mathit{obs}}^j\end{array}\right]}& \mathbf{M}\end{array}\right]\ge 0,& \text{static obstacle avoidance constraint}\\ \left[\begin{array}{ll}2\left(\mathit{cos}{\beta }^{\mathit{ij}}+\mu \right)& {\underbrace{\left[\begin{array}{l}{x}^i\left(k+1\right)\\ x^j\left(k+1\right)\end{array}\right]}}^T\\ \underbrace{\left[\begin{array}{l}{x}^i\left(k+1\right)\\ x^j\left(k+1\right)\end{array}\right]}& \mathbf{M}\end{array}\right]\ge 0,& \text{intervehicle avoidance constraint}\end{array}$$

where $x_{k+1}^i$ and ${\mathrm{\Lambda }}_k^i$ (which are components of ${\mathcal{L}}^i$) are the optimization variables. They are declared as SDP variables where ${\mathrm{\Lambda }}_k^i$ shapes the trajectories $x_{k+1}^i$ 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 leader-follower 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.

Figure 5.

Multiple virtual leaders graph topology with an undirected topology.

If $x_v^i\left(t\right)$ is the state of a virtual leader corresponding to vehicle $i$, then for each leader-follower vehicle pair ${\mathbf{x}}^i\left(t\right)={\left[x_v^i{\left(t\right)}^T x^i{\left(t\right)}^T\right]}^T$ the corresponding leader-follower Laplacian matrix is

$${\mathbf{L}}^t\left(t\right)=\left[\begin{array}{cc}0& 0\\ 0& -1\end{array}\right]\mathbf{.}$$E31

The corresponding collective dynamics of ${\mathbf{x}}^i\left(t\right)$ is

$${\dot{\mathbf{x}}}^i\left(t\right)=-\left[\begin{array}{cc}{\mathrm{\Lambda }}^i\left(t\right)& \mathbf{0}\\ \mathbf{0}& {\mathrm{\Lambda }}^i\left(t\right)\end{array}\right]\left({\mathbf{L}}^t\left(t\right)\otimes {\mathbf{I}}_3\right){\mathbf{x}}^i\left(t\right).$$E32

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

5. Simulation results

Due to limitation of space, two simulation results are presented for consensus with collision avoidance on the unit sphere, more simulation results are in [18, 20]. The first experiment is to test formation acquisition with avoidance on the sphere. The second experiment is to test arbitrary reconfigurations on the sphere with collision avoidance. Three different communication topologies used are shown in Figure 4. In 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.

Optimization software Sedumi [22] and Yalmip [23] running inside Matlab R2009a, were used for solving all the problems. The simulations were done on an Intel R Core(TM)2 Duo P8600 @ 2.40GHz with 2 GB RAM, running Windows 7.

5.1. Formation acquisition on the unit sphere with avoidance

In this experiment, ten vehicles converge to a formation on the sphere, which is realized by maintaining a relative spacing with each other while also avoiding a static obstacle, with $\alpha ={30}^{\mathrm{o}}$. Angle ${\beta }^{\mathit{ij}}={20}^{\mathrm{o}}$ is set to maintain the relative spacing between the vehicles $\forall \hspace{0.17em}i,j=1\cdots 10,i\ne j$. The initial positions are:

$$\begin{array}{ccc}{x}^1\left(0\right)& =& {\left[0.34170.55550.7581\right]}^T\\ x^2\left(0\right)& =& {\left[0.49600.12700.8589\right]}^T\\ x^3\left(0\right)& =& {\left[0.30450.94970.0730\right]}^T\\ x^4\left(0\right)& =& {\left[0.57350.79520.1967\right]}^T\\ x^5\left(0\right)& =& {\left[0.80050.38670.4580\right]}^T\\ x^6\left(0\right)& =& {\left[0.37270.73720.5637\right]}^T\\ x^7\left(0\right)& =& {\left[0.03550.51170.8585\right]}^T\\ x^8\left(0\right)& =& {\left[0.65530.74280.1371\right]}^T\\ x^9\left(0\right)& =& {\left[0.91880.24460.3094\right]}^T\\ x^{10}\left(0\right)& =& {\left[0.02610.87730.4792\right]}^T\end{array}$$

The result for Topology 1 is shown in Figure 6 (left), while the right figure shows the result obtained using Topology 3 – a cyclic graph which produces a circulant Laplacian L, whose dynamics leads to swirling motion. The proof is in [18].

Figure 6.

Ten-vehicle formation acquisition using topology 1 (left), and using topology 3 (right).

When relative spacing are specified between the vehicles, the motion obtained from this Laplacian is like the result obtained in [11]. However, when there is no relative spacing specified, the circular motion converges to a point. Using a circulant matrix such as that of Topology 3, one can vary the radius of the circular formation achieved $\left(r=\text{cos}\hspace{0.17em}{\theta }^{\mathit{ij}}\right)$; by setting ${\theta }^{\mathit{ij}}$ equal for all $i,j$ and varying its size with time. If the magnitude of angle $\theta$ is reduced, the radius of the circular formation structure obtained also reduces, and vice versa. Figure 7 (left) shows the result for setting ${\theta }^{\mathit{ij}}={30}^{\mathrm{o}}\forall \hspace{0.17em}i,j$ for four vehicles. The center and right figures show the results for ten vehicles as ${\theta }^{\mathit{ij}}$ moves gradually from ${20}^{\mathrm{o}}$ toward $0^{\mathrm{o}}$. When ${\theta }^{\mathit{ij}}=0\forall \hspace{0.17em}i,j$, the vehicles rendezvous to a point.

Figure 7.

Four-vehicle formation acquisition using topology 3 with ${\beta }^{\mathit{ij}}={30}^{\mathrm{o}}$ (left), and ten-vehicle formation acquisition, using topology 3, with ${\beta }^{\mathit{ij}}={20}^{\mathrm{o}}$ (center) and ${\beta }^{\mathit{ij}}=0^{\mathrm{o}}$ (right).

5.2. Collision free reconfiguration on the unit sphere with avoidance of no-fly zones

This is a more traditional reconfiguration problem which we try to solve by using the consensus based protocols presented in this chapter. Three flying vehicles (e.g. UAVs), are required to fly from their initial positions to given final positions. There are cross-paths (inter-vehicle collision constraints) in addition to no-fly zones (static obstacle constraints), between the initial and final positions. The initial positions are:

$$\begin{array}{ccc}{x}_0^1\left(0\right)& =& {\left[0.86590-0.4999\right]}^T\\ x_0^2\left(0\right)& =& {\left[0.4165-0.57210.7071\right]}^T\\ x_0^3\left(0\right)& =& {\left[-0.5878-0.8090\right]}^T\end{array}$$

The desired final positions are:

$$\begin{array}{ccc}{x}_f^1\left(0\right)& =& {\left[-0.4330-0.74990.4999\right]}^T\\ x_f^2\left(0\right)& =& {\left[-0.2939-0.9045-0.309\right]}^T\\ x_f^3\left(0\right)& =& {\left[0.9393-0.30520.1564\right]}^T\end{array}$$

For inter-vehicle collision avoidance, they are required to maintain a minimum safety distance of $r=\text{cos}{10}^{\mathrm{o}}$ units. Five no-fly zones are imposed on the vehicles at the following positions:

$$\begin{array}{ccc}{x}_{\mathit{obs}}^1& =& {\left[0.5237-0.72080.454\right]}^T\\ x_{\mathit{obs}}^2& =& {\left[0.2939-0.9045-0.309\right]}^T\\ x_{\mathit{obs}}^3& =& {\left[0-0.98770.1564\right]}^T\\ x_{\mathit{obs}}^4& =& {\left[0.5878-0.8090\right]}^T\\ x_{\mathit{obs}}^5& =& {\left[0-0.95110.309\right]}^T\end{array}$$

The radii of the no-fly zones are equal to $r$, therefore ${\beta }^{\mathit{ij}}={\alpha }^{\mathit{ij}}={10}^{\mathrm{o}}\forall \hspace{0.17em}i,j\left(i\ne j\right)$ for this simulation. The result is shown in Figure 8.

Figure 8.

Three-vehicle reconfiguration with collision avoidance and avoidance of no-fly zones.

On a final note, we have attempted to solve the problem of consensus in a spherical coordinate system by solving in on the unit sphere. The same unit sphere was used in [11]. This is convenient because the results are easier to visualize and compute on the unit sphere. The results presented here can be applied directly to real-life planetary navigation problems such as horizontal separation of aircraft [18, 20], simply by transforming actual position vectors into unit vectors in the unit sphere, solving to obtain the solution trajectories, and transforming the solutions back to actual desired trajectories in the real-world coordinates. The unit of measurement for implementation will therefore depend on the application at hand.