Modelling and Simulation of Marine Craft KINEMATICS

Modelling and Simulation of Marine Craft

KINEMATICSEdin OmerdicSenior Research FellowMobile & Marine Robotics Research CentreUniversity of Limerick

Mobile & Marine Robotics Research Centre

University of Limerick

Outline

KinematicsReference Frames

ECEF Frame

NED Frame

BODY Frame

UNITY Frame

UTM

Transformations BODY – NEDEuler Angles

Quaternions

Quaternions from Euler Angles

Euler Angles from Quaternions

Transformations ECEF – NEDLat-Lon from ECEF Coordinates

ECEF Coordinates from Lat-Lon



GNC Signal Flow

GNC Signal Flow

Weather routing

program

Trajectory

Generator

Motion Control

System

Control

Allocation

Fusion Marine Craft INS (GNSS + AHRS)

OA Sensors

(Radar, Cameras, etc.)

Observer

Weather

data

Waypoints

Guidance

System

Control

System

Obstacle

Avoidance

Module

Estimated

positions and

velocities Navigation

System

Waves, wind

and ocean

currents




ECEF Frame

Symbol 𝐞 = (𝒙𝐞, 𝒚𝐞, 𝒛𝐞)

Definition The Earth-centered Earth-fixed frame which rotate around North-South axis with angular speed 𝑤𝑒 = 7.2921 ∗ 10−5 𝑟𝑎𝑑/𝑠

Type Right-handed

Axes 𝑥e (from origin through intersection of Prime Meridian and Equator)

𝑦e𝑧e (from origin through North pole)

Origin Location of ECEF origin 𝑜e is fixed to the Centre of the Earth.

Assumption For marine craft moving at relatively low speed the Earth rotation can be neglected and e can be considered to be inertial, such

that Newton's laws apply.

Application Typically used for global guidance, navigation and control (for example, to describe motion and location of ships in transit between

continents).




NED Frame

Symbol 𝒏 = (𝒙𝒏, 𝒚𝒏, 𝒛𝒏)

Definition Tangent plane on the surface of Earth reference ellipsoid WGS 1984.

Type Right-handed

Axes 𝑥𝑛= North

𝑦𝑛= East

𝑧𝑛= Downwards, normal to the surface.

Origin Location of NED origin 𝑜𝑛 relative to ECEF frame 𝒆 = (𝒙𝒆, 𝒚𝒆, 𝒛𝒆) is determined using Lat and Lon.

Assumption 𝑛 is inertial, such that Newton's laws apply.

Application The position and orientation of the craft are described relative to 𝑛 .

𝑥𝑒

𝑦𝑒

𝑧𝑒

Equator

𝑥𝑛𝑦𝑛

𝑧𝑛

𝜆

𝜑

𝜆 − 𝐿𝑜𝑛𝑔𝑖𝑡𝑢𝑑𝑒𝜑 − 𝐿𝑎𝑡𝑖𝑡𝑢𝑑𝑒




BODY Frame

Symbol 𝒃 = (𝒙𝒃, 𝒚𝒃, 𝒛𝒃)

Definition Moving frame fixed to the craft.

Type Right-handed

Axes 𝑥𝑏= longitudinal axis (directed toward front);

𝑦𝑛= transversal axis (directed toward starboard);

𝑧𝑛= normal axis (directed toward bottom)

Origin Location of BODY origin 𝑜𝑏 is usually chosen to coincide with a point midships in

the water line for marine crafts.

Application The linear and angular velocities of the vessel are described relative to 𝑏 .




UNITY Frame

Symbol 𝒖 = (𝒙𝒖, 𝒚𝒖, 𝒛𝒖)

Definition Frame used for 3D visualisation in Unity 3D.

Type Left-handed

Axes 𝑥𝑢= East

𝑦𝑢= Up

𝑧𝑢= North.

Origin Location of UNITY origin 𝑜𝑢 relative to ECEF frame 𝒆 = (𝒙𝒆, 𝒚𝒆, 𝒛𝒆) is determined using Lat and Lon.

Application The position and orientation of all vessels are sent to Unity 3D for visualisation.




UTM

Symbol 𝑼𝑻𝑴 = (𝒙𝑼𝑻𝑴, 𝒚𝑼𝑻𝑴, 𝒛𝑼𝑻𝑴)

Definition The UTM system divides the Earth into 60 zones and uses a secant transverse Mercator projection in

each zone (conformal projection) to give location on the surface of the Earth. Each zone is segmented

into 20 latitude bands.

Type Right-handed

Axes 𝑥𝑈𝑇𝑀= Northing

𝑦𝑈𝑇𝑀= Easting

𝑧𝑈𝑇𝑀= Downwards, normal to the surface.

Origin Location of UTM origin 𝑜𝑈𝑇𝑀 is shifted such that Northing and Easting coordinates are always

positive. For this purpose artificial offset False Easting is added in Northern hemisphere, while both

False Easting and False Northing are used in Southern hemisphere. In vertical plane the UTM origin is

at sea level.




UTM (cont.)



KinematicsTransformations between Frames

Notation

𝒑𝑏/𝑈𝑇𝑀𝑈𝑇𝑀 =

𝑥𝑈𝑇𝑀𝑦𝑈𝑇𝑀𝑧𝑈𝑇𝑀

Position of vessel 𝑜𝑏 in UTM

𝒑𝑏/𝑛𝑛 =

𝑥𝑛𝑦𝑛𝑧𝑛

Position of vessel 𝑜𝑏 in NED

𝒗𝑏/𝑛𝑏 =

𝑢𝑣𝑤

Linear velocity of 𝑜𝑏 with

respect to {n} expressed in {b}𝒗𝑏/𝑛𝑛 Linear velocity of 𝑜𝑏 with

respect to {n} expressed in {n}

𝜽𝑛𝑏 =𝑅𝑃𝑌

Attitude (Euler angles) between

{n} and {b} 𝒘𝑏/𝑛𝑏 =

𝑝𝑞𝑟

Angular velocity of 𝑏 with

respect to {n} expressed in {b}

𝒑𝑛/𝑈𝑇𝑀𝑈𝑇𝑀 Position of NED origin 𝑜𝑛 in

UTM ሶ𝛉𝑛𝑏 =ሶ𝑅ሶ𝑃ሶ𝑌

