Collision Avoidance 103

Free Trajectories
100

90

80

70

60

50
y

40

30

20

10

–10
–10 –5 0 5 10 15 20 25
x

Figure 4.23 Complete trajectory set for 15 obstacles

15 obstacles resulted in a set of trajectories that avoided 10 of them. This took

approximately 1 s in computing time in MATLAB code.

4.3 Obstacle Avoidance of Unmapped Static Obstacles

If a UAV detects an unmapped obstacle by means of an on-board sensor,

the UAV has to re-plan the path either by varying the curvature between
the two waypoints under consideration or by re-planning the path using
an intermediate waypoint. The obstacles are tested by determining if the
obstacle safety circle intersects the UAV safety circle. If the intersection is not
empty, then re-planning is required. This can be done either by increasing the
curvature of the path or by creating an intermediate waypoint and producing
a new path that includes this new waypoint.
Assuming the obstacle circle Oobs and the UAV safety circle Osafe , the
condition for collision avoidance is

Oobs ∩ Osafe = ∅. (4.40)

104 Cooperative Path Planning of Unmanned Aerial Vehicles

4.3.1 Safety Circle Algorithm

Assuming that a group of UAVs are cooperating in the manner described
in Chapter 6, then the path planning algorithm described in that chapter
needs to be modified. A simple extension of the algorithm to modify the
paths of the UAVs by changing the curvature of the arcs of the path is given
as follows:

(i) Produce flyable paths for each UAV.

(ii) Change the course of the path to meet the safety constraints by increas-
ing the curvature.
(iii) Calculate the length of the paths.
(iv) Find the reference path.
(v) Increase the length of the shorter paths to the length of the reference
path. This results in paths of equal length.
(vi) Check again for the paths meeting the safety constraints.
(vii) If not, adjust the position of the new waypoint and increase the curvature
to meet the safety conditions to produce paths of equal length.

Although the above algorithm can be used to ensure collision avoidance,

handling unmapped obstacles is best achieved by generating intermediate
waypoints and/or poses that will avoid the obstacle, as this algorithm is
faster. This is because it does not depend on an iterative process of increasing
the curvature until an avoidance path is produced. The advantages of this
method is that it is efficient and implementation is simple.

4.3.2 Intermediate Waypoint Algorithm

When an obstacle intersection is detected, the path planner generates an
intermediate waypoint so that the obstacle is avoided. Consider a flyable
trajectory r(t) generated for a given set of poses and/or waypoints. The
obstacle avoidance algorithm calls the path planner to re-plan the nominal
path by selecting a new waypoint or pose. The schematic of the concept is
given in Figure 4.24. In the figure, the central hatched circle is the obstacle.
Points M and N on the safety circle are the intermediate poses, and the points
of intersection of the obstacle safety circle and the nominal path (dotted in
the figure) are given by X1 (entry) and X2 (exit). The intermediate waypoints
are generated by first drawing a line between the entry point X1 and the exit
point X2 . If a line orthogonal to this a constructed, it will intersect the obstacle
safety circle at two point N and M. These are then designated as the potential
intermediate waypoints. The intermediate waypoint is selected based on the
Collision Avoidance 105

X2 p2

N
r2(t)
C

M
X1

r (t )

r1(t )

p1

Figure 4.24 Threat handling by intermediate pose. Reprinted with permission

of Elsevier

location of the centre of the obstacle C, which is either to the left or the right
of this line. If the centre C is left to the line X1 –X2 , the intermediate pose
M is selected on the right-hand side of the obstacle region, and vice versa.
In the figure, the initial dotted path is r(t) and the modified solid line paths,
generated with the intermediate waypoint M, are r1 (t) and r2 (t). Hence the
multiple UAV path planning algorithm developed in Chapter 6 is modified
to give the following:

(i) Produce flyable paths for each UAV.

