Chapter 2 - Kinematics

Chapter 2 - Kinematics

2.1 Reference frames

2.2 Transformations between BODY and NED

2.3 Transformations between ECEF and NED

2.4 Transformations between BODY and FLOW

“The study of dynamics can be divided into two parts: kinematics, which treats only geometrical

aspects of motion, and kinetics, which is the analysis of the forces causing the motion”

2

Overall Goal of Chapters 2 to 8

The notation and representation are adopted from:

Fossen, T. I. (1991). Nonlinear Modeling and Control of Underwater Vehicles, PhD thesis, Department of Engineering Cybernetics, NTNU, June 1991.

Fossen, T. I. (1994). Guidance and Control of Ocean Vehicles, John Wiley and Sons Ltd. ISBN: 0-471-94113-1.

Represent the 6-DOF dynamics in a compact matrix-vector form according to:





J









M





C











D











g









g

₀









_wind





wave

#

#

2.1 Reference Frames

ECI {i}: Earth centered inertial frame; non-accelerating frame (fixed in space) in which Newton’s laws of motion apply.

ECEF {e}: Earth-Centered Earth-Fixed frame; origin is fixed in the center of the Earth but the axes rotate relative to the inertial frame ECI.

NED {n}: North-East-Down frame; defined relative to the Earth’s reference ellipsoid (WGS 84). BODY {b}: Body frame; moving coordinate frame fixed to the vessel.

x_b- longitudinal axis (directed from aft to fore) y_b- transversal axis (directed to starboard) z_b-normal axis (directed from top to bottom)

N E D  e x y l z e e BODY ECEF/ECI NED _et _e x y y x z i i e e i,ze ECEF/ECI

4

2.1 Reference Frames – Body-Fixed

Reference Points

• CG - Center of gravity

• CB - Center of buoyancy

• CF - Center of flotation

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

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

u

  u₁nn1  un2n2  u3nn3

un  u₁n,u₂n,u₃n Coordinate-free vector

n

i i  1, 2, 3 are the unit vectors that definen Coordinate form of uinn

forces and linear and positions and DOF moments angular velocities Euler angles

1 motions in the x-direction (surge) X u x 2 motions in the y-direction (sway) Y v y 3 motions in the z-direction (heave) Z w z 4 rotation about the x-axis (roll, heel) K p  5 rotation about the y-axis (pitch, trim) M q  6 rotation about the z-axis (yaw) N r 

2.1 Reference frames and 6-DOF motions

x_b y_b u (surge) r (yaw) v (sway) (heave) w (roll) p (pitch) q

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.

6

2.1 Reference Frames - Notation

Generalized position, velocity and force ECEF position: pb/e e _ x y z  3 Longitude and latitude en  l   S 2 NED position: pb/n n  N E D  3 Attitude (Euler angles) nb      S3 Body-fixed linear velocity v_bb_/n  u v w  3 Body-fixed angular velocity _bb_{/n } p q r  3 Body-fixed force: fb b _ X Y Z  3 Body-fixed moment mb b  K M N  3   pb/n n _{or p} b/n e ₎ nb ,   vb/n b _bb_/n ,   f_bb m_bb #

2.2 Transformations between

BODY and NED

Special orthogonal group of order 3:

SO



3







R

|

R





33

,

R

is orthogonal and det

R



1



Orthogonal matrices of order 3:

O



3







R

|

R





33

,

RR





R



R



I



RR  RR  I, detR  1 Rotation matrix: Since R is orthogonal, R1  R  to  _R from to _ from Example:

8





a

:



S







a

Cross-product operator as matrix-vector multiplication:

S  S  0 3 2 3 0 1 2 1 0 ,   1 2 3

where is a skew-symmetric matrix S  S

2.2 Transformations between

BODY and NED

2.2 Transformations between

BODY and NED

Eulers theorem on rotation:

R11  1 cos 12  cos R22  1 cos ₂2  cos R33  1 cos ₃2  cos R12  1 cos 12 3sin R21  1 cos 21 3sin R23  1 cos 23 1sin R32  1 cos 32 1sin R31  1 cos 31 2sin R13  1 cos 13 2sin R, I33 sin S  1 cos S 2  where   1,2,3, ||  1  vn_b/n  R_bnvb_b/n, R_bn : R, #

10

2.2.1 Euler Angle Transformation

Three principal rotations:

(2) Rotation over pitch angle about . Note that .  y v =v 2 2 1 x2 x3 y3 y2 u3 u2 v2 v3  

(1) Rotation over yaw angle about . Note that .  z w =w 3 3 2 x1 x2 z1 z2 u1 u2 w1 w2  U U

