Scribd
Upload
126 views2 pages

Appendix I: A. 2-DOF Barycenter Stabilization Model

This document provides mathematical models and block diagrams for modeling and controlling the 6 degrees of freedom motion of an underwater vehicle. It describes: 1) Developing a 2 DOF dynamic model using Lagrange's equations to model motion in the vehicle's body fixed frame. 2) Using integral state feedback control to augment the system model and place dominant closed-loop poles, with a block diagram of the controller. 3) Relating motion in the vehicle's body fixed frame to motion in an earth-fixed frame using rotation matrices, and developing the full 6 DOF equations of motion for a rigid body.

Uploaded by

nchubccl
Copyright
© Attribution Non-Commercial (BY-NC)
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
126 views2 pages

Appendix I: A. 2-DOF Barycenter Stabilization Model

This document provides mathematical models and block diagrams for modeling and controlling the 6 degrees of freedom motion of an underwater vehicle. It describes: 1) Developing a 2 DOF dynamic model using Lagrange's equations to model motion in the vehicle's body fixed frame. 2) Using integral state feedback control to augment the system model and place dominant closed-loop poles, with a block diagram of the controller. 3) Relating motion in the vehicle's body fixed frame to motion in an earth-fixed frame using rotation matrices, and developing the full 6 DOF equations of motion for a rigid body.

Uploaded by

nchubccl
Copyright
© Attribution Non-Commercial (BY-NC)
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 2

Appendix I

Figure 1. Block diagramof 6-DOF system.