(ii) Change the course of the path to meet the safety constraints by creating
intermediate waypoints.
(iii) Calculate the length of the paths.
(iv) Find the reference path.
(v) Increase the length of the shorter paths to the length of the reference
path. This results in paths of equal length.
(vi) Check again for the paths meeting the safety constraints.
(vii) If not, adjust the position of the new waypoint and increase the curvature
to meet the safety conditions to produce paths of equal length.
106 Cooperative Path Planning of Unmanned Aerial Vehicles

Restricted TARGET
Regions

BASE

Figure 4.25 UAVs in a static cluttered environment. Reprinted with permission

of ASME

4.4 Algorithmic Implementation

A schematic for a cluttered environment is shown in Figure 4.25. The obstacle

regions are modelled as polygons but will have safety circles associated with
them. The regions are assumed stationary and their positions are known.
The path planning of multiple UAVs through a sequence of poses can be
written as
ri,j−1 (q)
Ps,i,j−1 (xs,i,j−1 , ys,i,j−1 , θs,i,j−1 ) −−−−→ Pf,i,j (xf,i,j , yf ,i,j , θf,i,j ), (4.41)
 
i = 1, . . . , nUAV , j = 2, . . . , np , |κi (q)| < κmax , safe , length ,

where nUAV is the number of UAVs, np is the number of poses (including

the start and finish poses), the suffix i is for the ith UAV and the suffix j
is for the jth pose. Two UAVs are considered for implementation of the
algorithm. The UAVs are assumed to be homogeneous in their physical
capabilities, and hence they will both fly at the same speed and have the
Collision Avoidance 107

35

30 TARGET

25
UAV2

20
Y

15

10
BASE
UAV1
5

5 10 15 20 25
X

Figure 4.26 Dubins flyable paths of two UAVs in a cluttered environment

same curvature constraints. In previous chapters, Dubins, clothoid and PH

paths are generated, so we shall consider solutions based on each of these.

4.4.1 Dubins Path Modification

Figure 4.26 shows the paths of two UAVs in a cluttered environment. The
flights path of UAV2 intersects the obstacle and with the flight path of UAV1.
The curvatures of the circular arcs of the Dubins path are varied till the
flight path avoids the threat region. Figure 4.27 shows the new safe and
flyable path after increasing the curvature of the UAV2 path. Figure 4.28
shows the solution of the same problem by using an intermediate waypoint.
Once the obstacle intersection has been detected, an intermediate waypoint
is generated using the principle explained in section 4.3.2.

4.4.2 Clothoid Path Modification

Figure 4.29 shows nominal paths as a result of using clothoid paths. The
poses and threats are generated randomly. The distances between the poses
are at least twice as great as the minimum radius of curvature (maximum
curvature) of the UAV. This is to ensure that a flyable path exists between the
108 Cooperative Path Planning of Unmanned Aerial Vehicles

35

30 TARGET

25
UAV1
20
Y

15

10 UAV2
BASE

5 10 15 20 25
X

Figure 4.27 Re-planning the Dubins path of UAV2 by curvature adjustment

35

30 TARGET

25

20
Y

15 UAV2
Intermediate way point
10
BASE

5 UAV1

5 10 15 20 25
X

Figure 4.28 Re-planning the Dubins path of UAV2 using an intermediate

waypoint
Collision Avoidance 109

200

180 4

2 3
160
4
140 1
3
2
120 1
4
100 3
y

1 2
4
80

60 2
1 3
40 4

20 2 3
1

0
0 20 40 60 80 100 120 140 160 180 200
x
Initial paths (same as final paths if no modification)
Final paths after modification
Paths generated through intermediate waypoints

Figure 4.29 Five UAVs each with four waypoints in cluttered space. Reprinted
with permission of Elsevier

