Chapter 2 - Kinematics

2.1 Kinematic Preliminaries 2.2 Transformations between BODY and NED 2.3 Transformations between ECEF and NED 2.4 Transformations between ECEF and Flat-Earth Coordinates 2.5 Transformations between BODY and FLOW

“The study of dynamics can be divided into two parts: kinematics, which treats only geometrical aspects of BODY motion, and kinetics, which is the analysis of the forces causing the motion”

Overall Goal of Chapters 2 to 10 Represent the 6-DOF equations of motion in a compact matrix-vector form according to:

1 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) Chapter Goals

• Understand the geographic reference frame NED, the Earth-centered reference frame ECEF and the body-fixed reference frame BODY • Understand what FLOW axes are and why we use these axes for marine craft and aircraft • Be able to write down the differential equations relating BODY velocities to NED positions (Euler angles and unit quaternions) • Be able to: • Transform ECEF (x, y, z) positions to (longitude, latitude, height) and vice versa • Transform (longitude, latitude, height) to flat-Earth positions (x, y, z) and vice versa • Be able to define and visualize: • Angle of attack • Sideslip angle • Crab angle • Heading angle • Course angle

2 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.1 Vector Notation (Fossen 2021)

Coordinate-free vector (arrow notation)

n n n !u " u1!n1 # u2!n2 # u3!n3

!ni !i " 1,2,3" are the unit vectors that define #n$

Coordinate form of !u in ! n " (bold notation)

n n n n ! u ! !u1,u2,u3" ‘‘ The coordinate-free vector is expressed in {n}’’

3 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.1 Six-DOFs Motions

The notation is adopted from:

SNAME (1950). Nomenclature for Treating the Motion of a Submerged Body Through a Fluid. The Society of Naval Architects and Marine Engineers, Technical and Research Bulletin No. 1-5, April 1950, pp. 1-15.

4 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.1 Generalized Coordinates

For a marine craft not subject to any motion constraints

Number of independent (generalized) coordinates = DOFs

The term generalized coordinates refers to the parameters that describe the configuration of the craft relative to some reference configuration.

For marine craft, the generalized position and velocity vectors are

These quantities are all formulated in NED. It is advantageous to express the velocities of the craft in the BODY frame. In other words

The relationship between the velocity vector in NED and BODY will be derived on the subsequent pages.

5 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.1 Summary: 6-DOFs Vectors

6-DOF generalized position, velocity and force vectors expressed in {b}

6 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) zi, ze

2.1 Reference Frames we

Earth-Centered Reference Frames

ECI {i}: The Earth-centered inertial (ECI) frame {i} = (xi, yi, zi) is an inertial frame for terrestrial navigation, that is a nonaccelerating reference frame in which New- ton’s laws of ECEF/ECI we t motion apply. ye

yi ECEF {e}: The Earth-centered Earth-fixed (ECEF) reference frame {e} = xi (xe, ye, ze) has its origin oe fixed to the center of the Earth but the axes rotate relative to the inertial frame ECI, which is fixed in space. xe

xe - axis in the equatorial plane pointing towards the zero/prime meridian; same longitude as the Greenwich observatory ye - axis in the equatorial plane completing the right-hand frame ze - axis pointing along the Earth’s rotational axis

−5 The Earth rotation is ωie = 7.2921 × 10 rad/s and the Earth’s rotational vector expressed in {e} is

7 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.1 Reference Frames (cont.) ze

BODY NED Geographic Reference Frames (Tangent Planes) N E D Geographical reference frames are usually chosen as tangent planes on the surface of the Earth. µ

l ye ECEF/ECI • Terrestrial navigation: The tangent plane on the surface of the Earth moves with the craft and its location is specified by time- varying longitude-latitude values (l, μ). The tangent frame is xe usually rotated such that its axes points in the NED directions.

• Local navigation: The tangent plane is fixed at constant values NED {n}: North-East-Down frame; defined (l0 , μ0 ) and the position is computed with respect to a local relative to the Earth’s reference coordinate origin. The axes of the tangent plane are usually ellipsoid (WGS 84), usually as the chosen to coincide with the NED axes. tangent plane to the ellipsoid

“Flat-Earth navigation”: position is accurate to a smaller geographical area (10 km × 10 km). xb - axis points towards true North yb - axis points towards East

zb - axis points downwards normal to the Earth’s surface 8 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.1 Reference Frames (cont.) ze

Body-Fixed Reference Frames N NED E The body-fixed reference frame {b} = (xb, yb, zb) D BODY with origin ob is a moving coordinate frame that is fixed to the craft. µ

l y BODY {b}: For marine craft, the body axes are ECEF/ECI e chosen as:

xb- longitudinal axis (directed from aft to fore) xe

yb- transversal axis (directed to starboard)

zb-normal axis (directed from top to bottom)

FLOW {f}: Flow axes are used to align the x-axis with the craft’s velocity vector such that lift is perpendicular to the relative flow and drag is parallel. The transformation from FLOW to BODY axes is defined by two principal rotations where the rotation angles are the angle of attack α and the sideslip angle β. The main purpose of the flow axes is to simply the computations of lift and drag forces.

9 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.1 Body-Fixed Reference Points

The most important reference point:

The following time-varying points are expressed with respect to the CO

The CF is located a distance LCF from the CO in the x-direction

The center of flotation is the centroid of the water plane area Awp in calm water. The vessel will roll and pitch about this point.

10 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.2 Transformations between BODY and NED

Orthogonal matrices of order 3:

O!3" ! #R|R ! !3!3, RR" ! R!R ! I$

Special orthogonal group of order 3:

SO!3" ! #R|R ! !3!3, R is orthogonal and detR !1$

Rotation matrix: Example: RR! ! R!R ! I, det R ! 1

Since R is orthogonal, R!1 ! R!

11 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.2 Transformations between BODY and NED (cont.)

Cross-product operator as matrix-vector multiplication: ! ! a :! S!!"a

0 !!3 !2 !1 ! S!!" ! !S !!" ! !3 0 !!1 , ! " !2

!!2 !1 0 !3

where S ! ! S ! is a skew-symmetric matrix

The inverse operator is denoted:

12 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.1 Transformations between BODY and NED (cont.)

Euler’s theorem on rotation:

2 R!,"! I3#3 ! sin" S!"" ! !1 ! cos"" S !""

n n b n vb/n ! Rbvb/n, Rb :! R!," # ! ! ! !!1,!2,!3" , |!| ! 1 where

2 R11 ! !1 ! cos!" "1 " cos ! 2 R22 ! !1 ! cos!" "2 " cos ! ! 2 R33 ! !1 ! cos!" "3 " cos !

R12 ! !1 ! cos!" "1"2 ! "3 sin!

R21 ! !1 ! cos!" "2"1 " "3 sin!

R23 ! !1 ! cos!" "2"3 ! "1 sin!

R32 ! !1 ! cos!" "3"2 " "1 sin!

R31 ! !1 ! cos!" "3"1 ! "2 sin!

R13 ! !1 ! cos!" "1"3 " "2 sin!

13 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.1 Euler Angle Transformation x 3 x2 y u2 u U Three principal rotations: 3 (1) Rotation over yaw

angle y about z3 . Note that w =w . ! ! !0, 0, 1"! ! ! " 32 ! ! !0, 1, 0"! ! ! " v ! 3 y ! ! !1, 0, 0" ! ! " y 3 v2 y2

x1 u1 c! !s! 0 u q x 2 Rz,! ! s! c! 0 (2) Rotation over pitch 2

angle q about y2 . w 0 0 1 U 2 Note that v21 =v .

w1 c! 0 s!

Ry,! ! 0 1 0 z1 z2 !s! 0 c! v 1 y 1 0 0 f 1 y0b =y v=v2 Rx,! ! 0 c! !s! w=w (3) Rotation over roll 0 angle f about x . 0 s c 1 ! ! w Note that u =u f 1 12 U . 14 z0b =z z1 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.2 Euler Angle Transformation (cont.) Linear velocity transformation (zyx convention):

Euler angle rotation matrix

Small-angle approximation:

15 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.2 Euler Angle Transformation (cont.)

NED positions (continuous time and discrete time)

Euler’s method with sampling time h

Component form

16 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.2 Euler Angle Transformation (cont.)

Angular velocity transformation (zyx convention):

where

Small angle approximation: Notice that: o 1 0 !" 1. Singular point at ! ! " 90 !1 ! T!!!!nb" ! 0 1 "!# T! !!nb" " T!!!nb" 0 !# 1

17 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.2 Euler Angle Transformation (cont.)

Euler angle attitude representations ODE for Euler angles ODE for rotation matrix

Component form where

Must be combined with an algorithm for computation of the Euler angles from the rotation matrix

18 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.2 Euler Angle Transformation (cont.) Summary: 6-DOF kinematic equations

3-parameter representation ! !nb! !!,",#" with singularity at ! ! " 90o

Component form

19 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.2 Unit Quaternions

4-parameter representation -avoids the representation singularity of the Euler angles -numerical effective (no trigonometric functions)

! ! ! 3 ! Q ! !q|q q !1, q ! !!," " , " ! " and ! ! "" ! " !!1,!2,!3!