Euler rate vector

𝑹𝑏𝑛 𝜃𝑛𝑏 Transformation matrix of linear

velocity vector 𝒗𝑏/𝑛𝑏 from {b} to

𝒗𝑏/𝑛𝑛 in {n}

𝑻𝜃 𝜽𝒏𝒃 Transformation matrix of

angular velocity vector 𝒘𝑏/𝑛𝑏

from {b} to Euler rate vectorሶ𝛉𝑛𝑏 in {n}



KinematicsTransformations between Frames

Euler’s Rotation Theorem:

Geometry perspective

Any displacement of a rigid-body in 3D, such that a point on the rigid body remains fixed, is equivalent to a single rotation about some axis that passes through the fixed point.

Linear Algebra perspective

Any two Cartesian coordinate systems in 3D with a common origin are related by a rotation about some fixed axis passing through the origin.



KinematicsTransformations BODY-NEDEuler Angles

Linear Velocity Transformation

ሶ𝒑 Τ𝑏 𝑛𝑛 = 𝒗 Τ𝑏 𝑛

𝑛 = 𝑹𝑏𝑛 𝜽𝑛𝑏 𝒗 Τ𝑏 𝑛

𝑏 = 𝑹𝑧,𝑌𝑹𝑦,𝑃𝑹𝑥,𝑅𝒗 Τ𝑏 𝑛𝑏

𝑹𝑥,𝑅 =1 0 00 𝑐𝑅 −𝑠𝑅0 𝑠𝑅 𝑐𝑅

, 𝑹𝑦,𝑃 =𝑐𝑃 0 𝑠𝑃0 1 0

−𝑠𝑃 0 𝑐𝑃, 𝑹𝑧,𝑌=

𝑐𝑌 −𝑠𝑌 0𝑠𝑌 𝑐𝑌 00 0 1

𝑹𝑏𝑛 𝜽𝑛𝑏 =

𝑐𝑌𝑐𝑃 −𝑠𝑌𝑐𝑅 + 𝑐𝑌𝑠𝑃𝑠𝑅 𝑠𝑌𝑠𝑅 + 𝑐𝑌𝑐𝑅𝑠𝑃𝑠𝑌𝑐𝑃 𝑐𝑌𝑐𝑅 + 𝑠𝑌𝑠𝑃𝑠𝑅 −𝑐𝑌𝑠𝑅 + 𝑠𝑃𝑠𝑌𝑐𝑅−𝑠𝑃 𝑐𝑃𝑠𝑅 𝑐𝑃𝑐𝑅

𝑹𝑏𝑛 𝜽𝑛𝑏

−1 = 𝑹𝑏𝑛 𝜽𝑛𝑏

𝑇




Angular Velocity Transformation

ሶ𝜽𝑛𝑏 = 𝑻𝜃 𝜽𝒏𝒃 𝒘 Τ𝑏 𝑛𝑏 =

1 𝑠𝑅𝑡𝑃 𝑐𝑅𝑡𝑃0 𝑐𝑅 −𝑠𝑅0 𝑠 Τ𝑅 𝑐 𝑃 𝑐 Τ𝑅 𝑐 𝑃

𝒘 Τ𝑏 𝑛𝑏

Singularity for P=±90°




6 DOF Kinematic Equations

ሶ𝒑 Τ𝑏 𝑛𝑛

ሶ𝜽𝑛𝑏=

𝑹𝑏𝑛 𝜽𝑛𝑏 𝟎3×3𝟎3×3 𝑻𝜃 𝜽𝒏𝒃

𝒗 Τ𝑏 𝑛𝑏


ሶ𝜼 = 𝑱𝜣 𝜼 𝝂

Simulation ModelAttitude representation: Euler Angles

r

q

p

w

v

u

b

nb

b

nb

/

/

w

vv

nb

n

nb

θ

pη

/

Y

P

R

z

y

x

n

n

n

nb

n

nb

θ

pη /

Gain Integrator

0

00 /

nb

n

nb

θ

pη

KINEMATICS



KinematicsTransformations BODY-NEDQuaternions

Basis: 1,i,j,k𝑯 ≡ ℝ4

Addition:

𝑞 = 𝑎𝟏 + 𝑏𝒊 + 𝑐𝒋 + 𝑑𝒌

Scalar part

Vector part

“Real” quaternions: 𝑎𝟏 + 0𝒊 + 0𝒋 + 0𝒌

“Pure” quaternions: 0𝟏 + 𝑏𝒊 + 𝑐𝒋 + 𝑑𝒌

𝑎1𝟏 + 𝑏1𝒊 + 𝑐1𝒋 + 𝑑1𝒌 + 𝑎2𝟏 + 𝑏2𝒊 + 𝑐2𝒋 + 𝑑2𝒌= 𝑎1 + 𝑎2 𝟏 + 𝑏1 + 𝑏2 𝒊 + 𝑐1 + 𝑐2 𝒋 + 𝑑1 + 𝑑2 𝒌

Scalar Multiplication: 𝛼 𝑎𝟏 + 𝑏𝒊 + 𝑐𝒋 + 𝑑𝒌 = 𝛼𝑎𝟏 + 𝛼𝑏𝒊 + 𝛼𝑐𝒋 + 𝛼𝑑𝒌

Quaternion Multiplication:

𝑎1𝟏 + 𝑏1𝒊 + 𝑐1𝒋 + 𝑑1𝒌 𝑎2𝟏 + 𝑏2𝒊 + 𝑐2𝒋 + 𝑑2𝒌= 𝑎1𝑎2 − 𝑏1𝑏2 − 𝑐1𝑐2 − 𝑑1𝑑2 𝟏+ 𝑎1𝑏2 + 𝑏1𝑎2 + 𝑐1𝑑2 − 𝑑1𝑐2 𝒊+ 𝑎1𝑐2 − 𝑏1𝑑2 + 𝑐1𝑎2 + 𝑑1𝑏2 𝒋+ 𝑎1𝑑2 + 𝑏1𝑐2 − 𝑐1𝑏2 + 𝑑1𝑎2 𝒌




Addition:

Scalar part

Vector part

𝑟1, Ԧ𝑣1 + 𝑟2, Ԧ𝑣2 = 𝑟1 + 𝑟2, Ԧ𝑣1 + Ԧ𝑣2

