AIAA SciTech Forum 10.2514/6.

2018-1592
8–12 January 2018, Kissimmee, Florida
2018 AIAA Guidance, Navigation, and Control Conference

Waypoint Following for Fixed-Wing MAVs in 3D Space

Nikhil Kumar Singh∗ and Sikha Hota†

Department of Aerospace Engineering, Indian Institute of Technology Kharagpur, Kharagpur, West Bengal, India
721302

In this paper, optimal trajectory generation of a fixed-wing miniature aerial vehicle (MAV)
with bounded turn radius is considered for following a series of waypoints in three dimensional
(3D) space. Using optimal control theory, it was shown in literature that the CSC type path
is the optimal one for given initial and final positions and orientations when the points are
sufficiently far apart. Here, C stands for the circular turn of minimum turning radius of a
Downloaded by UNIVERSITY OF ADELAIDE -INTERNET on January 13, 2018 | http://arc.aiaa.org | DOI: 10.2514/6.2018-1592

MAV and S is for straight line path. As the value of minimum turn radius is different for
different 2D planes in 3D space, in the present work for computing the CSC path between
a pair of waypoints, different values of turn radii for initial and final maneuvers have been
used, which gives shorter path lengths compared to the conventional CSC paths. Finally, the
methodology has been extended to generate the optimal path in obstacle-filled environments to
explore a series of waypoints in sequence.

I. Nomenclature

X0 = initial position of the vehicle

Xf = final position of the vehicle
v1 = initial orientation (unit velocity vector) of the vehicle
v2 = final orientation (unit velocity vector) of the vehicle
va = airspeed of the aerial vehicle
η = maximum load factor
ψ = inclination angle of the 2D maneuver plane with XY plane
r = minimum turn radius in an inclined plane
rh = minimum turn radius in the horizontal plane
rv = minimum turn radius in the vertical plane
L (L) = lift force
W (W) = weight of the MAV
Fr (Fr ) = radial force
m = mass of the MAV
φ = banking angle
g (g) = acceleration due to gravity
X (X) = tangent vector between initial and final maneuver circle
x = unit tangent vector along X
P1 = intersection point between the initial maneuver circle and X
P2 = intersection point between the final maneuver circle and X
U1 (U1 ) = normal vector on the initial maneuver plane
U2 (U2 ) = normal vector on the final maneuver plane
u1 = unit normal vector on the initial maneuver plane
u2 = unit normal vector on the final maneuver plane
O1 = origin of the initial maneuver circle
O2 = origin of the final maneuver circle
ψ1 = angle between the initial maneuver plane and XY plane
ψ2 = angle between the final maneuver plane and XY plane
∗ Research Scholar, Department of Aerospace Engineering, Indian Institute of Technology Kharagpur, Kharagpur, West Bengal, India 721302,

Email: [email protected]
† Assistant Professor, Department of Aerospace Engineering, Indian Institute of Technology Kharagpur, Kharagpur, West Bengal, India 721302,

Email: [email protected]

Copyright © 2018 by the American Institute of Aeronautics and Astronautics, Inc.

All rights reserved.
 r1 = minimum turn radius of the initial maneuver circle
r2 = minimum turn radius of the final maneuver circle
θ1 = initial angular turn
θ2 = final angular turn
W 1 (W1 ) = radius vector pointing from X0 to O1
W 2 (W2 ) = radius vector pointing from X f to O2
w1 = unit radius vector pointing from X0 to O1
w2 = unit radius vector pointing from X f to O2
Y 1 (Y1 ) = radius vector pointing from P1 to O1
Y 2 (Y2 ) = radius vector pointing from P2 to O2
y1 = unit radius vector pointing from P1 to O1
y2 = unit radius vector pointing from P2 to O2
Downloaded by UNIVERSITY OF ADELAIDE -INTERNET on January 13, 2018 | http://arc.aiaa.org | DOI: 10.2514/6.2018-1592

Ce1 = center of the ellipse generated by the intersection of the obstacle and initial maneuver plane
Ce2 = center of the ellipse generated by the intersection of the obstacle and final maneuver plane
de1 = distance between the center of the circular base of obstacle and Ce1
de2 = distance between the center of the circular base of obstacle and Ce2

Note that the symbols in bold fonts are used to express vectors and symbols in parenthesis are used to express the
magnitude of those vectors.