(3) Rotation over roll angle about . Note that .  x u =u 1 1 2 z =z0 b z1 y1 y =y0 b v=v2  v1 w=w0 w1  U   1, 0, 0      0, 1, 0      0, 0, 1    Rx,  1 0 0 0 c s 0 s c Ry,  c 0 s 0 1 0 s 0 c Rz,  c s 0 s c 0 0 0 1

2.2.1 Euler Angle Transformation

Linear velocity transformation (

zyx

-convention):

Small angle approximation:

where R_bnnb  cc sc css ss ccs sc cc sss cs ssc s cs cc R_bnnb  I33  Snb  1    1    1 R_bnnb1  Rnbnb  Rx,Ry,Rz, R_bnnb : Rz,Ry,Rx, p _bn_/n  R_bnnbv_bb_/n #

12

NED positions (continuous time and discrete time):

2.2.1 Euler Angle Transformation

Component form:

Euler

integration p_bn_/n  Rn_bnbv_bb_/n #

N  ucoscos  vcossinsin  sincos  wsinsin  coscossin

Ė  usincos  vcoscos  sinsinsin  wsinsincos  cossin

D  usin  vcossin  wcoscos

#

# #

p_bn_/nk  1  p_bn_/nk  hR_bnnbkv_bb_/nk #

Angular velocity transformation (

zyx

-convention):

2.2.1 Euler Angle Transformation

where

1. Singular point at    90o

Small angle approximation: _{Notice that:}

T_1nb  1 0 s 0 c cs 0 s cc  Tnb  1 st ct 0 c s 0 s/c c/c T_nb  1 0  0 1  0  1 T_1nb  T_nb nb  Tnb_bb_/n #  b/n b   0 0  R_x_, 0  0  R_x_,Ry, 0 0  : T_1nbnb #

14

ODE for Euler angles: ODE for rotation matrix

2.2.1 Euler Angle Transformation

Component form:

  p  qsintan  rcostan

  qcos  rsin   q sin cos  r cos cos ,   90 o # #

# _{+ algorithm for computation}

of Euler angles from the rotation matrix

where

Euler angle attitude representations:

R_bnnb nb ,, nb  Tnb_bb_/n # R b n  R_bnS_bb_/n # S_bb_/n  0 r q r 0 p q p 0 #

Summary: 6-DOF kinematic equations:

2.2.1 Euler Angle Transformation

Component form:

3-parameter representation

with singularity at    90o

N  ucoscos  vcossinsin sincos

 wsinsin coscossin

Ė  usincos  vcoscos sinsinsin

 wsinsincos cossin D  usin  vcossin wcoscos

#

# #

  p  qsintan  rcostan

  qcos rsin   q sin cos  r cos cos ,   90 o # # # nb ,,    J  p_bn_/n nb  Rb n_ nb 033 033 Tnb v_bb_/n _bb_/n #

16

2.2.2 Unit Quaternions

4-parameter representation:

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

Q  q|qq 1,q  ,,   3 and     1,2,3

R,  I33  sin S  1  cos S 2



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

  cos  2   1,2,3  sin  2 q   1 2 3  cos  2  sin  2  Q R_bnq :  R,  I33  2S  2S2

2.2.2 Unit Quaternions

Linear velocity transformation

where

R_bnq 

1  2₂2  ₃2 212  3 213 2

212  3 1  22₁  ₃2 223 1

213  2 223  1 1  212  22

Component form (NED positions):

Rbnq1  R

b n_q qq  1

NB! must be integrated under the constraint or 2   1 2  2 2  3 2  ₁ N  u1  2₂2  2₃2 2v12  3  2w13  2 Ė  2u12  3  v1  2₁2  2₃2  2w23  1 D  2u13  2  2v23  1 w1  2₁2  2₂2 # # # p _bn_/n  Rn_bqv_bb_/n #

18

2.2.2 Unit Quaternions

Angular velocity transformation

Tqq  1₂ 1 2 3  3 2 3  1 2 1  , T_qqTqq  1₄I33 where   1₂ 1p 2q 3r 1  1 2 p  3q 2r 2  1 2 3p q 1r 3  1 2 2p 1q r # # # # Component form: NB! nonsingular to the price of one more parameter

Alternative representation (Kane 1983)

The equations are derived using

q  Tqq_bb_/n # _{q }    1 2  I33  S _bb_{/n #}

R



_bn



R

_bn

S





_bb_/n



4-parameter representation

Nonsingular but one more ODE is needed

Summary: 6-DOF kinematic equations (7 ODEs):