Scalar Multiplication:

Quaternion Multiplication:

𝑞 = 𝑟, Ԧ𝑣

𝛼 𝑟, Ԧ𝑣 = 𝛼𝑟, 𝛼 Ԧ𝑣

𝑟1, Ԧ𝑣1 𝑟2, Ԧ𝑣2 = 𝑟1𝑟2 − Ԧ𝑣1 ∙ Ԧ𝑣2, 𝑟1 Ԧ𝑣2 + 𝑟2 Ԧ𝑣1 + Ԧ𝑣1 × Ԧ𝑣2




Conjugate:

Scalar part

Vector part

𝑞∗ = 𝑟, − Ԧ𝑣 = 𝑟𝟏 − 𝑣𝑥𝒊 − 𝑣𝑦𝒋 − 𝑣𝑧𝒌

Norm:

𝑞 = 𝑟, Ԧ𝑣 = 𝑟𝟏 + 𝑣𝑥𝒊 + 𝑣𝑦𝒋 + 𝑣𝑧𝒌

𝑞 = 𝑞𝑞∗ = 𝑟2 + Ԧ𝑣 2 = 𝑟2 + 𝑣𝑥2 + 𝑣𝑦

2 + 𝑣𝑧2

𝑞𝑞∗ = 𝑟, Ԧ𝑣 𝑟, − Ԧ𝑣 = 𝑟2 + Ԧ𝑣 2, 0 = 𝑟2 + 𝑣𝑥2 + 𝑣𝑦

2 + 𝑣𝑧2 𝟏 + 0𝒊 + 0𝒋 + 0𝒌

𝑞1𝑞2∗ = 𝑞2

∗𝑞1∗

𝑞∗𝑞 = 𝑟,− Ԧ𝑣 𝑟, Ԧ𝑣 = 𝑟2 + Ԧ𝑣 2, 0 = 𝑞𝑞∗

𝑝𝑞 2 = 𝑝𝑞 𝑝𝑞 ∗ = 𝑝𝑞𝑞∗𝑝∗ = 𝑝 𝑞 2𝑝∗ = 𝑝𝑝∗ 𝑞 2 = 𝑝 2 𝑞 2

Inverse: 𝑞−1 =𝑞∗

𝑞 2

𝑞−1𝑞 = 𝑞𝑞−1 = 1




Unit Quaternion: 𝑞 = 1 ⇒ 𝑟2 + Ԧ𝑣 2 = 1 ⇒ ∃𝜃 ∈ 0, 𝜋 : cos 𝜃 = 𝑟, sin 𝜃 = Ԧ𝑣

𝑞 = 1 ⇒ 𝑞 = 𝑟, Ԧ𝑣 = r + Ԧ𝑣Ԧ𝑣

Ԧ𝑣= cos 𝜃 + sin 𝜃 Ԧො𝑣 = 𝑒

0,𝜃 ො𝑣cos 𝜃 sin 𝜃

Ԧො𝑣

Polar Decomposition: 𝑞 = 𝑞

𝑞

𝑞= 𝑞 cos 𝜃 + sin 𝜃 Ԧො𝑣 = 𝑞 𝑒

0,𝜃 ො𝑣

Unit Quaternion Unit

Quaternion

Unit Quaternion

Power: 𝑞𝑝 = 𝑞 𝑝𝑒0, 𝑝𝜃 ො𝑣

= 𝑞 𝑝 cos 𝑝𝜃 + sin 𝑝𝜃 Ԧො𝑣

Euler Formula(General Case): 𝑒 𝑟,𝑣 = 𝑒

𝑟,0 + 0, 𝑣 ො𝑣= 𝑒 𝑟,0 𝑒

0,𝜃 ො𝑣= 𝑒𝑟𝑒

0,𝜃 ො𝑣= 𝑒𝑟 cos 𝜃 , sin 𝜃 Ԧො𝑣




Euler Formula (“Pure” Quaternion):𝑞 = 0, Ԧ𝑣

𝑞2 = 0, Ԧ𝑣 0, Ԧ𝑣 = − Ԧ𝑣 2, 0

⋮

𝑞3 = − Ԧ𝑣 2, 0 0, Ԧ𝑣 = 0,− Ԧ𝑣 2 Ԧ𝑣

𝑞4 = 0,− Ԧ𝑣 2 Ԧ𝑣 0, Ԧ𝑣 = Ԧ𝑣 4, 0

𝑞5 = Ԧ𝑣 4, 0 0, Ԧ𝑣 = 0, Ԧ𝑣 4 Ԧ𝑣

𝑞6 = 0, Ԧ𝑣 4 Ԧ𝑣 0, Ԧ𝑣 = − Ԧ𝑣 6, 0

𝑞7 = − Ԧ𝑣 6, 0 0, Ԧ𝑣 = 0,− Ԧ𝑣 6 Ԧ𝑣

𝑒𝑞 = 1 + 𝑞 +𝑞2

2!+𝑞3

3!+𝑞4

4!+𝑞5

5!+𝑞6

6!+𝑞7

7!⋯

𝑒 0,𝑣 = 1,0 + 0, Ԧ𝑣 +− Ԧ𝑣 2, 0

2!+

0,− Ԧ𝑣 2 Ԧ𝑣

3!+

Ԧ𝑣 4, 0

4!+

0, Ԧ𝑣 4 Ԧ𝑣

5!⋯

𝑒 0,𝑣 = 1 −Ԧ𝑣 2

2!+

Ԧ𝑣 4

4!⋯ , 1 −

Ԧ𝑣 2

3!+

Ԧ𝑣 4

5!⋯

Ԧ𝑣

Ԧ𝑣= cos Ԧ𝑣 , sin Ԧ𝑣 Ԧො𝑣




Special case: Rotation About Perpendicular AxisԦ𝑣 , = cos𝜃 Ԧ𝑣 + sin 𝜃 ො𝑛 × Ԧ𝑣

ො𝑛 × Ԧ𝑣

Ԧ𝑣

Ԧ𝑣 ,

𝜃

0ො𝑛

cos 𝜃 Ԧ𝑣

sin 𝜃 ො𝑛 × Ԧ𝑣

𝑣 = 0, Ԧ𝑣

𝑛 = 0, ො𝑛