II. Introduction
ecently, UAVs are being heavily employed in several military applications, such as, target localization and
R interception, border patrolling, surveillance of enemy territory etc. Additionally, UAVs are the potential solutions
for several civilian and commercial applications. For example, it can improve the existing scenario in the field of traffic
monitoring, tracking high speed vehicles, following terrorists on run. Other important areas of its application are
performing search and rescue operations caused by any disaster, to obtain a birds-eye view of an area to identify the
source of huge sound or fire breakout or leakage from some chemical plant or to dictate the emission of any radio-active
particle from a nuclear plant and many more.
For many of these applications it is required that UAVs should follow a series of waypoints with desired orientations
while avoiding collision with buildings, trees, elevated grounds or other moving vehicles. The generated path should
also consider the constraint arising out from vehicle’s dynamics for example, for fixed-wing UAVs the constraints are
minimum turning radius, minimum speed, and maximum climb rate. In the civilian field, fixed-wing UAVs are mostly
used for long-distance, long-range and high-altitude missions. For surviving in longer duration mission generated path
should also be the optimal one, which also saves the energy consumption to some extent. This motivates a large number
of researchers to address the several issues of path planning for fixed-wing UAVs. In this paper, we shall consider the
optimal path planning of fixed-wing UAVs in three-dimensional environment to follow a sequence of waypoints with
given orientations without colliding with obstacles.
Dubins [1] obtained a shortest path geometrically for given initial and final positions and orientations in 2D plane.
Shkel and Lumelsky [2] proposed different sets of Dubins paths for large and small distances between initial and final
positions and orientations. Dubins path is of discontinuous curvature profile so Shanmugavel et al. [3] proposed a
quintic Pythagorian hodograph curves in 2D plane to generate a smoother path. Babaei and Mortazavi [4] proposed an
efficient algorithm for fast trajectory planning of unmanned aerial vehicles (UAVs) in a 2D plane without the prior
knowledge of midcourse waypoints. These midcourse waypoints are computed during online flight using optimal
control and geometric principles. In the presence of wind, optimal path planning of miniature air vehicles (MAVs)
are challenging, because the wind speed is about 20-60% of the air speed of MAVs. Hota and Ghose [5] presented an
optimal straight line path convergence method for miniature aerial vehicles in 2D plane both in the presence and in the
absence of wind conditions. Nelson et al. [6] proposed a vector field method for path following of straight line and
circular arcs by MAVs in the presence of constant wind but the generated path is not optimal. Park et al. [7] proposed a
nonlinear path following guidance method of UAVs for tracking the straight line and curved paths accurately in the
presence of wind. Hota and Ghose [8] presented the optimal method of trajectory generation for MAVs for tracking
waypoints as well as entire waypoint segments in the presence of wind.
Using optimal control theory, Sussmann [9] proved that the shortest path for given initial and final positions and
orientations in 3D space is either a helicoidal arc or CSC type paths. He showed that if the points are sufficiently far

2
 apart, the CSC path is the optimal one and for small distances the helicoidal path is the optimal. But how to compute the
CSC path in 3D was not described in [9]. Later Hota and Ghose [10] computed CSC paths for an aerial vehicle with
bounded turn radius using both numerical and geometric techniques. Above mentioned problem was discussed for lower
altitude variation in 3D space. Pharpatara et al. [11] discussed the 3D shortest path planning for a hypersonic glider in a
heterogeneous environment. In heterogeneous environment, the turn radius is not bounded due to change in air density
with higher altitude variation. Wang et al. [12] proposed a geometric approach for smooth transition between initial and
final positions of 3D vehicles. But all the above-said works only consider the path planning in the absence of obstacles.
In obstacle-filled environment, Lee et al. [13] proposed a molecular vision based method for obstacle avoidance of
small UAVs, where obstacles were detected using multi-scaled-oriented patches (MOPs) and the internal outline of the
obstacles were calculated through the scale-invariant feature-transform (SIFT) algorithm. Chen et al. [14] proposed a
dynamic path planning algorithm of routing UAVs for tracking a ground target in obstacle-filled environment. In this
work [14] the tangent vector field guidance (TVFG) algorithm and Lyapunov vector field guidance (LVFG) algorithm
Downloaded by UNIVERSITY OF ADELAIDE -INTERNET on January 13, 2018 | http://arc.aiaa.org | DOI: 10.2514/6.2018-1592

were developed to obtain the shortest path with UAV’s operational constraints for a given target’s position and the current
state of the UAV. Hota and Ghose [15] presented waypoint-based trajectory planning of fixed-wing MAVs in 3D space,
where transition between one waypoint segment to another occurs in a smooth manner without colliding with obstacles
and at the same time it satisfies the objective of passing through a desired distance from the associated waypoint. For
multiple UAVs, George et al. [16] proposed a reactive inverse proportional navigation algorithm for collision avoidance
to resolve the conflict among UAVs on a collision course while flying to their respective destinations. Reactive collision
avoidance algorithms to avoid collisions and to reduce near misses among UAVs in free flight in high density airspace
was also presented in [17]. Stastny et al. [18] proposed a morphing potential field navigation and nonlinear model
predictive control method for obstacle and collision avoidances in fixed-wing unmanned aerial systems with high speed
and high inertia, operating in proximal or congested settings. Pharpatara et al. [19] proposed an obstacle avoidance
trajectory planning of an aerial vehicles in 3D space using rapidly exploring random tree (RRT*) algorithm. Here, for
increasing the convergence speed of suboptimal solution of RRT* algorithm, it is combined with artificial potential
fields. Cicibas et al. [20] presented the comparison of 3D versus 4D (three spatial dimension and time dimension) path
planning of UAVs and shown that the result of 4D path planning is better than 3D in complex dynamic environment.
In the present work, waypoints are to be tracked by a fixed-wing MAVs in a sequential manner. The main difference
compared to the earlier works is that for computing the CSC path, different values of turn radius for different inclined
planes of initial and final maneuvers have been used and which gives shorter path lengths compared to the conventional
CSC paths. The method is very effective and fast enough to implement in real-time. Finally, the trajectory generation
methodology is extended to generate the optimal path in obstacle-filled environments. To do this a near-optimal path
which does not collide with obstacles has been constructed by taking a larger turn (turn radius of which is more than the
tightest feasible turn on that plane) or by adopting another CSC path (There are 4 possible CSC paths as presented in
Section IV)

III. Problem Definition