poses. The filled ellipses are the obstacles. Flyable paths are then produced
between the poses to give the nominal paths (dashed in the figure). The
obstacle intersections are handled by generating intermediate waypoints
and/or poses as explained in section 4.3.2. The intersection of paths with the
no-fly zones are determined iteratively for the clothoid arc segments, while
the intersection of lines with ellipses can be detected by simple geometry.
An intermediate pose or waypoint is produced for each intersection within
the obstacle safety zone. Following this, the paths are re-planned to pass
through the new waypoints.
From the figure, it is also seen that, when a path intersects with a single
obstacle, re-planning of a new path is simple, as it only has to avoid a single
obstacle. However, re-planning is difficult when obstacles appear in clusters,
as in the case of the second and third UAVs. In the case of the second UAV,
and in between the third and fourth waypoints, there are three obstacles
110 Cooperative Path Planning of Unmanned Aerial Vehicles

in a cluster. The first re-planning of the initial path intersects the other
obstacle and hence requires further re-planning to avoid all of the obstacles.
A similar situation arises for the third UAV, where a cluster of two obsta-
cles appear together. Finally, the curvature of the first four paths, ordered
from the top, are increased to that of the reference path, which is the path
length of fifth UAV. This is similar to the path planning for a cluttered envi-
ronment for mapped obstacles detailed in section 4.2 using the differential
geometric approach.

4.4.3 PH Path Modification

For the case of PH paths, first the initial paths have to be optimized for
curvature continuity, as the initial paths are only tangent-continuous. The
flight paths shown in Figure 4.30 are the initial nominal paths, which are
tangent-continuous with the start pose Ps and the finish pose Pf . As the paths

45

Pf
40 Pf

35

30

25
Y

20

15

10
Ps Ps

5 UAV2
UAV1

0
0 5 10 15 20 25 30 35 40
X

Figure 4.30 Initial paths (only tangent continuity) – PH 2D in cluttered space.

Reprinted with permission of ASME
Collision Avoidance 111

45

40 Pf Pf

35

30

25
Y

20

15

10 Ps
Ps

5
UAV1 UAV2

0
0 5 10 15 20 25 30 35 40
X

Figure 4.31 Flyable paths – PH 2D in cluttered space. Reprinted with permis-

sion of ASME

do not meet the maximum-curvature bound, they have to be optimized first

for the flyable path. The resulting path length of UAV1 is 35.96 units and
that of UAV2 is 35.92 units.
Figure 4.31 shows the UAV paths optimized for their curvatures. The
resulting lengths of the new paths are 40.69 and 37.13 units respectively.
Now the resulting paths do not meet the safety requirements, as intersections
with the obstacle safety circles and the UAV safety circles occur. The path of
UAV1 intersects with the obstacles, while that of UAV2 has a safety circle
intersection. Both the paths need further modification to their curvature.
Further increase in the curvature of the paths result in paths that meet
the safety constraints. Figure 4.32 shows the resulting safe flight paths. The
corresponding path lengths are now 42.57 and 41.12 units. As the length s1
is longer than s2 , the flight path r1 is designated as the reference path. The
length of r2 thus has to be increased to that of r1 to produce paths of equal
lengths. Figure 4.33 shows the final paths of equal length 42.57 units.
112 Cooperative Path Planning of Unmanned Aerial Vehicles

45

Tangent Pf
40 Pf
Circle

35 Bezier
polygon

30

25
Obstacles
Y

20

15

10
Ps
5
Ps
UAV1 UAV2
0
0 5 10 15 20 25 30 35 40
X

Figure 4.32 Feasible (safe and flyable) paths – PH 2D in cluttered space.

Reprinted with permission of ASME

4.4.4 Obstacle Avoidance in 3D

For obstacle avoidance in 3D, a simple case of flying through an urban area
containing buildings is considered. Each building is square in shape, and
the size of each building is different. The buildings are overlapping in this
example because the locations of the buildings are generated randomly. This
is not as artificial as it first appears, because urban environments are complex
and building shapes include such intersecting squares as representative
shapes. The scenario is shown in Figure 4.34. The heights and areas of the
buildings are generated randomly. As shown in the figure, each building is
enclosed by a cylinder of diameter equal to the length of the diagonal of the
square. This is to simplify the distance calculation between the buildings. If
the distance between the buildings is less than a threshold value, the UAV
cannot fly between them.
The positions of the obstacles are defined by means of the centres of the
encompassing safe distance circles. Delaunay triangulation (Berg et al. 2000)
Collision Avoidance 113

