Submarine Equilibrium Manifold

A nonlinear system is usually described by a differential equation in the form x =

f(x, u), where x is the system state and u the control input. The qualitative study of
the dynamics of the nonlinear system begins with the exploration of its equilibrium
points. At this end, the set of points (xe , ue ) in the state-input space at which the
vector field vanishes (x = 0) is studied. Therefore we impose

0 = f(xe , ue ) (1)

Under suitable conditions on the function f, the equation (1) provides an implicit-
ly defined hypersurface or, better, a differentiable manifold that we refer to as the
equilibrium manifold.

Similar considerations hold when studying a mechanical system, with some im-
portant modifications. Indeed, imposing the time derivative of the state (i.e., the
velocities and accelerations) to be zero reduces to perform a static analysis of the
mechanical system. However, there are other important cases (e.g., think to a rigid
body rotating with uniform velocity about a fixed axis) for which we would like to
say that the mechanical system is in equilibrium, even if some of the velocities and
accelerations are not zero. A submarine going around a circle at constant speed or
in a level flight condition can be appropriate examples in the context of this dis-
sertation. Those steady motions are known in the mechanical literature as relative
equilibria. Mechanical engineers working in the automotive or aerospace fields also
refer to a relative equilibrium as a trim condition.

The understanding of the system dynamics begins with a study of the set of
constant operating conditions, or trim trajectories. For vehicles, trim trajectories
are constant input trajectories for which the linear and angular velocities, viewed in
the body frame, are constant. That is

u = v = w = p = q = r = 0 . (2)

For underwater vehicles, as for aircraft, trim trajectories can be parametrized by

the choice of velocity V , flight path angle , and lateral acceleration alat . Figure 1
shows the geometry of the variables V and , as well as the angle of attack and the
sideslip angle , which are also useful in describing the submarine dynamics below.

At this point, generally speaking, the trim problem can be written as

Given V, and alat

Solve x = f(x, u) for x and u (3)
Subject to (2) .

In order to solve the trim problem for the submarine, the first thing to do is to
define the system model. The submarine model consist in the set of equations of

1
 Figura 1: Definition of velocity related variables

motion, which can be written as a set of 12 ordinary differential equations (ODEs)

x = u cos cos + v sin sin cos v cos sin

+w sin sin + w cos sin cos (4)
y = u cos sin + v cos cos + v sin sin sin
+w cos sin sin w sin cos (5)
z = u sin + v cos sin + w cos cos (6)

= p + r cos tan + q sin tan (7)

= q cos r sin (8)
r cos + q sin
= (9)
cos

AXIAL FORCE EQUATION

(m Xu ) u + mzG q myG r = (10)
2 2
(Xvr + m) vr + (Xwq m) wq + (Xqq + mxG ) q + (Xrr + mxG ) r myG pq
+ (Xrp mzG ) pr + Xq|q| q|q| + Xuu u2 + Xvv v 2 + Xww w 2 + Xw|w|w|w|
+Xr r r2 u2 + Xs s s2 u2 + Xb b b2 u2 (W B) sin


+ KT 0 n2 D 4 + KT J (1 wf )nD 3 u + KT J 2 (1 wf )2 D 2 u2 (1 t)

LATERAL FORCE EQUATION

(m Yv ) v (Yp + mzG ) p + (mxG Yr ) r = (11)
2 2
(Ywp + m) wp + (Yr m) ur + myG r + myG p mzG qr + (Ypq mxG ) pq

+Yr|r| r|r| + Yp up + Y u2 + Yv uv + Yv|v|N v v 2 + w 2 + (Yr + Yr ) r u2
+YvwN vw + (W B) cos sin

2
 NORMAL FORCE EQUATION
(m Zw ) w (Zq + mxG ) q + myG p = (12)
(m + Zq ) uq + (Zvp m) vp + mzG p2 + mzG q 2 mxG rp myG rq + Zq|q| q|q|

+Zrr r 2 + Zvr vr + Z u2 + Zw uw + Zvv v 2 + Z|w|u|w| + Zww |w| v 2 + w 2

+Zb b u2 + (Zs + Zs ) s u2 ZwwN w v 2 + w 2 + (W B) cos cos

ROLLING MOMENT EQUATION