The aim of this work is to generate the optimal trajectory of MAVs to pass through a defined series of waypoints as
is shown in Fig. 1. Here, for illustration purpose, the waypoints are denoted as X1, X2, X3, X4, X5, X6 and the MAV has
to visit them sequentially without colliding with the obstacles. For better utilization of its payload let us assume that at
every waypoint the orientation of the MAV should be at a particular direction, which is required by several surveillance
applications. For simplicity, let us assume that the obstacles are static in nature.
Here, the geometrical method shall be used for generating the path of MAVs. First, we shall consider two consecutive
waypoints, say, X1 and X2 . Let us assume that starting from a given orientation at the position X1 the MAV has to reach
to X2 with a given orientation. This problem was discussed in Sussmann [9] and it was proved that for a large distance
between given initial and final positions in 3D space, the shortest path between any two given configurations (positions
and orientations) is of the CSC type. These initial and final circles are of the minimum turn radius for the vehicle. By
taking the same value for the minimum turn radius for the both maneuvers, the shortest CSC path was computed in [10].
But as the value of minimum turn radius is different for different 2D planes in 3D space, (the details of which will be
presented in the next Section) in this present work the shortest path between two consecutive waypoints will be of CSC
type with different values of circular turns for initial and final maneuvers.
In a preliminary version of this work Singh and Hota [21] computed the optimal CSC path with different turn radii in
different 2D planes of 3D space for given initial position and orientation to final position and orientation. In the present
work detail of the path generation for following several waypoints along with the algorithm to generate sub-optimal

3
 paths in case the optimal path hits with obstacle, is presented.

X4

z-axis (m) X3
1000 X2
500
X5
0
Downloaded by UNIVERSITY OF ADELAIDE -INTERNET on January 13, 2018 | http://arc.aiaa.org | DOI: 10.2514/6.2018-1592

1000 1000

X1
500 X6 500

0 0

y-axis (m) x-axis (m)

-500 -500

Fig. 1 Defined series of waypoints in 3D space.

IV. Solution Approach

In this Section at first the optimal path will be computed between two waypoints considering different values of
minimum turn radii at different planes.

A. Computation of minimum turn radius of a 2D plane in 3D space

For path generation in 3D space the airspeed (va ) and the maximum load factor (η) of the vehicle are assumed to be
constant throughout the mission. As shown below the MAV has different values of minimum turn radii in different
inclined planes. The front view of the MAV is shown in Fig. 2, where the MAV’s plane of maneuver makes the angle ψ
with the horizontal plane (XY plane). The wings of the MAVs are banked at an angle φ to facilitate a turn on the tilted
plane. From the force-balance equations of the MAV we get,

W cos(ψ) = L cos(ψ + φ), (1)

Radial force Fr is defined as,

mva2
Fr =
= L sin(ψ + φ) − W sin ψ. (2)
r
By replacing η = L/W in equations (1) and (2), we get the turn radius of the inclined plane as,

va2
r= p . (3)
g(−sinψ + η2 + sin2 ψ − 1)

For all planes, the minimum radius of circular turn can occur only when the value of load factor is maximum. In
equation (3), if ψ = 0 (the plane corresponds to the horizontal plane or XY plane), we get turn radius rh as,

v2
rh = p a , (4)
g η2 − 1

4
 And if ψ = π/2 (the plane corresponds to the vertical plane or XZ plane), we get turn radius rv as,

va2
rv = , (5)
g(η − 1)
Note that, the value of the minimum turn radius on the vertical plane is more compared to the same on the horizontal
plane, or, rv > rh .

𝜙
Downloaded by UNIVERSITY OF ADELAIDE -INTERNET on January 13, 2018 | http://arc.aiaa.org | DOI: 10.2514/6.2018-1592

𝜙 𝜓

r
Inclined Plane 𝑾

Fig. 2 Geometry for computing the minimum turn radius in inclined plane.

B. 3D path generation between two consecutive waypoints

The geometrical method generates the optimal path of CSC type when initial and final points are situated sufficiently
far away. The principle of the path generation is the same as of 2D plane but the values of minimum turn radii are
different in different maneuver planes. The 2D plane on which the initial maneuver will take place will be called as the
initial maneuvering plane and similarly, the 2D plane on which the final maneuver will take place will be called as the
final maneuvering plane. Note that, the straight line maneuver between these two turns is along the straight line which
is the common tangent between these two circular turns and is also the intersecting line between the initial and final
maneuvering planes. A typical CSC path in 3D space is shown in Fig. 3. As mentioned earlier, the values of minimum
turn radii for the initial and final maneuvering planes are different and they are denoted as r1 and r2 , respectively. To
compute the CSC path, the following steps will be followed.
Let us define, X be the tangent vector between the two circular maneuvers as shown in Fig. 3. The unit vector along
X is,
X
x= (6)
X
The vector perpendicular to the initial maneuver plane is denoted as U1 . So

U1 = x × v1 (7)
U1
The unit normal vector to the initial maneuver plane is, u1 = . Following section IV-A, the minimum turn radius
U1
corresponds to the initial maneuver plane will be,

va2
r1 = p . (8)
g(−sinψ1 + η2 + sin2 ψ1 − 1)

Where ψ1 = cos−1 (u1 .(0, 0, 1)). The radius vector W 1 , which is oriented toward the center of the initial circle from its
initial position is,
W 1 = v1 × U1 = v1 × (x × v1 ) (9)

5
 Final circle

Final maneuver
𝑗 plane 𝒗2

𝑟2 𝑜2 𝒘2
𝑋𝑓
𝜃2
𝒚2 𝑼2

𝑿 𝑃2
𝑃1
𝒗1

𝒚1
Downloaded by UNIVERSITY OF ADELAIDE -INTERNET on January 13, 2018 | http://arc.aiaa.org | DOI: 10.2514/6.2018-1592

