9th IFAC Conference on Control Applications in Marine Systems

The International Federation of Automatic Control

September 17-20, 2013. Osaka, Japan

A Quaternion-Based LOS Guidance

Scheme for Path Following of AUVs
Anastasios M. Lekkas ∗ Thor I. Fossen ∗∗
∗
Department of Engineering Cybernetics,
Norwegian University of Science and Technology, NO-7491,
Trondheim, Norway (e-mail: anastasios.lekkas@itk.ntnu.no)
∗∗
Centre for Autonomous Marine Operations and Systems,
Department of Engineering Cybernetics,
Norwegian University of Science and Technology, NO-7491,
Trondheim, Norway (e-mail: fossen@ieee.org)

Abstract: This paper presents a quaternion version of the well-known Line-of-Sight (LOS)
guidance algorithm for marine applications. The transformation from Euler angles is achieved
by exploiting the nature of the quaternion structure and using fundamental half-angle formulae
from trigonometry. First, the Euler angles version of the LOS guidance algorithm is briefly
presented for two uncoupled cases: a) the horizontal xy-plane, and b) the vertical zx-plane.
Then, a coupled case is also considered and the transformation procedure is presented for all
three cases. The vehicle considered pertains to a 5-DOF kinematics model where the roll angle is
neglected, typical of torpedo-shaped Autonomous Underwater Vehicles (AUVs). Naturally, the
Euler angles representation of the system involves singularities which, in the general 3-D space
navigation case, should be avoided. The presented method aims at providing a singularity-free
and computationally-efficient version of the conventional LOS algorithm.

Keywords: Autonomous underwater vehicles, path following, LOS guidance, quaternions

1. INTRODUCTION autopilot convergence is usually studied by using cascaded

The Line-of-Sight (LOS) algorithm is a well-documented
guidance method in both marine and aerial vehicles ap-
plications, see for instance Yanushevsky (2011), Breivik
(2010), Breivik and Fossen (2009). Its main advantages
are its simplicity and efficiency in generating appropriate
heading reference trajectories such that, when tracked by
the heading autopilot, they guarantee the vehicle’s smooth
convergence to the desired path. The method is based on
a simple geometry which, when it comes to marine appli-
cations, refers to the LOS vector starting at the vehicle’s
position and passing through a point p(xlos , ylos ), which is
located on the path-tangential line at a lookahead distance
∆h > 0 ahead of the direct projection of the vehicle’s
position p(x, y) on to the path, this can be seen in Fig. 1.
The vehicle is then assigned to reach the constantly moving
point p(xlos , ylos ) and this induces the desired steering
behavior.
The performance of the algorithm is affected by factors
such as the value of the lookahead distance, as well as the
convergence speed of the heading autopilot. Regarding the
lookahead distance, large values will result in a smooth
convergence, without oscillations around the desired path
but also more time-consuming. On the other hand, small
values will drive the vehicle faster on the desired path, but
this will usually lead to an oscillatory behavior. For this
reason, several variable lookahead distance methods have
been proposed in the past (Pavlov et al., 2009; Oh and Sun,
2010; Lekkas and Fossen, 2012). The issue of the heading
systems theory. In that context, the subsystem consisting
of the heading autopilot and the vehicle is considered
to be a perturbation to the subsystem consisting of the
LOS guidance and the vehicle, for more information the
interested reader is referred to Børhaug and Pettersen
(2005), Lekkas and Fossen (2012).
It is a well known fact that, in the general case, the Euler
angles representation of the 6-DOF kinematics involves
singularities for the pitch angles θ = ±90◦ (Fossen, 2011).
Consequently, it is useful to derive a quaternion version
of the conventional LOS guidance for AUVs. Moreover,
the quaternion representation is more computationally
efficient compared to Euler angles since it does not include
trigonometric functions. This makes it even more suitable
for applications involving unmanned vehicles where the on-
board computational power might be more limited. This
paper serves as the first step toward this direction. The
quaternion representation of the LOS algorithm is derived
for two uncoupled 3-DOF cases: a) the horizontal xy-
plane, and b) the vertical zx-plane, and for a coupled case
where the sideslip angle is also a function of the vertical
motion. For each case, the terms of the guidance law are
transformed from Euler angles to quaternion by taking
into account the nature of quaternions that correspond to
rotations and using simple trigonometric identities.
The rest of this paper is organized as follows: Section 2
presents the vehicle model considered. In Section 3, the
Euler representation of the LOS guidance algorithm is
presented for the uncoupled horizontal and vertical planes

