Frequently used notation in this chapter.
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.
- path planning
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 ; sea navigation and ocean sampling ; 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,  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 .
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 : (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 .
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.
|Number of vehicles|
|Position vector of vehicle|
|Unit vector corresponding to vector|
|Control input of vehicle|
|Obstacle vector number|
|Offset vector between vehicles and|
|Angle between vehicle and obstacle|
|Angle between vehicles and|
|Minimum angular separation from obstacle number|
|Minimum angular separation between vehicles and|
|Stacked vector of position vectors|
|Stacked vector of control inputs|
|Laplacian-like stochastic matrix|
|A vector consisting of all zeros|
|Kronecker multiplication operator|
|Special Euclidean group|
|The set of positive definite matrices|
|The identity matrix|
|A positive definite matrix variable,|
|The consensus space for|
|Set of vertices of|
|Set of edges of|
|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 as a pair consisting of two finite sets having elements; a set of points called vertices , and a set of connecting lines called edges, or endpoints, or , of the vertices . Thus, an edge is incident with vertices and . Graph 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. implies ; otherwise it is called a directed graph (digraph), and it is symmetric. The quantity is called the order, and the size, respectively, of . The set of neighbors of node is denoted by . The number of edges incident with vertex is called the degree or valence of . Furthermore, the number of directed edges incident into is called the In-degree of , while the Out-degree is similarly defined as the number of edges incident out of the .
We define the adjacency matrix of of order as the matrix
For the undirected graph is always symmetric, while of a digraph G is symmetric if and only if is symmetric. The out-degree matrix of of order , is an 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 is similarly defined.
The Laplacian matrix of digraph of order , is the matrix
An important property of any Laplacian 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) are represented by vertices of the graph, while the edges of the graph represent communication links between them. Let denote the state of a vehicle and 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]
We determine that consensus has been achieved when as , . A more comprehensive presentation of the necessary mathematical tools for this work (including graph theory and consensus theory), can be found in .
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 , (referenced to a coordinate frame centered on the centroid of the sphere), a set of obstacles , , and the Laplacian matrix of their communication graph , 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 which implies that vectors and are unit vectors and must be kept so throughout the evolution of the trajectory vectors. The angle between the position vectors of vehicles and is , while is the angle between vehicle and obstacle . The control problem is to drive all to a consensus position or to a formation while avoiding each other and the 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.
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.
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 can communicate with at least one other neighboring vehicle. Given that is the number of vehicles in the neighborhood of that it can communicate with, then individually synthesizes a Laplacian-like stochastic matrix so that all are driven to consensus on the unit sphere. The synthesis of is as follows. A semidefinite matrix variable, for each is generated. Then
where are the position vectors of vehicles that is communicating with at time . For the purpose of analysis, the collective description for vehicles is given as
where, is the collective Laplacian matrix. Note that any 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
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 as
Eq. (10) is the discrete time version of or , which guarantees that or for vehicles, iff . Eq. (8) drives the positions 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 achieves global consensus asymptotically for L .
Proof: The proof , 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).
Proof: Note that if x belongs to the consensus space , then , (i.e. all vehicles have stopped moving). Because is the nullspace of , where . Meaning that once enters it stays there since there is no more motion. If consensus has not been achieved then , consider a Lyapunov candidate function ; unless . Then,
where for . This implies that approaches a point in as , which proves the claim. Eq. (11) is true for as long as 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 , and to avoid two constraint regions around and . The obstacle regions are defined by cones, whose base radii are and , respectively. Let the angle between vehicles and be , and that between vehicle and obstacle be . Then the requirements for collision avoidance are: , , , and , , , . They have the following equivalent quadratic constraints:
By using the Schur’s complement formula , 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 , , and , 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 i.e. or whose eigenvalues are all nonnegative (this is synonymous with in the Schur inequality). To make 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 , then
Let , then M is positive definite. Thus, following the Schur’s complement formula, the LMI equivalent of Eq. (12) becomes
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 , with .
4.3. Formulation of formation control on the unit sphere
Formation patterns are obtained by specifying a minimum angular separation of between any two vehicles and thereby defining relative spacing between individual vehicles. Using the avoidance strategy formerly described, the constraint 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 vehicles, the avoidance requirements result in extra 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 and the minimum angular separations between the vehicles is set at a constant value . Therefore, in addition to the four static obstacle avoidance constraints (such as Eqs. (24) to (29), with ), each vehicle has three more intervehicle collision avoidance constraints such as
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 and the plant Eq. (5) for each vehicle, find a feasible sequence of trajectories that satisfies the following constraints:
where and (which are components of ) are the optimization variables. They are declared as SDP variables where shapes the trajectories 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.
If is the state of a virtual leader corresponding to vehicle , then for each leader-follower vehicle pair the corresponding leader-follower Laplacian matrix is
The corresponding collective dynamics of is
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  and Yalmip  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 . Angle is set to maintain the relative spacing between the vehicles . 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 circulant Laplacian L, whose dynamics leads to swirling motion. The proof is in .
When relative spacing are specified between the vehicles, the motion obtained from this Laplacian is like the result obtained in . 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 ; by setting equal for all and 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 for four vehicles. The center and right figures show the results for ten vehicles as moves gradually from toward . When , the vehicles rendezvous to a point.
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 units. Five no-fly zones are imposed on the vehicles at the following positions:
The radii of the no-fly zones are equal to , therefore for this simulation. The result is shown in Figure 8.
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 . 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.