𝑣 , = 0, Ԧ𝑣 , = 0, cos 𝜃 Ԧ𝑣 + sin 𝜃 ො𝑛 × Ԧ𝑣 = cos 𝜃 , sin 𝜃 ො𝑛 0, Ԧ𝑣 = 𝑒0,𝜃 ො𝑛

0, Ԧ𝑣 = 𝑒0,𝜃 ො𝑛

𝑣

𝑛𝑣 = 0, ො𝑛 0, Ԧ𝑣 = 0, ො𝑛 × Ԧ𝑣

Solution using Quaternions:

𝑛2 = 0, ො𝑛 0, ො𝑛 = −1, 0

𝑣 ,= 𝑒0,𝜃 ො𝑛

𝑣

ො𝑛

0Ԧ𝑣

Ԧ𝑣 ,

𝜃

ො𝑛 × Ԧ𝑣

ො𝑛 = 1




Ԧ𝑣⊥, = cos𝜃 Ԧ𝑣⊥ + sin 𝜃 ො𝑛 × Ԧ𝑣⊥

ො𝑛 × Ԧ𝑣⊥

Ԧ𝑣⊥

Ԧ𝑣⊥,

𝜃

0ො𝑛

cos 𝜃 Ԧ𝑣⊥

sin 𝜃 ො𝑛 × Ԧ𝑣⊥

Ԧ𝑣 , = Ԧ𝑣∥, + Ԧ𝑣⊥

, = Ԧ𝑣∥ + cos 𝜃 Ԧ𝑣⊥ + sin 𝜃 ො𝑛 × Ԧ𝑣⊥

Rotation About General Axis

ො𝑛

0

Ԧ𝑣

Ԧ𝑣 ,

𝜃Ԧ𝑣⊥

Ԧ𝑣∥ = Ԧ𝑣∥,

Ԧ𝑣⊥,


cos 𝜃 Ԧ𝑣⊥





Vector Projection onto Line

Ԧ𝑣

𝑏Ԧ𝑣⊥

Ԧ𝑣∥

Ԧ𝑣 = Ԧ𝑣∥ + Ԧ𝑣⊥ ⇒ Ԧ𝑣⊥ = Ԧ𝑣 − Ԧ𝑣∥ = Ԧ𝑣 − 𝛼𝑏

𝑏 ⊥ Ԧ𝑣⊥ ⇒ 𝑏 ∙ Ԧ𝑣⊥ = 0

𝑏 ∙ Ԧ𝑣 − 𝛼𝑏 = 0

Ԧ𝑣∥ = 𝛼𝑏

𝑏 ∙ Ԧ𝑣 − 𝛼 𝑏 ∙ 𝑏 = 0

𝛼 =𝑏 ∙ Ԧ𝑣

𝑏 ∙ 𝑏=𝑏𝑇 Ԧ𝑣

𝑏𝑇𝑏

Ԧ𝑣∥ = 𝛼𝑏 =𝑏 ∙ Ԧ𝑣

𝑏 ∙ 𝑏𝑏 =

𝑏𝑇 Ԧ𝑣

𝑏𝑇𝑏𝑏 = 𝑏

𝑏𝑇 Ԧ𝑣

𝑏𝑇𝑏=𝑏𝑏𝑇

𝑏𝑇𝑏Ԧ𝑣

Projection Matrix

Scalar




Special Case:

Ԧ𝑣

𝑏 = ො𝑛

Ԧ𝑣⊥

Ԧ𝑣∥

Ԧ𝑣∥ =ො𝑛ො𝑛𝑇

ො𝑛𝑇 ො𝑛Ԧ𝑣 =

ො𝑛 ො𝑛𝑇

ො𝑛2 Ԧ𝑣 = ො𝑛 ො𝑛𝑇 Ԧ𝑣

𝑏 = ො𝑛 ො𝑛 = 1

Ԧ𝑣∥ =ො𝑛 ∙ Ԧ𝑣

ො𝑛 ∙ ො𝑛ො𝑛 =

ො𝑛 ∙ Ԧ𝑣

ො𝑛2 ො𝑛 = ො𝑛 ∙ Ԧ𝑣 ො𝑛

Projection Matrix

Scalar




Special Case:

Ԧ𝑣

Ԧ𝑣⊥

Ԧ𝑣∥

Ԧ𝑞 = sin𝜃

2

𝑏 = Ԧ𝑞 = sin𝜃

2ො𝑛

𝑏 = Ԧ𝑞 = sin𝜃

2ො𝑛

Ԧ𝑣∥ =Ԧ𝑞 Ԧ𝑞𝑇

Ԧ𝑞𝑇 Ԧ𝑞Ԧ𝑣 =

Ԧ𝑞 Ԧ𝑞𝑇

Ԧ𝑞 2 Ԧ𝑣 =Ԧ𝑞 Ԧ𝑞𝑇

sin2𝜃2

Ԧ𝑣

Ԧ𝑣∥ =Ԧ𝑞 ∙ Ԧ𝑣

Ԧ𝑞 ∙ Ԧ𝑞Ԧ𝑞 =

Ԧ𝑞 ∙ Ԧ𝑣

Ԧ𝑞 2 Ԧ𝑞 =Ԧ𝑞 ∙ Ԧ𝑣

sin2𝜃2

Ԧ𝑞

Projection Matrix

Scalar




Ԧ𝑣 , = Ԧ𝑣∥, + Ԧ𝑣⊥

, = Ԧ𝑣∥ + cos 𝜃 Ԧ𝑣⊥ + sin 𝜃 ො𝑛 × Ԧ𝑣⊥ = Ԧ𝑣∥ + cos𝜃 Ԧ𝑣 − Ԧ𝑣∥ + sin 𝜃 ො𝑛 × Ԧ𝑣 − Ԧ𝑣∥

= 1 − cos𝜃 Ԧ𝑣∥ + cos 𝜃 Ԧ𝑣 + sin 𝜃 ො𝑛 × Ԧ𝑣 − sin 𝜃 ො𝑛 × Ԧ𝑣∥0

Rodrigues Rotation Formula: Ԧ𝑣 , = cos 𝜃 Ԧ𝑣 + 1 − cos 𝜃 Ԧ𝑣 ∙ ො𝑛 ො𝑛 + sin 𝜃 ො𝑛 × Ԧ𝑣