(Ix Kp ) p (Kr + Izx ) r Ixy q + myG w (Kv + mzG ) v = (13)
2 2
(Iy Iz + Kqr ) qr + Izx pq Iyz r + Iyz q Ixy pr + myG uq myG vp
+ (Kwp mzG ) wp + (mzG + Kr ) ur + Kp|p|p|p| + Kr|r| r|r| + Kp up + K u2 + Kv uv
+Kv|v| v|v| + Kr r u2 + (yG W yB B) cos cos (zG W zB B) cos sin


KQ0 n2 D 5 + KQJ (1 wf )nD 4 u + KQJ 2 (1 wf )2 D 3 u2

PITCHING MOMENT EQUATION

(Iy Mq ) q Ixy p Iyz r + mzG u (Mw + mxG ) w = (14)
2 2
(Mrp + Iz Ix ) rp + Ixy qr Izx p + Izx r Iyz qp + (mzG + Mvr ) vr
mzG wq + (Mq mxG ) uq + mxG vp + Mq|q| q|q| + Mrr r 2 + M u2

+Mw uw + MwwN w v 2 + w 2 + Mvv v 2 + Mvw vw + M|w| u|w| + Mww |w| v 2 + w 2
+Mb b u2 + (Ms + Ms ) s u2 (xG W xB B) cos cos (zG W zB B) sin

YAWING MOMENT EQUATION

(Iz Nr ) r Iyz q (Np + Izx ) p + (mxG Nv ) v myG u = (15)
2 2
(Ix Iy + Npq ) pq + Iyz rp Ixy q + Ixy p Izx rq + mxG wp + (Nr mxG ) ur

myG vr + myG wq + Nr|r| r|r| + Np up + N u2 + Nv uv + Nv|v|N v v 2 + w 2
+ (Nr + Nr ) r u2 + NvwN vw + (xG W xB B) cos sin + (yG W yB B) sin

Equations (4)-(9) describe the kinematics while equations (10)-(15) describe the
dynamics. As it can be seen in that set of equations, the presence of the variables
which describe the trim trajectories it is not found. Then, what one needs to do is to
rewrite the submarine model in terms of such variables (V, , alat ) and perhaps using
the angles (, ). In order to do that, for the linear velocity vector, from Figure (1)
we can easily write
u = V cos cos
v = V sin (16)
w = V sin cos ,

3
while for the angular velocity vector, from equations (7)-(9) we have

p = sin
q = cos + sin cos (17)
r = sin + cos cos

Differentiating respect to time and applying (17), we have

0 = sin cos
0 = cos sin + sin cos + cos cos sin sin (18)
0 = sin cos + cos cos sin cos cos sin

One of the solutions given by

= 0 = 0 = 0
= 0 = 0 = 0
= 0 = 0 = 0 t

0 , 0 and 0 are constants. Now, we need a relation between and (V, , alat ). In
order to do that, we can establish the relation between the linear velocity expressed
in the inertial frame (x, y, z) with that expressed in the velocity frame using V ,
and , as

x V
y = Rz ()Ry () 0 (19)
z 0

where

cos 0 sin
Ry () = 0 1 0 (20)
sin 0 cos

and

cos sin 0
Rz () = sin cos 0 (21)
0 0 1

Taking the time derivative of (19) we obtain the accelerations

x 0 1 0 0 0 1 V V
y = Rz () 1 0 0 + 0 0 0 Ry () 0 + Ry () 0 ,

z 0 0 0 1 0 0 0 0
(22)

4
or, equivalently,

atan x 0 V V
alat = RTz () y = 0 0 Ry () 0 + Ry () 0 (23)
avert z 0 0 0 0

where atan and alat are the tangential and lateral (or normal) accelerations, respec-
tively, to the projected two dimensional (horizontal plane) curve, and avert is the
vertical acceleration. The expression for the acceleration vector is

atan sin cos V sin + V cos
alat = V cos + V 0 = V cos (24)
avert cos sin V cos V sin

For the trim trajectory case, it is easy to show that = . In order to do that,
we equate the two ways of expressing the linear velocity vector in the inertial frame,
one of them coming from the velocity vector in body frame and the other coming
from velocity in the velocity frame

V u
Rz ()Ry () 0 = Rz ()Ry ()Rx () v
(25)
0 w

differentiating with respect to time we have

0 1 0 V 0 0 1 V V
Rz () 1 0 0 Ry () 0 + Rz ()Ry () 0 0 0 0 + Rz ()Ry () 0 =
0 0 0 0 1 0 0 0 0

