CH 2

1
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
BODY
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)

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:
q` Jqv
Mv` Cvv Dvv gq g
0

wind

wave
#
#

3
2.1 Reference Frames
ECI {i}: Earth centered inertial frame; non-accelerating frame (fixed in space) in which Newtons
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 Earths 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
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 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
wp
in calm water. The
vessel will roll and pitch about this point.
u u
1
n
n
1
u
2
n
n
2
u
3
n
n
3
u
n
u
1
n
, u
2
n
, u
3
n
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 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 0
6 rotation about the z-axis (yaw) N r
2.1 Reference frames and 6-DOF motions
x
b
y
b
z
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:
p
b/e
e

x
y
z

3
Longitude and
latitude
O
en

l
j
S
2
NED
position:
p
b/n
n

N
E
D

3
Attitude
(Euler angles)
O
nb

0
S
3
Body-fixed
linear
velocity
v
b/n
b

u
v
w

3
Body-fixed
angular
velocity

b/n
b

p
q
r

3
Body-fixed
force:
f
b
b
X
Y
Z

3
Body-fixed
moment
m
b
b

K
M
N

3
q
p
b/n
n
or p
b/n
e
)
O
nb
, v
v
b/n
b
b/n
b
,
f
b
b
m
b
b
#

7
2.2 Transformations between
BODY and NED
Special orthogonal group of order 3:
SO3 R|R
33
, R is orthogonal and det R =1
Orthogonal matrices of order 3:
O3 R|R
33
, RR
R I
RR
R I, det R 1
Rotation matrix:
Since R is orthogonal,
R
1
R
v
to
R
from
to
v
from
Example:

8
k a : Ska
Cross-product operator as matrix-vector multiplication:
Sk S
k
0 z
3
z
2
z
3
0 z
1
z
2
z
1
0
, k =
z
1
z
2
z
3
where is a skew-symmetric matrix S S

BODY and NED

