Frequently used notation in this chapter.

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

### Keywords

- consensus
- path planning
- avoidance
- optimization
- LMI

## 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 *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 |
---|---|

Number of vehicles | |

Vehicle number | |

Position vector of vehicle | |

Unit vector corresponding to vector | |

Control input of vehicle | |

Obstacle vector number | |

Offset vector between vehicles | |

Angle between vehicle | |

Angle between vehicles | |

Minimum angular separation from obstacle number | |

Minimum angular separation between vehicles | |

Stacked vector of | |

Stacked vector of | |

Laplacian matrix, | |

Laplacian-like stochastic matrix | |

A vector consisting of all zeros | |

Kronecker multiplication operator | |

Special Euclidean group | |

The set of | |

The | |

A positive definite matrix variable, | |

The consensus space for | |

Graph | |

Set of vertices of | |

Set of edges of | |

Vertex | |

Endpoint or edge | |

Neighbors of | |

Adjacency matrix of | |

Out-degree matrix of | |

A vector or matrix in the Schur’s inequality | |

A positive definite matrix in the Schur’s inequality | |

A symmetric matrix in the Schur’s inequality | |

A positive definite matrix variable | |

A positive semidefinite matrix |

## 2. Mathematical background

This section briefly describes the mathematical basis of consensus theory.

### 2.1. Basic graph theory

We define a graph *vertices* *edges,* *endpoints*, *incident* with vertices *undirected* if for every edge connecting two vertices, communication between the vertices is possible in both directions across the edge, i.e. *directed graph* (*digraph*), and it is *symmetric*. The quantity *order*, and *size*, respectively, of *neighbors* of node **incident with** vertex *degree* or *valence* of *directed* edges **incident into** *In-degree* of *Out-degree* is similarly defined as the number of edges **incident out** of the

We define the *adjacency matrix*

For the undirected graph *always symmetric*, while *out-degree matrix*

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

The *Laplacian matrix*

An important property of any Laplacian

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

We determine that consensus has been achieved when

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

The problem is illustrated in Figure 1; the unit sphere is centered on

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 *stochastic matrix*

where

where,

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

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

Eq. (10) is the discrete time version of

Theorem 1: As long as the associated (static) communication graph of **L** has a spanning tree, the strategy **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

**Proof:** Note that if x belongs to the *consensus space*

where

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

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

where

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

Multiply Eq. (20) by 2 and we have

We desire a positive definite *μ* which is larger than the largest absolute value of the eigenvalues of

Let **M** is positive definite. Thus, following the Schur’s complement formula, the LMI equivalent of Eq. (12) becomes

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

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

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

Formation patterns are obtained by specifying a minimum angular separation of

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

where

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

If

The corresponding collective dynamics of

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

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

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

### 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:

The desired final positions are:

For inter-vehicle collision avoidance, they are required to maintain a minimum safety distance of

The radii of the no-fly zones are equal to

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.