𝜃1 Initial maneuver
𝑋0 𝑜1
𝒘1 plane
𝑼1 𝑟1

Initial circle
O
𝑖
𝑘

Fig. 3 Geometry of CSC path in 3D space for given initial and final positions and orientations.

The position vector of the center of the initial circle OO1 can be written as,

OO1 = OX0 + r1 w1 (10)

W1
Where w1 = . Then, the radius vector Y 1 , which is oriented towards the center of the initial circle from the point P1
W1
(where the aerial vehicle starts following the straight line path) is

Y 1 = x × U1 = x × (x × v1 ) (11)

Now, OP1 can be computed as,

OP1 = OO1 − r1 y1 = OX0 + r1 w1 − r1 y1 (12)
Y1
Where y1 = . The vector perpendicular to the final maneuver plane is
Y1
U2 = x × v2 . (13)
U2
Where the unit normal vector to the final maneuver plane is u2 = . Again following section IV-A, the minimum turn
U2
radius of the final maneuver circle is calculated as,

va2
r2 = p . (14)
g(−sinψ2 + η2 + sin2 ψ2 − 1)

Where ψ2 = cos−1 (u2 .(0, 0, 1)). The radius vector W 2 which is oriented toward the center of the final circle from its
final position is,
W 2 = −v2 × U2 = −v2 × (x × v2 ). (15)
The center position vector of the final circle OO2 is,

OO2 = OX f + r2 w2 (16)

6
 𝑗 𝑗 𝒗2

𝒗1 r2
𝑿 𝒘2 Xf
𝜃1 P1
O2 𝜃2 𝑼2
Q 𝒚1 𝜃2 2
𝜃1 2
M
2
𝒚2 𝑁 𝜃2
𝜃1
2 O1
𝑅
𝑿
X0 𝒘1 P2
r1
𝑼1

O 𝑖
O 𝑖
Downloaded by UNIVERSITY OF ADELAIDE -INTERNET on January 13, 2018 | http://arc.aiaa.org | DOI: 10.2514/6.2018-1592

𝑘
𝑘

(a) Initial maneuver turn (b) Final maneuver turn

Fig. 4 Calculation of initial and final maneuver turns in 3D space.

W2
Where, w2 = . Then, the radius vector Y 2 , which is oriented towards the center of the final circle from the point P2
W2
(where the aerial vehicle starts following the final circular turn) is,

Y 2 = −x × U2 = −x × (x × v2 ) (17)

Now, OP2 can be computed as,

OP2 = OO2 − r2 y2 = OX f + r2 w2 − r2 y2 (18)
Y2
Where, y2 = . Now, we can find the value of X by solving the following nonlinear equations.
Y2
OP2 − OP1 = X = (OX f + r2 w2 − r2 y2 ) − (OX0 + r1 w1 − r1 y1 ). (19)

Which gives,
OP2 − OP1 = X = OX f − OX0 − r1 (w1 − y1 ) − r2 (y2 − w2 ). (20)
From Fig. 4(a) we get,
θ1 θ1
r1 (w1 − y1 ) = X0 P1 = X0 Q + QP1 = r1 tan( )v1 + r1 tan( )x (21)
2 2
From Fig. 4(b) we get,
θ2 θ2
r2 (y2 − w2 ) = P2 X f = P2 R + RX f = r2 tan( )x + r2 tan( )v2 (22)
2 2
Here, θ 1 and θ 2 follow the following relationships: cos θ 1 = v1 .x andcos θ 2 = v2 .x. Using (21) and (22) the equation
(20) can be written as,
θ1 θ2
X = OX f − OX0 − r1 tan (x + v1 ) − r2 tan (x + v2 ) (23)
2 2
Solving these nonlinear equations, we can obtain X, θ 1 , θ 2 , r1 and r2 . Now, the total length of the path can be computed
as,
The total length of the CSC path = first arc length (r1 θ 1 ) + straight line length (X) + second arc length (r2 θ 2 ).
It is important to note that there can be four types of CSC paths for this problem. The complete set of equations is,
θ1 θ2
X = OX f − OX0 ∓ r1 tan (x + v1 ) ∓ r2 tan (x + v2 ) (24)
2 2
Four CSC Paths
The above-said four CSC paths will help us by providing with the multiple options of path generation for waypoint

7
 following problem in the presence of obstacles. The longer CSC path can be followed once the shorter CSC path(s)
collides with the obstacles, the details of which has been presented in Section IV-D. In the following example a pair of
initial and final configurations have been selected to illutrate the existance of 4 CSC paths. In Fig. 5, the four CSC
paths are shown for given initial and final conditions. Here X0 = (0, 0, 0), v1 = (0, 1, 0), X f = (300, 400, 500), and
v2 = (0.8270, −0.3678, 0.4252). The other values of parameters are shown in Table 2. The lengths of the CSC paths are
as follows: CSC1= 742.1491 m, CSC2= 1277.2 m, CSC3= 1367.9 m, CSC4 = 1840.7 m.

CSC1
Downloaded by UNIVERSITY OF ADELAIDE -INTERNET on January 13, 2018 | http://arc.aiaa.org | DOI: 10.2514/6.2018-1592

CSC2
CSC3
CSC4
v1
1000 v2
X0
Xf
600
z-axis (m)

500

400
0
200
-500
600 0
400
200 x-axis (m)
0
y-axis (m) -200 -200

Fig. 5 Four CSC paths in 3D space.