Component form:

q  ,1,2,3   1₂ 1p 2q 3r 1  1 2 p 3q 2r 2  1 2 3p q 1r 3  1 2 2p 1q r # # # #

2.2.2 Unit Quaternions

N  u1  2₂2  2₃2  2v12  3 2w13  2 Ė  2u12  3 v1  212  232 2w23  1 D  2u13  2 2v23  1  w1  212  222 # # #    J  p_bn_/n q  R_bnq 033 043 Tqq v_bb_/n _bb_/n #

20

Discrete-time algorithm for unit quaternion normalization

qq  ₁2  2₂  ₃2  2  1

2.2.2 Unit Quaternions

Algorithm ( Discrete- TimeNormalizationoftheUnit Quaternions ) 1. k  0.Computeinitialvaluesofqk  0.

2. Forsimplicity,EulerIntegrationimpliesthat

qk1  qkhTqqk_bb_/nk #

wherehisthesamplingtime.

3. Normalization

qk1  qk1

qqk1q 

qk1

qk1qk1 4. Letk  k1andreturntoStep2.

Acontinuoustimealgorithmforunitquaternionnormalizationcanbeimplementedbynotingthat

d dtq

_{q  2q}_T

qqbb/n  0 #

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.

In Simulinkthis is avoided by introducing feedback:

 q  Tqq_nbb  ₂ 1  qqq d dt q _{q  2q}_T qqnb b _{ 1  q}_qq_{q  1  q}_qq_q 0 if q is initialized as a unit vector   0 (typically 100 x  1  qq

Change of coordinates (x=0 gives )

x  x1  x linearization about x=0 gives x  x

qq  1

2.2.2 Unit Quaternions

d

22

2.2.3 Quaternions from Euler Angles

Ref. Shepperd (1978)

Algorithm (QuaternionsFromEulerAngles)

1. Given the Euler angles , , and . Let the transformation matrix R_bn according to be written : Rbn : 

R 11 R 12 R 13 R 21 R 22 R 23 R 31 R 32 R 33 2. The trace of R_be is computed as : R 44  trRbn  R 11  R 22  R 33

3. Let 1  i  4 be the index corresponding to : R ii  maxR 11,R 22,R 33,R 44 4. Compute p i corresponding to Index i of Step 3 according to :

p i  12R ii R 44 where the sign ascribed to p i can be chosen to be either positive or negative .

5. Compute the other three p i- values from :

p 4p 1  R 32 R 23 p 2p 3  R 32  R 23 p 4p 2  R 13 R 31 p 3p 1  R 13  R 31 p 4p 3  R 21 R 12 p 1p 2  R 21  R 12

by dividing the 3 equations containing the component p i with the known value of p i ( from Step 4 ) on both sides .

6. Compute the Euler parameters q  ,1,2,3 according to :

  p 4/2 and j  p j/2 j  1,2,3

2.2.4 Euler Angles from Quaternions

Require that the rotation matrices of the two kinematic representations are equal:

q  ,1,2,3 cc sc  css ss ccs sc cc sss cs  ssc s cs cc  R11 R12 R13 R21 R22 R23 R31 R32 R33

Algorithm: One solution is:

  atan2R32,R33   sin1_R 31  tan1 R31 1 R₃₁2 ;   90o   atan2R21,R11 # # #

where atan2(y,x) is the 4-quadrant arctangent of the real parts of the elements of x and y, satisfying:

  atan2y,x  

24           

2.3 Transformation between

ECEF and NED

ECEF {e}-frame NED {n}-frame Longitude: l (deg) Latitude:  (deg) Ellipsoidal height: h (m)

A point on or above the Earth’s surface is uniquely determined by:

h

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

2.3.1 Longitude and Latitude transformations

The transformation between the ECEF and NED velocity vectors is:

Two principal rotations:

1. a rotation l about the z-axis

2. a rotation ( ) about the   /2 y-axis.

en l,  S2 Rneen  Rz,lRy, 2  cosl sinl 0 sinl cosl 0 0 0 1 cos_  ₂  0 sin_  ₂  0 1 0 sin_ ₂  0 cos ₂  Rneen 

coslsin sinl coslcos

sinlsin cosl sinlcos

cos 0 sin

p_be_/e  Re_nenp_bn_/e  R_neenR_bnnbv_bb_/e #

26

2.3.1 Flat Earth Navigation

For flat Earth navigation it can be assumed that the NED tangent plane is fixed on the surface of the Earth-that is, l and  are constants, by assuming that the

operating radius of the vessel is limited:

en l,  S2

en  constant Rn

e

en  Ro  constant

Rneen 

coslsin sinl coslcos

sinlsin cosl sinlcos

cos 0 sin

p_be_/e  Renenp_bn_/e  RneenRbnnbvbb/e #

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 parameters longitude l, latitude  and height h.

Transformation:

2.3.2

Longitude/Latitude from ECEF

Coordinates

and height h



en





l

,







p

_be_/e





x

,

y

,

z



 p_be_/e  x,y,z p_be_/e  x,y,z

28

N



re2

re2cos2rp2sin2

2.3.2 Longitude/Latitude from ECEF

Coordinates

l



atan

y_xe_e



tan  z_pe 1 e2 N N h 1 h  _cosp_ N # #

while latitude  and height h are implicitly computed by:

p

_be_/e





x

,

y

,

z





Parameters Comments

re  6 378 137 m Equatorial radius of ellipsoid (semimajor axis)

rp  6 356 752 m Polar axis radius of ellipsoid (semiminor axis)

e  7. 292115 105 rad/s Angular velocity of the Earth

e  0. 0818 Eccentricity of ellipsoid

e



1





r_rp_e



2

2.3.2

Longitude/Latitude from ECEF

Coordinates

Algorithm (Transformationof x e,y e,z e tol ,,h )

1. Compute p  x e2  y e2.

2. Compute the approximate value  0 from :

tan 0  z _p e1e 21 3. Compute an approximate value N from :

N  r e2

r e2cos2 0  r p2sin2 0 4. Compute the ellipsoidal height by :

h  _cosp _

0 N  0 . 5. Compute an improved value for the latitude by :

tan  z _p e 1e 2 N  0 N  0  h

1

6. Check for another iteration step : if | 0 |< tol where tol is a small number , then the iteration is complete . Otherwise set  0   and continue with Step 3 .

30

2.3.3 ECEF Coordinates from

Longitude/Latitude

Ref. Heiskanen (1967)

The transformation from for given heights en l, h to is given by

x y z  N  hcoscosl N  hcossinl rp2 re2 N  h sin p_be_/e  _x,y,z

2.4 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.

xb  - xstab zb yb U xflow

32

2.4.1 Definitions of Course, Heading and

Sideslip Angles

The relationship between the angular variables course, heading, and sideslip is important for maneuvering of a vehicle in the horizontal plane (3 DOF) .

The terms course and heading are used interchangeably in much of the literature on guidance, navigation and control of marine vessels, and this leads to confusion.

Definition (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 by the right-hand screw convention

Definition: 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 by the right-hand screw convention.

Definition: Sideslip (drift) angle β: The angle from the BODY x-axis to the velocity vector of the vehicle,

positive rotation about the BODY z-axis frame by the right-hand screw convention

    

  arcsin _Uv  small   _Uv

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

β_SNAME = -β

The sideslip definition follows the sign convention used by the aircraft community, for instance as in Nelson (1998) and Stevens (1992). This definition is more intuitive from a guidance point-of-view than SNAME (1950).

Note that the heading angle equals the course angle ( = χ) when the sway velocity v = 0, that is when there is no sideslip.

Course angle:

Sideslip (drift) angle:

2.4.1 Definitions of Course, Heading and

Sideslip Angles

34 Ry,  cos 0 sin 0 1 0 sin 0 cos , Rz,  Rz,  cos sin 0 sin cos 0 0 0 1 u v w  Ry,Rz, U 0 0 u  Ucoscos v  Usin w  Usincos # # # or vstab  Ry,vb vflow  Rz,vstab # # xb _ - xstab zb yb U xflow

2.4.2 Sideslip and Angle of Attack

U  u2  _v2 _#

2.4.2 Sideslip and Angle of Attack

Extension to Ocean Currents

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

ur  u  uc vr  v  vc wr  w  wc # # # Ur  ur2  vr2  wr2 # ur  Urcosrcosr vr  Ursinr wr  Ursinrcosr # # # r  tan1 w_urr r  sin1 vr Ur # # Hence,

36

Lift and drag forces can then be computed as a function of forward speed and transformed back to BODY coordinates.

u  Ucoscos v  Usin w  Usincos # # # Linear approximation: u  U, v  U, w  U

BODY to FLOW axes:

 u,v,w,p,q,r flow  _U,_,_,p,q,r flow  _T_U _T_U _diag_{1, 1/U, 1/U, 1, 1, 1} xb  - xstab zb yb U xflow

2.4.2 Sideslip and Angle of Attack

TUMTU1flow _{ TUNTU}1flow  _T_U _#