Open access peer-reviewed chapter

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

By Innocent Okoloko

Submitted: May 16th 2017Reviewed: September 22nd 2017Published: September 26th 2018

DOI: 10.5772/intechopen.71216

Downloaded: 515


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.


  • 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 SE3: (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 spacesand 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 neighborswithin its sensor view. Each vehicle can therefore use the Laplacian matrixof the communication graph Lin 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 constrainedand unconstrainedspaces 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.

nNumber of vehicles
iVehicle number i
xiPosition vector of vehicle i
x̂Unit vector corresponding to vector x
ui,ẋiControl input of vehicle i
xobsjObstacle vector number j
xijoffOffset vector between vehicles iand j
φijAngle between vehicle iand obstacle j
θijAngle between vehicles iand j
αiMinimum angular separation from obstacle number i
βijMinimum angular separation between vehicles iand j
xStacked vector of nposition vectors
u,ẋStacked vector of ncontrol inputs
LLaplacian matrix, L=DGAG
L,LiLaplacian-like stochastic matrix
0A vector consisting of all zeros
Kronecker multiplication operator
SE3Special Euclidean group
SmThe set of m×mpositive definite matrices
InThe n×nidentity matrix
ΛA positive definite matrix variable, ΛSm
CThe consensus space for x,C=xx1=x2==xn
VSet of vertices of G
ESet of edges of G
viVertex viV
vivjEndpoint or edge vivjE
NiNeighbors of vi; Ni=vjV:vivjE
AGAdjacency matrix of G; AG=aij
DGOut-degree matrix of G; DG=dij
SA vector or matrix in the Schur’s inequality
RA positive definite matrix in the Schur’s inequality
QA symmetric matrix in the Schur’s inequality
MA positive definite matrix variable
GA 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 Gas a pair VEconsisting of two finite sets having elements; a set of points called verticesV=12n, and a set of connecting lines called edges,Evivj:vivjVjior endpoints, Eijor vivj, of the vertices [15]. Thus, an edge is incidentwith vertices viand vj. Graph Gis said to be undirectedif for every edge connecting two vertices, communication between the vertices is possible in both directions across the edge, i.e. vivjEimplies vjviE; otherwise it is called a directed graph(digraph), and it is symmetric. The quantity Vis called the order, and Ethe size, respectively, of G. The set of neighborsof node viis denoted by Ni=vjV:vivjE. The number of edges incident withvertex vis called the degreeor valenceof v. Furthermore, the number of directededges incident intovis called the In-degreeof v, while the Out-degreeis similarly defined as the number of edges incident outof the v.

We define the adjacency matrixAG=aijof Gof order nas the n×nmatrix


For the undirected graph AGis always symmetric, while AGof a digraph G is symmetric if and only if Gis symmetric. The out-degree matrixDG=dijof Gof order n, is an n×nmatrix


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

The Laplacian matrixL=lijof digraph Gof order n, is the n×nmatrix


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

2.2. Basic consensus theory

The basic consensusproblem 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) ii=1nare represented by vertices of the graph, while the edges of the graph represent communication links between them. Let xidenote the state of a vehicle iand xis 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]


We determine that consensus has been achieved when xixjxijoffas t, ij. 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 xit0R3, i=1,,n(referenced to a coordinate frame centered on the centroid of the sphere), a set of obstacles xobsjR3, 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 0which implies that vectors xiand xobsjare unit vectors and must be kept so throughout the evolution of the trajectory vectors. The angle between the position vectors of vehicles iand jis θij, while φikis the angle between vehicle iand obstacle k. The control problem is to drive all xito a consensus position or to a formation while avoiding each other and the xobsjalong 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: consensusand 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, swarmingand 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 trajectoriesfor each vehicle on the sphere, which satisfies normand 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 ican communicate with at least one other neighboring vehicle. Given that τis the number of vehicles in the neighborhood of ithat it can communicate with, then i,i=1nindividually synthesizes a Laplacian-like stochastic matrixLiso that all xiare driven to consensus on the unit sphere. The synthesis of Liis as follows. A semidefinite matrix variable, ΛiS3for each iis generated. Then

Lit=τΛ1itΛ2itΛτit,ẋit=τΛ1itΛ2itΛτitx1Tt x2TtxτTtT=Litx1Tt x2TtxτTtT,E5

where xiTt,i=1,,τare the position vectors of vehicles that iis communicating with at time t. For the purpose of analysis, the collective description for nvehicles is given as


where, L=lij,ij=1nis the collective Laplacian matrix. Note that any Λiis 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


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 ias


Eq. (10) is the discrete time version of xitTẋit=0or xtTẋt=0, which guarantees that xitTxit=1or xtTxt=nfor nvehicles, iff xi0=1i. Eq. (8) drives the positions xi0to 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 Lhas a spanning tree, the strategy ẋt=Lxtachieves 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 Lis connected. Note that this proof had already been presented in [20].

Proof:Note that if x belongs to the consensus spaceC=xx1=x2==xn, then ẋ=0, (i.e. all vehicles have stopped moving). Because Cis the nullspace of Lt, where Ltx=0x. Meaning that once xenters Cit stays there since there is no more motion. If consensus has not been achieved then xC, consider a Lyapunov candidate function V=xTΓx; V>0unless xC. Then,