Unit quaternion (Euler parameter) rotation matrix (Chou 1992):

2 R!,! ! I3"3 " sin ! S!!" " !1 ! cos!" S !!"

! " ! ! cos # 2 "1 cos q 2 Q ! ! # ! "2 ! " ! sin 2 ! ! !!1,!2,!3" ! " sin 2 "3

20 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.2 Unit Quaternions (cont.)

Linear velocity transformation NB! must be integrated under the constraint

2 2 2 2 ! ! "1 ! "2 ! "3 " 1

Component form (NED positions)

21 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.2 Unit Quaternions (cont.) Angular velocity transformation NB! nonsingular to the price of one more parameter

Alternative representation using the quaternion product

22 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.2 Unit Quaternions (cont.)

Summary: 6-DOF kinematic equations (7 ODEs) 4-parameter representation ! q ! !!,"1, "2, "3! Nonsingular but one more ODE is needed

Component form:

23 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.2 Unit Quaternions (cont.)

Discrete-time algorithm for unit quaternion normalization

24 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.2 Unit Quaternions (cont.) Continuous-time algorithm for unit quaternion normalization

If q is initialized as a unit vector, then it will remain a unit vector.

However, integration of the quaternion vector q from the differential equation will introduce numerical errors that will cause the length of q to deviate from unity.

This is avoided by introducing feedback:

! ! 0 (typically 100!

x! " !!x!1 ! x" linearization about x = 0 gives x! " !!x

25 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.2 Unit Quaternion from Euler Angles

26 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.2 Euler Angles from a Unit Quaternion

Since the rotation matrices of the two kinematic representations are equal:

! ! !nb! !!,",#" q ! !!,"1, "2, "3!

c!c" !s!c# ! c!s"s# s!s# ! c!c#s" R11 R12 R13

s!c" c!c# ! s#s"s! !c!s# ! s"s!c# " R21 R22 R23

!s" c"s# c"c# R31 R32 R33

Euler angle solutions:

where atan2(y,x) is the 4-quadrant inverse tangent confining the result to

27 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.3 Transformation between ECEF and NED

A point on or above the Earth’s surface is ze uniquely determined by: NED {n}-frame h Longitude: l (deg) N Latitude: µ (deg) E Ellipsoidal height: h (m) D

l ECEF ye {e}-frame

xe NED axes definitions: N – North axis is pointing North E – East axis is pointing East D – Down axis is pointing down in the normal direction to the Earth’s surface

28 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.3 Longitude and Latitude Transformations

The transformation between the ECEF and NED velocity vectors is: ! 2 !en! !l,!" ! S Two principal rotations: 1. a rotation l about the z-axis 2. a rotation (! ! ! " / 2 ) about the y-axis.

29 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.3 Longitude/Latitude from ECEF Coordinates

Satellite navigation system measurements are given in the ECEF frame: Not to useful for the operator.

Presentation of terrestrial position data is therefore made in terms of the ellipsoidal parameter's longitude l, latitude µ and height h.

N E Transformation D

µ l, µ and h l ye

xe 30 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.3 Longitude/Latitude from ECEF Coordinates (cont.)

Parameters Comments re ! 6 378 137 m Equatorial radius of ellipsoid (semimajor axis) WGS-84 rp ! 6 356 752 m Polar axis radius of ellipsoid (semiminor axis) !5 !e ! 7.292115 ! 10 rad/s Angular velocity of the Earth e ! 0.0818 Eccentricity of ellipsoid

2 N re rp 2 ! 2 2 2 2 e ! 1 ! ! re " re cos !"rp sin !

while latitude µ and height h are implicitly computed by

31 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.3 Longitude/Latitude from ECEF Coordinates (cont.)

Ref. Hofman-Wellenhof et al. (2004) 32 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.3 ECEF Coordinates from Longitude/Latitude

! The transformation from ! en ! ! l , ! " for given heights h to

is given by

33 Ref. Heiskanen (1967) Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.3 ECEF Coordinates from Longitude/Latitude (cont.)

34 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.4 Transformations between ECEF and Flat-Earth Coordinates

For local flat-Earth navigation it can be assumed that the NED tangent plane is fixed on the surface of the Earth.

Assume that the NED tangent plane is located at l0 and μ0 such that

The ECEF coordinates satisfy the differential equation

Flat Earth is a good approximation for ships and floating structures operating in a limited region.

Flat Earth is a bad approximation for global waypoint tracking control systems for marine craft since (l, μ) will vary largely for vessels in transit between the different continents.

35 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.4 Longitude, Latitude and Height from Flat-Earth Coordinates

