Ch2

1

Chapter 2 - Kinematics

2.1 Reference frames 2.2 Transforma0ons between BODY and NED 2.3 Transforma0ons between ECEF and NED 2.4 Transforma0ons between BODY and FLOW

The study of dynamics can be divided into two parts: kinema0cs, which treats only geometrical aspects of mo0on, and kine0cs, which is the analysis of the forces causing the mo0on

BODY

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

2

Overall Goal of Chapters 2 to 8

The nota0on and representa0on are adopted from: Fossen, T. I. (1991). Nonlinear Modeling and Control of Underwater Vehicles, PhD thesis, Department of Engineering Cyberne6cs, 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!!" " g0 ! $ " $wind " $wave

# #


3

2.1 Reference Frames

ECI {i}: Earth centered iner0al frame; non-accelera0ng frame (xed in space) in which Newtons laws of mo0on apply.

ECEF {e}: Earth-Centered Earth-Fixed frame; origin is xed in the center of the Earth but the axes rotate rela0ve to the iner0al frame ECI.

NED {n}: North-East-Down frame; dened rela0ve to the Earths reference ellipsoid (WGS 84). BODY {b}: Body frame; moving coordinate frame xed to the vessel.

xb- longitudinal axis (directed from a_ to fore) yb- transversal axis (directed to starboard) zb-normal axis (directed from top to bo`om)

N

ED

e

x

yl

z

e

e

BODY

ECEF/ECI

NED

e t

e

x y

y

x

z

i i

e

e

i z e,

ECEF/ECI


4

2.1 Reference Frames Body-Fixed Reference Points

CG - Center of gravity CB - Center of buoyancy CF - Center of ota0on CF is located a distance LCF from CO in the x-direc0on

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

u! " u1nn!1 # u2nn!2 # u3nn!3

un ! !u1n,u2n,u3n"!

Coordinate-free vector

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

Coordinate form of u! in !n"


5

forces and linear and positions and

DOF moments angular velocities Euler angles

1 motions in the x-direction (surge) X u x2 motions in the y-direction (sway) Y v y3 motions in the z-direction (heave) Z w z4 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

xb

yb

zb

u ( )surge

r ( )yaw

v ( )sway

( )heavew

( )rollp

( )pitchqThe nota0on is adopted from: SNAME (1950). Nomenclature for Trea0ng the Mo0on of a Submerged Body Through a Fluid. The Society of Naval Architects and Marine Engineers, Technical and Research Bulle6n No. 1-5, April 1950, pp. 1-15.


6

2.1 Reference Frames - Notation

Generalized posi0on, velocity and force

ECEFposition:

pb/ee !xyz

! !3Longitude andlatitude

!en !l!

! S2

NEDposition:

pb/nn !NED

! !3Attitude(Euler angles)

!nb !"

#

$

! S3

Body-fixedlinearvelocity

vb/nb !uvw

! !3Body-fixedangularvelocity

"b/nb !

pqr

! !3

Body-fixedforce:

fbb !

XYZ

! !3Body-fixedmoment

mbb !KMN

! !3

! !pb/nn !or pb/ne )

"nb, # !

vb/nb

$b/nb

, % !fbb

mbb #


7

2.2 Transformations between BODY and NED

Special orthogonal group of order 3:

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

Orthogonal matrices of order 3:

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

RR! ! R!R ! I, detR ! 1

Rota0on matrix:

Since R is orthogonal, R!1 ! R!! to ! R from

to ! from

Example:


8

! ! a :! S!!"a

Cross-product operator as matrix-vector mul0plica0on:

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


9

2.2 Transformations between BODY and NED Eulers theorem on rota0on:

R11 ! !1 ! cos!" "12 " cos!R22 ! !1 ! cos!" "22 " cos!R33 ! !1 ! cos!" "32 " 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!

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

where

! ! !!1,!2,!3"!, |!| ! 1

!

vb/nn ! Rbnvb/nb , Rbn :! R!," #


10

2.2.1 Euler Angle Transformation Three principal rota0ons:

(2) Rotation over pitch angle about . Note that .

yv =v

2

2 1

x2x3

y3y2

u3u2

v2

v3

(1) Rotation over yaw angle about . Note that .

zw =w

3

3 2

x1

x2

z1 z2

u1

u2

w1

w2

U

U

(3) Rotation over roll angle about . Note that .

xu =u

1

1 2

z =z0 b z1

y1y =y0 bv=v2

v1

w=w0

w1

U

! ! !1, 0, 0"! ! ! "

! ! !0, 1, 0"! ! ! "

! ! !0, 0, 1"! ! ! "

Rx,! !1 0 00 c! !s!0 s! c!

Ry,! !c! 0 s!0 1 0!s! 0 c!

Rz,! !c! !s! 0s! c! 00 0 1


11

2.2.1 Euler Angle Transformation Linear velocity transforma0on (zyx-conven0on):

Small angle approxima0on:

where

Rbn!!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#

Rbn!!!nb" ! I3"3 ! S!!!nb" "1 "!# !$!# 1 "!%"!$ !% 1

Rbn!!nb"!1 ! Rnb!!nb" ! Rx,!! Ry,"! Rz,#!Rbn!!nb" :! Rz,!Ry,"Rx,#

p! b/nn ! Rbn!"nb"vb/nb #


12

NED posi0ons (con0nuous 0me and discrete 0me):

2.2.1 Euler Angle Transformation

Component form:

Euler integra0on


N! " u cos!!"cos!"" # v!cos!!"sin!"" sin!#" ! sin!!"cos!#""# w!sin!!" sin!#" # cos!!"cos!#" sin!"""

" u sin!!"cos!"" # v!cos!!"cos!#" # sin!#"sin!"" sin!!""# w!sin!"" sin!!"cos!#" ! cos!!" sin!#""

D! " !u sin!"" # vcos!"" sin!#" # wcos!""cos!#"

#

# #

pb/nn !k ! 1" " pb/nn !k" ! hRbn!!nb!k""vb/nb !k" #


13

Angular velocity transforma0on (zyx-conven0on):


where

1. Singular point at

! ! " 90oSmall angle approxima0on: No0ce 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"#b/nb # !b/nb !!"

00

# Rx,!!0""

0# Rx,!! Ry,"!

00#"

:! T$!1!"nb""# nb #


14

ODE for Euler angles: ODE for rota0on matrix


Component form:

!! " p # qsin" tan ! # rcos" tan !"! " qcos" ! rsin"

#! " q sin"cos! # rcos"cos! , ! " $90

o

# #

# + algorithm for computa0on of Euler angles from the rota0on matrix

where

Euler angle adtude representa0ons:

Rbn!!nb"!nb! !!,",#"!

!" nb ! T"!!nb"#b/nb # R! b

n ! RbnS!"b/nb " #

S!!b/nb " !

0 !r qr 0 !p!q p 0

#


15

Summary: 6-DOF kinema0c equa0ons:


Component form:

3-parameter representa0on with singularity at ! ! " 90o

N! " u cos!cos" # v!cos!sin"sin# ! sin!cos#"# w!sin!sin# # cos!cos#sin""

" u sin!cos" # v!cos!cos# # sin#sin"sin!"# w!sin"sin!cos# ! cos!sin#"

D! " !u sin" # vcos"sin# # wcos"cos#

#

# #

!! " p # q sin! tan" # rcos! tan""! " q cos! ! rsin!

#! " q sin!cos" # rcos!cos" , " " $90

o

# #

#

!nb! !!,",#"!!! " J!!""

#

p# b/nn

$# nb"

Rbn!$nb" 03!303!3 T$!$nb"

vb/nb

%b/nb

#


16

2.2.2 Unit Quaternions 4-parameter representa0on:

-avoids the representa0on singularity of the Euler angles -numerical eec0ve (no trigonometric func0ons)

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

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

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

! ! cos "2

! ! !!1,!2,!3"! ! " sin "2

q !

!

"1"2"3

!cos #2! sin #2

! Q

Rbn!q! : ! R!,! ! I3"3 " 2!S!!! " 2S2!!"


17

2.2.2 Unit Quaternions Linear velocity transforma0on

where

Rbn!q! !1 ! 2!!22 " !32" 2!!1!2 ! !3"" 2!!1!3 " !2""2!!1!2 " !3"" 1 ! 2!!12 " !32" 2!!2!3 ! !1""2!!1!3 ! !2"" 2!!2!3 " !1"" 1 ! 2!!12 " !22"

Component form (NED posi0ons):

Rbn!q"!1 ! Rbn!q"!

q!q ! 1NB! must be integrated under the constraint or !2 ! "12 ! "22 ! "32 " 1

N! " u!1 ! 2!22 ! 2!32" # 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"

# # #

p! b/nn ! Rbn!q"vb/n

b #


18

2.2.2 Unit Quaternions Angular velocity transforma0on

Tq!q" ! 12

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

, Tq!!q"Tq!q" ! 14 I3#3

where

!! " ! 12 !"1p # "2q # "3r"

"! 1 " 12 !!p ! "3q # "2r"

"! 2 " 12 !"3p # !q ! "1r"

"! 3 " 12 !!"2p # "1q # !r"

#

#

#

#

Component form:

NB! nonsingular to the price of one more parameter

Alterna0ve representa0on (Kane 1983)

The equa0ons are derived using

q! ! Tq!q""b/nb # q! !!"

"!! 12

!"!

!I3"3 # S!""#b/nb #

R! bn ! RbnS!"b/nb "


19

4-parameter representa0on Nonsingular but one more ODE is needed

Summary: 6-DOF kinema0c equa0ons (7 ODEs):

Component form:

q ! !!,"1, "2, "3!!

!! " ! 12 !"1p # "2q # "3r"

"! 1 " 12 !!p ! "3q # "2r"

"! 2 " 12 !"3p # !q ! "1r"

"! 3 " 12 !!"2p # "1q # !r"

#

#

#

#

2.2.2 Unit Quaternions

N! " u!1 ! 2!22 ! 2!32" # 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# b/nn

q#"

Rbn!q" 03!304!3 Tq!q"

vb/nb

$b/nb

#


20

Discrete-0me algorithm for unit quaternion normaliza0on

q!q ! !12 " !22 " !32 " "2 ! 1



21

Discrete-0me algorithm for unit quaternion normaliza0on



22

Con0nuous-0me algorithm for unit quaternion normaliza0on:

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

However, integra0on of the quaternion vector q from the dieren0al equa0on will introduce numerical errors that will cause the length of q to deviate from unity. In Simulink this is avoided by introducing feedback:

!q " Tq!q"!nbb #!2 !1 ! q

!q"q

ddt !q

!q" ! 2q!Tq!q"!nbb " !!1 ! q!q"q!q ! !!1 ! q!q"q!q0 if q is ini0alized as a unit vector

! ! 0 (typically 100!

x ! 1 ! q!qChange of coordinates (x=0 gives )

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

q!q ! 1


ddt !q

!q" ! 2q!Tq!q"!b/nb ! 0 #


23

2.2.3 Quaternions from Euler Angles Ref. Shepperd (1978)


24

2.2.4 Euler Angles from Quaternions Require that the rota0on matrices of the two kinema0c representa0ons 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 R13R21 R22 R23R31 R32 R33

Algorithm: One solu0on is:

! ! atan2!R32,R33"

" ! !sin!1!R31" ! ! tan!1 R311 ! R312

; " " "90o

# ! atan2!R21,R11"

#

#

#

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

!! " atan2!y,x" " !

Rbn!!nb" :! Rbn!q" !nb! !!,",#"!


25

N

ED

e

x

yl

z

e

e

2.3 Transformation between ECEF and NED

ECEF {e}-frame

NED {n}-frame

Longitude: l (deg) La0tude: m (deg) Ellipsoidal height: h (m)

A point on or above the Earths surface is uniquely determined by:

h

NED axes deni0ons: N - North axis is poin0ng North E - East axis is poin0ng East D Down axis is poin0ng down in the normal direc0on to the Earths surface


26

2.3.1 Longitude and Latitude transformations The transforma0on between the ECEF and NED velocity vectors is:

Two principal rota0ons: 1. a rota0on l about the z-axis 2. a rota0on ( ) about the y-axis. !! ! "/2

!en! !l,!"! ! S2

Rne!!en" ! Rz,lRy,!!! "2 !cos l ! sin l 0sin l cos l 00 0 1

cos !!! ! "2 " 0 sin!!! !"2 "

0 1 0

! sin!!! ! "2 " 0 cos !!! !"2 "

Rne!!en" !! cos l sin! ! sin l ! cos lcos!! sin l sin! cos l ! sin lcos!cos! 0 ! sin!

p! b/ee ! Rne!"en"p! b/en ! Rne!"en"Rbn!"nb"vb/eb #



27

2.3.1 Flat Earth Navigation For at Earth naviga0on it can be assumed that the NED tangent plane is xed on the surface of the Earth-that is, l and m are constants, by assuming that the opera0ng radius of the vessel is limited:

!en! !l,!"! ! S2

!en ! constant Rne!!en" ! Ro ! constant

Rne!!en" !! cos l sin! ! sin l ! cos lcos!! sin l sin! cos l ! sin lcos!cos! 0 ! sin!

p! b/ee ! Rne!"en"p! b/en ! Rne!"en"Rbn!"nb"vb/eb #


28

Satellite naviga0on system measurements are given in the ECEF frame: Not to useful for the operator. Presenta0on of terrestrial posi0on data is therefore made in terms of the ellipsoidal parameters longitude l, la0tude m and height h. Transforma0on:

2.3.2 Longitude/Latitude from ECEF Coordinates

and height h

!en! !l,!"!pb/ee ! !x,y,z"!

pb/ee ! !x,y,z"!

pb/ee ! !x,y,z"!


29

N ! re2

re2 cos2!"rp2 sin2!


l ! atan! yexe "

tan! ! zep 1 ! e2 NN " h

!1

h ! pcos! ! N

#

#

while la0tude m and height h are implicitly computed by:

pb/ee ! !x,y,z"!Parameters Commentsre ! 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 Earthe ! 0.0818 Eccentricity of ellipsoid

e ! 1 ! ! rpre "2


30



Ref. Hofman-Wllenhof et al. (2004)

31

2.3.3 ECEF Coordinates from Longitude/Latitude

Ref. Heiskanen (1967)

The transforma0on from for given heights h to is given by !en! !l,!"!

xyz

!

!N " h"cos!cos l!N " h"cos!sin lrp2

re2N " h sin!

pb/ee ! !x,y,z"!


32

2.4 Transformation between BODY and FLOW FLOW axes are o_en used to express hydrodynamic data. The FLOW axes are found by rota0ng the BODY axis system such that resul0ng x-axis is parallel to the freestream ow. In FLOW axes, the x-axis directly points into the rela0ve ow 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 -

xstabzb

yb

Uxflow


33

2.4.1 Definitions of Course, Heading and Sideslip Angles The rela0onship 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, naviga0on and control of marine vessels, and this leads to confusion.


DeniKon (Course angle ): The angle from the x-axis of the NED frame to the velocity vector of the vehicle, posi0ve rota0on about the z-axis of the NED frame by the right-hand screw conven0on

DeniKon: Heading (yaw) angle : The angle from the NED x-axis to the BODY x-axis, posi0ve rota0on about the z-axis of the NED frame by the right-hand screw conven0on.

DeniKon: Sideslip (driT) angle : The angle from the BODY x-axis to the velocity vector of the vehicle, posi0ve rota0on about the BODY z-axis frame by the right-hand screw conven0on

34

! ! " " #

! ! arcsin vU! small" ! ! vU

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

SNAME = -

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

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

Course angle:

Sideslip (driT) angle:

2.4.1 Definitions of Course, Heading and Sideslip Angles

U ! u2 " v2 #


35

Ry,! !cos! 0 sin!0 1 0

!sin! 0 cos!, Rz,!" ! Rz,"! !

cos" sin" 0!sin" cos" 00 0 1

uvw

! Ry,!! Rz,!"!U00

u ! Ucos!cos"v ! Usin"w ! Usin!cos"

# # #

or

vstab ! Ry,!vb

v flow ! Rz,!"vstab # #

xb -

xstabzb

yb

Uxflow

2.4.2 Sideslip and Angle of Attack

U ! u2 " v2 #


36

2.4.2 Sideslip and Angle of Attack Extension to Ocean Currents For a marine cra_ exposed to ocean currents, the concept of rela0ve veloci0es is introduced. The rela0ve veloci0es are:

ur ! u ! ucvr ! v ! vcwr ! w ! wc

# # #

Ur ! ur2 " vr2 " wr2 #

ur ! Ur cos!!r"cos!"r"vr ! Ur sin!"r"wr ! Ur sin!!r"cos!"r"

# # #

!r ! tan!1 wrur"r ! sin!1 vrUr

#

#

Hence,


New expressions for sideslip and angle of a`ack, which includes the eect of ocean currents

37

Some interesKng observaKons regarding sideslip: 1) A vehicle moving on a straight line in calm water

(U > 0 and v = 0) will have a zero sideslip angle

2) As soon as you start to turn, the sway velocity will be non-zero and consequently the vehicle will sideslip, . This is referred to as sideslip due to turning.

3) A vehicle is also exposed to environmental forces, which induces a ow velocity (wind/current). This again gives a second contribu0on to sideslip. Moreover,


= 0

=vrUr

6= 0

! ! arcsin vU! small" ! ! vU


38

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

u ! Ucos!cos"v ! Usin"w ! Usin!cos"

# # #

Linear approxima0on:

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 -

xstabzb

yb

Uxflow


T!U"MT!U"!1!" flow ! T!U"NT!U"!1!flow " T!U"# #


Ch2

Documents

body frame

control of vehicles

control of underwater

control of ocean vehicles

coordinate frame

submerged body

reference frames eci

accelera0ng frame