Consensus-Based Multipath Planning with Collision Avoidance Using Linear Matrix Inequalities

Abstract

Consensus theory has been widely applied to collective motion planning related to coordinated motion. However, when the collective motion is highly irregular and adversarial, the basic consensus theory does not guarantee collision avoidance by default. As collision avoidance is a central problem of path planning, the incorporation of avoidance into the consensus algorithm is a subject of research. This work presents a new method of incorporating collision avoidance into the consensus algorithm, by applying the concept of constrained orientation control, where orientation constraints are represented as a set of linear matrix inequalities (LMI) and solved by semidefinite programming (SDP). The developed algorithm is used to simulate consensus-based multipath planning with collision avoidance for a team of communicating soccer robots.

1. Introduction

Path planning has found practical applications in areas such as entertainment (e.g. robot soccer) [1]; self-driving vehicles (e.g. Google’s self-driving cars) [2]; intelligent highways [3], and multiple unmanned space systems [4]. Because of the potential applications, the topic of multipath planning has been studied extensively, for example in [5, 6, 7, 8, 9, 10, 11].

The simplicity and potential of consensus algorithms to generate collective behaviors, such as flocking, platooning, rendezvous, and other formation configurations, make it an attractive choice for solving certain problems in multiagent control. However, the basic consensus algorithm collision avoidance mechanism is not developed for adversarial situations (i.e., opposite or attacking motion). To extend the power of the algorithm, it is therefore necessary to develop more powerful collision avoidance capabilities.

Next, we consider the basic approaches to collision avoidance in consensus. Some researchers, for example, [12, 13], approached the avoidance problem by introducing potential forces such as attraction and repulsion. However, the potential force algorithms were not developed for adversarial reconfigurations, for example, vehicles moving in opposite directions. Potential functions also have a problem of getting into local minima, coupled with slow speed of convergence. It is observed in [12] that any repulsion based on potential functions alone is not sufficient to guarantee consensus-based collision avoidance. Moreover, the attitude change maneuver presented in [12] was not developed for three-dimensional space (see [14] for a comprehensive literature survey on this topic).

Thus, in this work, we present an approach which we previously developed [5, 9] for incorporating collision avoidance into the consensus framework by applying quadratically constrained attitude control (Q-CAC), via semidefinite programming (SDP), using linear matrix inequalities (LMI). The main benefit of this approach is that it can solve the collision avoidance problem in adversarial situations and any configurations, and the formulation can be applied to two-dimensional as well as three-dimensional spaces. Table 1 shows the notation frequently used in this chapter.

Notation	Meaning
x^i	Position vector of vehicle number i
(x^ij)^off	Offset vector of vehicles i and j
x	Stacked vector of more than one position vector
x^off	Stacked vector of more than one offset vector
$u^i,{\dot{x}}^i$	Control input of vehicle i
$\mathbf{u},\dot{\mathbf{x}}$	Stacked vector of control inputs of more than one vehicle
L	Laplacian matrix
${\mathcal{S}}^m$	The set of m × m positive-definite matrices
S	Bounding sphere or circle of a vehicle or obstacle
ε	Width of safety region
r^∗	Radius of S
r	r^∗ + ε
v^i	Attitude vector of vehicle i
$v_{\mathit{obs}}^i$	Obstacle vector of vehicle i
$v_{\mathit{obs}}^{\mathit{ij}}$	Obstacle vector of vehicle i emanating from vehicle j
D^ij	Euclidean distance between vehicles i and j
L^ij	Line passing through the mid points of vehicles i and j
ρ^ij	Perpendicular bisector of L^ij separating vehicles i and j
PL^i	Plane passing through the midpoint of vehicle i
l^ij	Line of intersection of PL^i and PL^j
$d_x^i$	Distance from x^i to l^ij (for 3D) or p^ij(for 2D)
$d_v^i$	Distance from v^i to l^ij (for 3D) or p^ij(for 2D)
z^i	A point on the Z axis of PL^i
p^ij	Point of intersection of the lines passing through $\overrightarrow{x^i\left(t\right)v^i\left(t\right)}$ and $\overrightarrow{x^j\left(t\right)v^j\left(t\right)}$
N^i	Normal vector perpendicular to x^i, v^i, and z^i
D	Attitude control plant matrix, $\mathbf{D}\in {\mathcal{S}}^m$
⊗	Kronecker multiplication operator
A	State or plant matrix for dynamics of x
B	Input matrix for dynamics of x for input u
F	Feedback controller matrix
K	Proportional constant
Ip	Identity matrix of size p × p
Γ	Γ = L ⊗ Ip
Η	A vector or matrix in the Schur inequality
R	A positive-definite matrix in the Schur inequality
Q	A symmetric matrix in the Schur inequality
η	Positive real number for scaling the consensus term
β	Positive real number for scaling the proportional term

Table 1.

Frequently used notation in this chapter.

2. Problem statement

The basic consensus problem is that of driving the states of a team of communicating agents to a common value by distributed protocols based on their communication graph. The agents (or vehicles) i(i = 1, ⋯, n) are represented by vertices of the graph, whereas the edges of the graph represent communication links between them. Let x_i denote the state of a vehicle i and x is the stacked vector of the states of all vehicles. For systems modeled by first-order dynamics, the following first-order consensus protocol (or its variants) has been proposed, for example in [12, 13]