where z=Γx0for xC. This implies that xapproaches a point in Cas t, which proves the claim. Eq. (11) is true for as long as Lis 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 x1t, x2tand x3tto avoid two constraint regions around xobs1and xobs2. The obstacle regions are defined by cones, whose base radii are r1and r2, respectively. Let the angle between vehicles iand jbe θij, and that between vehicle iand obstacle kbe φik. Then the requirements for collision avoidance are: φ11α1, φ21α1, φ31α1, and φ12α2, φ22α2, φ32α2, tt0tf. They have the following equivalent quadratic constraints:


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 Q=QT, R=RT, and R>0, is equivalent to, and can be represented by the linear matrix inequality


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 G,i.e. G>0,or whose eigenvalues are all nonnegative (this is synonymous with Rin the Schur inequality). To make Gpositive 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


Let M=μI6+03I3I3031, then Mis 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 xobs, with α=30o.

Figure 2.

Four-vehicle rendezvous on a unit sphere with collision avoidance of a static obstacle. The figure shows the evolution of thex,y,zpositions of the four vehiclesx1,x1,x3,x4from 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 βijtbetween any two vehicles iand jthereby defining relative spacing between individual vehicles. Using the avoidance strategy formerly described, the constraint θijβiji,jis 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 nvehicles, the avoidance requirements result in extra Pn2=n!n2!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 α=30oand the minimum angular separations between the vehicles is set at a constant value βij=20oi,j. Therefore, in addition to the four static obstacle avoidance constraints (such as Eqs. (24) to (29), with α1=α), each vehicle has three more intervehicle collision avoidance constraints such as


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 thex,y,zpositions of the four vehiclesx1,x1,x3,x4from initial to final positions.

Figure 4.

Topology 1 (left) is afully connectedcommunication graph withnoleader, topology 2 (center) is acycliccommunication graph withoneleader, node 1, and topology 3 (right) is acycliccommunication graph withnoleader.


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 xit0,i=1nand the plant Eq. (5) for each vehicle, find a feasible sequence of trajectories that satisfies the following constraints:

xk+1i=xkitLitxki,dynamics constraintxikTxk+1ixki=0,normconstraint2cosαij+μxik+1xobsjTxik+1xobsjM0,static obstacle avoidance constraint2cosβij+μxik+1xjk+1Txik+1xjk+1M0,intervehicle avoidance constraint

where xk+1iand Λki(which are components of Li) are the optimization variables. They are declared as SDP variables where Λkishapes the trajectories xk+1ito 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 xvitis the state of a virtual leader corresponding to vehicle i, then for each leader-follower vehicle pair xit=xvitT xitTTthe corresponding leader-follower Laplacian matrix is


The corresponding collective dynamics of xitis


This configuration was applied in the reconfiguration experiment in Section 5.2. Practical application of this strategy to the problem of separationin 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 connectedcommunication graph with noleader, Topology 2 (center) is a cycliccommunication graph with oneleader, node 1, and Topology 3 (right) is a cycliccommunication graph with noleader.

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 α=30o. Angle βij=20ois set to maintain the relative spacing between the vehicles i,j=110,ij. The initial positions are:


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 circulantLaplacian 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 r=cosθij; by setting θijequal for all i,jand varying its size with time. If the magnitude of angle θis reduced, the radius of the circular formation structure obtained also reduces, and vice versa. Figure 7 (left) shows the result for setting θij=30oi,jfor four vehicles. The center and right figures show the results for ten vehicles as θijmoves gradually from 20otoward 0o. When θij=0i,j, the vehicles rendezvous to a point.

Figure 7.

Four-vehicle formation acquisition using topology 3 withβij=30o(left), and ten-vehicle formation acquisition, using topology 3, withβij=20o(center) andβij=0o(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:


The desired final positions are:


For inter-vehicle collision avoidance, they are required to maintain a minimum safety distance of r=cos10ounits. Five no-fly zones are imposed on the vehicles at the following positions:


The radii of the no-fly zones are equal to r, therefore βij=αij=10oi,jijfor 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.

© 2018 The Author(s). Licensee IntechOpen. This chapter is distributed under the terms of the Creative Commons Attribution 3.0 License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

How to cite and reference

Link to this chapter Copy to clipboard

Cite this chapter Copy to clipboard

Innocent Okoloko (September 26th 2018). Multi-Path Planning on a Sphere with LMI-Based Collision Avoidance, Advanced Path Planning for Mobile Entities, Rastislav Róka, IntechOpen, DOI: 10.5772/intechopen.71216. Available from:

chapter statistics

515total chapter downloads

More statistics for editors and authors

Login to your personal dashboard for more detailed statistics on your publications.

Access personal reporting

Related Content

This Book

Next chapter

Multi-Spacecraft Attitude Path Planning Using Consensus with LMI-Based Exclusion Constraints

By Innocent Okoloko

Related Book

First chapter

Software‐Defined Optical Networking (SDON): Principles and Applications

By Yongli Zhao, Yuqiao Wang, Wei Wang and Xiaosong Yu

We are IntechOpen, the world's leading publisher of Open Access books. Built by scientists, for scientists. Our readership spans scientists, professors, researchers, librarians, and students, as well as business professionals. We share our knowledge and peer-reveiwed research papers with libraries, scientific and engineering societies, and also work with corporate R&D departments and government entities.

More About Us