Sufficient distance between two consecutive waypoints for the existence of a CSC path: As the optimal path for
small distances between waypoints may be of helicoidal type and the computation of which is not simple as described in
[9], in this present work it is assumed that the waypoints are sufficiently far apart. In the following, we quantify the
distance required between two consecutive waypoints for the existence of the CSC paths.
It can be observed that CSC path can exist only if there is a common tangent between the initial and final circular turns.
Now, if the distance between these two circles are not large enough and the circles touch each other the path will become
CC type. Let us consider the geometry of the CC path shown in Fig. 6. Form this figure one can write that,
||X0 X f || = ||OX f − OX0 || = ||(OX f − OP) + (OP − OX0 )|| (25)
Using triangular inequality we get,
||X0 X f || = ||OX f − OX0 || ≤ ||(OX f − OP)|| + ||(OP − OX0 )|| (26)
Where, ||(OX f − OP)|| ≤ 2r2 and ||(OP − OX0 )|| ≤ 2r1 . Again note that, r1 ≤ rv and r2 ≤ rv
So to ensure that there is a common tangent between these two turns or to ensure that CSC path exists for given initial
and final configurations, one needs to check whether ||X0 X f || > 4rv .

C. Path generation through n waypoints in obstacle-free environment

1. Algorithm for Generating the Trajectory in Obstacle-Free Environment

1. Enter the total number of waypoints (n ≥ 2) and coordinates of them (X1, X2, X3, ..., Xn ) with desired orientations.

8
 O2
r2
P

X0 Xf
r1
O1
Downloaded by UNIVERSITY OF ADELAIDE -INTERNET on January 13, 2018 | http://arc.aiaa.org | DOI: 10.2514/6.2018-1592

Fig. 6 Sufficient distance between initial and final points to obtain CSC path.

2. Enter MAV data: airspeed (va ), the maximum load factor (η).
3. Initialize i = 1, where i is the number of times the CSC path to be computed between a pair of waypoints.
4. Compute the shortest CSC path as described in Section IV-A and IV-B.
5. Set i = i + 1.
6. If i < n − 1 then goto Step 4, else Exit.

D. Path generation through n waypoints in obstacle-filled environment

1. Collision checking
Now consider the scenario where the workspace is filled with building shaped obstacles. The MAV has to fly through
several waypoints in this workspace as required by several surveillance applications. For better utility of its payload let
us assume that at every waypoint the desired orientation is specified. For simplicity, the buildings can be approximated
as square-shaped at base with different heights and sizes. The following steps have been used for collision checking.
1. Each building can be assumed to be enclosed by a cylinder, the diameter of which is equal to the diagonal of
the squared-shaped base of the building plus a small value equivalent to the safety margin, which is shown in
Fig. 7(a). Let us describe the k th obstacle as [Obsxk , Obsyk , Obshk , Obsrk ], where on ground the center of the
k th obstacle is [Obsxk , Obsyk ], Obshk is the height of the k th building and Obsrk is the radius of the cylindrical
representation of the k th building. Here although the waypoints are situated in obstacle-free environment but the
CSC path connecting two waypoints may collide with obstacles.
2. As discussed in Section IV-B, two planes are involved in developing the CSC path between a pair of waypoints. In
case both these initial and final maneuver planes do not intersect the cylindrical representation of the obstacle,
there is no collision and the optimal CSC path can be opted for exploring. Else, if any of these two planes of the
optimal CSC path intersects with an obstacle, one needs to find out the ellipse(s) that is created by the intersection
of the plane(s) with the cylinder. Now if any portion of the CSC path intersects the ellipse or any of these ellipses
(for intersection with both the planes), the collision is going to occur and one needs to choose an alternative path
by either choosing another CSC path or by increasing the radius of turn of the circular path of the optimal CSC
type path.
3. The following steps can be followed to check whether any of these two planes is intersected by k th obstacle. For
improving the clarity in the presentation let us consider the steps to check whether k th obstacle is intersected
by the initial maneuvering plane of the CSC path. The similar procedure can be followed to check whether k th
obstacle is intersected by the final maneuvering plane of the CSC path. An ellipse is formed if a cylindrical object
is intersected by a plane, which is shown in Fig. 7(b). The center of the ellipse, Ce1 is

Ce1 = (Obsxk , Obsyk , 0) + de1 (0, 0, 1) (27)

9
 where, de1 is the distance between the center of the ellipse and the center of the obstacle on XY plane and its
direction is along the Z-axis.
As Ce1 is a point on the intersecting plane, the vectors (OX1 − OCe1 ) and u1 are perpendicular. So,
(OX1 − OCe1 ).u1 = 0 (28)
The length de1 can be obtained as follows,
(OX1 − (Obsxk , Obsyk , 0)).u1
de1 = (29)
(0, 0, 1).u1
It can be noted that if the obstacle is not tall enough, it may not be intersected by the maneuvering plane of CSC
path. Note that the intersection with the initial maneuvering plane is possible only if
de1 − Obsrk tanψ1 < Obshk
Downloaded by UNIVERSITY OF ADELAIDE -INTERNET on January 13, 2018 | http://arc.aiaa.org | DOI: 10.2514/6.2018-1592

(30)
Similarly, k th obstacle can be intersected by the final maneuvering plane of a CSC path only if
de2 − Obsrk tanψ2 < Obshk (31)
where, de2 can be computed from
(OX2 − (Obsxk , Obsyk , 0)).u2
de2 = (32)
(0, 0, 1).u2