$$\dot{\mathbf{x}}\left(t\right)=-\mathbf{L}\left(\mathit{x}\left(\mathit{t}\right)-{\mathit{x}}^{\mathit{off}}\right).$$E1

Consensus is said to have been achieved when ‖xⁱ − x^j‖ → (x^ij)^off, as t → ∞, ∀i ≠ j.

The consensus-based multipath planning with collision avoidance problem can be stated as follows: Given a set of vehicles i, with initial positions xⁱ(t₀), desired final positions $x_d^i$, at time t_f, a set of obstacles with positions $x_{\mathit{obs}}^j\left(j=1,\cdots ,m\right)$, and the Laplacian matrix of their communication graph L find a sequence of collision-free trajectories from t₀ to t_f such that $x^i\left(t_f\right)=x_d^i\forall i$. Protocol (Eq. (1)) on its own does not solve the collision avoidance problem in adversarial situations. A comprehensive presentation of the necessary mathematical tools for this work (including graph theory and consensus theory) can be found in [14].

3. Solutions

In this section, we develop solutions to the problem stated in Section 2.

3.1. Consensus-based arbitrary reconfigurations

It was shown that for the dynamic system.

$$\dot{\mathbf{x}}=\mathbf{Ax}+\mathbf{Bu},$$E2

there exists a stabilizing feedback controller F, such that the protocol

$$\dot{\mathbf{x}}=\mathbf{Ax}+\mathbf{BFu}$$E3

drives x to x_f [15]. Here, x = [x¹, ⋯, xⁿ] is a stacked vector of the initial positions of the vehicles, u =  − Γ(x − x^off), Γ = L ⊗ I_p, I_p is the identity matrix of size p × p, and p is the state dimension of the vehicles.

To begin, we first consider the reference consensus path planning problem. To this end, the following protocol is proposed for a leader-follower communication graph architecture

$$\mathbf{u}=-\mathrm{\Gamma }\left(\mathbf{x}-{\mathbf{x}}^{\mathit{off}}\right)+\mathbf{K}\left({\mathbf{x}}^{\mathit{off}}-\mathbf{x}\right).$$E4

The corresponding protocol for a leaderless architecture is

$$\mathbf{u}=-\mathrm{\Gamma }\left(\mathbf{x}-{\mathbf{x}}^{\mathit{off}}\right)+\mathbf{K}\left({\mathbf{x}}_{\mathit{d}}-\mathbf{x}\right),$$E5

where x_d ≠ x^off is the desired final position and is different from the formation configuration, K = ϵI_n, (0 < ϵ ≪ 1), and n is the dimension of x.

Theorem 1 The time-varying system (Eq. (2)) achieves consensus.

Proof: see [14].

Figure 1 shows a simulation of consensus-based reconfiguration, using the communication graph in Figure 2, which is an example of a leader-follower graph. Node 1 is the leader, and each of the other nodes is connected to their adjacent neighbors. In Figure 1, the dots inside small circles indicate initial positions, whereas the dot in the diamond is the initial position of the leader. The stars indicate desired final positions. The larger circles with dashed lines are positions where collisions occurred, and the diameters of the circles indicate the size of intersection of the safety regions of the vehicles. The simulation proves that for arbitrary reconfigurations, the basic consensus algorithm does not guarantee collision avoidance.

Figure 1.

Consensus-based reconfiguration in adversarial situation using topology.

Figure 2.

Topology: a leader-follower graph.

3.2. Quadratically constrained attitude control-based collision avoidance

The collision avoidance problem is that of avoiding static obstacles and other moving vehicles while driving the state of a vehicle from one point to another. For simplicity, we approximate a vehicle or an obstacle by S, as shown in Figure 3. A nonspherical obstacle may be represented by a polygon as shown in Figure 4. For the S-type obstacle (or vehicle), let the obstacle be centered on a point x_obs; it is desired that the time evolution of any vehicle state xⁱ(t) from t₀ to t_f should avoid the constraint region shown in Figure 3.

Figure 3.

Constrained control problem for a static spherical obstacle.

Figure 4.

Constrained control problem for static nonspherical obstacle.

The feasible region is thus defined by

$$x_{\mathit{\text{feas}}}=\left\{x\in {\mathbb{R}}^{m\times m}|‖x-x_{\mathit{obs}}‖>r^{\ast }\right\},m\in \mathbb{R},$$E6

where r^∗ is the radius of S, bounded by a safety region of width ε.

There is no direct representation of the nonlinear nonconvex equation (Eq. (6)) as LMI. However, some non-LMI methods, for example, mixed integer linear programming (MILP) [7], have been developed for approximating its solution. In this section, we present an approach, which we previously developed in [5, 9, 10, 14], based on the principles of quadratically constrained attitude control (Q-CAC) algorithm [16], initially developed for the spacecraft attitude control problem.