978-3-902823-52-6/2013 © IFAC 245 10.3182/20130918-4-JP-3022.00070

IFAC CAMS 2013
September 17-20, 2013. Osaka, Japan

pk+1 pn 3. EULER REPRESENTATION OF THE LOS

(xlos , ylos )
p GUIDANCE LAW
h

pk xe
3.1 Horizontal Plane LOS Guidance
U
xb In the case of decoupled horizontal plane path-following we
LOS
ye vector
assume that θ = 0◦ , consequently the kinematics equation
to be considered is:
ẋ = u cos(ψ) − v sin(ψ), (9)
ẏ = u sin(ψ) + v cos(ψ), (10)
p
ψ̇ = r. (11)
xn
y b
The horizontal-plane speed Uh is given by:
p
Uh := u2 + v 2 , (12)
and is assumed to be positive and bounded:
yn
Uh,min ≤ Uh ≤ Uh,max , 0 < Uh,min . (13)
From (12)–(13) it is implied that the vessel always has
Fig. 1. Line-of-sight guidance geometry for straight lines at least a nonzero surge speed. The reason for setting a
in the xy plane. Here the sideslip angle is equal to minimum positive speed Uh,min is related to the stability
zero. proof of the LOS algorithm and a more rigorous approach
as well as the coupled case. The quaternion transformation is outside the scope of this paper and can be found in
for the uncoupled horizontal plane is given in Section 4, Lekkas and Fossen (2013). The model (9)–(11) includes
for the uncoupled vertical plane in Section 5 and for the only absolute velocities and describes the motion of an
coupled case in Section 6. Some simulation results can be underactuated vehicle since only two out of three DOF’s
found in Section 7, and Section 8 concludes the paper. can be controlled independently, namely the yaw angle and
the surge velocity.
2. VEHICLE MODEL
Path Following Objective: Assuming that the vehicle is
2.1 Vehicle Dynamics assigned to converge to the line connecting the waypoints
WPk –WPk+1 , the along-track and the cross-track error for
Following the methodology of Børhaug and Pettersen a given vehicle position (x, y) are given by:
(2005), for the path-following task we can neglect the roll
   
xe x − xk
angle, hence for an underactuated autonomous vehicle the = R> (γp ) , (14)
ye y − yk
following 5-DOF dynamic model can be used:
where (xk , yk ) is the position of the k-th waypoint ex-
η̇ = J(η)ν, (1) pressed in the NED frame, and the rotation matrix from
Mν̇ + C(ν)ν + D(ν)ν + g(η) = τ , (2) the inertial frame to the path-fixed reference frame is given
where M is the mass and inertia matrix, C(ν) is the by:  
Coriolis and centripetal matrix, D(ν) is the damping cos(γp ) − sin(γp )
R(γp ) = ∈ SO(2). (15)
matrix, g(η) describes the gravitational and buoyancy sin(γp ) cos(γp )
forces, and τ includes the control forces and moments. Moreover,
Accordingly, the generalized position and velocity are xe = (x − xk ) cos(γp ) + (y − yk ) sin(γp ), (16)
recognized as: ye = −(x − xk ) sin(γp ) + (y − yk ) cos(γp ), (17)
η = (x, y, z, θ, ψ)T , ν = (u, v, w, q, r)T , (3) where γp is the horizontal-plane path-tangential angle:
where (x, y, z) is the vehicle’s inertial position in Cartesian γp = atan2(yk+1 − yk , xk+1 − xk ), (18)
coordinates, θ is the pitch angle and ψ is the yaw angle.
Then, the associated control objective for horizontal plane
In addition, u is the surge velocity, v is the sway velocity,
straight-line path-following is:
w is the heave velocity, q is the pitch rate and r is the yaw
rate. lim ye (t) = 0. (19)
t→+∞

2.2 Vehicle Kinematics Note that the along-track error xe does not need to be
minimized in a path-following scenario, the contrary is
The model considers only absolute velocities and is the true for applications that impose temporal constraints.
following, see Børhaug and Pettersen (2005): The lookahead-based guidance law is given by (see Breivik
and Fossen (2009)):
ẋ = u cos (ψ) cos (θ) − v sin (ψ) + w cos (ψ) sin (θ), (4)
−ye
 
