Chapter 2 - · PDF fileChapter 2 - Kinematics ... 2.2.1 Euler Angle Transformation ... -avoids the representation singularity of the Euler angles-numerical effective

1

Chapter 2 - Kinematics

2.1Referenceframes2.2TransformationsbetweenBODYandNED2.3TransformationsbetweenECEFandNED2.4TransformationsbetweenBODYandFLOW

“Thestudyofdynamics canbedividedintotwoparts: kinematics,whichtreatsonlygeometricalaspectsofmotion,andkinetics,whichistheanalysisoftheforcescausingthemotion”

BODY

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

2

Overall Goal of Chapters 2 to 8

Thenotationandrepresentationareadoptedfrom:

Fossen,T.I.(1991). NonlinearModelingandControlofUnderwaterVehicles,PhDthesis,DepartmentofEngineeringCybernetics,NTNU,June1991.

Fossen,T.I.(1994).GuidanceandControlofOceanVehicles,JohnWileyandSonsLtd.ISBN:0-471-94113-1.

Representthe6-DOFdynamicsinacompactmatrix-vectorformaccordingto:

!" ! J!!"#M#" " C!#"# " D!#"# " g!!" " g0 ! $ " $wind " $wave

# #


3

2.1 Reference Frames

ECI{i}: Earthcenteredinertialframe;non-acceleratingframe(fixedinspace)inwhichNewton’slawsofmotionapply.

ECEF{e}: Earth-CenteredEarth-Fixedframe;originisfixedinthecenteroftheEarthbuttheaxesrotaterelativetotheinertialframeECI.

NED{n}: North-East-Downframe;definedrelativetotheEarth’sreferenceellipsoid(WGS84).BODY{b}: Bodyframe;movingcoordinateframefixedtothevessel.

xb- longitudinalaxis(directedfromafttofore)yb- transversalaxis(directedtostarboard)zb-normalaxis(directedfromtoptobottom)

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 - Centerofgravity• CB- Centerofbuoyancy• CF - Centerofflotation

CFislocatedadistanceLCF fromCOinthex-direction

ThecenterofflotationisthecentroidofthewaterplaneareaAwp incalmwater.Thevesselwillrollandpitchaboutthispoint.

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

un ! !u1n,u2n,u3n"!

Coordinate-freevector

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

Coordinateformof 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

( )pitchqThenotationisadoptedfrom:

SNAME(1950). NomenclatureforTreatingtheMotionofaSubmergedBodyThroughaFluid.TheSocietyofNavalArchitectsandMarineEngineers,TechnicalandResearchBulletinNo.1-5,April1950,pp.1-15.


6

2.1 Reference Frames - Notation

Generalizedposition,velocityandforce

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 betweenBODY and NED

Specialorthogonal groupoforder3:

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

Orthogonal matricesoforder3:

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

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

Rotationmatrix:

SinceR isorthogonal, R!1 ! R!

! to ! R fromto ! from

Example:


8

! ! a :! S!!"a

Cross-productoperatorasmatrix-vectormultiplication:

S!!" ! !S!!!" !

0 !!3 !2!3 0 !!1!!2 !1 0

, ! "

!1!2!3

whereisaskew-symmetricmatrixS ! !S!

2.2 Transformations betweenBODY and NED


9

2.2 Transformations betweenBODY and NEDEulerstheoremonrotation:

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 TransformationThreeprincipalrotations:

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

yv =v

2

2 1

x2x3

y3

y2

u3

u2

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

y1

y =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

Linearvelocitytransformation(zyx-convention):

Smallangleapproximation:

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

NEDpositions(continuoustimeanddiscretetime):


Component form:

Eulerintegration


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/n

n !k" ! hRbn!!nb!k""vb/n

b !k" #


13

Angularvelocitytransformation(zyx-convention):


where

1. Singularpointat ! ! " 90o

Smallangleapproximation: Noticethat:

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

ODEforEulerangles:ODEforrotationmatrix


Componentform:

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

#! " q sin"cos! # r cos"

cos! , ! " $90o

# #

# + algorithmforcomputationofEuleranglesfromtherotationmatrix

where

Eulerangleattituderepresentations:

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

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

n ! RbnS!"b/n

b " #

S!!b/nb " !

0 !r qr 0 !p!q p 0

#


15

Summary:6-DOFkinematicequations:


Componentform:

3-parameterrepresentation

withsingularityat ! ! " 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" # r cos!

cos" , " " $90o

# #

#

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

#

p# b/nn

$# nb"

Rbn!$nb" 03!3

03!3 T$!$nb"

vb/nb

%b/nb

#


16

2.2.2 Unit Quaternions4-parameterrepresentation:

-avoidstherepresentationsingularityoftheEulerangles-numericaleffective(notrigonometricfunctions)

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

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

Unitquaternion(Eulerparameter)rotationmatrix(Chou1992):

! ! 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 QuaternionsLinearvelocitytransformation

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"

Componentform(NEDpositions):

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

q!q ! 1NB! mustbeintegratedundertheconstraintor!2 ! "1

2 ! "22 ! "3

2 " 1

N! " u!1 ! 2!22 ! 2!3

2" # 2v!!1!2 ! !3"" # 2w!!1!3 # !2""

Ė " 2u!!1!2 # !3"" # v!1 ! 2!12 ! 2!3

2" # 2w!!2!3 ! !1""

D! " 2u!!1!3 ! !2"" # 2v!!2!3 # !1"" # w!1 ! 2!12 ! 2!2

2"

# # #

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


18

2.2.2 Unit QuaternionsAngularvelocitytransformation

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"

#

#

#

#

Componentform:

NB! nonsingulartothepriceofonemoreparameter

Alternativerepresentation(Kane1983)

Theequationsarederivedusing

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

"!! 1

2!"!

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

R! bn ! RbnS!"b/nb "


19

4-parameterrepresentation

NonsingularbutonemoreODEisneeded

Summary:6-DOFkinematicequations(7ODEs):

Componentform:

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!3

2" # 2v!!1!2 ! !3"" # 2w!!1!3 # !2""

Ė " 2u!!1!2 # !3"" # v!1 ! 2!12 ! 2!3

2" # 2w!!2!3 ! !1""

D! " 2u!!1!3 ! !2"" # 2v!!2!3 # !1"" # w!1 ! 2!12 ! 2!2

2"

# # #

!! " J!!""

#

p# b/nn

q#"

Rbn!q" 03!3

04!3 Tq!q"vb/nb

$b/nb

#


20

Discrete-timealgorithmforunitquaternionnormalization

q!q ! !12 " !2

2 " !32 " "2 ! 1



21

Continuous-timealgorithmforunitquaternionnormalization:

Ifq isinitializedasaunitvector,thenitwillremainaunitvector.

However,integrationofthequaternionvectorq fromthedifferentialequationwillintroducenumericalerrorsthatwillcausethelengthofq todeviatefromunity.

InSimulink thisisavoidedbyintroducingfeedback:

!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!q0ifqisinitializedasaunitvector