At any time t, suppose the safety region of vehicle i centered on xⁱ(t) intersects the safety region of an obstacle, obs, centered on x_obs. Let v(t) be the unit vector extending from the centre of x_obs or xⁱ(t) in the direction of the point of intersection. The vectors v(t) will be different for each vehicle or obstacle. Considering the case shown in Figure 3, assume x_obs is known and v(t) is also known in the frame of obs. Then, to guide vehicle i safely around the obstacle, define a unit vector vⁱ(t) in the direction of v(t) in the frame of obs. The vector vⁱ(t) will be regarded as an imaginary vector whose direction can be constrained to change with time. The vector vⁱ(t) can then be used to find a sequence of trajectories around obs which guides i from xⁱ(t₀) to xⁱ(t_f) without violating (Eq. (6)).

The problem reduces to the Q-CAC problem. It is desired that the angle θ between vⁱ(t) and v(t) should be larger than some given angle ∅, ∀t. The constraint is

$$v^i{\left(t\right)}^{T}v\left(t\right)\le \mathit{cos}\varnothing ,\forall t\in \left[t_0,t_f\right].$$E7

The idea is to control the angle between the unit vectors vⁱ(t) and v(t). This implies that one of the vectors vⁱ(t) or v(t) must remain static, whereas the other moves with time. Vector vⁱ(t) is used to control the position of the vehicle; therefore, vⁱ(t) will move with time. The positions of vⁱ(t) define a trajectory path for xⁱ(t). Thus, xⁱ(t) is forced to move on the surface of the safety region bounding S. At some time t_k, xⁱ(t) will arrive close to a point indicated by vⁱ(t_k), at which a translation to xⁱ(t_f) is unconstrained. This is shown by the black dots on the boundary of the safety region in Figure 4. To obtain the unit vector v(t), the actual vector extending from the centre of x_obs or xⁱ(t) in the direction of the point of intersection is normalized. After the solution vⁱ(t) is obtained as a unit vector, vⁱ(t) is multiplied by r = r^∗ + ε to obtain the actual safe trajectory.

Let v(t) = [vⁱ(t)^Tv(t)^T]^T, then the dynamics of v(t) is defined as

$$\dot{\mathbf{v}}\left(\mathit{t}\right)=\mathbf{D}\left(\mathit{t}\right)\mathbf{v}\left(t\right),$$E8

where $\mathbf{D}\in {\mathcal{S}}^{\mathit{pn}}$, p is the dimension of the state vector xⁱ, and n is the number of vehicles. The above differential equation represents the rotational dynamics of the two vectors contained in v(t). D is a semidefinite matrix variable whose contents are unknown. Its purpose is to vary the angle between the two vectors in v(t) with time while also keeping them normalized.

The discrete time equivalent of the above differential equation is

$$\mathbf{v}\left(k+1\right)=\mathit{\Delta t}\mathbf{D}\left(\mathit{k}\right)\mathbf{v}\left(\mathit{k}\right),$$E9

where k = 0, ⋯, N (N∆t = t_f) is the discrete time equivalent of t and ∆t is the discretization time-step. To implement Eq. (9), D is declared in a semidefinite program which chooses the appropriate values to rotate the vectors in v(t) while satisfying norm constraints. Note in the above discretization of the differential equation, the identity matrix cannot be added to the solution; instead, the matrix D is chosen implicitly to satisfy the rotation. The vectors in v(t) are unit vectors; they are not translating, but they are rotating and must be preserved as unit vectors.

To enforce the attitude constraint (Eq. (7)) in a SDP, it should be represented as a LMI using the Schur complement formula described in [17]. The Schur complement formula states that the inequality

$$\mathbf{H}{\mathbf{R}}^{-1}{\mathbf{H}}^{\mathbf{T}}-\mathbf{Q}\le 0,$$E10

where Q = Q^T, R = R^T, and R > 0 are equivalent to and can be represented by the linear matrix inequality

$$\left[\begin{array}{ll}\mathit{Q}& \mathit{H}\\ {\mathit{H}}^T& \mathit{R}\end{array}\right]\ge 0.$$E11

Note that Eq. (7) is equivalent to

$${\displaystyle \underset{\mathit{v}{\left(\mathit{t}\right)}^{\mathit{T}}}{\underbrace{\left[v^i{\left(t\right)}^{T}v{\left(t\right)}^T\right]}}}\left[\begin{array}{ll}{\mathit{0}}_3& \frac{1}{2}{\mathit{I}}_3\\ \frac{1}{2}{\mathit{I}}_3& {\mathit{0}}_3\end{array}\right]\left[\begin{array}{c}{v}^i\left(t\right)\\ v\left(t\right)\end{array}\right]\le \mathit{cos}\varnothing ,$$E12

which also implies that

$$\underset{\mathit{v}{\left(\mathit{t}\right)}^{\mathit{T}}}{\underbrace{\left[v^i,{\left(t\right)}^T,v,{\left(t\right)}^T\right]}}\underset{\mathbf{G}}{\underbrace{\left[\begin{array}{cc}{\mathit{0}}_3& {\mathit{I}}_3\\ {\mathit{I}}_3& {\mathit{0}}_3\end{array}\right]}}\left[\begin{array}{c}{v}^i\left(t\right)\\ v\left(t\right)\end{array}\right]\le 2\mathit{cos}\varnothing ,$$E13