ez = (0,0,1)

Safety Margin

Obs
h
k
Squared-sized Ce1
building base

d e1

(Obs , Obs , 0)
Obs x y
k k
r
k
(a) (b)

Fig. 7 (a) Cylindrical approximation of a squared-sized building base (b) Intersection between a cylindrical
obstacle and the initial maneuvering plane.

4. As mentioned earlier, if the cylindrical obstacle is intersected by any of these planes (initial and/or final maneuvering
plane(s)), one needs to check further whether the 3D CSC path intersects those ellipse(s) described in the previous
step. To check this, consider the fact that if the 3D CSC path intersects those ellipse(s), then XY projection of the
3D CSC path and base of the obstacle shall also intersect each other. So to make the analysis simpler, we shall
project the 3D CSC path on XY plane to get a path of ellipse-straight line-ellipse (ESE) type as shown in Fig. 8(a).
Here, the first ellipse corresponds to the projection of the initial circular turn of CSC path on XY plane, and the
second ellipse corresponds to the projection of the final circular turn of CSC path on XY plane. Then it is only
required to check whether this ESE path and circular base of the obstacle on the XY plane intersect each other (as
shown in Fig. 8(b)). It can be noted that an ellipse and circle may intersect at most four points as illustrated in Fig.
9 for the projected ellipse of initial maneuver turn and the base of the cylindrical obstacle. One needs to ensure
that only the portion of ESE path corresponding to the trajectory of the vehicle as highlighted in Fig. 9 should not
be a part of the intersection. For this a geometrical method has been followed and the detail of which has been
skipped here for the brevity of the document.

10
 1200

1000

800
z-axis (m)

Projection of final
CSC trajectory circle
600 Y-axis P22
Projection of straight
Downloaded by UNIVERSITY OF ADELAIDE -INTERNET on January 13, 2018 | http://arc.aiaa.org | DOI: 10.2514/6.2018-1592

line segment b22

400
O22
a22
200 P11
0 d ce2
1000
O11
Projection of CSC a11
trajectory
b11 d ce1
500 Oobs
Obsrk
0 200 0 -200 Projection of initial
x-axis (m) 800 600 400 circle Obstacle base

y-axis (m)
O X-axis

(a) (b)

Fig. 8 (a) Projection of a CSC path is an ellipse-straight line-ellipse (ESE) type path on XY plane, and (b) Base
of an obstacle and the projection of a 3D CSC path on XY plane.

Projection of the
P11 initial maneuver
turn
P11 A11

X 01 O11 A11 X 01 O11

A12
(a) Base of the (b)
obstacle

P11 A11
Projection of the
initial maneuver A11 A12
circle P11
X 01 O11 A12
X 01 O11
A13
A14 A13
(c)
(d)
Fig. 9 Possible intersection of the initial ellipse and the circular base of the obstacle.

11
 Table 1 Initial and final condition for simulation

Case Study X0 (m) v1 X f (m) v2 CSC path length (m)

√ √
Case 1 (0, 0, 0) (1, 1, 1)/ 3 (300, 400, 500) (1, 1, 1)/ 3 707.41
√ √
Case 2 (0, 0, 0) (1, 1, 1)/ 3 (−300, 400, 500) (1, 1, 1)/ 3 749.48
√ √
Case 3 (0, 0, 0) (1, 1, 1)/ 3 (−300, −400, 500) (1, 1, 1)/ 3 952.27
√ √
Case 4 (0, 0, 0) (1, 1, 1)/ 3 (300, −400, 500) (1, 1, 1)/ 3 778.96

Table 2 Simulation parameters

Downloaded by UNIVERSITY OF ADELAIDE -INTERNET on January 13, 2018 | http://arc.aiaa.org | DOI: 10.2514/6.2018-1592

Parameters Value
Airspeed (va ) 15 m/s
Minimum turn radius in the 34.56 m
horizontal plane rh
Minimum turn radius in the vertical 114.56 m
plane rv
Maximum load factor (η) 1.2002
Acceleration due to gravity (g) 9.81 m/s2

2. Algorithm for generating the trajectory in obstacle-filled environment

1. Enter the total number of waypoints (n ≥ 2) and coordinates of them (X1, X2, X3, ..., Xn ).
2. Enter MAV data: airspeed (va ), maximum load factor (η).
3. Initialize i = 1.
4. Compute the optimal CSC path from Xi to Xi+1 by following the method in Section IV-B.
5. Initialize j = 1, where j = 1, 2, ...q number of static obstacles.
6. Check the collision between j th obstacle and the optimal CSC path by following the method described in Section
IV-D.
7. If collision occurs, set Collision=1.
8. If Collision==1, compute other CSC path by increasing the turn radius of the CSC path for one turn or for both the
turns or follow other CSC paths form another three possible CSC paths.
9. Check the collision between the new path and j th obstacle. If collision occurs set Collision=1 and go to Step 8,
else set Collision=0.
10. If Collision==0, set j = j + 1; go to Step 6 until j = q.
11. Set i = i + 1, go to Step 4 until i = n − 1.

V. Simulation Results

A. Trajectory generation between two waypoints

To show the efficiency of the path generation algorithm a number of configurations are considered for simulation
studies as shown in Table 1. It may be noted that the geometrical distances between initial to final positions are the same
for all cases and the value is equal to 707.11 m. The other simulation parameters are tabulated in Table 2. The optimal
CSC paths for Case 1 to Case 4 are shown in Fig. 10(a)−10(d) and the path lengths are listed in Table 1.

