3.3.1. Detecting and resolving a simple collision

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

2 4

3

3

vi ð Þ k þ 2

vi

5 M

vj ð Þ k þ 2

2 4

vj

5 M

i, j ¼ 1, ⋯, n, i 6¼ 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

> ð Þ� <sup>t</sup> xj ð Þ<sup>t</sup> � � �

which can be computed using position feedback data determined by onboard or external

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

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

sion avoidance behavior for fast collision-free reconfiguration for the team of vehicles.

� ≤ r <sup>i</sup> <sup>þ</sup> <sup>r</sup>

(t) and xj

(tf). We identify three different basic collision configurations as: (i) simple

obsð Þ k þ 2

obsð Þ k þ 2

3 5 T

3 5

T

≥ 0, (21)

≥ 0, (22)

<sup>j</sup> � �, (23)

(t) and the desired final states

2 cos <sup>∅</sup> <sup>þ</sup> <sup>μ</sup> � �

obsð Þ k þ 2

2 cos <sup>∅</sup> <sup>þ</sup> <sup>μ</sup> � �

obsð Þ k þ 2

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

DijðÞ¼ <sup>t</sup> xi

vi ð Þ k þ 2

2 4

vi

vj ð Þ k þ 2

2 4

3.3. Conflict resolution for multiple vehicles

sensors or communicated among the vehicles.

Consider two vehicles i and j, whose current states are x<sup>i</sup>

the collision configurations.

(tf) and xj

are xi

vj

constraints:

12 Advanced Path Planning for Mobile Entities

is considered next.

A simple collision problem is any configuration in which Dij(t) ≤ (r i + r j ) and the current vector directions (or attitude vectors) vi (t) and vj (t) of vehicles i and j are on different sides of the plane or infinite line Lij(t) passing through the points x<sup>i</sup> (t), xj (t); and the attitude vectors xi ð Þ<sup>t</sup> vi ð Þ<sup>t</sup> ! and xj ð Þ<sup>t</sup> vj ð Þ<sup>t</sup> ! are not parallel. Note that when xi ð Þ<sup>t</sup> vi ð Þ<sup>t</sup> ! and xj ð Þ<sup>t</sup> vj ð Þ<sup>t</sup> ! 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.

This is the easiest collision problem to solve because the attitude vectors are already on opposite sides of Lij(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 θ<sup>i</sup> (t) and

θj (t) and choosing the next feasible trajectories r ∗ /2 along the new direction vectors xi ð Þ<sup>t</sup> vi ð Þ<sup>t</sup> !

and xj ð Þ<sup>t</sup> vj ð Þ<sup>t</sup> ! , the new trajectories are bound to satisfy the feasible regions separated by ρij(t), provided ε<sup>i</sup> > 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 Dij(t) ≤ (r i + r j ) and vi (t) T vj (t) ≈ π rad. Figure 7(a) illustrates the head-on collision problem.

Figure 6. Simple collision problem.

The paths from the current positions xi (t) and xj (t) to the goal positions xi (tf) and xj (tf) lead to a configuration in which the attitude vectors xi ð Þ<sup>t</sup> vi ð Þ<sup>t</sup> ! and xj ð Þ<sup>t</sup> vj ð Þ<sup>t</sup> ! 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 headconfiguration 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].

3D) of intersection can be computed for both vectors. Figure 8 is an example of a cross-path

Note that for the avoidance process, the attitude control algorithm attempts to expand the

