5

Path-Following
Guidance
The previous chapters have detailed the design of paths through complex
environments to enable the UAVs to coordinate their search and rendezvous
activities. In order for the UAVs to complete their missions, we require them
to be able to follow the planned paths accurately. To that end, guidance
algorithms are required to ensure correct following of the paths. The analysis
will assume that an autopilot has been designed to enable the UAV to follow
velocity and heading commends accurately. Although this is not a topic for
this book, there are many references that confirm that the design of such
autopilots has been demonstrated in flight.
In order to fly along or close to the planned path, the UAV may be
assigned to track the trajectory or to follow the path. The first case is called
trajectory tracking, where the path is parameterized by time and the UAV
has to track a point as it moves along the path. The second method is called
path following, in which the UAV has to stay close to the path without
having to track an explicit point. In both methods, the guidance system tries
to reduce the distance error between the UAV and the path. The distance
error is the orthogonal projection of the UAV’s position onto the path. The
guidance algorithm needs to generate a smooth path that meets the kinematic
constraints of the UAV and to minimise the error. The guidance algorithm
must also be able to make the UAV track straight paths and curved paths.

Cooperative Path Planning of Unmanned Aerial Vehicles

Antonios Tsourdos, Brian White and and Madhavan Shanmugavel
© 2011 John Wiley & Sons, Ltd. ISBN: 978-0-470-74129-0
120 Cooperative Path Planning of Unmanned Aerial Vehicles

Some path-following guidance algorithms guide the UAV from one

waypoint to the next, but this can result in oscillations of the UAV path,
which produces a ‘slalom’-type trajectory rather than a straight-line tra-
jectory. The waypoint-following control becomes difficult when the UAV
location is disturbed, by wind, for example, when it results in oscillations
around the waypoints. Following curved trajectories is also challenging with
this method (Park et al. 2004).
A better way is to track the path joining the waypoints (Caravita et
al. 2007). This path-following method ensures that the error is minimised.
The path-following method was originally reported in the field of mobile
robotics, where it is used to reduce the distance between the robot’s position
and the path, and the angle between the velocity vector and the tangent
on the path. Nonlinear control for path following ensures stabilisation and
convergence in mobile robots (Micaelli and Samson 1992, 1993). A Lyapunov
direct method is used in Kim and Oh (1999) to prove convergence. A gain
scheduling controller with linearization of the error vector about the pre-
planned path is reported in Pascoal et al. (2006). A comprehensive review
of path following for ocean vehicles is reported in Fossen (1994). A method
similar to the potential field and vector field method is used for path planning
of micro-UAVs in Nelson et al. (2007). Here we present the carrot guidance
algorithm for the UAV.
Although in practical systems the UAV has an autopilot that has its own
dynamics, it will be assumed that the UAV will instantaneously attain the
desired turn rate required by the guidance algorithm. This is not usually a
problem if the autopilot that determines the UAV response is fast compared
with the dynamic response of the guidance system. A more complex analysis
is required if the dynamics of the UAV, as well as the kinematics of the
guidance system, are considered together. An integrated guidance and
control method is used to follow waypoints in Kaminer et al. (1998).

5.1 Path Following the Dubins Path

Here, we consider a UAV following a Dubins path, as shown in Figure 5.1.

The figure shows the UAV axes tb and nb , which represent the x and y axes
of the UAV in body axes, with θ as the yaw angle of the UAV. There is also
a set of axes that are fixed in the Dubins curve (tc and nc ), which have an
origin on the curve such that the UAV lies along the normal axis nc . Hence,
as the UAV moves in inertial space, the Dubins curve axes will move along
the curve to maintain the UAV position on the normal vector nc .
Path-Following Guidance 121

tc tb
nb
nc θ
φ pb
pc

Figure 5.1 Guidance geometry

In inertial axes, we have

V I = R(θ )V b , (5.1)

where Vb is the UAV speed, and θ is the body axes rotation angle with
respect to the inertial axes. Hence
⎛ ⎞
cos(θ ) − sin(θ ) 0
R(θ ) = ⎝ sin(θ ) cos(θ ) 0 ⎠ ,
0 0 1
⎛ ⎞
Vb
Vb = ⎝ 0 ⎠ . (5.2)
0

If a set of path axes are defined that are attached to the UAV via the normal
vector nc , then the UAV position in inertial axes is given by

pI = pc + R(φ)pu , (5.3)

where
⎛ ⎞
cos(φ) − sin(φ) 0
R(φ) = ⎝ sin(φ) cos(φ) 0 ⎠,
0 0 1
⎛ ⎞
0
pu = ⎝ d ⎠ , (5.4)
0
122 Cooperative Path Planning of Unmanned Aerial Vehicles

where d is the line connecting the point on the Dubins curve and the UAV,
pu is the unit vector along this line and φ is the arc angle of the resulting axes.
Differentiating with respect to time yields