B. Trajectory generation for n consecutive waypoints in obstacle-free environment

Here, we have considered six waypoints:, X1, X2, X3, X4, X5, X6 as given in Table 3 in sequence. Without loss in
generality it is assumed that at every waypoint the orientation is towards the line joining the current waypoint to the next
waypoint. For nth waypoint the orientation is the same as that of the (n − 1)th waypoint. But in practice these directions
can be specified from the application field. The combined trajectory is shown in Fig. 11 and the computation time and

12
 600 optimal path
v1
Downloaded by UNIVERSITY OF ADELAIDE -INTERNET on January 13, 2018 | http://arc.aiaa.org | DOI: 10.2514/6.2018-1592

600
optimal path v2
v1 400 X0

z-axis (m)
z-axis (m)

400 v2 Xf
X0
200 Xf
200

0
0

400 400
400
100
200 200 0
200 -100
0 -200
y-axis (m) 0 x-axis (m) -300
y-axis (m) 0 x-axis (m)

(a) Case 1 (b) Case 2

600
optimal path optimal path
500 v1 600 v1
v2 v2
400 X0 500 X0
z-axis (m)

Xf Xf
300 400
z-axis (m)

200 300

100 200

0 100
200
0
0
200 400
-200 0
-400 0 100 -200 200
-200 -100
y-axis (m) -300 -400
x-axis (m) 0
y-axis (m) x-axis (m)

(c) Case 3 (d) Case 4

Fig. 10 Optimal CSC path in 3D space (for Case 1-4).

13
 Table 3 Combined trajectory of obstacle avoidance by changing CSC path and increasing turn radius

Case Type of Waypoint Length of Computation

study trajectory configuration the path Time
(a) Combined trajectory for waypoint following X1 = (0, 0, 0) m 4047.1 m 0.43 sec
in obstacle free environment X2 = (300, 400, 500) m
(b) Combined trajectory for obstacle avoidance by X3 = (700, 100, 950) m 4362.3 m 1.08 sec
adopting other CSC paths from the set of X4 = (900, 700, 600) m
4 CSC paths as described in section IV-B X5 = (−100, 500, 700) m
(c) Combined trajectory for obstacle avoidance X6 = (−400, −100, 800) m 4106.3 m 1.01 sec
Downloaded by UNIVERSITY OF ADELAIDE -INTERNET on January 13, 2018 | http://arc.aiaa.org | DOI: 10.2514/6.2018-1592

by increasing turn radius of the optimal

CSC path

length of the path are tabulated in Table 3. It can be clearly seen that the computation time is reasonably low, which
shows that the algorithm can be implemented for real time applications.

Waypoints
Desired orientations
Optimal path through waypoints
X4

1000
z-axis (m)

500 X3
X2
0 X5
1000

500 X1 1000
X6

500
0
0
y-axis (m)
-500 x-axis (m)
-500

Fig. 11 Optimal path through a series of waypoints.

C. Trajectory generation for n consecutive waypoints in obstacle-filled environment

In obstacle-filled environment, the same six waypoints as considered above are considered with the same orientations,
but now there are obstacles in the environment. The optimal CSC path between X2 and X3 collides with one obstacle as
shown in Fig. 12, so two different ways have been used to avoid obstacles. In the first case, to compute path between X2
and X3 a different CSC path has been used from the set of four CSC paths as described in Section IV-B and the resulted
path is also plotted in Fig. 12. The second method is to increase the turn radius of the both the turns of the optimal CSC
to get a longer CSC path which does not collide with obstacles as shown in Fig. 13. The length of total path and the
computational time are also tabulated in Table 3. All the simulations have been done in Intel(R) Core(TM) i5-3470
CPU @ 3.20 GHz with 8.00 GB RAM. Again, the computational time is reasonably less to implement the algorithm in
real time in obstacle-filled environment.

14
 Waypoints
Desired orientations
Path colliding with obstacles
Path without colliding with obstacles

X4

1500 X3
z-axis (m)

1000
X2
Downloaded by UNIVERSITY OF ADELAIDE -INTERNET on January 13, 2018 | http://arc.aiaa.org | DOI: 10.2514/6.2018-1592

500 X5

0
1000 1000
X6 X1
500 500

0 0

y-axis (m) x-axis (m)

-500 -500

Fig. 12 Combined trajectory to avoid obstacles by adopting a different CSC path from 4 possible CSC paths
in Section IV-B.

Waypoints
Desired orientations
Path colliding with obstacles
Path without colliding with obstacles

X4

X3
1500
z-axis (m)

1000 X2
X5
500
0 1000
1000
X6 X1
500
500
0
0
y-axis (m) x-axis (m)
-500 -500

Fig. 13 Combined trajectory to avoid obstacles by taking a larger turn (whose turn radius is more than the
minimum turn radius of the vehicle).

15
 VI. Conclusion and Future Work
In this paper, the trajectory has been generated for exploring a series of waypoints in 3D space. The optimal CSC
paths for given initial and final positions and orientations have been generated using 3D geometry. In these CSC paths,
the different values of turn radii for different inclined planes of initial and final maneuvers have been used in contrast to
the conventional CSC path where the value of turn radius was assumed to be the same for all 2D planes in 3D space.
This method is very effective and fast enough to implement in real-time when the initial and final points are given, and
they are situated sufficiently far from each other. Finally, the combined trajectory has been generated in obstacle-filled
environment, where the collision with obstacles has been avoided by adopting another CSC path of larger length or by
taking a larger turn (increasing turn radius of MAVs). In future, the generated trajectory will be implemented using a
6-DOF model of a fixed-wing MAV. Extending the work in dynamic environment where the obstacles are moving in
nature is another possible direction of future works.
Downloaded by UNIVERSITY OF ADELAIDE -INTERNET on January 13, 2018 | http://arc.aiaa.org | DOI: 10.2514/6.2018-1592