Note also that some of the eigenvalues of the G in Eq. (13) are nonpositive. To make the matrix positive definite, one only needs to shift the eigenvalues of G, by choosing a positive real number μ which is larger than the largest absolute value of the eigenvalues of G, then

$$\underset{\mathit{v}{\left(\mathit{t}\right)}^{\mathit{T}}}{\underbrace{\left[\begin{array}{cc}{v}^i{\left(t\right)}^T& v{\left(t\right)}^T\end{array}\right]}}\left(\mu {\mathit{I}}_6+\left[\begin{array}{cc}{\mathit{0}}_3& {\mathit{I}}_3\\ {\mathit{I}}_3& {\mathit{0}}_3\end{array}\right]\right)\underset{\mathit{v}\left(\mathit{t}\right)}{\underbrace{\left[\begin{array}{c}{v}^i\left(t\right)\\ v\left(t\right)\end{array}\right]}}\le 2\left(\mathit{cos}\varnothing +\mu \right).$$E14

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

$$\left[\begin{array}{cc}2\left(\mathit{cos}\varnothing +\mu \right)& \mathit{v}{\left(\mathit{t}\right)}^{\mathit{T}}\\ \mathbf{v}\left(\mathit{t}\right)& \mathbf{M}\end{array}\right]\ge 0.$$E15

For collision avoidance, the dynamic system (Eq. (8)) is solved whenever it is required, subject to the attitude constraint (Eq. (15)) and norm constraints ‖vⁱ(t)‖ = 1 and ‖v(t)‖ = 1. Thus, the optimization problem of collision avoidance is essential to find a feasible vⁱ subject to the following constraints:

$${\mathbf{v}}_{k+1}=∆t\mathbf{D}\left(t\right){\mathbf{v}}_k,$$E16

$${\mathbf{v}}_{\mathit{k}}^{\mathit{T}}\left({\mathit{v}}_{\mathit{k}+1}-{\mathit{v}}_{\mathit{k}}\right)=0,$$E17

$$\left[\begin{array}{cc}2\left(\mathit{cos}\varnothing +\mu \right)& \mathbf{v}{\left(\mathit{t}\right)}^{\mathit{T}}\\ \mathbf{v}\left(\mathit{t}\right)& \mathbf{M}\end{array}\right]\ge 0.$$E18

Eq. (17) is essentially the discrete time version of $\mathbf{v}{\left(t\right)}^T\dot{\mathbf{v}}\left(t\right)=0$ which guarantees that v(t)^Tv(t) = 2, if‖vⁱ(0)‖ = 1 and ‖v(0)‖ = 1. This solution works for 2D and 3D spaces. The next step is to extend the formulation to the case of dynamic obstacles. First, consider two vehicles i and j with states xⁱ(t), x^j(t) and attitude vectors vⁱ(t), v^j(t), respectively. Collision avoidance requires that they must avoid each other always. As shown in Figure 5, any time their safety regions are violated and the point of their intersection in the coordinate frame of i is $v_{\mathit{obs}}^i\left(t\right)$.

Figure 5.

Constrained control problem for dynamic obstacles.

The avoidance requirements are

$${\theta }^i\left(t\right)\ge \varnothing \equiv v^i{\left(t\right)}^T{v}_{\mathit{obs}}^i\left(t\right)\le \mathit{cos}\varnothing ,$$E19

$${\theta }^j\left(t\right)\ge \varnothing \equiv v^j{\left(t\right)}^T{v}_{\mathit{obs}}^j\left(t\right)\le \mathit{cos}\varnothing ,$$E20

∀t ∈ [t₀, t_f],

where ∅ ≥ π/2. For this dynamic situation, it is sufficient to enforce the following avoidance constraints:

$$\left[\begin{array}{cc}2\left(\mathit{cos}\varnothing +\mu \right)& {\left[\begin{array}{c}{\mathit{v}}^{\mathit{i}}\left(\mathit{k}+\mathit{2}\right)\\ {\mathit{v}}_{\mathit{obs}}^{\mathit{i}}\left(\mathit{k}+\mathit{2}\right)\end{array}\right]}^{\mathit{T}}\\ \left[\begin{array}{c}{\mathit{v}}^{\mathit{i}}\left(\mathit{k}+\mathit{2}\right)\\ {\mathit{v}}_{\mathit{obs}}^{\mathit{i}}\left(\mathit{k}+\mathit{2}\right)\end{array}\right]& \mathbf{M}\end{array}\right]\ge 0,$$E21

$$\left[\begin{array}{cc}2\left(\mathit{cos}\varnothing +\mu \right)& {\left[\begin{array}{c}{\mathit{v}}^{\mathit{j}}\left(\mathit{k}+\mathit{2}\right)\\ {\mathit{v}}_{\mathit{obs}}^{\mathit{j}}\left(\mathit{k}+\mathit{2}\right)\end{array}\right]}^{\mathit{T}}\\ \left[\begin{array}{c}{\mathit{v}}^{\mathit{j}}\left(\mathit{k}+\mathit{2}\right)\\ {\mathit{v}}_{\mathit{obs}}^{\mathit{j}}\left(\mathit{k}+\mathit{2}\right)\end{array}\right]& \mathbf{M}\end{array}\right]\ge 0,$$E22