9
BODY and NED
Eulers theorem on rotation:
R
11
1 cos [ z
1
2
cos [
R
22
1 cos [ z
2
2
cos [
R
33
1 cos [ z
3
2
cos [
R
12
1 cos [ z
1
z
2
z
3
sin[
R
21
1 cos [ z
2
z
1
z
3
sin[
R
23
1 cos [ z
2
z
3
z
1
sin[
R
32
1 cos [ z
3
z
2
z
1
sin[
R
31
1 cos [ z
3
z
1
z
2
sin[
R
13
1 cos [ z
1
z
3
z
2
sin[
R
z,[
= I
33
sin[ Sk 1 cos [ S
2
k
where
k z
1
, z
2
, z
3
, |k| 1
[
v
b/n
n
R
b
n
v
b/n
b
, R
b
n
: R
z,[
#

10
2.2.1 Euler Angle Transformation
Three principal rotations:

(2) Rotation over pitch
angle about .
Note that .
y
v =v
2
2 1
x
2
x
3
y
3
y
2
u
3
u
2
v
2
v
3
(1) Rotation over yaw

angle about .
Note that .
z
w =w
3
3 2
x
1
x
2
z
1
z
2
u
1
u
2
w
1
w
2
U
U
(3) Rotation over roll
angle about .
Note that
.
x
u =u
1
1 2
z =z
0 b
z
1
y
1
y =y
0 b
v=v
2
v
1
w=w
0
w
1
U
k 1, 0, 0
[
k 0, 1, 0
[ 0
k 0, 0, 1
[
R
x,

1 0 0
0 c s
0 s c
R
y,0

c0 0 s0
0 1 0
s0 0 c0
R
z,

c s 0
s c 0
0 0 1

11

Linear velocity transformation (zyx-convention):
Small angle approximation:
where
R
b
n
O
nb

cc0 sc cs0s ss ccs0
sc0 cc ss0s cs s0sc
s0 c0s c0c
R
b
n
oO
nb
I
33
SoO
nb

1 o o0
o 1 o
o0 o 1
R
b
n
O
nb
1
R
n
b
O
nb
R
x,
R
y,0
R
z,
R
b
n
O
nb
: R
z,
R
y,0
R
x,
p`
b/n
n
R
b
n
O
nb
v
b/n
b
#

12
NED positions (continuous time and discrete time):
Component form:
Euler
integration
p`
b/n
n
R
b
n
O
nb
v
b/n
b
#
N
ucos cos0 vcos sin0 sin sin cos

wsin sin cos cos sin0
usincos0 vcos cos sin sin0 sin
wsin0 sin cos cos sin
D
usin0 vcos0 sin wcos0 cos

#
#
#
p
b/n
n
k 1 p
b/n
n
k hR
b
n
O
nb
kv
b/n
b
k #

13
Angular velocity transformation (zyx-convention):
where
1. Singular point at 0 90
o
Small angle approximation:
Notice that:
T
1
O
nb

1 0 s0
0 c c0s
0 s c0c
T
O
nb

1 st0 ct0
0 c s
0 s/c0 c/c0
T
oO
nb

1 0 o0
0 1 o
0 o 1
T
1
O
nb
T
O
nb
O
`
nb
T
O
nb
b/n
b
#
b/n
b

0
0
R
x,
0
0
0
R
x,
R
y,0
0
0
: T
1
O
nb
O
`
nb
#

14
ODE for Euler angles: ODE for rotation matrix
Component form:
0
p qsintan0 r cos tan0
qcos r sin
q
sin
cos 0
r
cos
cos 0
, 0 90
o
#
#
#
+ algorithm for computation
of Euler angles
from the rotation matrix
where
Euler angle attitude representations:
R
b
n
O
nb
O
nb
, 0,
O
`
nb
T
O
nb
b/n
b
#
R
`
b
n
R
b
n
S
b/n
b
#
S
b/n
b

0 r q
r 0 p
q p 0
#

15
Summary: 6-DOF kinematic equations:
Component form:
3-parameter representation

with singularity at
0 90
o
N
ucoscos0 vcossin0sin sincos

wsinsin coscossin0
usincos0 vcoscos sinsin0sin
wsin0sincos cossin
D
usin0 vcos0sin wcos0cos

#
#
#
p qsintan0 rcostan0
0
qcos rsin
q
sin
cos0
r
cos
cos0
, 0 90
o
#
#
#
O
nb
, 0,
q Jqv
p`
b/n
n
O
`
nb

R
b
n
O
nb
0
33
0
33
T
O
nb
v
b/n
b
b/n
b
#

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 = p, c
, c
3
and p c = c
1
, c
2
, c
3
R
[,k
I
33
sin[ Sk 1 cos [ S
2
k
Unit quaternion (Euler parameter) rotation matrix (Chou 1992):
p cos
[
2
c c
1
, c
2
, c
3
ksin
[
2
q
p
c
1
c
2
c
3
cos
[
2
k sin
[
2
Q
R
b
n
q : R
p,c
I
33
2pSc 2S
2
c

17
Linear velocity transformation
where
R
b
n
q
1 2c
2
2
c
3
2
2c
1
c
2
c
3
p 2c
1
c
3
c
2
p
2c
1
c
2
c
3
p 1 2c
1
2
c
3
2
2c
2
c
3
c
1
p
2c
1
c
3
c
2
p 2c
2
c
3
c
1
p 1 2c
1
2
c
2
2
Component form (NED positions):

R
b
n
q
1
R
b
n
q
q 1
NB! must be integrated under
the constraint or
p
2
c
1
2
c
2
2
c
3
2
1
N
u1 2c
2
2
2c
3
2
2vc
1
c
2
c
3
p 2wc
1
c
3
c
2
p
2uc
1
c
2
c
3
p v1 2c
1
2
2c
3
2
2wc
2
c
3
c
1
p
D
2uc
1
c
3
c
2
p 2vc
2
c
3
c
1
p w1 2c
1
2
2c
2
2
#
#
#
p`
b/n
n
R
b
n
qv
b/n
b
#

18
Angular velocity transformation
T
q
q
1
2
c
1
c
2
c
3
p c
3
c
2
c
3
p c
1
c
2
c
1
p
, T
q
qT
q
q
1
4
I
33
where
p
1
2
c
1
p c
2
q c
3
r
c
1

1
2
pp c
3
q c
2
r
c
2

1
2
c
3
p pq c
1
r
c
3

1
2
c
2
p c
1
q pr
#
#
#
#
Component form:
NB! nonsingular to
the price of one more
parameter

Alternative representation (Kane 1983)
The equations are derived using

q` T
q
q
b/n
b
#
q`
p
c`

1
2
c
pI
33
Sc

b/n
b
#
R
`
b
n
R
b
n
S
b/n
b


19
4-parameter representation

Nonsingular but one more
ODE is needed
Summary: 6-DOF kinematic equations (7 ODEs):
Component form:
q = p, c
1
, c
2
, c
3
p
1
2
c
1
p c
2
q c
3
r
c
1

1
2
pp c
3
q c
2
r
c
2

1
2
c
3
p pq c
1
r
c
3

1
2
c
2
p c
1
q pr
#
#
#
#
N
u1 2c
2
2
2c
3
2
2vc
1
c
2
c
3
p 2wc
1
c
3
c
2
p
2uc
1
c
2
c
3
p v1 2c
1
2
2c
3
2
2wc
2
c
3
c
1
p
D
2uc
1
c
3
c
2
p 2vc
2
c
3
c
1
p w1 2c
1
2
2c
2
2
#
#
#
q Jqv
p`
b/n
n
q`

R
b
n
q 0
33
0
43
T
q
q
v
b/n
b
b/n
b
#

20
Discrete-time algorithm for unit quaternion normalization
q
q c
1
2
c
2
2
c
3
2
p
2
1
Algorithm ( Discrete - Time Normalization of the Unit Quaternions )
1 . k 0 . Compute initial values of q k 0 .
2 . For simplicity , Euler Integration implies that
q k 1 q k h T q
q k
b / n
b
k #
where h is the sampling time .
3 . Normalization
q k 1
q k 1
q q k 1 q

q k 1
q

k 1 q k 1
4 . Let k k 1 and return to Step 2 .
A continuous time algorithm for unit quaternion normalization can be implemented by noting that
d
dt
q

q 2 q

T
q
q
b / n
b
0 #

21
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 Simulink

this is avoided by introducing feedback:
q T
q
q
nb
b

,
2
1 q
qq
d
dt
q
q 2q
T
q
q
nb
b
,1 q
qq
q ,1 q
qq
q
0 if q is initialized as
a unit vector
, 0 (typically 100
x 1 q
q Change of coordinates (x=0 gives )

x ,x1 x x ,x
linearization about x=0 gives
q
q 1
d
dt
q
q 2q
T
q
q
b/n
b
0 #

22
2.2.3 Quaternions from Euler Angles
Ref. Shepperd (1978)
Algorithm ( Quaternions From Euler Angles )
1 . Given the Euler angles , 0 , and . Let the transformation matrix R
b
n
according to be written :
R
b
n
O :
R 1 1 R 1 2 R 1 3
R
2 1
R
2 2
R
2 3
R 3 1 R 3 2 R 3 3
2 . The trace of R
b
e
O is computed as : R
4 4
tr R
b
n
R
1 1
R
2 2
R
3 3
3 . Let 1

i

4 be the index corresponding to : R
ii
max R
1 1
, R
2 2
, R
3 3
, R
4 4

4 . Compute p
i
corresponding to Index i of Step 3 according to :
p i 1 2 R ii

R 4 4
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 4 p 1
R 3 2

R 2 3 p 2 p 3
R 3 2
R 2 3
p 4 p 2
R 1 3

R 3 1 p 3 p 1
R 1 3
R 3 1
p 4 p 3
R 2 1

R 1 2 p 1 p 2
R 2 1
R 1 2
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 p , c
1 , c
2 , c
3

according to :
p p 4 / 2 and c
j
p j / 2 j 1 , 2 , 3

23
2.2.4 Euler Angles from Quaternions
Require that the rotation matrices of the two kinematic representations are equal:
q = p, c
1
, c
2
, c
3
cc0 sc cs0s ss ccs0

sc0 cc ss0s cs s0sc
s0 c0s c0c

R
11
R
12
R
13
R
21
R
22
R
23
R
31
R
32
R
33
Algorithm: One solution is:
atan2R
32
, R
33
0 sin
1
R
31
tan
1
R
31
1 R
31
2
; 0 90
o
atan2R
21
, R
11
#
#
#
where atan2(y,x) is the 4-quadrant arctangent of the real parts of the elements
of x and y, satisfying:
m atan2y, x m
R
b
n
O
nb
: R
b
n
q O
nb
, 0,

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 Earths 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 Earths surface

25
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 y-axis.
j m/2
O
en
l, j
S
2
R
n
e
O
en
R
z,l
R
y,j
m
2

cosl sinl 0
sinl cosl 0
0 0 1
cosj
m
2
0 sinj
m
2

0 1 0
sinj
m
2
0 cosj
m
2

R
n
e
O
en

cosl sinj sinl cosl cosj
sinl sinj cosl sinl cosj
cosj 0 sinj
p`
b/e
e
R
n
e
O
en
p`
b/e
n
R
n
e
O
en
R
b
n
O
nb
v
b/e
b
#
p`
b/n
n
R
b
n
O
nb
v
b/n
b
#

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:
O
en
l, j
S
2
O
en
constant
R
n
e
O
en
R
o
constant
R
n
e
O
en

cosl sinj sinl cosl cosj
sinl sinj cosl sinl cosj
cosj 0 sinj
p`
b/e
e
R
n
e
O
en
p`
b/e
n
R
n
e
O
en
R
b
n
O
nb
v
b/e
b
#

27
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
O
en
l, j
p
b/e
e
x, y, z
p
b/e
e
x, y, z
p
b/e
e
x, y, z

28
N
r
e
2
r
e
2
cos
2
jr
p
2
sin
2
j
Coordinates
l atan
y
e
x
e

tanj
z
e
p
1 e
2 N
N h
1
h
p
cos j
N
#
#
while latitude and height h are implicitly
computed by:
p
b/e
e
x, y, z
Parameters Comments
r
e
6 378 137 m Equatorial radius of ellipsoid (semimajor axis)
r
p
6 356 752 m Polar axis radius of ellipsoid (semiminor axis)
o
e
7. 292115 10
5
rad/s Angular velocity of the Earth
e 0. 0818 Eccentricity of ellipsoid
e 1
r
p
r
e

2

29
Coordinates
Algorithm ( Transformation of x e , y e , z e
to l , j , h )
1 . Compute p x
e
2
y
e
2
.
2 . Compute the approximate value j 0 from :
tan j 0
z e
p
1

e
2

1
3 . Compute an approximate value N from :
N
r
e
2
r
e
2
cos
2
j 0 r
p
2
sin
2
j 0
4 . Compute the ellipsoidal height by :
h
p
cos j
0

N 0 .
5 . Compute an improved value for the latitude by :
tan j
z e
p
1

e
2
N 0
N 0 h
1
6 . Check for another iteration step : if | j

j 0 | < tol where tol is a small number , then the iteration is complete .
Otherwise set j 0 j and continue with Step 3 .

30
2.3.3 ECEF Coordinates from
Longitude/Latitude
Ref. Heiskanen (1967)
The transformation from for given heights h to is given by O
en
l, j
x
y
z

N h cosjcosl
N h cosjsinl
r
p
2
r
e
2
N h sinj
p
b/e
e
x, y, z

31
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.
x
b
-
x
stab
z
b
y
b
U
x
flow

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

33
_ [
[ arcsin
v
U
[ small
[
v
U
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
U u
2
v
2
#

34
R
y,o

coso 0 sino
0 1 0
sino 0 coso
, R
z,[
R
z,[

cos [ sin[ 0
sin[ cos [ 0
0 0 1
u
v
w
R
y,o
R
z,[
U
0
0
u Ucos ocos [
v Usin[
w Usinocos [
#
#
#
or
v
stab
R
y,o
v
b
v
flow
R
z,[
v
stab
#
#
x
b
-
x
stab
z
b
y
b
U
x
flow
2.4.2 Sideslip and Angle of Attack
U u
2
v
2
#

35
Extension to Ocean Currents

For a marine craft exposed to ocean currents, the concept of relative velocities is
introduced. The relative velocities are:
u
r
u u
c
v
r
v v
c
w
r
w w
c
#
#
#
U
r
u
r
2
v
r
2
w
r
2
#
u
r
U
r
coso
r
cos[
r
v
r
U
r
sin[
r
w
r
U
r
sino
r
cos[
r
#
#
#
o
r
tan
1
w
r
u
r
[
r
sin
1
v
r
U
r
#
#
Hence,

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

u Ucos ocos [
v Usin[
w Usinocos [
#
#
#
Linear approximation:
u U, v [U, w oU
BODY to FLOW axes:
v =u, v, w, p, q, r
v
flow
U, [, o, p, q, r
v
flow
TUv
TU diag1, 1/U, 1/U, 1, 1, 1
x
b
-
x
stab
z
b
y
b
U
x
flow
TUMTU
1
v`
flow
TUNTU
1
v
flow
TU #

CH 2

Hochgeladen von

Dokumentinformationen

Originalbeschreibung:

Originaltitel

Copyright

Verfügbare Formate

Dieses Dokument teilen

Dokument teilen oder einbetten

Freigabeoptionen

Stufen Sie dieses Dokument als nützlich ein?

Sind diese Inhalte unangemessen?

Copyright:

Verfügbare Formate

CH 2

Hochgeladen von

Copyright:

Verfügbare Formate

1

2.2 Transformations between

(1) Rotation over yaw

ucos cos0 vcos sin0 sin sin cos

usin0 vcos0 sin wcos0 cos

p qsintan0 r cos tan0

ucoscos0 vcossin0sin sincos

usin0 vcos0sin wcos0cos

Component form (NED positions):

q Change of coordinates (x=0 gives )

cc0 sc cs0s ss ccs0

2.3 Transformation between

Das könnte Ihnen auch gefallen