References
[1] Dubins, L. E., “On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal
positions and tangents,” American Journal of mathematics, Vol. 79, No. 3, 1957, pp. 497–516.

[2] Shkel, A. M., and Lumelsky, V., “Classification of the Dubins set,” Robotics and Autonomous Systems, Vol. 34, No. 4, 2001, pp.
179–202.

[3] Shanmugavel, M., Tsourdos, A., White, B. A., and Żbikowski, R., “Differential geometric path planning of multiple UAVs,”
Journal of Dynamic Systems, Measurement, and Control, Vol. 129, No. 5, 2007, pp. 620–632.

[4] Babaei, A., and Mortazavi, M., “Fast trajectory planning based on in-flight waypoints for unmanned aerial vehicles,” Aircraft
Engineering and Aerospace Technology, Vol. 82, No. 2, 2010, pp. 107–115.

[5] Hota, S., and Ghose, D., “A modified Dubins method for optimal path planning of a miniature air vehicle converging to a
straight line path,” In American Control Conference (ACC), IEEE, 2009, pp. 2397–2402.

[6] Nelson, D. R., Barber, D. B., McLain, T. W., and Beard, R. W., “Vector field path following for miniature air vehicles,” IEEE
Transactions on Robotics, Vol. 23, No. 3, 2007, pp. 519–529.

[7] Park, S., Deyst, J., and How, J. P., “Performance and Lyapunov stability of a nonlinear path-following guidance method,”
Journal of Guidance Control and Dynamics, Vol. 30, No. 6, 2007, pp. 1718–1728.

[8] Hota, S., and Ghose, D., “Curvature-constrained trajectory generation for waypoint following for miniature air vehicle,”
Proceedings of the Institution of Mechanical Engineers, Part G: Journal of Aerospace Engineering, Vol. 228, No. 11, 2014, pp.
2066–2082.

[9] Sussmann, H. J., “Shortest 3-dimensional paths with a prescribed curvature bound,” Proceedings of the Decision and Control
Conference, IEEE, Vol. 4, 1995, pp. 3306–3312.

[10] Hota, S., and Ghose, D., “Optimal path planning for an aerial vehicle in 3D space,” In Decision and Control Conference (CDC),
IEEE, 2010, pp. 4902–4907.

[11] Pharpatara, P., Hérissé, B., and Bestaoui, Y., “3D-shortest paths for a hypersonic glider in a heterogeneous environment,”
IFAC-Papers OnLine, Vol. 48, No. 9, 2015, pp. 186–191.

[12] Wang, Y., Wang, S., and Tan, M., “Path generation of autonomous approach to a moving ship for unmanned vehicles,” IEEE
Transactions on Industrial Electronics, Vol. 62, No. 9, 2015, pp. 5619–5629.

[13] Lee, J.-O., Lee, K.-H., Park, S.-H., Im, S.-G., and Park, J., “Obstacle avoidance for small UAVs using monocular vision,”
Aircraft Engineering and Aerospace Technology, Vol. 83, No. 6, 2011, pp. 397–406.

[14] Chen, H., Chang, K., and Agate, C. S., “UAV path planning with tangent-plus-Lyapunov vector field guidance and obstacle
avoidance,” IEEE Transactions on Aerospace and Electronic Systems, Vol. 49, No. 2, 2013, pp. 840–856.

[15] Hota, S., and Ghose, D., “Waypoint-based trajectory planning of fixed-wing MAVs in 3D space,” Journal of Intelligent &
Robotic Systems, Vol. 86, No. 1, 2017, pp. 95–113.

[16] George, J., and Ghose, D., “A reactive inverse PN algorithm for collision avoidance among multiple unmanned aerial vehicles,”
American Control Conference, IEEE, 2009, pp. 3890–3895.

16
 [17] Manathara, J. G., and Ghose, D., “Reactive collision avoidance of multiple realistic UAVs,” Aircraft Engineering and Aerospace
Technology, Vol. 83, No. 6, 2011, pp. 388–396.

[18] Stastny, T. J., Garcia, G. A., and Keshmiri, S. S., “Collision and obstacle avoidance in unmanned aerial systems using morphing
potential field navigation and nonlinear model predictive control,” Journal of Dynamic Systems, Measurement, and Control,
Vol. 137, No. 1, 2015, pp. 1–10.

[19] Pharpatara, P., Hérissé, B., and Bestaoui, Y., “3-D trajectory planning of aerial vehicles using RRT,” IEEE Transactions on
Control Systems Technology, Vol. 25, No. 3, 2017, pp. 1116–1123.

[20] Cicibas, H., Demir, K. A., and Arica, N., “Comparison of 3D versus 4D path planning for unmanned aerial vehicles,” Defence
Science Journal, Vol. 66, No. 6, 2016, pp. 651–664.

[21] Singh, N. K., and Hota, S., “Optimal path planning for fixed-wing UAVs in 3D space,” Accepted in ICTACEM, IIT Kharagpur,
Downloaded by UNIVERSITY OF ADELAIDE -INTERNET on January 13, 2018 | http://arc.aiaa.org | DOI: 10.2514/6.2018-1592

2017.

17