$$i,j=1,\cdots ,n,i\ne j.$$

Note that (k + 2) is used because the optimization is performed two steps ahead of time to ensure that the future trajectories are collision free. However, when this avoidance protocol is applied to dynamic collision avoidance, some vehicle configurations pose challenges and this is considered next.

3.3. Conflict resolution for multiple vehicles

A collision between two vehicles i and j is imminent at time t whenever

$$D^{\mathit{ij}}\left(t\right)=‖x^i\left(t\right)-x^j\left(t\right)‖\le \left(r^i+r^j\right),$$E23

which can be computed using position feedback data determined by onboard or external sensors or communicated among the vehicles.

There are two aspects of collision problems: (i) collision detection and (ii) collision response. Collision detection is the computational problem of detecting the intersection of two or more objects. This can be done either using sensors or numerically using concepts from linear algebra and computational geometry. Collision response is the initiation of the appropriate avoidance maneuver. In this section, we present methods to detect different configurations of collisions and classify them. Then, an appropriate response technique is developed for each of the collision configurations.

Consider two vehicles i and j, whose current states are xⁱ(t) and x^j(t) and the desired final states are xⁱ(t_f) and x^j(t_f). We identify three different basic collision configurations as: (i) simple collision; (ii) head-on collision; and (iii) cross-path collision. Solutions will be developed for each of these configurations, and when combined synergistically, they will provide sufficient collision avoidance behavior for fast collision-free reconfiguration for the team of vehicles.

3.3.1. Detecting and resolving a simple collision

A simple collision problem is any configuration in which D^ij(t) ≤ (rⁱ + r^j) and the current vector directions (or attitude vectors) vⁱ(t) and v^j(t) of vehicles i and j are on different sides of the plane or infinite line L^ij(t) passing through the points xⁱ(t), x^j(t); and the attitude vectors $\overrightarrow{x^i\left(t\right)v^i\left(t\right)}$ and $\overrightarrow{x^j\left(t\right)v^j\left(t\right)}$ are not parallel. Note that when $\overrightarrow{x^i\left(t\right)v^i\left(t\right)}$ and $\overrightarrow{x^j\left(t\right)v^j\left(t\right)}$ are not parallel, a point or line of intersection can be computed for both vectors. Examples of simple collision problems are shown in Figures 5 and 6.

Figure 6.

Simple collision problem.

This is the easiest collision problem to solve because the attitude vectors are already on opposite sides of L^ij(t). Considering Figure 6 (b), the plane or line ρ^ij(t) tangent to the point of intersection of both vehicles constrains the current motion spaces of the vehicles to either of the two sides of the plane at time t. A pure optimization-based solution will attempt to search the space on the right side of ρ^ij(t) to seek for a point which is closest to the goal of i, and this will be used as the next trajectory. The algorithm will also search the left side of ρ^ij(t) to find the next trajectory for j. Once the positions are updated, a new ρ^ij(t) is computed.

Indeed, the solution is provided by the basic collision avoidance protocols (Eqs. (21) and (22)) without having to do a set search. It is easy to observe that by expanding the angles θⁱ(t) and θ^j(t) and choosing the next feasible trajectories r^∗/2 along the new direction vectors $\overrightarrow{x^i\left(t\right)v^i\left(t\right)}$ and $\overrightarrow{x^j\left(t\right)v^j\left(t\right)}$, the new trajectories are bound to satisfy the feasible regions separated by ρ^ij(t), provided εⁱ > r^∗i for any i. The rest of the avoidance strategies developed in the remaining part of this section are attempts to reduce more complex collision configurations to a simple collision configuration.

3.3.2. Detecting and resolving a head-on collision

A head-on collision problem is any configuration in which D^ij(t) ≤ (rⁱ + r^j) and vⁱ(t)^Tv^j(t) ≈ π rad. Figure 7(a) illustrates the head-on collision problem.

Figure 7.

Head-on collision problem.

The paths from the current positions xⁱ(t) and x^j(t) to the goal positions xⁱ(t_f) and x^j(t_f) lead to a configuration in which the attitude vectors $\overrightarrow{x^i\left(t\right)v^i\left(t\right)}$ and $\overrightarrow{x^j\left(t\right)v^j\left(t\right)}$ are parallel (or close to parallel) and in opposite directions, in the sense that a point of intersection cannot be computed. Figure 7(b)–(d) shows several examples of head-on collision. Figure 7(b) is a direct head-on collision because the vectors $\overrightarrow{x^i\left(t\right)v^i\left(t\right)}$ and $\overrightarrow{x^j\left(t\right)v^j\left(t\right)}$ are lying directly on L^ij(t). Figure 7(c) is an approximate head-on collision and Figure 7(d) is a head-on collision that can be easily converted to a simple collision configuration.

For the configurations in Figure 7(b) and (c), the Q-CAC formulation presented earlier easily solves this problem without any modifications to the algorithm. However, whenever $v^i{\left(t\right)}^T{v}_{\mathit{obs}}^i\left(t\right)\approx 0$ for any i, the optimization algorithm takes some significant time to solve. Even though the resulting trajectory is desirable, this delay is undesirable for real-time collision avoidance. Therefore, whenever this configuration is encountered for any two vehicles, a one-step elementary evasive maneuver is initiated, in which either vⁱ(t) or v^j(t) is rotated by a small angle ψ > 0. This rotation effectively transforms the head-on collision configuration to a simple collision configuration. Once this is done, the avoidance constraints defined in Eqs. (21) and (22) solve in real time. The trajectory obtained using this strategy for two-vehicle reconfiguration with head-on collision avoidance is shown in [14].