ẏ = u sin (ψ) cos (θ) + v cos (ψ) + w sin (ψ) sin (θ), (5) ψd = γp + arctan . (20)
ż = −u sin (θ) + w cos (θ), (6) ∆h
In the presence of external disturbances, or during turns,
θ̇ = q, (7) the heading angle ψd and the course angle χd are not
1 aligned anymore and are related in the following way:
ψ̇ = r, cos (θ) 6= 0. (8)
cos(θ) χd = ψd + β, (21)

246
IFAC CAMS 2013
September 17-20, 2013. Osaka, Japan

and therefore the desired heading angle is: xn

−ye
 
ψd = γp + arctan − β. (22)
∆h
where
β = atan2(v, u), (23) Uh
which is equal to the orientation of the vehicle’s velocity u xb
vector Uh with respect to the body-fixed frame. In other
words, (23) is the angle between the vehicle’s velocity ori- v
entation and the vehicle’s heading. This is the commonly
known as sideslip, or drift, angle. yb
Uh = u

Stability Result: It has been shown that choosing the xb

desired heading as in (22) results in a κ-exponentially
stable equilibrium point ye = 0, see Lekkas and Fossen
(2013). This implies that the equilibrium point ye = 0 is
UGAS and ULES, see Lefeber (2000). yb

3.2 Vertical Plane LOS Guidance

This problem is similar to the one presented in Section

3.1. In the case of decoupled vertical plane path-following yn
of underwater vehicles, it is common to assume that the
yaw angle is constant, ψ = ψ0 . Consequently the kinematic Fig. 2. Sideslip angle β during a turn. There are no
equations to be considered are: external disturbances acting on the system in this
ẋ = u cos(ψ0 ) cos(θ) − v sin(ψ0 ) + w cos(ψ0 ) sin(θ), (24) case, hence the zero sideslip angle when the vehicle
ż = −u sin(θ) + w cos(θ), (25) moves forward. During a turn, however, there will be
a nonzero sway velocity v and, consequently, a β 6= 0.
θ̇ = q. (26)
with ∆v denoting the lookahead distance for the vertical-
Apparently, for this problem the vertical plane total speed plane LOS and where
is defined as:
α = atan2(w, u) (36)
p
Uv := u2 + w2 > 0, (27)
and is assumed to be positive and bounded: is the commonly known as the angle of attack.
Uv,min ≤ U ≤ Uv,max , 0 < Uv,min . (28) 3.3 LOS Guidance for 3-D Coupled Motions
Path Following Objective: Similarly to the case for 1
the horizontal LOS guidance is now coupled with the
surface vessels, we assume that the vehicle is supposed
vertical motion of the AUV. Hence 5-DOF’s are considered
to converge to the line connecting the waypoints W Pk –
and the kinematics is given by (4)–(8) The total speed in
W Pk+1 , the along-track and the cross-track error for a
this case is:
given vehicle position (x, z) are given by: p
    U := u2 + v 2 + w2 . (37)
xe x − xk Due to the system’s structure it is reasonable to consider
= R> (αp ) , (29)
ze z − zk the two following subsystems in order to solve the problem:
where (xk , zk ) is the position of the k-th waypoint ex- a) a depth controller for (6)–(7), and b) a LOS guidance for
pressed in the NED frame, and the rotation matrix from (4)–(5) and (8). The heave-pitch subsystem is uncoupled
the inertial frame to the path-tangential frame is given by: with respect to the rest of the states, while on the other

cos(αp ) sin(αp )
 hand (4)–(5) and (8) imply that the horizontal motion is