0 1 0 u 0 0 1 u
Rz () 1 0 0 Ry ()Rx () v + Rz ()Ry () 0 0 0 Rx () v

0 0 0 w 1 0 0 w
(26)

0 0 0 u u
+Rz ()Ry ()Rx () 0 0 1 v + Rz ()Ry ()Rx () v

0 1 0 w w

because of the definition of the trim trajectories u = v = w = 0, V = = 0 and

from the solution of ((18)) = = 0, we have

0 1 0 V 0 1 0 u
1 0 0 Rz ()Ry () 0 = 1 0 0 Rz ()Ry ()Rx () v (27)

0 0 0 0 0 0 0 w

then = .

alat = V cos (28)

5
 Replacing the value for from (28) in the angular velocity vector we have
alat
p= sin
V cos
alat
q= sin cos (29)
V cos
alat
r= cos cos
V cos
It is very important to realize that in the submarine model the position vector
in the inertial frame (x, y, z) as well as the yaw angle do not play any roll in
the dynamics of the submarine, therefore one can consider a reduced model for the
submarine dynamics form by (7), (8) and (10)-(15). At this point, the only relation
missing is between with , , and . In order to find such expression, we equate
the two ways of expressing the linear velocity vector in the inertial frame, one of
them coming from the velocity vector in body frame and the other coming from
velocity in the velocity frame

V u
Rz ()Ry () 0 = Rz ()Ry ()Rx () v
(30)
0 w

Knowing that

u V
v = Ry ()Rz () 0 (31)
w 0

And premultiplying (30) by eT3 to avoid the rotation over the zaxis

eT3 Ry ()e1 = eT3 Ry ()Rx ()Ry ()Rz ()e1 (32)

with

1 0 0
Rx () = 0 cos() sin() (33)
0 sin() cos()

and Ry () and Rz () defined as in (20) and (21), we have

sin = sin cos cos + cos (sin sin + cos sin cos ) (34)
| {z } | {z }
w1 w3

(34) can be seen as the zcoordinate of rotating degrees the vector [w1 , 0, w3]T
over the yaxis. Figure (2) shows such rotation. From that Figure it is clear that

=+ (35)

6
 x
w1

sin

x

w3
z
z

Figura 2: ???

furthermore, that
 
1 w3
= tan (36)
w1
then
   
1 w3 1 sin sin + cos sin cos
= + tan = + tan . (37)
w1 cos cos
Finally, for the submarine case, the trim problem can be written as
Given V, , alat and b
Solve (7),(8), (10)-(15) for x and u (38)
Subject to (2) .
Next, we will present some interesting results from the solution of (38), which
also helps to analyze the system.

Simulations for the submarine trim trajectories

1. Finding the critical velocity for level flight. Level flight implies zero vertical
flight path angle and zero lateral acceleration, since we want the submarine
goes in straight line. In order to find the critical velocity, we search for the
equilibrium points in a velocity range from 0 10m/s, plotting in the same
figure the stern plane s and the pitch angle as functions of velocity V .
Results are shown in Figure 3.

7
 (deg), s (deg) vs. V (m/s)
10
pitch angle
8 stern plane

10
0 1 2 3 4 5 6 7 8 9 10

Figura 3: Critical speed for level flight

2. Looking for unstability.

The common sign of unstability is related with the singular values of the system
dynamic matrix A, that is, the linearized system poles. As it is well known,
if such poles have real part positive they are unstable and, furthermore, the
system is unstable. Considering the submarine system described as

x = f(x(t), u(t)), (39)

the linearization about a trajectory = (x(), u()) is given by

z(t) = fx (x(), u()) z(t) + fu (x(), u()) v(t). (40)

| {z } | {z }
A B

2.1 Turning
In Figure 4 we show the root locus for the linearized system when de-
scribing circle of different radius in a constant depth and velocity, which
means V constant, = 0 and 0 alat 0,05g.
The system becomes unstable for |alat | 0,0067g, while turning at V =
7m/s.
For the values of alat that make the system unstable is easy design a
constant state feedback matrix K which stabilizes the system and let it
describe the circle in a particular depth...
The type of trajectories we find with the stabilizing matrix K are showed
in Figure 5
Doing such kind of exploration around alat , we can also show the diameter
of the circle described, the motion needed in the rudder and in the stern

8
 Submarine Root Locus, V = 7 m/s, = 0, alat= 0.007g 0.075g, b = 0
