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{A}\mathbf{x} + \mathbf{B}\mathbf{u},\tag{2}
$$

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

$$
\dot{\mathbf{x}} = \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{F}\mathbf{u} \tag{3}
$$

Figure 1. Consensus-based reconfiguration in adversarial situation using topology.

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

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

7

Figure 2. Topology: a leader-follower graph.

drives x to x<sup>f</sup> [15]. Here, x = [x<sup>1</sup> , ⋯, x<sup>n</sup> ] is a stacked vector of the initial positions of the vehicles, <sup>u</sup> <sup>=</sup> � <sup>Γ</sup>(<sup>x</sup> � <sup>x</sup>off), <sup>Γ</sup> <sup>=</sup> <sup>L</sup> <sup>⊗</sup> <sup>I</sup>p, <sup>I</sup><sup>p</sup> is the identity matrix of size <sup>p</sup> � <sup>p</sup>, and <sup>p</sup> 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} = -\Gamma(\mathbf{x} - \mathbf{x}^{off}) + \mathbf{K}(\mathbf{x}^{off} - \mathbf{x}).\tag{4}$$

The corresponding protocol for a leaderless architecture is

$$\mathbf{u} = -\Gamma(\mathbf{x} - \mathbf{x}^{off}) + \mathbf{K}(\mathbf{x}\_d - \mathbf{x}) \tag{5}$$

where <sup>x</sup><sup>d</sup> 6¼ <sup>x</sup>off is the desired final position and is different from the formation configuration, K = eIn, (0 < e ≪ 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.

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

Figure 1. Consensus-based reconfiguration in adversarial situation using topology.

Figure 2. Topology: a leader-follower graph.

situations. A comprehensive presentation of the necessary mathematical tools for this work

<sup>u</sup> <sup>=</sup> � <sup>Γ</sup>(<sup>x</sup> � <sup>x</sup>off), <sup>Γ</sup> <sup>=</sup> <sup>L</sup> <sup>⊗</sup> <sup>I</sup>p, <sup>I</sup><sup>p</sup> is the identity matrix of size <sup>p</sup> � <sup>p</sup>, and <sup>p</sup> is the state dimension of

To begin, we first consider the reference consensus path planning problem. To this end, the

where <sup>x</sup><sup>d</sup> 6¼ <sup>x</sup>off is the desired final position and is different from the formation configuration,

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

following protocol is proposed for a leader-follower communication graph architecture

x\_ ¼ Ax þ Bu, (2)

x\_ ¼ Ax þ BFu (3)

] is a stacked vector of the initial positions of the vehicles,

<sup>u</sup> ¼ �<sup>Γ</sup> <sup>x</sup> � <sup>x</sup>off <sup>þ</sup> K xoff � <sup>x</sup> : (4)

<sup>u</sup> ¼ �<sup>Γ</sup> <sup>x</sup> � <sup>x</sup>off <sup>þ</sup> K xð Þ <sup>d</sup> � <sup>x</sup> , (5)

(including graph theory and consensus theory) can be found in [14].

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

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

, ⋯, x<sup>n</sup>

The corresponding protocol for a leaderless architecture is

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

basic consensus algorithm does not guarantee collision avoidance.

K = eIn, (0 < e ≪ 1), and n is the dimension of x.

3.1. Consensus-based arbitrary reconfigurations

It was shown that for the dynamic system.