3.3.3. Detecting and resolving cross-path collision for two vehicles

A cross-path collision problem is any configuration in which D^ij(t) ≤ (rⁱ + r^j) and the current vector directions vⁱ(t) and v^j(t) are on the same side of L^ij(t), and the attitude vectors $\overrightarrow{x^i\left(t\right)v^i\left(t\right)}$ and $\overrightarrow{x^j\left(t\right)v^j\left(t\right)}$ are not parallel. Because the vectors are not parallel, a point (for 2D) or line (for 3D) of intersection can be computed for both vectors. Figure 8 is an example of a cross-path collision problem.

Figure 8.

Cross-path collision trajectory.

Note that for the avoidance process, the attitude control algorithm attempts to expand the angles θⁱ(t) and θ^j(t) to an angle = π/2. Based on this initial configuration, therefore, vⁱ(t) and v^j(t) will remain parallel or close to parallel, but not in opposite directions. If this continues, the desired goal positions may never be reached, or may be reached after a great deal of effort. To resolve this problem, it is required to determine whether the two vehicles are indeed in a cross-path configuration. The task is therefore to see if there exists a point or line of intersection between $\overrightarrow{x^i\left(t\right)v^i\left(t\right)}$ and $\overrightarrow{x^j\left(t\right)v^j\left(t\right)}$, and if such an intersection lies on one side of L^ij.

3.3.4. Determining cross-path collision in 3D and 2D.

To determine cross-path collision between i and j in 3D, two planes PLⁱ and PL^j are defined, both parallel to the z axes of the world coordinate frame (Figure 9). Each plane must contain the vectors $\overrightarrow{x^i\left(t\right)v^i\left(t\right)}$ and $\overrightarrow{x^j\left(t\right)v^j\left(t\right)}$ as shown in the figure. Therefore, the plane PLⁱ is defined as the set (Nⁱ(t), xⁱ(t), vⁱ(t), z(t)), where zⁱ(t) is a point chosen above or below xⁱ(t) or vⁱ(t) on the z axis and Nⁱ(t) is the normal vector perpendicular to xⁱ(t), vⁱ(t), and zⁱ(t). Once N^j(t) is similarly defined, the intersection of the two planes can be computed using techniques from computational geometry. If the two planes are not parallel, the computation of planes’ intersection will return a line l^ij. Once this line is determined, the next step is check if it is on one side of the plane parallel to the z axis and containing the points xⁱ(t) and x^j(t).

Figure 9.

Determination of cross-path collision in 3D.

An easy way to do this is to compute the perpendicular distances from the points xⁱ(t), vⁱ(t), x^j(t), and v^j(t), to l^ij.

Let the corresponding distances be:

$$d_x^i\left(t\right)=‖x^i\left(t\right)-l^{\mathit{ij}}‖,$$E24

$$d_v^i\left(t\right)=‖v^i\left(t\right)-l^{\mathit{ij}}‖,$$E25

$$d_x^j\left(t\right)=‖x^j\left(t\right)-l^{\mathit{ij}}‖,$$E26

$$d_v^j\left(t\right)=‖v^j\left(t\right)-l^{\mathit{ij}}‖.$$E27

If $d_v^i\left(t\right)\le d_x^i\left(t\right)$ and $d_v^j\left(t\right)\le d_x^j\left(t\right)$, then the line of intersection is in front of both vehicles, and a cross-path collision is imminent as shown in Figure 9(a) and (b). Otherwise, there is no cross-path conflict as shown in Figure 9(c).

The analysis is simpler in the 2D case. Instead of l^ij, we search for a point p^ij, which is the point of intersection of the lines passing through $\overrightarrow{x^i\left(t\right)v^i\left(t\right)}$ and $\overrightarrow{x^j\left(t\right)v^j\left(t\right)}$ as shown in Figure 10. If indeed such a point is found, we use p^ijinstead of l^ijin the previous set of equations.

Figure 10.

The point p^ij is the point of intersection of the infinite lines passing through direction vectors $\overrightarrow{x^i\left(t\right)v^i\left(t\right)}$ and $\overrightarrow{x^j\left(t\right)v^j\left(t\right)}$. The position of p^ij in relation to both direction vectors determines if a cross-path collision is imminent. If p^ij is in front of both vectors as in (a) and (b), then a cross-path collision is imminent; otherwise, no cross-path collision is imminent as in (c).

Figure 11 shows an illustration of the computation of $d_x^i$ and $d_v^i$ for any i.

Figure 11.

Continuing the explanation from Figure 10, $d_x^i$ is the distance from any xⁱ (vehicle i) top^ij, whereas $d_v^i$ is the distance from vⁱ to p^ij, that is, the distance of the outer boundary (where vⁱ lies) of the safety region of vehicle i to p^ij.