A. 2-DOF barycenter Stabilization Model
The dynamic model of 2-DOF barycenter system can be
developed from Lagranges equation, which considers the total
energy of all masses in the equivalent model with reference to
fishs body fixed frame. Hence, the dynamic model of system
can be written in 2-nd matrix form as
11 12
21 22
B
B
J J C
J J C
u u u
| | |
t t u u
t t | |
( ( ( ( (
=
( ( ( ( (
(


(A1)
where:
2 2 2 2 2
11
. . . . .
x x x x y y p p b
J m l m h m h m h Bh = + + + +
2
12
. .sin( )
y y
J m h u =
2
21
. .sin( )
y y
J m h u =
2 2 2 2 2 2
22
1
{ . . . . . .
2
y y x x x x y y p p b
J m l m l m h m h m h Bh = + + + + +

2 2 2
. .cos(2 ) . .cos(2 ) . .cos(2 )
x x y y x x
m h m h m l u u u + +

2 2
. .cos(2 ) . .cos(2 ) . . .sin(2 )}
p p b x x x
m h Bh m h l u u | + +

2 2 2
( . . ).sin ( ).sin(2 )
p p b
C B m h Bh
u u
u | = + +
2
1
.cos( ).{4. . . .cos( ) 2.[ 2. .
2
x x x x x
C B m h l m l
| |
u | = + +

2 2 2 2
3. . 3. . ( . . ).cos(2 )].sin( )}
p p b p p b
m h Bh m h Bh | u + + + +

.{ . .cos( ).cos( ) . .cos( ).sin( )
x x
g m l myly
u
t u | | u = +
( . . ).cos( ).sin( )}
x x p p
m h m h | u +
.{ . .cos( ) . .cos( ).sin( )
y y x x
g m l m l
|
t | u | = +
( . . ).cos( ).sin( ) . .sin( ).sin( )}
x x p p x x
m h m l m l | u | u + +
. . .cos( ).cos( )
B b
gBh
u
t | u =
. . .cos( ).cos( )
B b
gBh
|
t u | =
B. 2-DOF barycenter Control
In this paper we utilize Integral State Feedback (ISF)
controller to achieve zero steady state error of tracking reference
angles. The ISF controller design is based on linearized state
space model, which can be written as
x

= Bu Ax + (A2)
y =Cx (A3)
We insert two integrators state z
1
and z
2
in the forward path
between the error comparator and the plant. Therefore, the
system equations can be augmented. Thus new matrix
expression is given by
2 2 2 2 2 2 2 2
2 2 2 2 2 2 2 2
0 0 1
0 0 2
ref
x x x x
x x x x ref
A B x x u
C I z z u
u

(
( ( ( ( ( (
= + + (
( ( ( ( ( (

(A4)

2 2 2 2
[ 0 ]
x x c
y C x = (A5)
The block diagram of the closed-loop sub control system is
shown in Fig. 2.
} }
ref ref
,| u
out out
,| u

z z u

x x

Figure 2. Block diagramof ISF controller for control 2-DOF barycenter system.
The system is controllable, in order to place dominant poles
to the closed-loop system, we consider characteristic equation
c c c
K B A sI + =0. Consequently, the K
c
is state feedback gain
and the control law is given as

c c
u K x = (A6)
where:
[ ]
c I
K K K =
[ ]
c
x x z =

C. Mathematical Model of 6-DOF Motion
The kinetic relation between the fishs body fixed frame and
the fish motion in earth-fixed frame is expressed by
| |
T
x y z q | u = (A7)
| |
T
v u w p q r u = (A8)
( ) J v q q = (A9)
( ) 0
( )
0 ( )
R
J
T
q
O
O (
=
(
O

(A10)
where:
( )
c c s c c s s s s c c s
R s c c c s s s c s s s c
s c s c c
u | u | | | u
u | | u | u |
u u | u |
+ + (
(
O = + +
(
(

.
1
( ) 0
0
s t c t
T c s
s c c c
| u | u
| |
| u | u
O
(
(
O =
(
(

.
While, c(.) =cos(.), s(.) =sin(.), and t(.) =tan(.). Note that
( ) T
O
O is undefined for 2 u t = . However, when the
perturbations are small, such that (|, u, ) may be treated as
small angles
*
, the ( ) T
O
O can be approximated by p | =

, q u =

,
and r = . As a result, the equation of motion of the rigid body is
given by
( ) ( )
RB RB H
v v v gq + = M C (A11)
where:
0 0 0 . 0
0 0 . 0 .
0 0 0 . 0
0 . 0 0
. 0 . 0 0
0 . 0 0
G
G G
G
RB
G x zx
G G y
G zx z
m mz
m mz mx
m mx
M
mz I I
mz mx I
mx I I
(
(

(
(
= (

(
(

(
(



41 51 61
42 52 62
43 53 63
41 42 43 54 64
51 52 53 54 65
61 62 63 64 65
0 0
0 0 0
0 0 0
u uu
RB
p
q
r rr
C C u c c c
c c c
c c c
C
c c c C c c
c c c c C c
c c c c c C C r
( +
(

(
(

(
=

(
(

(
(
+




41
. .
G
c mz r = ,
54
. .
zx zr
c I p I r =

42
. c mw = ,
61
.( . )
G
c m x r v = +

43
.( . )
G
c m z p v = ,
62
. c mu =

51
.( . )
G
c m x q w = ,
63
. .
G
c mx p =

52
.( . . )
G G
c m z r x p = + ,
63
. .
G
c mx p =

53
.( . )
G
c m z q u = + ,
65
. .
zx x
c I r I p =

| |
1 2
| 0 0 |
H H H F
F
| u
t t t ( = =




( ).sin( )
( ).cos( ).sin( )
( ).cos( ).cos( )
( )
. .cos( ).sin( )
. .sin( )
0
G
G
B W
B W
B W
g
z B
z B
u
u |
u |
q
u |
|
(
(

(
(
=
(
(
(
(
(


While, | |
G G G G
r x y z = is the vector from the body origin
to the center of gravity. For underwater vehicles, set
G
x =0 at
the origin and
G
y =0 provided port-starboard symmetry and. In
conclusion, the block diagram of 6-DOF motion in North-East-
Down frame (NED frame) and its control system is depicted in
Fig. 1.

*
Michael V. Cook, Flight Dynamics Principle, ELSEVIER, USA, 2007.

You might also like