Ԧ𝑣∥ = Ԧ𝑣 ∙ ො𝑛 ො𝑛 Ԧ𝑣∥

Rotation About General Axis

ො𝑛

0

Ԧ𝑣

Ԧ𝑣 ,

𝜃Ԧ𝑣⊥


Ԧ𝑣⊥,


1 − cos 𝜃 Ԧ𝑣∥

cos 𝜃 Ԧ𝑣


Ԧ𝑣⊥, = cos𝜃 Ԧ𝑣⊥ + sin 𝜃 ො𝑛 × Ԧ𝑣⊥


Ԧ𝑣⊥

Ԧ𝑣⊥,

𝜃

0ො𝑛

cos 𝜃 Ԧ𝑣⊥





Rotation About General Axis: Solution using Quaternions

𝑣 = 0, Ԧ𝑣

𝑛 = 0, ො𝑛

𝑣∥ = 0, Ԧ𝑣∥

𝑣⊥ = 0, Ԧ𝑣⊥Lema 2: 𝑒

0,𝜃 ො𝑛𝑣⊥ = 𝑣⊥𝑒

0,−𝜃 ො𝑛

Lema 1: 𝑒0,𝜃 ො𝑛

𝑣∥ = 𝑣∥𝑒0,𝜃 ො𝑛

Ԧ𝑣 , = Ԧ𝑣∥, + Ԧ𝑣⊥

, = Ԧ𝑣∥ + Ԧ𝑣⊥,

𝑣 , = 0, Ԧ𝑣 ,

𝑣 , = 0, Ԧ𝑣 , = 0, Ԧ𝑣∥, + Ԧ𝑣⊥

, = 0, Ԧ𝑣∥ + Ԧ𝑣⊥,

𝑣 , = 𝑒0,𝜃2ො𝑛𝑒

0,−𝜃2ො𝑛𝑣∥ + 𝑒

0,𝜃2ො𝑛𝑒

0,𝜃2ො𝑛𝑣⊥ = 𝑒

0,𝜃2ො𝑛𝑣∥𝑒

0,−𝜃2ො𝑛+ 𝑒

0,𝜃2ො𝑛𝑣⊥𝑒

0,−𝜃2ො𝑛

𝑣 , = 𝑣∥ + 𝑣⊥, = 𝑣∥ + 𝑒

0,𝜃 ො𝑛𝑣⊥

Lema 1 Lema 2

𝑣 , = 𝑒0,𝜃2ො𝑛

𝑣∥ + 𝑣⊥ 𝑒0,−

𝜃2ො𝑛

= 𝑒0,𝜃2ො𝑛𝑣𝑒

0,−𝜃2ො𝑛

ො𝑛

0

Ԧ𝑣

Ԧ𝑣 ,

𝜃Ԧ𝑣⊥


Ԧ𝑣⊥,

Ԧ𝑞 = sin𝜃

2ො𝑛

Ԧ𝑣∥ =Ԧ𝑞 ∙ Ԧ𝑣

Ԧ𝑞 ∙ qԦ𝑞




Rodrigues Rotation Formula:

Ԧ𝑣 , = 1 − cos 𝜃 Ԧ𝑣 ∙ ො𝑛 ො𝑛 − Ԧ𝑣 + Ԧ𝑣 + cos 𝜃 Ԧ𝑣 + sin 𝜃 ො𝑛 × Ԧ𝑣

= 1 − cos 𝜃 Ԧ𝑣 ∙ ො𝑛 ො𝑛 − Ԧ𝑣 + 1 − cos 𝜃 + cos 𝜃 Ԧ𝑣 + sin 𝜃 ො𝑛 × Ԧ𝑣

Ԧ𝑣∥

− Ԧ𝑣⊥

Ԧ𝑣 , = Ԧ𝑣 + 1 − cos 𝜃 Ԧ𝑣 ∙ ො𝑛 ො𝑛 − Ԧ𝑣 + sin 𝜃 ො𝑛 × Ԧ𝑣

ො𝑛

0

Ԧ𝑣

Ԧ𝑣 ,

𝜃Ԧ𝑣⊥


Ԧ𝑣⊥,

1 − cos 𝜃 − Ԧ𝑣⊥


ො𝑛 × Ԧ𝑣 ≔ 𝑺 𝒏 𝐯

ො𝑛 × ො𝑛 × Ԧ𝑣 = Ԧ𝑣 ∙ ො𝑛 ො𝑛 − Ԧ𝑣 = − Ԧ𝑣⊥

𝑺 𝒏 𝑺 𝒏 𝐯 = 𝑺2 𝒏 𝐯 = 𝒏𝒏𝑻 − 𝑰𝟑×𝟑 𝐯

= 𝒏𝒏𝑻 𝐯 − 𝑰𝟑×𝟑𝐯 = 𝐯∥ − 𝐯 = −𝐯⊥

𝐯, = 𝑰𝟑×𝟑 + 1 − cos 𝜃 𝑺2 𝒏 + sin 𝜃𝑺 𝒏 𝐯

𝑹𝒏,𝜃

Ԧ𝑣 , = cos𝜃 Ԧ𝑣 + 1 − cos 𝜃 Ԧ𝑣 ∙ ො𝑛 ො𝑛 + sin 𝜃 ො𝑛 × Ԧ𝑣




Rotation Formula using Quaternions:

𝑞 = 𝑒0,𝜃2ො𝑛

= cos𝜃

2, sin

𝜃

2ො𝑛 = 𝑞0, Ԧ𝑞 , 𝑞0

2+ Ԧ𝑞 2 = 1

𝑣 , = 𝑒0,𝜃2ො𝑛

𝑣∥ + 𝑣⊥ 𝑒0,−

𝜃2ො𝑛


0,−𝜃2ො𝑛

= 𝑞𝑣𝑞∗

𝑣 , = 0, 𝑰𝟑×𝟑 + 1 − cos 𝜃 𝑺2 𝒏 + sin 𝜃𝑺 𝒏 Ԧ𝑣 =

= 0, 𝑰𝟑×𝟑 + 2sin2𝜃

2𝑺2 𝒏 + 2 sin

𝜃

2cos

𝜃

2𝑺 𝒏 Ԧ𝑣

𝑣 , = 0, 𝑰𝟑×𝟑 + 2𝑺2 sin𝜃

2𝒏 + 2 cos

𝜃