R(αp ) = ∈ SO(2). (30) coupled with the vertical plane motion via the pitch angle
− sin(αp ) cos(αp )
θ and the heave velocity w. According to the analysis in
Moreover, Lekkas and Fossen (2013), the solution for the heave-pitch
xe = (x − xk ) cos(αp ) − (z − zk ) sin(αp ), (31) subsystem is the same as in Section 3.2. Regarding the
ze = (x − xk ) sin(αp ) + (z − zk ) cos(αp ), (32) horizontal motion it was shown that the cross-track error
where αp is the vertical path-tangential angle: is given by:
p
αp = atan2(−(zk+1 − zk ), (xk+1 − xk )). (33) ẏe = (Uv2 cos2 (θ − α) + v 2 ) sin (ψ − γp + βv ) (38)
| {z }
Consequently, the associated control objective for vertical ζ
plane straight-line path-following is: where:
lim ze (t) = 0. (34) βv = atan2 (v, Uv cos (θ − α)) . (39)
t→+∞
Similarly to the horizontal cross-track error case, we as- The desired yaw angle in this case is:
−ye
 
sume that the desired pitch angle is perfectly tracked at
ψd = γp − βv + arctan . (40)
all times and choose the desired pitch angle as: ∆h
 
ze Consequently, the only difference w.r.t. the solution of
θd = αp + α + arctan , (35)
∆v Section 3.1 is the coupled sideslip angle βv .

247
IFAC CAMS 2013
September 17-20, 2013. Osaka, Japan

4. HORIZONTAL LOS GUIDANCE IN QUATERNION x, y , r

FORM xk , yk d r
Horizontal LOS Heading Autopilot

This section deals with transforming the horizontal LOS

guidance (22) in quaternion form. A quaternion qψ repre-
⌘, v
senting a rotation of an angle ψ around the z-axis can be Waypoints AUV rle;r

expressed as follows (Kuipers, 1999):

   
ψ ψ
qψ = cos +k sin . (41) xk , zk ✓d
2 2 Vertical LOS Depth Autopilot
q

| {z } | {z }
η ε3 x, z ✓, q

4.1 Transforming ψr to the quaternion qψr Fig. 3. Block diagram of the LOS guidance for the uncou-
pled horizontal and vertical planes.
First we define: γ  γ 
p p
ye2 + ∆2h := Rh2 (42) qγp = cos + k sin , (57)
2 2
−ye ∆h

sin (ψr ) = , cos (ψr ) = , (43) ∆y
γp = atan2 , (58)
Rh Rh ∆x
Then, since and, for the sake of notational brevity, we define:
   
ψr ψr q
sin (ψr ) = 2 sin cos , (44) Rxy = ∆x2 + ∆y 2 , (59)
2 2
consequently the quaternion transformation gives:
 
ψr
cos (ψr ) = 2 cos2 − 1, (45)
2
s
∆x + Rxy
we get: η γp = ± , (60)
2Rxy
sin (ψr ) = 2ε3 η, (46)
∆y
2
cos (ψr ) = 2η − 1. (47) ε3γp = p . (61)
2Rxy (∆x + Rxy )
Combining (42)–(43) with (44)–(45) yields:
−ye
2ε3r ηr = p 2 , (48) 4.3 Transforming β
∆h + ye2
∆h Implementing the same procedure for the sideslip angle β
2ηr2 − 1 = p 2 , (49)
∆h + ye2 yields:
   
and solving w.r.t. ηr and ε3r gives: β β
qβ = cos + k sin , (62)
r
∆h + Rh 2 2
v
ηr = ± , (50)
2Rh β = atan2 . (63)
−ye u
ε3r = (51) which gives:
2Rh η r
−ye u+U
=p . (52) ηβ = ± , (64)
2Rh (∆h + Rh ) 2U
v
It is important to note that either the positive or negative ε3β = p . (65)
value for ηr can be chosen, hence affecting the sign of εr 2U (u + U )
accordingly. Both cases correspond to the same rotation,
as it is shown by (A.5). Therefore, the corresponding 4.4 Horizontal Rotation Quaternion
quaternion, qψr is written:
A quaternion-based control law system will make use of
r
∆h + R h −ye
qψr = + kp . (53) the horizontal rotation quaternion, qh , which is given by
2Rh 2R(∆h + R) the quaternion products of qψr , qβ , qγp . Therefore:
It can be confirmed that (53) corresponds to a rotation qh = qψr ⊗ qβ ⊗ qγp . (66)
quaternion by computing the norm:
Due to the fact that all the rotations considered in (66)
kqk = η 2 + εT ε (54) are around the z-axis, the order of the multiplication is
2
∆h + R h ye not important. Note, however that this is a special case.
= + (55)
2Rh 2Rh (∆h + Rh )
= 1. (56) 5. VERTICAL LOS GUIDANCE IN QUATERNION
FORM
4.2 Transforming γp to the quaternion qγp
The transformation procedure is similar to the one in
Similarly to Section 5.1 we have: Section 4. A quaternion qθ representing a rotation of an