Figure 12(a) is a cross-path collision configuration, but (b) is a simple collision configuration.

Figure 12.

The effects of cross-path conflict resolution.

The solution strategy adopted is to convert any cross-path configuration such as Figure 12 (a) to a simple configuration such as (b). To do this, one only must move either vⁱ(t) or v^j(t) to the other side of L^ij(t) (or onto the line L^ij(t)). A simple strategy to decide which v(t) should be moved to obtain smoother phase transition is to measure θⁱ(t) and θ^j(t). If θ^j(t) < θⁱ(t), then v^j(t) should be moved. This is done by swapping v^j(t) and $v_{\mathit{obs}}^j\left(t\right)$, which immediately results in a simple collision reconfiguration. Thereafter, when the Q-CAC algorithm expands θ^j(t), it is the former $v_{\mathit{obs}}^j\left(t\right)$ (which is now the new v^j(t)) that moves, whereas the former v^j(t) (which is now the new $v_{\mathit{obs}}^j\left(t\right)$) remains static.

Therefore, if a cross-path trajectory is determined, to resolve the problem it is sufficient to swap the variables in one of the avoidance constraints (Eq. (21) or Eq. (22)). For example, Eq. (21) may be left as it is and Eq. (22) is rewritten in the form

$$\left[\begin{array}{cc}2\left(\mathit{cos}\varnothing +\mu \right)& {\left[\begin{array}{c}{\mathit{v}}_{\mathit{obs}}^{\mathit{j}}\left(\mathit{k}+\mathit{2}\right)\\ {\mathit{v}}^{\mathit{j}}\left(\mathit{k}+\mathit{2}\right)\end{array}\right]}^{\mathit{T}}\\ \left[\begin{array}{c}{\mathit{v}}_{\mathit{obs}}^{\mathit{j}}\left(\mathit{k}+\mathit{2}\right)\\ {\mathit{v}}^{\mathit{j}}\left(\mathit{k}+\mathit{2}\right)\end{array}\right]& \mathit{M}\end{array}\right]\ge 0.$$E28

The trajectories obtained by applying this strategy to cross-path collision avoidance for two vehicles in 2D and 3D are shown in [14].

3.3.5. Resolving cross-path collision for more than two vehicles

If more than two vehicles are involved as shown in Figure 13, for any vehicle i, whose attitude vector vⁱ(t) is in a cross-path configuration with vehicles j and k, we are concerned only about the two bounding obstacle vectors $v_{\mathit{obs}}^{\mathit{ij}}\left(t\right)$ and $v_{\mathit{obs}}^{\mathit{ik}}\left(t\right)$.

Figure 13.

Three-vehicle cross-path trajectory problem.

In order not to get into a stalemate situation (undesirable for aircraft), only positive nonzero velocities are required to be generated. We adopt a counterclockwise avoidance measure to achieve this, where, for each vehicle, the left bounding obstacle vector is always chosen as the cross-path obstacle vector for avoidance. For example, for k to turn counterclockwise, it chooses the vector $v_{\mathit{obs}}^{\mathit{ki}}\left(t\right)$ to avoid instead of $v_{\mathit{obs}}^{\mathit{kj}}\left(t\right)$. Thus, for the configuration of Figure 13, the following set of attitude constraints is enforced:

$$\left[\begin{array}{cc}2\left(\mathit{cos}\varnothing +\mu \right)& {\left[\begin{array}{c}{\mathit{v}}_{\mathit{obs}}^{\mathit{ij}}\left(\mathit{k}+\mathit{2}\right)\\ {\mathit{v}}^{\mathit{i}}\left(\mathit{k}+\mathit{2}\right)\end{array}\right]}^{\mathit{T}}\\ \left[\begin{array}{c}{\mathit{v}}_{\mathit{obs}}^{\mathit{ij}}\left(\mathit{k}+\mathit{2}\right)\\ {\mathit{v}}^{\mathit{i}}\left(\mathit{k}+\mathit{2}\right)\end{array}\right]& \mathit{M}\end{array}\right]\ge 0,$$E29

$$\left[\begin{array}{cc}2\left(\mathit{cos}\varnothing +\mu \right)& {\left[\begin{array}{c}{\mathit{v}}_{\mathit{obs}}^{\mathit{jk}}\left(\mathit{k}+\mathit{2}\right)\\ {\mathit{v}}^{\mathit{j}}\left(\mathit{k}+\mathit{2}\right)\end{array}\right]}^{\mathit{T}}\\ \left[\begin{array}{c}{\mathit{v}}_{\mathit{obs}}^{\mathit{jk}}\left(\mathit{k}+\mathit{2}\right)\\ {\mathit{v}}^{\mathit{j}}\left(\mathit{k}+\mathit{2}\right)\end{array}\right]& \mathit{M}\end{array}\right]\ge 0,$$E30