2𝑺 sin

𝜃

2𝒏 Ԧ𝑣

𝑹𝑏𝑛 𝒒 = 𝑹 𝑞0,𝑞

𝑣 , = 0, 𝑰𝟑×𝟑 + 2𝑺2 Ԧ𝑞 + 2𝑞0𝑺 Ԧ𝑞 Ԧ𝑣

ො𝑛

0

Ԧ𝑣

Ԧ𝑣 ,

𝜃Ԧ𝑣⊥


Ԧ𝑣⊥,

2𝑞0𝑺 Ԧ𝑞 Ԧ𝑣

2𝑺2 Ԧ𝑞 Ԧ𝑣

Ԧ𝑞 = sin𝜃

2ො𝑛

𝑺2 Ԧ𝑞 = Ԧ𝑞 Ԧ𝑞𝑇 − Ԧ𝑞 2𝑰𝟑×𝟑




𝑣 , = 𝑒0,𝜃2ො𝑛

𝑣∥ + 𝑣⊥ 𝑒0,−

𝜃2ො𝑛

=

= 𝑒0,𝜃2ො𝑛𝑣∥ 𝑒

0,−𝜃2ො𝑛+ 𝑒

0,𝜃2ො𝑛𝑣⊥ 𝑒

0,−𝜃2ො𝑛

𝑣 , = 𝑒0,𝜃 ො𝑛

𝑣∥ + 𝑣⊥ = 𝑒0,𝜃 ො𝑛

𝑣∥ + 𝑒0,𝜃 ො𝑛

𝑣⊥Wrong Formula:

Correct Formula:

ො𝑛

0

Ԧ𝑣

Ԧ𝑣 ,

𝜃Ԧ𝑣⊥

Ԧ𝑣∥,

Ԧ𝑣⊥,

Ԧ𝑣∥Ԧ𝑣∥,

Ԧ𝑣⊥,

Ԧ𝑝∥,

Ԧ𝑝⊥,

ො𝑛

0

Ԧ𝑣

Ԧ𝑣 ,

Ԧ𝑣⊥

Ԧ𝑣∥

Step 1:

Ԧ𝑝⊥,

Ԧ𝑝∥,

ො𝑛

0

Ԧ𝑣

Ԧ𝑣 ,

Ԧ𝑣⊥

Ԧ𝑣⊥,

Ԧ𝑣∥, = Ԧ𝑣∥

Step 2:

Ԧ𝑝⊥,

Ԧ𝑝∥,




Rotation Formula using Quaternions:

𝑞 = 𝑒0,𝜃2ො𝑛

= cos𝜃

2, sin

𝜃

2ො𝑛 = 𝑞0, Ԧ𝑞 , 𝑞0

2+ Ԧ𝑞 2 = 1

𝑣 , = 𝑒0,𝜃2ො𝑛

𝑣∥ + 𝑣⊥ 𝑒0,−

𝜃2ො𝑛


0,−𝜃2ො𝑛

= 𝑞𝑣𝑞∗

Quaternion Rotation Operator:

𝑣 , = 0, 𝑞02 − Ԧ𝑞 2 𝑰𝟑×𝟑 + 2Ԧ𝑞 Ԧ𝑞𝑇 + 2𝑞0𝑺 Ԧ𝑞 Ԧ𝑣

𝑣 , = 𝐿𝑞 𝑣 ≔ 𝑞𝑣𝑞∗ = 𝑞0, Ԧ𝑞 0, Ԧ𝑣 𝑞0, − Ԧ𝑞 = 0, 𝑞02 − Ԧ𝑞 2 Ԧ𝑣 + 2 Ԧ𝑞 ∙ Ԧ𝑣 Ԧ𝑞 + 2𝑞0 Ԧ𝑞 × Ԧ𝑣

ො𝑛

0

Ԧ𝑣

Ԧ𝑣 ,

𝜃Ԧ𝑣⊥


Ԧ𝑣⊥,

2𝑞0𝑺 Ԧ𝑞 Ԧ𝑣

2 Ԧ𝑞 Ԧ𝑞𝑇 Ԧ𝑣 = 2 Ԧ𝑞 ∙ Ԧ𝑣 Ԧ𝑞

𝑞02 − Ԧ𝑞 2 Ԧ𝑣

Ԧ𝑞 = sin𝜃

2ො𝑛

𝑞02 − Ԧ𝑞 2 Ԧ𝑣 = 𝑞0

2 + Ԧ𝑞 2 − 2 Ԧ𝑞 2 Ԧ𝑣 = 1 − 2sin2𝜃

2Ԧ𝑣 = cos 𝜃 Ԧ𝑣

1 − cos 𝜃 Ԧ𝑣∥ = 2sin2𝜃

2

Ԧ𝑞 ∙ Ԧ𝑣

Ԧ𝑞 ∙ qԦ𝑞 = 2sin2

𝜃

2

Ԧ𝑞 ∙ Ԧ𝑣

Ԧ𝑞 2Ԧ𝑞 =

= 2sin2𝜃

2

Ԧ𝑞 ∙ Ԧ𝑣

sin2𝜃2

Ԧ𝑞 = 2 Ԧ𝑞 ∙ Ԧ𝑣 Ԧ𝑞 = 2 Ԧ𝑞 Ԧ𝑞𝑇 Ԧ𝑣 = 2 Ԧ𝑞 Ԧ𝑞𝑇 Ԧ𝑣

Scalar Projection Matrix




Linear Velocity Transformation

ሶ𝒑 Τ𝑏 𝑛𝑛 = 𝒗 Τ𝑏 𝑛

𝑛 = 𝑹𝑏𝑛 𝒒 𝒗 Τ𝑏 𝑛

𝑏

𝑹𝑏𝑛 𝒒 = 𝑹 𝑞0,𝑞 = 𝑰𝟑×𝟑 + 2𝑺2 Ԧ𝑞 + 2𝑞0𝑺 Ԧ𝑞

𝑹𝑏𝑛 𝒒 =

1 − 2 𝑞22 + 𝑞3

2 2 𝑞1𝑞2 − 𝑞3𝑞0 2 𝑞1𝑞3 + 𝑞2𝑞02 𝑞1𝑞2 + 𝑞3𝑞0 1 − 2 𝑞3

2 + 𝑞12 2 𝑞2𝑞3 − 𝑞1𝑞0