248
IFAC CAMS 2013
September 17-20, 2013. Osaka, Japan

x, y, w,✓ , r and, for the sake of notational brevity, we define:

p
xk , yk
Horizontal LOS
d
Heading Autopilot Rzx = ∆z 2 + ∆x2 , (82)
r consequently the quaternion transformation gives:
r
⌘, v ∆x + Rzx
Waypoints AUV ηα = ± , (83)
2Rzx
rle;r

∆x
ε3αp = p . (84)
zd
q
2Rzx (∆z + Rzx )
Depth Autopilot

z, ✓, q 5.3 Transforming α to the quaternion qα

Fig. 4. Block diagram of the LOS guidance for the coupled Implementing the same procedure for the angle of attack
horizontal and vertical planes. α yields:
α α
angle θ around the y-axis can be expressed as follows qα = cos + j sin , (85)
(Kuipers, 1999): 2  2
w
α = atan2 . (86)
   
θ θ
qθ = cos +j sin . (67) u
2 2 which gives:
| {z } | {z } r
η ε3 u + Uv
ηα = ± , (87)
5.1 Transforming θr to the quaternion qθr 2Uv
w
ε3α = p . (88)
2Uv (u + Uv )
ze2 + ∆2v = Rv2 (68)
ze ∆v 5.4 Vertical Rotation Quaternion
sin (θr ) = , cos (θr ) = , (69)
Rv Rv
Then, since As was the case in the previous section, we now calculate
    the quaternion qv that describes the total rotation around
θr θr the y-axis for the vertical LOS case:
sin (θr ) = 2 sin cos , (70)
2 2
  qv = qθr ⊗ qα ⊗ qαp . (89)
θr
cos (θr ) = 2 cos2 − 1, (71)
2 6. LOS GUIDANCE FOR 3-D COUPLED MOTIONS
we get: IN QUATERNION FORM
sin (θr ) = 2ε3 η, (72)
2
cos (θr ) = 2η − 1. (73) As discussed earlier, the only difference in this case is the
coupled sideslip angle βv . Assuming perfect pitch tracking
Combining (68)–(69) with (70)–(71) yields: (θ = θd ), we have from (35):
ze
2ε3r ηr = p , (74) θ − α = arctan (−∆z/∆x) + arctan (ze /∆v ),
∆2v + ze2
= arctan ((ze ∆x − ∆v ∆z)/(∆x∆v + ze ∆z)), (90)
∆h
2ηr2 − 1 = p
| {z }
, (75) λ1
∆2v + ze2
then we calculate the quantity:
and solving w.r.t. ηr and ε3r gives: q
λ2 := cos(θ − α) = 1/( 1 + λ21 ). (91)
r
∆v + R v
ηr = ± , (76)
2Rv The transformation in this case gives:
ze s
ε3r = (77) Uv λ2 + ζ
2Rv η ηβv = ± , (92)
ze 2ζ
=p . (78)
2Rv (∆v + Rv ) ν
ε3βv = p . (93)
Therefore, the corresponding quaternion, qθr is written: 2ζ(Uv λ2 + ζ)
r
∆v + Rv ze
qr = + jp . (79) 7. SIMULATIONS
2Rv 2R(∆v + Rv )

5.2 Transforming αp to the quaternion qαp In order to test the validity of the transformation method-
ology presented in the previous sections, a simple nu-
Similarly to Section 5.1 we have: merical example was constructed and implemented. The
α 
p
α 
p
lookahead distance was chosen as ∆h = 4 m and the cross-
qαp = cos + j sin , (80) track error increased linearly from −10 m to +10 m with a
2 2
step of .01 m, hence generating 2001 samples. In Fig. 5 the
−∆z

αp = atan2 , (81) quaternion computed by (53) is presented and Fig. 6 shows
∆x the corresponding Euler angles. It can be observed that the

249
IFAC CAMS 2013
September 17-20, 2013. Osaka, Japan

1 Appendix A. QUATERNION FUNDAMENTALS

0.5 η In this paper, the quaternion attitude representation is