dpI dpc dR(φ) dφ dp

= + pu + R(φ) u ,
dt dt dφ dt dt
ṗI = ṗc + R̂(φ)φ̇pu + R(φ)ṗu , (5.5)

where
⎛ ⎞
− sin(φ) − cos(φ) 0
R̂(φ) = ⎝ cos(φ) − sin(φ) 0 ⎠ . (5.6)
0 0 0

Now

ṗI = V I
⎞⎛
Vb
= R(φ) ⎝ 0 ⎠ (5.7)
0

and
⎞⎛
Vc
ṗc = R(φ) ⎝ 0 ⎠ . (5.8)
0

We also have
⎛ ⎞
0
ṗu = ⎝ ḋ ⎠ , (5.9)
0

where Vc is the speed of movement of the point pc along the path.

Equation (5.5) can thus be written as
⎛ ⎞ ⎛ ⎞ ⎛ ⎞
Vb Vc 0
R(θ ) ⎝ 0 ⎠ = R(φ) ⎝ ḋ ⎠ + R̂(φ)φ̇ ⎝ d ⎠ . (5.10)
0 0 0
Path-Following Guidance 123

Written in terms of the path axes, we have

⎛ ⎞ ⎛ ⎞ ⎛ ⎞
Vb Vc 0
R (φ)R(θ ) ⎝ 0 ⎠ = ⎝ ḋ ⎠ + R (φ)R̂(φ)φ̇ ⎝ d ⎠ (5.11)
0 0 0

or
⎛ ⎞ ⎛ ⎞ ⎛ ⎞
Vb Vc 0
RV ⎝ 0 ⎠ = ⎝ ḋ ⎠ + Rφ φ̇ ⎝ d ⎠ , (5.12)
0 0 0

where
⎛ ⎞⎛ ⎞
cos(φ) sin(φ) 0 cos(θ ) − sin(θ ) 0
RV = ⎝ − sin(φ) cos(φ) 0 ⎠ ⎝ sin(θ ) cos(θ ) 0 ⎠
0 0 1 0 0 1
⎛ ⎞
cos(φ) cos(θ ) + sin(φ) sin(θ ) − cos(φ) sin(θ ) + sin(φ) cos(θ ) 0
= ⎝ − sin(φ) cos(θ ) + cos(φ) sin(θ ) sin(φ) sin(θ ) + cos(φ) cos(θ ) 0 ⎠
0 0 1
⎛ ⎞
cos(θ − φ) − sin(θ − φ) 0
= ⎝ sin(θ − φ) cos(θ − φ) 0 ⎠ (5.13)
0 0 1

and
⎛ ⎞⎛ ⎞
cos(φ) sin(φ) 0 − sin(φ) − cos(φ) 0
Rφ = ⎝ − sin(φ) cos(φ) 0 ⎠ ⎝ cos(φ) − sin(φ) 0 ⎠
0 0 1 0 0 0
⎛ ⎞
0 −1 0
= ⎝ 1 0 0 ⎠. (5.14)
0 0 0

Hence we obtain
⎛ ⎞⎛ ⎞ ⎛ ⎞ ⎛ ⎞ ⎛ ⎞
cos(θ − φ) − sin(θ − φ) 0 Vb Vc 0 −1 0 0
⎝ sin(θ − φ) cos(θ − φ) 0 ⎠ ⎝ 0 ⎠ = ⎝ ḋ ⎠ + ⎝ 1 0 0 ⎠ φ̇ ⎝ d ⎠ .
0 0 1 0 0 0 0 0 0
(5.15)
124 Cooperative Path Planning of Unmanned Aerial Vehicles

Expanding the equations gives

Vb cos(θ − φ) = Vc − φ̇d,
Vb sin(θ − φ) = ḋ (5.16)

or
Vc = Vb cos(θ − φ) + φ̇d, (5.17)
ḋ = Vb sin(θ − φ). (5.18)

Equation (5.17) gives the speed of the path axes origin along the path.
Equation (5.18) gives the dynamics of the distance d from the path.
Now, the rate of change of φ, the curvature of the path κ and the speed Vc
of the point on the path pc are related by

φ̇ = κVc . (5.19)

Hence
Vc = Vb cos(θ − φ) + κVc d
(1 − κd)Vc = Vb cos(θ − φ)
Vb cos(θ − φ)
Vc = . (5.20)
(1 − κd)
This becomes indeterminate when the distance from the path coincides with
the turn centre of the arc of the path. This occurs when

(1 − κd) = 0
1
d= . (5.21)
κ
This is a condition where Vc becomes indeterminate. For this condition, the
UAV passes through the centre of rotation of the arc of the Dubins path.

5.2 Linear Guidance Algorithm

Consider a straight-line path, where, without loss of generality, the line is at

an angle given by φ = 0. We also have φ̇ = 0. Hence

Vc = Vb cos(θ ),
ḋ = Vb sin(θ ), (5.22)