drives x to x<sup>f</sup> [15]. Here, x = [x<sup>1</sup>

the vehicles.

Proof: see [14].

3. Solutions

6 Advanced Path Planning for Mobile Entities

#### 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 xobs; it is desired that the time evolution of any vehicle state xi (t) from t<sup>0</sup> to tf should avoid the constraint region shown in Figure 3.

The feasible region is thus defined by

$$\mathbf{x}\_{\text{fus}} = \{ \mathbf{x} \in \mathbb{R}^{m \times m} || \mathbf{x} - \mathbf{x}\_{\text{obs}} || > r^\* \}, \ m \in \mathbb{R}, \tag{6}$$

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

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

of an obstacle, obs, centered on xobs. Let v(t) be the unit vector extending from the centre of xobs

(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 xobs 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

(t) intersects the safety region

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

(t) will be regarded as an

: (7)

(t) will move with time. The positions of

<sup>∗</sup> + ε to obtain the actual safe trajectory.

, and n is the number of vehicles. The

(t) is forced to move on the surface of the safety

v\_ð Þ¼ t Dð Þt vð Þt , (8)

vð Þ¼ k þ 1 ΔtDð Þ k vð Þ k , (9)

(t) and v(t). This implies that one of

(t) can

(tf)

9

(t) is

(tk), at which

(t0) to xi

(t) and v(t)

At any time t, suppose the safety region of vehicle i centered on x<sup>i</sup>

should be larger than some given angle ∅, ∀t. The constraint is

The idea is to control the angle between the unit vectors vi

used to control the position of the vehicle; therefore, v<sup>i</sup>

where D ∈Spn, p is the dimension of the state vector xi

The discrete time equivalent of the above differential equation is

(t) define a trajectory path for x<sup>i</sup>

(t) is obtained as a unit vector, vi

region bounding S. At some time tk, xi

vi

(t) in the direction of v(t) in the frame of obs. The vector vi

imaginary vector whose direction can be constrained to change with time. The vector vi

then be used to find a sequence of trajectories around obs which guides i from xi

The problem reduces to the Q-CAC problem. It is desired that the angle θ between vi

(t). Thus, xi

, then the dynamics of v(t) is defined as

ð Þ<sup>t</sup> Tv tð Þ <sup>≤</sup> cos <sup>∅</sup>, <sup>∀</sup>t<sup>∈</sup> <sup>t</sup>0; tf

safety region in Figure 4. To obtain the unit vector v(t), the actual vector extending from 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

where k = 0, ⋯, N (NΔt = tf) is the discrete time equivalent of t and Δt is the discretization timestep. 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;

angle between the two vectors in v(t) with time while also keeping them normalized.

(t) is multiplied by r = r

(t) or v(t) must remain static, whereas the other moves with time. Vector vi

(t) will arrive close to a point indicated by vi

(tf) is unconstrained. This is shown by the black dots on the boundary of the

(t) in the direction of the point of intersection is normalized. After the solution

attitude control problem.

without violating (Eq. (6)).

or xi

vector vi

the vectors vi

a translation to xi

centre of xobs or xi

(t) T v(t) T ] T

Let v(t)=[vi

vi

vi

where r <sup>∗</sup> 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],

Figure 3. Constrained control problem for a static spherical obstacle.

Figure 4. Constrained control problem for static nonspherical obstacle.

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<sup>i</sup> (t) intersects the safety region of an obstacle, obs, centered on xobs. Let v(t) be the unit vector extending from the centre of xobs or xi (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 xobs 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 vi (t) in the direction of v(t) in the frame of obs. The vector vi (t) will be regarded as an imaginary vector whose direction can be constrained to change with time. The vector vi (t) can then be used to find a sequence of trajectories around obs which guides i from xi (t0) to xi (tf) without violating (Eq. (6)).

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

$$\left[\boldsymbol{v}^{i}(t)^{T}\boldsymbol{v}(t)\right]\leq\cos\bigotimes,\forall t\in\left[t\_{0},t\_{f}\right].\tag{7}$$

The idea is to control the angle between the unit vectors vi (t) and v(t). This implies that one of the vectors vi (t) or v(t) must remain static, whereas the other moves with time. Vector vi (t) is used to control the position of the vehicle; therefore, v<sup>i</sup> (t) will move with time. The positions of vi (t) define a trajectory path for x<sup>i</sup> (t). Thus, xi (t) is forced to move on the surface of the safety region bounding S. At some time tk, xi (t) will arrive close to a point indicated by vi (tk), at which a translation to xi (tf) 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 xobs or xi (t) in the direction of the point of intersection is normalized. After the solution vi (t) is obtained as a unit vector, vi (t) is multiplied by r = r <sup>∗</sup> + ε to obtain the actual safe trajectory.

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

3.2. Quadratically constrained attitude control-based collision avoidance

centered on a point xobs; it is desired that the time evolution of any vehicle state xi

xfeas <sup>¼</sup> <sup>x</sup> <sup>∈</sup> <sup>R</sup><sup>m</sup>�<sup>m</sup>jk k <sup>x</sup> � xobs <sup>&</sup>gt; <sup>r</sup>

<sup>∗</sup> is the radius of S, bounded by a safety region of width ε.

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

8 Advanced Path Planning for Mobile Entities

where r

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

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

<sup>∗</sup> f g, m <sup>∈</sup> <sup>R</sup>, (6)

(t) from t<sup>0</sup> to tf

$$
\dot{\mathbf{v}}(t) = \mathbf{D}(t)\mathbf{v}(t), \tag{8}
$$

where D ∈Spn, p is the dimension of the state vector xi , 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}(k+1) = \Delta t \mathbf{D}(k)\mathbf{v}(k),\tag{9}$$

where k = 0, ⋯, N (NΔt = tf) is the discrete time equivalent of t and Δt is the discretization timestep. 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{^I H \mathbf{R}^{-1} H^T - Q \le 0},\tag{10}$$

optimization problem of collision avoidance is essential to find a feasible vi subject to the

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

2 cos <sup>∅</sup> <sup>þ</sup> <sup>μ</sup> � � <sup>v</sup>ð Þ<sup>t</sup> <sup>T</sup> vð Þt M

(0)k = 1 and kv(0)k = 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

that they must avoid each other always. As shown in Figure 5, any time their safety regions are

(t), vj

ð Þt T vi

ð Þt T v j

∀t ∈[t0, tf],

" #

vT

Eq. (17) is essentially the discrete time version of <sup>v</sup>ð Þ<sup>t</sup> <sup>T</sup>

(t) and attitude vectors vi

θi

θj

Figure 5. Constrained control problem for dynamic obstacles.

violated and the point of their intersection in the coordinate frame of i is vi

ð Þ<sup>t</sup> <sup>≥</sup> <sup>∅</sup> � <sup>v</sup><sup>i</sup>

ð Þ<sup>t</sup> <sup>≥</sup> <sup>∅</sup> � <sup>v</sup><sup>j</sup>

v<sup>k</sup>þ<sup>1</sup> ¼ ΔtDð Þt vk, (16)

<sup>k</sup> ð Þ¼ vkþ<sup>1</sup> � vk 0, (17)

≥ 0: (18)

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

obsð Þt .

T v(t) 11

v\_ðÞ¼ t 0 which guarantees that v(t)

(t), respectively. Collision avoidance requires

obsð Þt ≤ cos ∅, (19)

obsð Þt ≤ cos ∅, (20)

following constraints:

= 2, ifkvi

with states x<sup>i</sup>

(t), xj

The avoidance requirements are

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

$$
\begin{bmatrix} \mathbf{Q} & \mathbf{H} \\ \mathbf{H}^T & \mathbf{R} \end{bmatrix} \succeq \mathbf{0}.\tag{11}
$$

Note that Eq. (7) is equivalent to

$$\underbrace{\begin{bmatrix} \boldsymbol{v}^{i}(t)^{T} & \boldsymbol{v}(t)^{T} \end{bmatrix}}\_{\boldsymbol{v}(t)^{T}} \begin{bmatrix} \mathbf{0}\_{3} & \frac{1}{2}\mathbf{I}\_{3} \\\\ \frac{1}{2}\mathbf{I}\_{3} & \mathbf{0}\_{3} \end{bmatrix} \begin{bmatrix} \boldsymbol{v}^{i}(t) \\\\ \boldsymbol{v}(t) \end{bmatrix} \leq \cos \mathcal{Q}\_{i} \tag{12}$$

which also implies that

$$\underbrace{\begin{bmatrix} \boldsymbol{v}^{i}(t)^{T}\boldsymbol{v}(t)^{T} \\ \end{bmatrix}}\_{\boldsymbol{v}(t)^{T}} \underbrace{\begin{bmatrix} \mathbf{0}\_{3} & \mathbf{I}\_{3} \\ \mathbf{I}\_{3} & \mathbf{0}\_{3} \\ \end{bmatrix}}\_{\mathbf{G}} \begin{bmatrix} \boldsymbol{v}^{i}(t) \\ \boldsymbol{v}(t) \end{bmatrix} \leq 2\cos\mathfrak{D},\tag{13}$$

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

$$
\underbrace{\begin{bmatrix} v^i(t)^T & v(t)^T \end{bmatrix}}\_{v(t)^T} \left(\mu I\_6 + \begin{bmatrix} \mathbf{0}\_3 & I\_3 \\ I\_3 & \mathbf{0}\_3 \end{bmatrix}\right) \underbrace{\begin{bmatrix} v^i(t) \\ v(t) \end{bmatrix}}\_{v(t)} \le 2\left(\cos\mathfrak{D} + \mu\right). \tag{14}
$$

Let M ¼ μI<sup>6</sup> þ 0<sup>3</sup> I<sup>3</sup> I<sup>3</sup> 0<sup>3</sup> � � � � �<sup>1</sup> , then M is positive definite. Therefore, following the Schur complement formula, the LMI equivalent of Eq. (14) is

$$
\begin{bmatrix}
2\left(\cos\mathcal{Q}+\mu\right) & \mathbf{v}(\mathbf{t})^T\\ \mathbf{v}(\mathbf{t}) & \mathbf{M}
\end{bmatrix} \ge 0. \tag{15}
$$

For collision avoidance, the dynamic system (Eq. (8)) is solved whenever it is required, subject to the attitude constraint (Eq. (15)) and norm constraints <sup>k</sup>vi (t)k = 1 and kv(t)k = 1. Thus, the optimization problem of collision avoidance is essential to find a feasible vi subject to the following constraints:

$$\mathbf{v}\_{k+1} = \Delta t \mathbf{D}(t) \mathbf{v}\_{k'} \tag{16}$$

$$\mathbf{v}\_{\mathbf{k}}^{\mathrm{T}}(\boldsymbol{\upsilon}\_{\mathbf{k}+1} - \boldsymbol{\upsilon}\_{\mathbf{k}}) = \mathbf{0},\tag{17}$$

$$
\begin{bmatrix}
\mathbf{2}\left(\cos\mathfrak{Q}+\mu\right) & \mathbf{v}(t)^T\\ 
\mathbf{v}(t) & \mathbf{M}
\end{bmatrix} \ge 0. \tag{18}
$$

Eq. (17) is essentially the discrete time version of <sup>v</sup>ð Þ<sup>t</sup> <sup>T</sup> v\_ðÞ¼ t 0 which guarantees that v(t) T v(t) = 2, ifkvi (0)k = 1 and kv(0)k = 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<sup>i</sup> (t), xj (t) and attitude vectors vi (t), vj (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 vi obsð Þt .

The avoidance requirements are

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

HT � <sup>Q</sup> <sup>≤</sup> <sup>0</sup>, (10)

≥ 0: (11)

≤ cos ∅, (12)

≤ 2 cos ∅, (13)

<sup>≤</sup> 2 cos <sup>∅</sup> <sup>þ</sup> <sup>μ</sup> � �: (14)

≥ 0: (15)

(t)k = 1 and kv(t)k = 1. Thus, the

, and R > 0 are equivalent to and can be represented by the linear matrix

HR�<sup>1</sup>

Q H H<sup>T</sup> R � �

> 1 2 I3

3 7 5 vi ð Þt v tð Þ

vi ð Þt v tð Þ

" #

ð Þt v tð Þ

" #


, then M is positive definite. Therefore, following the Schur

" #

03

2 6 4

1 2 I<sup>3</sup> 0<sup>3</sup>

0<sup>3</sup> I<sup>3</sup> I<sup>3</sup> 0<sup>3</sup> � �


number μ which is larger than the largest absolute value of the eigenvalues of G, then

μI<sup>6</sup> þ

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

> 0<sup>3</sup> I<sup>3</sup> I<sup>3</sup> 0<sup>3</sup> � � � � vi

2 cos <sup>∅</sup> <sup>þ</sup> <sup>μ</sup> � � v tð Þ<sup>T</sup> vð Þt M

For collision avoidance, the dynamic system (Eq. (8)) is solved whenever it is required, subject

" #

inequality

where Q = QT

inequality

, R = RT

10 Advanced Path Planning for Mobile Entities

Note that Eq. (7) is equivalent to

which also implies that

Let M ¼ μI<sup>6</sup> þ

vi

vi ð Þ<sup>t</sup> Tv tð Þ<sup>T</sup> h i |fflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflffl} v tð Þ<sup>T</sup>

vi

0<sup>3</sup> I<sup>3</sup> I<sup>3</sup> 0<sup>3</sup> � � � � �<sup>1</sup>

ð Þ<sup>t</sup> <sup>T</sup> v tð Þ<sup>T</sup> � � |fflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflffl} v tð Þ<sup>T</sup>

complement formula, the LMI equivalent of Eq. (14) is

to the attitude constraint (Eq. (15)) and norm constraints <sup>k</sup>vi

ð Þ<sup>t</sup> <sup>T</sup> v tð Þ<sup>T</sup> h i |fflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflffl} v tð Þ<sup>T</sup>