$$\left[\begin{array}{cc}2\left(\mathit{cos}\varnothing +\mu \right)& {\left[\begin{array}{c}{\mathit{v}}_{\mathit{obs}}^{\mathit{ki}}\left(\mathit{k}+\mathit{2}\right)\\ {\mathit{v}}^{\mathit{k}}\left(\mathit{k}+\mathit{2}\right)\end{array}\right]}^{\mathit{T}}\\ \left[\begin{array}{c}{\mathit{v}}_{\mathit{obs}}^{\mathit{ki}}\left(\mathit{k}+\mathit{2}\right)\\ {\mathit{v}}^{\mathit{k}}\left(\mathit{k}+\mathit{2}\right)\end{array}\right]& \mathit{M}\end{array}\right]\ge 0.$$E31

3.4. Consensus with Q-CAC–based avoidance

Once a safe attitude vector vⁱ(k) is computed at time k for any i, the next position xⁱ(k + 1) is computed as a point a distance r^∗i/2 from the current position, along the vector vⁱ(k). Note that vⁱ(k) is normalized to keep the computed control bounded. Whether there are intersections of the safety regions or not, one can guarantee the safety of the algorithm by bounding the control size within the interval 0 < uⁱ ≤ r^∗i/2. This means that a vehicle never steps beyond its safety region at any single time step.

Another important consideration is the size of control computed at each time using Laplacian matrices, which is directly proportional to the algebraic connectivity of the communication graph, and inversely proportional to the magnitude of the current time k. This means that, while the early values of u are large and therefore unsafe for collision avoidance (and must be bounded), the latter values of u are very small and therefore slow down the rate of convergence. One can observe that collisions are less likely to occur in the latter times when the vehicles are closer to their goal positions; consequently, convergence is slower at that time. Therefore, there is need to obtain constantly bounded control u which can guarantee both collision avoidance and a high speed of convergence. The following modifications to Eq. (4) and Eq. (5) were proposed in our previous works [5, 9, 14]. For the leader-follower architecture,

$$\mathbf{u}=-\eta {\mathit{log}}_{10}\left(k+1\right)\frac{\mathit{\Delta t}}{2{\lambda }_2\left(\mathit{L}\right)}\mathrm{\Gamma }\left(\mathit{x}-{\mathit{x}}^{\mathit{off}}\right)-\beta {\mathit{log}}_{10}\left(k+1\right)\frac{\mathit{\Delta t}}{2{\lambda }_2\left(\mathit{L}\right)}\mathbf{K}\left(\mathit{x}-{\mathit{x}}^{\mathit{off}}\right).$$E32

And for the leaderless architecture,

$$\mathbf{u}=-\eta {\text{log}}_{10}\left(\mathrm{k}+1\right)\frac{\Delta t}{2{\mathrm{\lambda }}_2\left(\mathbf{L}\right)}\mathrm{\Gamma }\left(\mathit{x}-{\mathit{x}}^{\mathit{off}}\right)-\beta {\text{log}}_{10}\left(\mathrm{k}+1\right)\frac{\Delta t}{2{\mathrm{\lambda }}_2\left(\mathbf{L}\right)}\mathbf{K}\left(\mathit{x}-{\mathit{x}}_{\mathit{d}}\right),$$E33

where λ₂(L) is the second smallest eigenvalue of the Laplacian L. The parameter η is for scaling the consensus term and β is for scaling the proportional term in Eqs. (32) and (33). The logarithmic term log₁₀(k + 1) and the term $\frac{\Delta t}{2{\lambda }_2\left(\mathit{L}\right)}$ are used to reduce ‖u‖ when k is small and increase ‖u‖ when k is large. The choices of parameters η and β should depend on the radius of S and safety region ε for each vehicle. Alternatively, one may choose to compute an unbounded u using Eqs. (4) or (5), then for each $u^i>\frac{{r^{\ast }}^i}{2}$, normalize uⁱ and set $u^i=\frac{{r^{\ast }}^i}{2}{u}^i$.

The step-by-step procedure for implementing the algorithm including a flowchart can be found in Ref. [14].

4. Simulation results

To demonstrate the solutions developed in this chapter, we revisit the experiment presented in Figure 1. The robots are homogeneous, and S for each robot is 85 mm, ε = 90 mm, whereas the dimensions of the soccer pitch are 6050 mm x 4050 mm. In Figure 14 (a), Eq. (5) was applied with the cyclic communication topology with one leader (Figure 2). In Figure 14 (b), Eq. (33) was applied with a full communication topology (i.e., every vehicle can communicate with each other). The simulation was done with MATLAB R2009a on an Intel^® Core(TM)2 Duo P8600 @ 2.40 GHz with 2 GB RAM, running Windows 7. For Figure 14(a), the multipath planning problem took 244 time-steps to solve, resulting in a total computation time of 7.343 s, in which 203 avoidance attempts were made, and there were no collisions. For Figure 14(b), using a full communication topology, the computational time was 0.0131 s, and there were no collisions.

Figure 14.

Collision-free reconfiguration: (a) using topology with Eq. (5) and (b) using fully connected graph with Eq. (33).

In [14], more simulations and analyses are presented, together with the limitations of this approach, which remains to be explored for future development.

5. Conclusion

In this chapter, we considered consensus-based multipath planning. An approach to incorporating collision avoidance in adversarial situations in the consensus algorithm by applying Q-CAC is presented. Simulation results are presented here to show that for a sizable number of vehicles, collision avoidance and fast convergence are guaranteed. Future work will include implementation on a team of mobile robots and autonomous aerial vehicles.