(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 crosspath configuration. The task is therefore to see if there exists a point or line of intersection

(t) are on the same side of Lij(t), and the attitude vectors xi

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

are not parallel. Because the vectors are not parallel, a point (for 2D) or line (for

(t) to an angle = π/2. Based on this initial configuration, therefore, vi

, and if such an intersection lies on one side of Lij

i + r j

http://dx.doi.org/10.5772/intechopen.71288

) and the current

.

ð Þ<sup>t</sup> vi ð Þ<sup>t</sup> ! 15

(t) and

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

(t) and vj

vector directions vi

ð Þ<sup>t</sup> vj ð Þ<sup>t</sup> !

collision problem.

(t) and θ<sup>j</sup>

ð Þ<sup>t</sup> <sup>v</sup><sup>i</sup> ð Þ<sup>t</sup> !

Figure 8. Cross-path collision trajectory.

and xj

ð Þ<sup>t</sup> vj ð Þ<sup>t</sup> !

and xj

angles θ<sup>i</sup>

between xi

vj

A cross-path collision problem is any configuration in which Dij(t) ≤ (r

on collision because the vectors xi ð Þ<sup>t</sup> vi ð Þ<sup>t</sup> ! and xj ð Þ<sup>t</sup> vj ð Þ<sup>t</sup> ! are lying directly on Lij(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 vi ð Þt T vi obsð Þt ≈ 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<sup>i</sup> (t) or v<sup>j</sup> (t) is rotated by a small angle ψ > 0. This rotation effectively transforms the head-on collision

Figure 7. Head-on collision problem.

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

The paths from the current positions xi

14 Advanced Path Planning for Mobile Entities

on collision because the vectors xi

Figure 7. Head-on collision problem.

vi ð Þt T vi

configuration in which the attitude vectors xi

converted to a simple collision configuration.

(t) and xj

and xj

vehicles, a one-step elementary evasive maneuver is initiated, in which either v<sup>i</sup>

ð Þ<sup>t</sup> vi ð Þ<sup>t</sup> !

ð Þ<sup>t</sup> vi ð Þ<sup>t</sup> !

ð Þ<sup>t</sup> vj ð Þ<sup>t</sup> !

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-

an approximate head-on collision and Figure 7(d) is a head-on collision that can be easily

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

rotated by a small angle ψ > 0. This rotation effectively transforms the head-on collision

obsð Þt ≈ 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

(t) to the goal positions xi

ð Þ<sup>t</sup> vj ð Þ<sup>t</sup> !

are lying directly on Lij

and xj

(tf) and xj

are parallel (or close to

(tf) lead to a

(t). Figure 7(c) is

(t) or v<sup>j</sup>

(t) is

A cross-path collision problem is any configuration in which Dij(t) ≤ (r i + r j ) and the current vector directions vi (t) and vj (t) are on the same side of Lij(t), and the attitude vectors xi ð Þ<sup>t</sup> vi ð Þ<sup>t</sup> ! and xj ð Þ<sup>t</sup> vj ð Þ<sup>t</sup> ! 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.

Note that for the avoidance process, the attitude control algorithm attempts to expand the angles θ<sup>i</sup> (t) and θ<sup>j</sup> (t) to an angle = π/2. Based on this initial configuration, therefore, vi (t) and vj (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 crosspath configuration. The task is therefore to see if there exists a point or line of intersection between xi ð Þ<sup>t</sup> <sup>v</sup><sup>i</sup> ð Þ<sup>t</sup> ! and xj ð Þ<sup>t</sup> vj ð Þ<sup>t</sup> ! , and if such an intersection lies on one side of Lij .

Figure 8. Cross-path collision trajectory.

#### 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<sup>i</sup> and PL<sup>j</sup> are defined, both parallel to the z axes of the world coordinate frame (Figure 9). Each plane must contain the vectors xi ð Þ<sup>t</sup> vi ð Þ<sup>t</sup> ! and xj ð Þ<sup>t</sup> vj ð Þ<sup>t</sup> ! as shown in the figure. Therefore, the plane PL<sup>i</sup> is defined as the set (N<sup>i</sup> (t), xi (t), vi (t), z(t)), where z i (t) is a point chosen above or below xi (t) or vi (t) on the z axis and N<sup>i</sup> (t) is the normal vector perpendicular to xi (t), v<sup>i</sup> (t), and z i (t). Once N<sup>j</sup> (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 xi (t) and xj (t).

An easy way to do this is to compute the perpendicular distances from the points xi (t), vi (t), xj (t), and v<sup>j</sup> (t), to l ij .

Let the corresponding distances be:

$$d\_x^i(t) = \left||\mathbf{x}^i(t) - l^{\vec{\eta}}\right||,\tag{24}$$

The analysis is simpler in the 2D case. Instead of l

indeed such a point is found, we use pijinstead of l

Figure 11 shows an illustration of the computation of di

of intersection of the lines passing through xi

xj ð Þ<sup>t</sup> <sup>v</sup><sup>j</sup> ð Þ<sup>t</sup> !

imminent as in (c).

distance from vi

Figure 11. Continuing the explanation from Figure 10, d<sup>i</sup>

to pij, that is, the distance of the outer boundary (where v<sup>i</sup>

ij, we search for a point pij, which is the point

http://dx.doi.org/10.5772/intechopen.71288

as shown in Figure 10. If

(t) or vj

(t) to the

17

ð Þ<sup>t</sup> <sup>v</sup><sup>i</sup> ð Þ<sup>t</sup> !

and

<sup>v</sup> is the

.

ð Þ<sup>t</sup> vi ð Þ<sup>t</sup> !

Figure 12(a) is a cross-path collision configuration, but (b) is a simple collision configuration. The solution strategy adopted is to convert any cross-path configuration such as Figure 12 (a)

other side of Lij(t) (or onto the line Lij(t)). A simple strategy to decide which v(t) should be

to a simple configuration such as (b). To do this, one only must move either v<sup>i</sup>

Figure 10. The point pij is the point of intersection of the infinite lines passing through direction vectors xi

. The position of pij in relation to both direction vectors determines if a cross-path collision is imminent. If pij is in front of both vectors as in (a) and (b), then a cross-path collision is imminent; otherwise, no cross-path collision is

and xj

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

<sup>x</sup> and di

ð Þ<sup>t</sup> vj ð Þ<sup>t</sup> !

ijin the previous set of equations.

<sup>v</sup> for any i.

<sup>x</sup> is the distance from any <sup>x</sup><sup>i</sup> (vehicle <sup>i</sup>) topij, whereas di

lies) of the safety region of vehicle i to pij

$$d\_v^i(t) = \left\| \left| \upsilon^i(t) - l^{\vec{\eta}} \right| \right\|\_{\prime} \tag{25}$$

$$d\_x^j(t) = \left\| \left| x^j(t) - l^j \right| \right\|\_{\prime} \tag{26}$$

$$d\_v^j(t) = \left\| v^j(t) - l^{ij} \right\|. \tag{27}$$

If d<sup>i</sup> <sup>v</sup>ð Þ<sup>t</sup> <sup>≤</sup> <sup>d</sup><sup>i</sup> <sup>x</sup>ð Þ<sup>t</sup> and dj <sup>v</sup>ð Þ<sup>t</sup> <sup>≤</sup> <sup>d</sup><sup>j</sup> <sup>x</sup>ð Þt , 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 crosspath conflict as shown in Figure 9(c).

Figure 9. Determination of cross-path collision in 3D.

The analysis is simpler in the 2D case. Instead of l ij, we search for a point pij, which is the point of intersection of the lines passing through xi ð Þ<sup>t</sup> vi ð Þ<sup>t</sup> ! and xj ð Þ<sup>t</sup> vj ð Þ<sup>t</sup> ! as shown in Figure 10. If indeed such a point is found, we use pijinstead of l ijin the previous set of equations.

Figure 11 shows an illustration of the computation of di <sup>x</sup> and di <sup>v</sup> for any i.

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

and xj

ð Þ<sup>t</sup> vj ð Þ<sup>t</sup> !

i

(t) is the normal vector perpendicular to xi

(t), z(t)), where z

plane parallel to the z axis and containing the points xi

the vectors xi

z axis and N<sup>i</sup>

return a line l

(t), and v<sup>j</sup>

xj

If d<sup>i</sup>

<sup>v</sup>ð Þ<sup>t</sup> <sup>≤</sup> <sup>d</sup><sup>i</sup>

the set (N<sup>i</sup>

ð Þ<sup>t</sup> vi ð Þ<sup>t</sup> !

16 Advanced Path Planning for Mobile Entities

ij

(t), to l ij . Let the corresponding distances be:

<sup>x</sup>ð Þ<sup>t</sup> and dj

path conflict as shown in Figure 9(c).

Figure 9. Determination of cross-path collision in 3D.

<sup>v</sup>ð Þ<sup>t</sup> <sup>≤</sup> <sup>d</sup><sup>j</sup>

(t), xi (t), vi

To determine cross-path collision between i and j in 3D, two planes PL<sup>i</sup> and PL<sup>j</sup> are defined, both parallel to the z axes of the world coordinate frame (Figure 9). Each plane must contain

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

An easy way to do this is to compute the perpendicular distances from the points xi

<sup>x</sup>ðÞ¼ <sup>t</sup> xi

<sup>v</sup>ðÞ¼ <sup>t</sup> vi

<sup>x</sup>ðÞ¼ <sup>t</sup> xj

<sup>v</sup>ðÞ¼ <sup>t</sup> vj

ð Þ� t l ij 

ð Þ� t l ij 

ð Þ� t l ij 

ð Þ� t l ij 

cross-path collision is imminent as shown in Figure 9(a) and (b). Otherwise, there is no cross-

di

di

dj

dj

as shown in the figure. Therefore, the plane PL<sup>i</sup>

(t) is a point chosen above or below xi

(t), v<sup>i</sup>

(t) and xj

<sup>x</sup>ð Þt , then the line of intersection is in front of both vehicles, and a

. Once this line is determined, the next step is check if it is on one side of the

(t), and z i

(t).

is defined as

(t) is similarly

(t) on the

(t), vi (t),

(t) or vi

(t). Once N<sup>j</sup>

, (24)

, (25)

, (26)

: (27)

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

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<sup>i</sup> (t) or vj (t) to the other side of Lij(t) (or onto the line Lij(t)). A simple strategy to decide which v(t) should be

Figure 10. The point pij is the point of intersection of the infinite lines passing through direction vectors xi ð Þ<sup>t</sup> <sup>v</sup><sup>i</sup> ð Þ<sup>t</sup> ! and xj ð Þ<sup>t</sup> <sup>v</sup><sup>j</sup> ð Þ<sup>t</sup> ! . The position of pij in relation to both direction vectors determines if a cross-path collision is imminent. If pij 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. Continuing the explanation from Figure 10, d<sup>i</sup> <sup>x</sup> is the distance from any <sup>x</sup><sup>i</sup> (vehicle <sup>i</sup>) topij, whereas di <sup>v</sup> is the distance from vi to pij, that is, the distance of the outer boundary (where v<sup>i</sup> lies) of the safety region of vehicle i to pij .

Figure 12. The effects of cross-path conflict resolution.

moved to obtain smoother phase transition is to measure θ<sup>i</sup> (t) and θ<sup>j</sup> (t). If θ<sup>j</sup> (t) < θ<sup>i</sup> (t), then vj (t) should be moved. This is done by swapping v<sup>j</sup> (t) and v j obsð Þt , which immediately results in a simple collision reconfiguration. Thereafter, when the Q-CAC algorithm expands θ<sup>j</sup> (t), it is the former v j obsð Þ<sup>t</sup> (which is now the new vj (t)) that moves, whereas the former vj (t) (which is now the new v j obsð Þt ) 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

$$\begin{bmatrix} 2(\cos \mathcal{Q} + \mu) \\\\ \begin{bmatrix} \mathbf{v}\_{obs}^{j}(\mathbf{k} + \mathbf{2})\\\\ \mathbf{v}^{j}(\mathbf{k} + \mathbf{2}) \end{bmatrix}^{T} \\\\ \begin{bmatrix} \mathbf{v}\_{obs}^{j}(\mathbf{k} + \mathbf{2})\\\\ \mathbf{v}^{j}(\mathbf{k} + \mathbf{2}) \end{bmatrix} \end{bmatrix} \ge 0. \tag{28}$$

cross-path obstacle vector for avoidance. For example, for k to turn counterclockwise, it

kj

2 4

vi ð Þ k þ 2

5 M

2 4

vj ð Þ k þ 2

5 M

obsð Þ k þ 2

obsð Þ k þ 2

3 5 T

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

http://dx.doi.org/10.5772/intechopen.71288

19

3 5 T

obsð Þt . Thus, for the configuration of Figure 13,

≥ 0, (29)

≥ 0, (30)

obsð Þt to avoid instead of v

vij

2 4

vi ð Þ k þ 2

vjk obsð Þ k þ 2

2 4

vj ð Þ k þ 2

2 cos <sup>∅</sup> <sup>þ</sup> <sup>μ</sup> � � vij

2 cos <sup>∅</sup> <sup>þ</sup> <sup>μ</sup> � � vjk

3

3

obsð Þ k þ 2

the following set of attitude constraints is enforced:

Figure 13. Three-vehicle cross-path trajectory problem.

chooses the vector vki

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 vi (t) is in a cross-path configuration with vehicles j and k, we are concerned only about the two bounding obstacle vectors v ij obsð Þ<sup>t</sup> and vik obsð Þt .

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

Consensus-Based Multipath Planning with Collision Avoidance Using Linear Matrix Inequalities http://dx.doi.org/10.5772/intechopen.71288 19

Figure 13. Three-vehicle cross-path trajectory problem.

moved to obtain smoother phase transition is to measure θ<sup>i</sup>

should be moved. This is done by swapping v<sup>j</sup>

Figure 12. The effects of cross-path conflict resolution.

18 Advanced Path Planning for Mobile Entities

obsð Þ<sup>t</sup> (which is now the new vj

may be left as it is and Eq. (22) is rewritten in the form

vj

2 4

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

vj ð Þ k þ 2

obsð Þt ) remains static.

vehicles in 2D and 3D are shown in [14].

the two bounding obstacle vectors v

former v j

the new v

vector vi

j

(t) and θ<sup>j</sup>

(t) and v j

obsð Þ k þ 2

vj ð Þ k þ 2

5 M

The trajectories obtained by applying this strategy to cross-path collision avoidance for two

If more than two vehicles are involved as shown in Figure 13, for any vehicle i, whose attitude

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

(t) is in a cross-path configuration with vehicles j and k, we are concerned only about

obsð Þt .

(t)) that moves, whereas the former vj

3 5 T

simple collision reconfiguration. Thereafter, when the Q-CAC algorithm expands θ<sup>j</sup>

2 cos <sup>∅</sup> <sup>þ</sup> <sup>μ</sup> � � vj

3

obsð Þ k þ 2

ij

obsð Þ<sup>t</sup> and vik

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)

> 2 4

(t). If θ<sup>j</sup>

obsð Þt , which immediately results in a

(t) < θ<sup>i</sup>

≥ 0: (28)

(t), then vj

(t) (which is now

(t), it is the

(t)

cross-path obstacle vector for avoidance. For example, for k to turn counterclockwise, it chooses the vector vki obsð Þt to avoid instead of v kj obsð Þt . Thus, for the configuration of Figure 13, the following set of attitude constraints is enforced:

$$\begin{bmatrix} 2(\cos \mathcal{Q} + \mu) \\\\ \begin{bmatrix} \boldsymbol{v}\_{obs}^{j}(\boldsymbol{k} + 2) \\\\ \boldsymbol{v}^{j}(\boldsymbol{k} + 2) \end{bmatrix}^{T} \end{bmatrix} \ge 0,\tag{29}$$
 
$$\begin{bmatrix} 2(\cos \mathcal{Q} + \mu) \\\\ \begin{bmatrix} 2(\cos \mathcal{Q} + \mu) \\\\ \boldsymbol{v}^{j}(\boldsymbol{k} + 2) \end{bmatrix}^{T} \begin{bmatrix} \boldsymbol{v}\_{obs}^{j}(\boldsymbol{k} + 2) \\\\ \boldsymbol{v}^{j}(\boldsymbol{k} + 2) \end{bmatrix}^{T} \end{bmatrix} \ge 0,\tag{30}$$

$$\begin{bmatrix} 2(\cos \mathcal{Q} + \mu) & \begin{bmatrix} v\_{obs}^{ki}(k+2) \\\\ v^k(k+2) \end{bmatrix}^T \\\\ \begin{bmatrix} v\_{obs}^{ki}(k+2) \\\\ v^k(k+2) \end{bmatrix} & \mathcal{M} \end{bmatrix} \ge 0. \tag{31}$$

4. Simulation results

no collisions.

5. Conclusion

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

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

http://dx.doi.org/10.5772/intechopen.71288

21

In [14], more simulations and analyses are presented, together with the limitations of this

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

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

approach, which remains to be explored for future development.

#### 3.4. Consensus with Q-CAC–based avoidance

Once a safe attitude vector vi (k) is computed at time k for any i, the next position x<sup>i</sup> (k + 1) is computed as a point a distance r ∗i /2 from the current position, along the vector vi (k). Note that vi (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<sup>i</sup> ≤ 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 \log\_{10}(k+1) \frac{\Delta t}{2\lambda\_2(L)} \Gamma(\mathbf{x} - \mathbf{x}^{\theta \theta}) - \beta \log\_{10}(k+1) \frac{\Delta t}{2\lambda\_2(L)} \mathbf{K}(\mathbf{x} - \mathbf{x}^{\theta \theta}). \tag{32}$$

And for the leaderless architecture,

$$\mathbf{u} = -\eta \log \mathbf{u}\_{10}(\mathbf{k} + 1) \frac{\Delta t}{2\lambda\_2(\mathbf{L})} \Gamma \left( \mathbf{x} - \mathbf{x}^{\rho \mathbf{f}} \right) - \beta \log \mathbf{u}\_{10}(\mathbf{k} + 1) \frac{\Delta t}{2\lambda\_2(\mathbf{L})} \mathbf{K}(\mathbf{x} - \mathbf{x}\_d), \tag{33}$$

where λ2(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 log10(k + 1) and the term <sup>Δ</sup><sup>t</sup> <sup>2</sup>λ2ð Þ <sup>L</sup> are used to reduce <sup>k</sup>u<sup>k</sup> when <sup>k</sup> is small and increase kuk 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<sup>i</sup> > <sup>r</sup>∗<sup>i</sup> <sup>2</sup> , normalize ui and set ui <sup>¼</sup> <sup>r</sup>∗<sup>i</sup> <sup>2</sup> ui .

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