ε1
0
used, where the unit quaternion q corresponding to a
ε2 rotation in R3 is defined as:
ε3
−0.5 q = η + ε1 i + ε2 j + ε3 k, (A.1)
−1 with kqk = 1. The complex conjugate of q is given by:
0 500 1000 1500 2000 2500
q∗ = η − ε1 i − ε2 j − ε3 k, (A.2)
Fig. 5. Quaternion elements computed by the transforma- and the inverse quaternion is given by:
tion algorithm. q−1 = q∗ . (A.3)
Following (Kuipers, 1999, Ch. 5.9), let us associate an
2
angle λq with the quaternion q. Then, the image w ∈ R3
Roll
1 of a vector v ∈ R3 due to the rotation by an angle λq can
Pitch
Yaw
be expressed in quaternion form:
0 wq = qvq∗ . (A.4)
−1 The rotation corresponding to the negative of a quaternion
is the same as the rotation due to the quaternion:
−2 wq = w−q . (A.5)
0 500 1000 1500 2000 2500
Moreover, the product of two quaternions q1 , q2 is defined
Fig. 6. Euler angles corresponding to the quaternion com- as:
puted by the transformation algorithm η1 η2 − εT
 
q1 ⊗ q2 := 1 ε2 . (A.6)
. η2 ε1 + η1 ε2 + ε1 × ε2
2
REFERENCES
yaw
1 Børhaug, E. and Pettersen, K.Y. (2005). Cross-track
control for underactuated autonomous vehicles. In
0
Proceedings of the 44th IEEE Conference on Decision
−1 and Control, and the European Control Conference,
602–608. Seville, Spain.
−2 Breivik, M. (2010). Topics in guided motion control of
0 500 1000 1500 2000 2500
marine vehicles. Ph.D. thesis, Norwegian University of
Fig. 7. Euler angles computed by arctan(−ye /∆h ) Science and Technology.
. Breivik, M. and Fossen, T.I. (2009). Guidance laws
for autonomous underwater vehicles, chapter 4, 51–76.
computed quaternion corresponds to a rotation around the INTECH Education and Publishing.
z-axis. Moreover, Fig. 7 shows the values computed by the Fossen, T.I. (2011). Handbook of Marine Craft Hydrody-
term arctan(−ye /∆h ) and the result is that the plot is the namics and Motion Control. John Wiley and Sons Ltd.
same as the one in Fig. 6, hence confirming the validity of Kuipers, J.B. (1999). Quaternions and rotation sequences.
the transformations leading to (53). Princeton university press.
Lefeber, E. (2000). Tracking control of nonlinear mechan-
ical systems. Ph.D. thesis, University of Twente.
8. CONCLUSIONS Lekkas, A.M. and Fossen, T.I. (2012). A time-varying
lookahead distance guidance law for path following. In
This paper proposed a simple technique in order to trans- 9th IFAC Conference on Manoeuvring and Control of
form the conventional LOS guidance algorithm in quater- Marine Craft. Arenzano, Italy.
nion form. The main motivation is to avoid singularities Lekkas, A.M. and Fossen, T.I. (2013). Line-of-Sight Guid-
in the more general 6-DOF case and also get a more ance for Path Following of Marine Vehicles, chapter 5,
computationally-efficient algorithm. The transformation 63–92. LAP LAMBERT Academic Publishing (O. Gal,
methodology was presented here for the uncoupled mo- Ed.).
tions in the horizontal and vertical planes, as well as a Oh, S.R. and Sun, J. (2010). Path following of underac-
coupled case. Future work includes the transformation of tuated marine surface vessels using line-of-sight based
the LOS guidance corresponding to the coupled 6-DOF model predictive control. Ocean Engineering, 37(2-3),
motions. 289–295.
Pavlov, A., Nordahl, H., and Breivik, M. (2009). MPC-
based optimal path following for underactuated vessels.
ACKNOWLEDGEMENTS In 8th IFAC International Conference on Manoeuvring
and Control of Marine Craft, 340–345. Guaruja, Brazil.
This work was supported by the Norwegian Research Yanushevsky, R. (2011). Guidance of Unmanned Aerial
Council through the Centre for Autonomous Marine Op- Vehicles. CRC Press.
erations and Systems at NTNU.

250