0.15

0.1

0.05

0.05

0.1

0.15

0.2
0.6 0.5 0.4 0.3 0.2 0.1 0 0.1

Figura 4: Root Locus for turning

x(m) vs t(s), advance motion y(m) vs t(s), lateral motion x 10

6 z(m) vs t(s), depth motion (deg) vs t(s), roll angle
150 200 1.5 15.3419

180 15.3418
1
100 160
15.3418

140
0.5 15.3418
50 120
15.3418
100 0
15.3418
0 80
0.5 15.3418
60

15.3418
50 40
1
20 15.3418

100 0 1.5 15.3418

0 50 100 150 200 250 300 350 400 450 500 0 50 100 150 200 250 300 350 400 450 500 0 50 100 150 200 250 300 350 400 450 500 0 50 100 150 200 250 300 350 400 450 500
(deg) vs t(s), pitch angle (deg) vs t(s), yaw angle u(m/s) vs t(s), surge velocity v(m/s) vs t(s), sway velocity
5.1136 2500 6.9482 0.3941

5.1136 0.3941
6.9482
2000
5.1136 0.3941

6.9482
5.1136 0.3941
1500

5.1136 6.9482 0.3941

1000
5.1136 0.3941
6.9482

5.1136 0.3941
500
6.9482
5.1136 0.3941

5.1136 0 6.9482 0.3941

0 50 100 150 200 250 300 350 400 450 500 0 50 100 150 200 250 300 350 400 450 500 0 50 100 150 200 250 300 350 400 450 500 0 50 100 150 200 250 300 350 400 450 500
w(m/s) vs t(s), heave velocity p(m/s) vs t(s), roll rate q(m/s) vs t(s), pitch rate r(m/s) vs t(s), yaw rate
0.7529 0.3577 1.0576 3.855

3.855
0.3577
0.7529 1.0576
3.855
0.3577
0.7529 1.0576 3.855

0.3577
3.855
0.7529 1.0576

0.3577 3.855

0.7529 1.0576
3.855
0.3577

0.7529 1.0576 3.855

0.3577
3.855
0.7529 1.0576
0.3577
3.855

0.7529 0.3577 1.0576 3.855

0 50 100 150 200 250 300 350 400 450 500 0 50 100 150 200 250 300 350 400 450 500 0 50 100 150 200 250 300 350 400 450 500 0 50 100 150 200 250 300 350 400 450 500
r(deg) vs t(s), rudder 6 b(deg) vs t(s), sail plane s(deg) vs t(s), stern plane n(rps) vs t(s), propeller speed
x 10 2.154
9.7171 2 8.2619

2.154
1.5
8.2619
2.154
9.7171 1
8.2619
2.154
0.5
8.2619 2.154
9.7171 0
2.154
8.2619
0.5
2.154
8.2619
9.7171 1
2.154

8.2619
1.5 2.154

9.7171 2 2.154
0 50 100 150 200 250 300 350 400 450 500 0 50 100 150 200 250 300 350 400 450 500 0 50 100 150 200 250 300 350 400 450 500 0 50 100 150 200 250 300 350 400 450 500

Figura 5: Trim trajectories for a circular motion

9
 Rudder and Stern plane, V = 7 m/s, = 0, alat= 0.007g 0.075g, b = 0 Roll angle, V = 7 m/s, = 0, alat= 0.007g 0.075g, b = 0 Angle of attack, V = 7 m/s, = 0, alat= 0.007g 0.075g, b = 0
Rudder
20 16 10
Stern plane

15
14 8
10

5
12 6
0

5 10 4

10
8 2
15

20
6 0
25

30 4 2
0 0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08 0 0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08 0 0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08
Sideslip angle, V = 7 m/s, = 0, alat= 0.007g 0.075g, b = 0 Evolution circle diameter, V = 7 m/s, = 0, alat= 0.007g 0.075g, b = 0 Propeller speed, V = 7 m/s, = 0, alat= 0.007g 0.075g, b = 0
1 1500 2.5

2.4
1.5

2.3
1000
2
2.2

2.1
2.5
500
2

3
1.9

3.5 0 1.8
0 0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08 0 0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08 0 0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08

Figura 6: Some extra characteristics for the turning motion as functions of the lateral
acceleration.

angle and also the propeller revolutions; all of the as functions of such
acceleration, as can be seen in Figure 6.
2.1 Straight line motion

10