! ! 0 (typically 100!

x ! 1 ! q!qChangeofcoordinates(x=0gives)

x! " !!x!1 ! x" x! " !!xlinearizationaboutx=0gives

q!q ! 1


ddt !q

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


22

2.2.3 Quaternions from Euler AnglesRef.Shepperd (1978)


23

2.2.3 Quaternions from Euler Angles


24

2.2.4 Euler Angles from QuaternionsRequirethattherotationmatricesofthetwokinematicrepresentationsareequal:

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: Onesolutionis:

! ! atan2!R32,R33"

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

; " " "90o

# ! atan2!R21,R11"

#

#

#

whereatan2(y,x) isthe4-quadrantarctangentoftherealpartsoftheelementsofx andy,satisfying:

!! " atan2!y,x" " !

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


25

N

ED

e

x

yl

z

e

e

2.3 Transformation betweenECEF and NED

ECEF{e}-frame

NED{n}-frame

Longitude:l (deg)Latitude:µ (deg)Ellipsoidalheight:h (m)

Apointon orabove theEarth’ssurfaceisuniquelydeterminedby:

h

NEDaxesdefinitions:N - NorthaxisispointingNorthE - EastaxisispointingEastD – DownaxisispointingdowninthenormaldirectiontotheEarth’ssurface


26

2.3.1 Longitude and Latitude TransformationsThetransformationbetweentheECEFandNEDvelocityvectorsis:

Twoprincipalrotations:1. arotationlaboutthez-axis2. arotation()aboutthey-axis.!! ! "/2

!en! !l,!"! ! S2

Rne!!en" ! Rz,lRy,!!! "2!

cos l ! sin l 0

sin l cos l 0

0 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

SatellitenavigationsystemmeasurementsaregivenintheECEFframe:Nottousefulfortheoperator.

Presentationofterrestrialpositiondata isthereforemadeintermsoftheellipsoidalparameterslongitudel,latitudeµ andheighth.

Transformation:

2.3.2 Longitude/Latitude from ECEF Coordinates

andheighth

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

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

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


28

N ! re2

re2 cos2!"rp2 sin2!


l ! atan! yexe "

tan! ! zep 1 ! e2 N

N " h!1

h ! pcos! ! N

#

#

whilelatitudeµ andheighth areimplicitlycomputedby:

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


29



Ref. Hofman-Wllenhof et al. (2004)

30

2.3.3 ECEF Coordinates from Longitude/Latitude

Ref.Heiskanen (1967)

Thetransformationfromforgivenheightsh toisgivenby!en! !l,!"!

xyz

!

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

re2N " h sin!

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


31

2.4 Transformation between BODY and FLOW FLOWaxesareoftenusedtoexpresshydrodynamicdata.TheFLOWaxesarefoundbyrotatingtheBODYaxissystemsuchthatresultingx-axisisparalleltothefreestreamflow.

InFLOWaxes,thex-axisdirectlypointsintotherelativeflowwhilethez-axisremainsinthereferenceplane,butrotatessothatitremainsperpendiculartothex-axis.They-axiscompletestheright-handedsystem.

xb-

xstabzb

yb

Uxflow


32

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 Transformation between BODY and FLOW

U ! u2 " v2 #

Principalrotations:

Velocitytransformation:


33

2.4 Transformation between BODY and FLOW ExtensiontoOceanCurrents

Foramarinecraftexposedtooceancurrents,theconceptofrelativevelocitiesisintroduced.Therelativevelocitiesare:

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,


Sideslipangle(SSA)andangleofattack(AOA)

34

2.4.1 Definitions of Course, Heading and Sideslip Angles

Sideslip angle:

Crab angle:

Course angle:

Heading (yaw) angle

Speed over ground:

Relative speed:

Current speed:

Current direction:

GNSS measures course angle 𝜒and speed over ground UCompass measures heading angle 𝜓Currents can be measured by an Acoustic Doppler Current Profiler (ADCP)

North

East

Ocean Current Triangle: Horizontal Plane

35

2.4.1 Definitions of Course, Heading and Sideslip AnglesTherelationshipbetweentheangularvariablescourse,heading,andsideslipisimportantformaneuveringofavehicleinthehorizontalplane(3DOF).

Thetermscourseandheadingareusedinterchangeablyinmuchoftheliteratureonguidance,navigationandcontrolofmarinecraft,andthisleadstoconfusion.


Definition(Courseangleχ): Theanglefrom thex-axisoftheNEDframeto thevelocityvectorofthevehicle,positiverotationaboutthez-axisoftheNEDframebytheright-handscrewconvention.MeasuredusingGNSS(orHPRunderwater).

Definition:Heading(yaw)angleψ:Theanglefrom theNEDx-axisto theBODYx-axis,positiverotationaboutthez-axisoftheNEDframebytheright-handscrewconvention.Measuredusingacompass.

Differencebetweencourseandheadingangles:

36



Definition(Crabangleβ): Theanglefrom theBODYx-axisto thevelocityvectorofthevehicle,positiverotationabouttheBODYz-axisframebytheright-handscrewconvention:

Thecrabangleisafunctionoftheswayvelocityandspeedoverground:

Definition(Sideslipangleβr): Thesideslipangleisdefinedintermsofrelativevelocities:

North

East

Remark: InSNAME(1950)andLewis(1989)thesideslipangleformarinecraftisdefined as:

βSNAME=-βr

Weusethesignconventionbytheaircraftcommunitye.g.Nelson(1998)andStevens(1992).Thisdefinitionismoreintuitivefromaguidancepoint-of-viewthanSNAME(1950).

37

Someinterestingobservationsregardingsideslip:

1) Avehiclemovingonastraightlineincalmwater(U > 0 andv =0)willhaveazerocrabangle

2) Assoonasyoustarttoturn,theswayvelocitywillbenon-zeroandconsequently,.Thecrabanglecorrespondstotheamountofcorrectionavehiclemustbeturnedinordertomaintainthedesiredcourse.

3) Avehicleisalsoexposedtoenvironmentalforces,whichinducesaflowvelocity(wind/current).Thisforcesthevehicleto“sideslip”.Moreover,


� = 0

� 6= 0

! ! arcsin vU

! small" ! ! v

U


!r ! tan!1 wrur

"r ! sin!1 vrUr

#

#

38


Relative flight path angle:

Flight path angle:

Angle of attack:

Pitch angle:

Speed over ground:

Relative speed:

Current speed:

GNSS measures flight path γand speed over ground UAHRS or INS measure pitch angle 𝜃Currents can be measured by an Acoustic Doppler Current Profiler (ADCP)

North

Down


Ocean Current Triangle: Vertical Plane

Documents

Chapter 2 - · PDF fileChapter 2 - Kinematics ... 2.2.1 Euler Angle Transformation ... -avoids the representation singularity of the Euler angles-numerical effective