2 𝑞1𝑞3 − 𝑞2𝑞0 2 𝑞2𝑞3 + 𝑞1𝑞0 1 − 2 𝑞12 + 𝑞2

2

𝑞 = 𝑞0, Ԧ𝑞 = 𝑞0𝟏 + 𝑞1𝒊 + 𝑞2𝒋 + 𝑞3𝒌

𝑞0 = cos𝜃

2

Ԧ𝑞 = sin𝜃

2ො𝑛

𝑹𝑏𝑛 𝒒 −1 = 𝑹𝑏

𝑛 𝒒 𝑇





ሶ𝑹𝒃𝑛 𝒒 = 𝑹𝑏

𝑛 𝒒 𝑺 𝒘 Τ𝑏 𝑛𝑏 ⇒ ሶ𝒒 = 𝑻𝒒 𝒒 𝒘 Τ𝑏 𝑛

𝑏

𝑞 = 𝑞0, Ԧ𝑞 = 𝑞0𝟏 + 𝑞1𝒊 + 𝑞2𝒋 + 𝑞3𝒌

𝑞0 = cos𝜃

2

Ԧ𝑞 = sin𝜃

2ො𝑛

𝑻𝒒𝑇 𝒒 𝑻𝒒 𝒒 =

1

4𝑰𝟑×𝟑

𝑻𝒒 𝒒 =1

2

−𝑞1𝑞0

−𝑞2 −𝑞3−𝑞3 𝑞2

𝑞3−𝑞2

𝑞0𝑞1

−𝑞1𝑞0

𝑻𝒒 𝒒 =1

2

−Ԧ𝑞𝑇

𝑞0𝑰𝟑×𝟑 + 𝑺 Ԧ𝑞



KinematicsTransformations BODY-NEDQuaternions versus Euler Angles


𝑻𝒒 𝒒 =1

2

−𝑞1𝑞0

−𝑞2 −𝑞3−𝑞3 𝑞2

𝑞3−𝑞2

𝑞0𝑞1

−𝑞1𝑞0

𝑻𝜃 𝜽𝒏𝒃 =1 𝑠𝑅𝑡𝑃 𝑐𝑅𝑡𝑃0 𝑐𝑅 −𝑠𝑅0 𝑠 Τ𝑅 𝑐 𝑃 𝑐 Τ𝑅 𝑐 𝑃

Singularity for P=±90°

Quaternions Euler Angles




6 DOF Kinematic Equations


ሶ𝒒=

𝑹𝑏𝑛 𝒒 𝟎3×3𝟎4×3 𝑻𝒒 𝒒

𝒗 Τ𝑏 𝑛𝑏


ሶ𝜼 = 𝑱𝒒 𝜼 𝝂

Simulation ModelAttitude representation: Quaternions

r

q

p

w

v

u

b

nb

b

nb

/

/

w

vv

q

pη

n

nb /

3

2

1

0

/

q

q

q

q

z

y

x

n

n

n

n

nb

q

pη

Gain Integrator

0

00 /

q

pη

n

nb

KINEMATICS



KinematicsTransformations BODY-NEDQuaternions from Euler Angles

Euler Angles → Quaternions

𝒒 =

𝑞0𝑞1𝑞2𝑞3

=

𝑐 𝑅/2 𝑐 𝑃/2 𝑐 𝑌/2 + 𝑠 𝑅/2 𝑠 𝑃/2 𝑠 𝑌/2

𝑠 𝑅/2 𝑐 𝑃/2 𝑐 𝑌/2 − 𝑐 𝑅/2 𝑠 𝑃/2 𝑠 𝑌/2

𝑐 𝑅/2 𝑠 𝑃/2 𝑐 𝑌/2 + 𝑠 𝑅/2 𝑐 𝑃/2 𝑠 𝑌/2

𝑐 𝑅/2 𝑐 𝑃/2 𝑠 𝑌/2 − 𝑠 𝑅/2 𝑠 𝑃/2 𝑐 𝑌/2



KinematicsTransformations BODY-NEDEuler Angles from Quaternions

Quaternions → Euler Angles

𝜽𝒏𝒃 =𝑅𝑃𝑌

=

atan2 2 𝑞2𝑞3 + 𝑞1𝑞0 , 1 − 2 𝑞12 + 𝑞2

2

asin 2 𝑞0𝑞2 − 𝑞3𝑞1

atan2 2 𝑞0𝑞3 + 𝑞1𝑞2 , 1 − 2 𝑞22 + 𝑞3

2



KinematicsTransformations ECEF-NEDLat-Lon Transformations

ሶ𝒑 Τ𝑏 𝑒𝑒 = 𝑹𝑛

𝑒 𝜽𝑒𝑛 𝑹𝑏𝑛 𝜽𝑛𝑏 𝒗 Τ𝑏 𝑛

𝑏 = 𝑹𝑧,𝜆𝑹𝑦,−𝜑−𝜋2


𝑏

𝑛

𝑒

𝑥𝑒

𝑦𝑒

𝑧𝑒

Equator

𝑥𝑛𝑦𝑛

𝑧𝑛

𝜆

𝜑

𝜆 − 𝐿𝑜𝑛𝑔𝑖𝑡𝑢𝑑𝑒𝜑 − 𝐿𝑎𝑡𝑖𝑡𝑢𝑑𝑒

𝑹𝑛𝑒 𝜽𝑒𝑛

𝑹𝑦,−𝜑−

𝜋2=

𝑐 −𝜑 −𝜋

20 𝑠 −𝜑 −

𝜋

20 1 0

−𝑠 −𝜑 −𝜋

20 𝑐 −𝜑 −

𝜋

2

𝑹𝑧,𝑌 =𝑐𝜆 −𝑠𝜆 0𝑠𝜆 𝑐𝜆 00 0 1

NED → ECEF



KinematicsTransformations ECEF-NEDLat-Lon Transformations

NED → ECEF

ሶ𝒑 Τ𝑏 𝑒𝑒 = 𝑹𝑛

𝑒 𝜽𝑒𝑛 ሶ𝒑 Τ𝑏 𝑛𝑛 =

−𝑐𝜆𝑠𝜑 −𝑠𝜆 −𝑐𝜆𝑐𝜑−𝑠𝜆𝑠𝜑 𝑐𝜆 −𝑠𝜆𝑐𝜑𝑐𝜑 0 −𝑠𝜑