$$\mathcal{O}^i(t) \ge \mathcal{O} \equiv \upsilon^i(t)^T \upsilon^i\_{\text{obs}}(t) \le \cos \mathcal{Q}\_i \tag{19}$$

$$\mathcal{O}^{j}(t) \succeq \mathcal{Q} \equiv \upsilon^{j}(t)^{\top} \upsilon^{j}\_{obs}(t) \leq \cos \mathcal{Q},\tag{20}$$

∀t ∈[t0, tf],

Figure 5. Constrained control problem for dynamic obstacles.

#### 12 Advanced Path Planning for Mobile Entities

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

$$\begin{bmatrix} 2(\cos \mathcal{Q} + \mu) \\\\ \begin{bmatrix} \mathbf{v}^j(k+2) \\\\ \mathbf{v}^j\_{abs}(k+2) \end{bmatrix}^T \\\\ \begin{bmatrix} \mathbf{v}^j(k+2) \\\\ \mathbf{v}^j\_{abs}(k+2) \end{bmatrix} & \mathbf{M} \\\\ \begin{bmatrix} 2(\cos \mathcal{Q} + \mu) & \begin{bmatrix} \mathbf{v}^j(k+2) \\\\ \mathbf{v}^j\_{abs}(k+2) \end{bmatrix}^T \\\\ \begin{bmatrix} \mathbf{v}^j(k+2) \\\\ \mathbf{v}^j\_{abs}(k+2) \end{bmatrix} & \mathbf{M} \\\\ i, j = 1, \cdots, n, i \neq j. \end{bmatrix} \end{bmatrix} \ge 0,\tag{22}$$