45

Tangent Pf
Pf
40 Circle

35
Bezier
30 polygon

25
Obstacles
Y

20

15

10
Ps
5 Ps
UAV1 UAV2
0
0 5 10 15 20 25 30 35 40 45
X

Figure 4.33 Paths of equal lengths – PH 2D in cluttered space. Reprinted with

permission of ASME

1000

800

600
100
400
50
Z

200
0 Y
0 200 400 600 0
800 1000
X

Figure 4.34 Obstacle avoidance in 3D

114 Cooperative Path Planning of Unmanned Aerial Vehicles

is applied on this database, which connects each point in the database to its
natural neighbours, thus forming a connectivity graph of obstacles to their
immediate neighbours. The UAV has to fly across these connecting lines
between the obstacles. In order to make sure that the distances between the
obstacles are large enough for the UAV, any connecting lines between the
obstacles whose length is less than the safety radius of the UAV is deleted
from the connectivity graph. Figure 4.35 shows the 2D projection of the
triangulation of 10 obstacles (buildings). The figure shows the connectivity
between their neighbourhoods – for example, the obstacle no. 9 at the bottom
right corner is connected to its neighbours 1, 4 and 8, but not to any others.
This helps to decide whether the UAV can fly in-between the obstacles.
This is done by calculating the available distance between the obstacles,
considering the size (radius) of the buildings. The small open circles are the
centre points (nodes) of the lines through which the UAV can fly. The nodes
are waypoints for the UAVs. Route planning is done by finding the shortest
route through these nodes. The Dijkstra search algorithm (Dijkstra 1959) is
used to find the route.
The waypoints can be connected using either Dubins, clothoid, or PH
paths. Here a Dubins path is used to connect the nodes. The height of the
waypoints can be chosen depending on the possible climb rate of the UAV.
The simulations in Figures 4.34 and 4.36 use 30 obstacles. The grey lines
are the triangulation of obstacle positions, and the thin black lines are the

1000

800

600

Y
400

1000
200 800
600
400
0 200 X
0

Figure 4.35 Path planning with obstacle avoidance in 3D

Collision Avoidance 115

1000

900

800

700

600

500
Y

400

300

200

100

0
0 200 400 600 800 1000
X

Figure 4.36 Obstacle avoidance in 3D (2D projection)

triangulation formed by the nodes including the start point (node no. 1) and
goal point (node no. 32). The numbers on the buildings are the label numbers
for each building.

References

Agarwal, P. K., Biedl, T., Lazard, S., Robbins, S., Suri, S. and Whitesides, S. 2002.
Curvature-constrained shortest paths in a convex polygon. SIAM Journal of Com-
puting, 31(6), 1814–1851.
Assaf, D. and Sharlin-Bilitzky, A. 1994. Dynamic search for a moving target. Journal
of Applied Probability, 31(2), 438–457.
Beard, R., McLain, T., Goodrich, M. and Anderson, E. 2002. Coordinated target
assignment and intercept for unmanned air vehicles. IEEE Transactions on Robotics
and Automation, 18(6), 911–922.
Benkoski, S. J., Monticino, M. G. and Weisinger, J. R. 1991. A survey of the search
theory literature. Naval Research Logistics, 38(4), 468–494.
Berg, M., Kreveld, M., Overmars, M. and Schwarzkopf, O. 2000. Computation Geometry,
Algorithms and Applications. Springer.
Bicchi, A. and Pallottino, L. 2000. An optimal cooperative conflict resolution for air
traffic management systems. IEEE Transactions on Intelligent Transportation Systems,
1(4), 221–232.