Given a local NED position with coordinate origin (l0, μ0) and reference height href in meters above the surface of the Earth, the change in longitude and latitude is

ssa is the smallest signed angle confining the argument to the interval [−π, π)

36 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) Smallest Signed Angle – Smallest Difference Between Two Angles

37 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.4 Flat-Earth Coordinates from Longitude, Latitude and Height

The NED positions (xn,yn,zn) with respect to a flat-Earth coordinate system with origin (l0 , μ0 ) and reference height href are computed as (Farrell 2008)

38 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.5 Definitions of Course, Heading and Crab Angles

The relationship between the course angle, heading angle and crab angle is important for maneuvering of a vehicle in the horizontal plane. The terms course and heading are used interchangeably in much of the literature on guidance, navigation and control and this leads to confusion. Course angle χ The angle from the x axis of the NED frame to the velocity vector of the vehicle, positive rotation about the z axis of the NED frame. Measured using GNSS (or HPR under water) Heading (yaw) angle ψ The angle from the NED x axis to the BODY x axis, positive rotation about the z axis of the NED frame. Measured using a compass

Crab angle βc Given by the formula:

39 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.5 Crab Angle (Amplitude-Phase Form)

Proof:

These equations can be expressed in amplitude- phase form

Amplitude

Phase

40 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.5 Extensions to Ocean Currents: Angle of Attack and Sideslip Angle

For a marine craft exposed to ocean currents, the concept of relative velocities is introduced. The relative velocities are

2 2 2 Ur ! ur " vr " wr #

where uc, vc and wc are the current velocities and

is the current speed.

41 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.5 Definitions of Course, Heading, Crab and Sideslip Angles

Ocean Current Triangle:

North Horizontal Plane

Sideslip angle:

Crab angle:

Course angle:

Heading (yaw) angle

East Speed over ground:

Relative speed:

Current speed:

Current direction:

GNSS measures course angle � and speed over ground U Compass measures heading angle � 42 Currents can be measured by an Acoustic Doppler Current Profiler (ADCP) Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.5 Crab Angle versus Sideslip Angle

Crab angle The crab angle is a function of the sway velocity and the North speed over ground

Sideslip angle The sideslip angle is defined in terms of relative velocities

East

Remark: In SNAME (1950) and Lewis (1989) the sideslip angle for marine craft is defined as:

βSNAME = -β We use the sign convention by the aircraft community e.g. Nelson (1998) and Stevens (1992). This definition is more intuitive from a guidance point-of-view than SNAME (1950).

43 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.5 Crab Angle versus Sideslip Angle

Some interesting observations

1) A vehicle moving on a straight line in calm water (U > 0 and v = 0) will have a zero-crab angle

2) As soon as you start to turn, the sway velocity will be non-zero and consequently, . The crab angle corresponds to the amount of correction a vehicle must be turned in order to maintain the desired course.

3) A vehicle is also exposed to environmental forces, which induces a flow velocity (wind/current). This forces the vehicle to “sideslip”. Moreover,

NB! Crab angle is the special case of the sideslip angle, for which the current speed Vc = 0

44 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.5 Definitions of Angle of Attack and Relative Flight Path

North

Relative flight path angle:

Angle of attack:

Pitch angle:

Down Speed over ground:

Relative speed: Ocean Current Triangle: Current speed: Vertical Plane

GNSS measures flight path γ and speed over ground U AHRS or INS measure pitch angle � Currents can be measured by an Acoustic Doppler Current Profiler (ADCP) 45 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.5 Transformation between BODY and FLOW

FLOW axes are often used to express hydrodynamic data. The FLOW axes are found by rotating the BODY axis system such that resulting x-axis is parallel to the freestream flow.

In FLOW axes, the x-axis directly points into the relative flow while the z-axis remains in the reference plane but rotates so that it remains perpendicular to the x-axis. The y-axis completes the right-handed system.

x b U a -b xflow

xstab

zb 46 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.5 Rotation Matrix between BODY and FLOW

y U ! u2 " v2 # b

Velocity transformation: x b U a -b xflow

xstab

zb Principal rotations:

cos! 0 sin! cos" sin" 0 ! Ry,! ! 0 1 0 , Rz,!" ! Rz," ! !sin " cos" 0 !sin! 0 cos! 0 0 1

47 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) 2.5 Summary: Angle of Attack and Sideslip Angle

Relative velocities

Angle of attack and sideslip angle

x b U a -b xflow

xstab

zb 48 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen) Chapter Goals - Revisited

49 Lecture Notes TTK 4190 Guidance, Navigation and Control of Vehicles (T. I. Fossen)