3.3.1. Detecting and resolving a simple collision

or infinite line Lij(t) passing through the points x<sup>i</sup>

are not parallel. Note that when xi

(t) and choosing the next feasible trajectories r

3.3.2. Detecting and resolving a head-on collision

Figure 7(a) illustrates the head-on collision problem.

A head-on collision problem is any configuration in which Dij(t) ≤ (r

directions (or attitude vectors) vi

shown in Figures 5 and 6.

ρij(t) is computed.

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

> r ∗i

Figure 6. Simple collision problem.

collision configuration.

provided ε<sup>i</sup>

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

θj

and xj

A simple collision problem is any configuration in which Dij(t) ≤ (r

(t) and vj

(t), xj

and xj

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

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

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

of intersection can be computed for both vectors. Examples of simple collision problems are

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

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>

of this section are attempts to reduce more complex collision configurations to a simple

∗

, the new trajectories are bound to satisfy the feasible regions separated by ρij(t),

for any i. The rest of the avoidance strategies developed in the remaining part

i + r j

(t) of vehicles i and j are on different sides of the plane

(t); and the attitude vectors xi

/2 along the new direction vectors xi

i + r j ) and vi

(t) T vj

) and the current vector

are not parallel, a point or line

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

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

and

13

(t) and

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

(t) ≈ π rad.

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^{\vec{y}}(t) = \left\| \mathbf{x}^{i}(t) - \mathbf{x}^{j}(t) \right\| \le \left( r^{i} + r^{j} \right), \tag{23}$$

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<sup>i</sup> (t) and xj (t) and the desired final states are xi (tf) and xj (tf). 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.