ሶ𝑥𝑛ሶ𝑦𝑛ሶ𝑧𝑛

𝑹𝑛𝑒 𝜽𝑒𝑛

North Velocity

East Velocity

Down Velocity

NEDECEF

ECEF → NED

ሶ𝒑 Τ𝑏 𝑛𝑛 = 𝑹𝑒

𝑛 𝜽𝑒𝑛 ሶ𝒑 Τ𝑏 𝑒𝑒 = 𝑹𝑛

𝑒𝑇 𝜽𝑒𝑛 ሶ𝒑 Τ𝑏 𝑒𝑒 =

−𝑐𝜆𝑠𝜑 −𝑠𝜆𝑠𝜑 𝑐𝜑−𝑠𝜆 𝑐𝜆 0−𝑐𝜆𝑐𝜑 −𝑠𝜆𝑐𝜑 −𝑠𝜑

ሶ𝑥𝑒ሶ𝑦𝑒ሶ𝑧𝑒

𝑹𝑒𝑛 𝜽𝑒𝑛

ECEFNED


ሶ𝒑 Τ𝑏 𝑒𝑒

Speed & Heading

Speed = ሶ𝑥𝑛2 + ሶ𝑦𝑛

2

Heading = tan−1𝑦𝑛𝑥𝑛



KinematicsTransformations ECEF-NEDGeodetic Latitude vs Geocentric Latitude

Raw GNSS (GPS, GLONASS and Gallileo) output is given in Cartesian ECEF frame.𝒑 Τ𝑏 𝑒𝒆

It is presented to users in terms of longitude , latitude and height relative to WGS-84 ellipsoid.𝜆 𝜑 ℎ

𝜑 − 𝐺𝑒𝑜𝑑𝑒𝑡𝑖𝑐 𝐿𝑎𝑡𝑖𝑡𝑢𝑑𝑒𝜓 − 𝐺𝑒𝑜𝑐𝑒𝑛𝑡𝑟𝑖𝑐 𝐿𝑎𝑡𝑖𝑡𝑢𝑑𝑒

𝜓 𝜑 = tan−1 1 − 𝑒2 tan𝜑

𝑎 = 6378137.0𝑚 𝑠𝑒𝑚𝑖 − 𝑚𝑎𝑗𝑜𝑟 𝑎𝑥𝑖𝑠

WGS-84 parameters:

𝑏 = 6356752.3142𝑚 𝑠𝑒𝑚𝑖 − 𝑚𝑖𝑛𝑜𝑟 𝑎𝑥𝑖𝑠

𝑒 =𝑎2−𝑏2

𝑎2𝑒𝑐𝑐𝑒𝑛𝑡𝑟𝑖𝑐𝑖𝑡𝑦

North Pole

𝜑𝜓

ℎ

Tangent

Normal

Ellipsoid

𝑎

𝑏 𝑁

𝑓 = 𝑎 − 𝑏 /𝑎 "flattening" 𝑝𝑎𝑟𝑎𝑚𝑒𝑡𝑒𝑟

𝑒2 = 2𝑓 − 𝑓2

Geodetic Latitude vs Geocentric Latitude

𝑒 , =𝑎2 − 𝑏2

𝑏2



KinematicsTransformations ECEF-NEDHeight Datums

1. Mean Sea Level (MSL) Datum (height relative to geoid)2. GPS height (height above WGS-84 ellipsoid)

ℎ

Earth’s surface

Geoid

WGS-84 Ellipsoid

𝐻

𝑀

𝜀𝑑

ℎ − 𝑒𝑙𝑙𝑖𝑝𝑠𝑜𝑖𝑑𝑎𝑙 ℎ𝑒𝑖𝑔ℎ𝑡 𝑔𝑒𝑜𝑑𝑒𝑡𝑖𝑐

𝐻 − 𝑜𝑟𝑡ℎ𝑜𝑚𝑒𝑡𝑟𝑖𝑐 ℎ𝑒𝑖𝑔ℎ𝑡 𝑀𝑆𝐿

𝑀 − 𝑔𝑒𝑜𝑖𝑑 𝑠𝑒𝑝𝑎𝑟𝑎𝑡𝑖𝑜𝑛 𝑢𝑛𝑑𝑢𝑙𝑎𝑡𝑖𝑜𝑛 𝑀 ≤ 100𝑚

ℎ ≈ 𝐻 +𝑀

𝜀𝑑 − 𝑑𝑒𝑓𝑙𝑒𝑐𝑡𝑖𝑜𝑛 𝑜𝑓 𝑡ℎ𝑒 𝑣𝑒𝑟𝑡𝑖𝑐𝑎𝑙 ≈ 0



KinematicsTransformations ECEF-NEDECEF Coordinates from Lat-Lon

Lat-Lon → ECEF Coordinates

𝒑 Τ𝑏 𝑒𝒆 =

𝑥𝑒𝑦𝑒𝑧𝑒

=

𝑁 + ℎ cos𝜑 cos 𝜆

𝑁 + ℎ cos𝜑 sin 𝜆

𝑏2

𝑎2𝑁 + ℎ sin 𝜆

𝑁 =𝑎2

𝑎2cos2𝜑 + 𝑏2sin2𝜑𝑃𝑟𝑖𝑚𝑒 𝑉𝑒𝑟𝑡𝑖𝑐𝑎𝑙 𝑜𝑓 𝐶𝑢𝑟𝑣𝑎𝑡𝑢𝑟𝑒 (𝑚)

𝑁



KinematicsTransformations ECEF-NEDLat-Lon from ECEF Coordinates

ECEF Coordinates → Lat-Lon

𝜆 = tan−1𝑦𝑒𝑥𝑒

𝑝 = 𝑥𝑒2 + 𝑦𝑒

2

𝜃 = tan−1𝑧𝑒𝑎

𝑝𝑏

𝜑 = tan−1𝑧𝑒 + 𝑒 ,2𝑏sin3𝜃

𝑝 − 𝑒2𝑎cos3𝜃

ℎ =𝑝

cos𝜑− 𝑁

Modelling and Simulation of Marine Craft KINEMATICS

Documents

Transcript of Modelling and Simulation of Marine Craft KINEMATICS