Aim of this lecture:

• Define the problem.

• Design of control strategies.

• Underwater Vehicle Model.

• Kinematics Equations of Motion.
• Linearizing the Kinematic model by feedback linearization
• Feedforward control law

• Feedback control law

• Dynamics error equation
• Kinematics control law

• Home exercise

Contents
1 Trajectory Tracking Control 3

2 Underwater Vehicle Model 4

2.1 Reference Frames . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
2.2 Kinematic Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.3 Linearization of Kinematic Model . . . . . . . . . . . . . . . . . . 10

3 Kinematic Control Law 11

3.1 Feedfoward and Feedback Control Model . . . . . . . . . . . . . . 12
3.2 Dynamics Error Equation of Closed-Loop Kinematic Control Sys-
tem . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
3.3 Dynamic Control Law Reference Velocities to Track the Trajectory 14

4 Exercises 19
4.1 Exercise 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
4.2 Exercise 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
4.3 Exercise 3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

1
List of Figures
1 Space Trajectory . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2 Rigid body AUV with six degree of freedon . . . . . . . . . . . . 4
3 Referencial Frames . . . . . . . . . . . . . . . . . . . . . . . . . . 4
4 Position Vectors AUV . . . . . . . . . . . . . . . . . . . . . . . . 6
5 Fullyactuated AUV with Six Fixed Thrusters . . . . . . . . . . . 8
6 Underactuated AUV with Four Fixed Thrusters . . . . . . . . . . 9
7 Linearized System . . . . . . . . . . . . . . . . . . . . . . . . . . 10
8 Kinematics Position Control Loop Diagram . . . . . . . . . . . . 12

2
 Trajectory Tracking - AUV Kinematics Model

May 24, 2019

1 Trajectory Tracking Control

The aim is to introduce the concept of trajectory tracking control of an
underactuated Autonomous Underwater Vehicle (AUV).
The problem of Trajectory tracking control to be addressed here is the
design of control strategies for tracking a space trajectory.

1
z

−1

−2
2 2
1 1
0 0
−1 −1
y −2−2 x

Figure 1: Space Trajectory

The approach involves a cascade control strategy which brings to a

control law consisting of:
• a kinematic control law, derived from the vehicle kinematic model,
which gives the velocities that forces the AUV to track the reference
trajectory.
• a dynamic control law, derived from the vehicle nonlinear multivariable
dynamic model, which gives the torques that forces the system to track
the above velocity reference values given by the kinematic control
law.
To develop this topic we first need to model an AUV, which usually
involves the study divided into two parts, Kinematics and Kinetics.

3
2 Underwater Vehicle Model
The AUV is modeled as a rigid body with six degree of freedon since six
independent coordinates are necessary to determine his position and
orientation.

CG

Figure 2: Rigid body AUV with six degree of freedon

2.1 Reference Frames

In order to discuss the motion of the vehicle through the environment we
need first to define the reference frames.
To analyze the motion of the head of an AUV with 6 degree of freedom
(DOF), it is convenient to represent its coordinates into three frames:
• an inertial reference frame I xi − yi − zi , which is fixed on the surface
of Earth.
• a body-fixed reference frame B xb − yb − zb , which is fixed on the
center of mass CG of the AUV.
• a point-fixed reference frame E xe − ye − ze , which is fixed into the
head of the AUV.
These frames can be seen in Figure (3).

B E
CG xb xe
yb ye
zb ze

yi

I xi
zi

Figure 3: Referencial Frames

4
 Using the standard notation of ocean engineering, the general motion
of an AUV in 6 DOF can be described by the following vectors:

η1ICG = {x y z}T η2ICG = {φ θ ψ}T I

ηCG = {η1ICG η2ICG }T (1)

v1BCG = {u + uC v + vC w + wC }T v2BCG = {p q r}T B

vCG = {v1BCG v2BCG }T
(2)
η1IE = {xE yE zE }T v1BE = {uE vE wE }T (3)
v1BC = {uC vC wC }T (4)
r1BE = {x y z }T (5)
in which
I
• ηCG describes the linear and angular position vector of the CG (i.e.
orientation) of the vehicle with respect to the earth-fixed reference
frame.
B
• vCG describes the linear and angular velocities vector of the CG of
the vehicle with respect to the body-fixed reference frame.
• η1IE describes the linear position vector of point E of the vehicle with
respect to the earth-fixed reference frame.

• v1BE describes the linear velocities vector of point E of the vehicle

with respect to the body-fixed reference frame.
• v1BC describes the linear velocities vector of the current of the sea
with respect to the body-fixed reference frame.
• r1BE describes the linear position vector of point E of the vehicle with
respect to the body-fixed reference frame.

5
 B r1BE E
CG xb xe
yb ye
zb ze
I I
ηCG ηE
yi

I xi
zi

Figure 4: Position Vectors AUV

The transformation between the velocities of the body-fixed v1BCG and

v2BCG
and the earth-fixed η̇1ICG and η̇2ICG can be written as:

η̇1ICG = J1BI
CG
(η2ICG )v1BCG

η̇2ICG = J2BI
CG
(η2ICG )v2BCG
in which J1BI
CG
and J2BI
CG
are transformation matrices which are related
through the functions of the Euler angles: roll (φ), pitch (θ) and yaw (ψ),
given by:
 
cos ψcosθ −sinψcosθ + cosψsinθsinφ sinψsinφ + cosψsinθcosφ
J1BI
CG
=  sin ψcosθ cosψcosθ + sinψsinθsinφ −cosψsinφ + sinψsinθcosφ 
sin θ cosθsinφ cosθcosφ
 
1 sinθtanθ cosθtanθ
J2BI
CG
=  0 cosψ −sinφ 
0 sinφ/cosθ cosφ/cosθ
The relationship between linear velocity of the gravity center (CG)
v1BCG and the linear velocity of the head point (E) v1BE is given by:

v1BE = v1BCG + v2BCG × r1BE (6)

The relationship between angular velocity of the gravity center (CG)

v2BCG and the angular velocity of the head point (E) v2BE is the same given
by:
v2BE = v2BCG (7)

2.2 Kinematic Model

Kinematics is the branch of mechanics primarily concerned with describing
the motions of objects without any concern as to the forces and moments

6
which generate the said motion. Modeling of the kinematic states involves
the study of the geometrical aspects of motion.
In this section, the kinematic equations of motion of the head E o an
AUV moving in a 3D space are presented.
In order to obtain the kinematic control law to ensure the position desired
E of the AUV follows the trajectory, the following kinematic model is consid-
ered:
η̇1IE = J1BI
CG
(η2ICG )v1BE (8)
η̇2IE = J2BI
CG
(η2ICG )v2BE (9)
in which the velocity inputs to be used to control the AUV can be some, any
B
or all the CG velocities vCG = {u, v, w, p, q, r} and the controlled position
outputs can be again some, any or all of the absolute position vector of
I
the head point E, ηE = {xe , ye , ze , φ, θ, ψ}. Notice that the number of velocity
inputs must be the same as the number of the position outputs.
Equations (8) and (9) can be assembled together as follows:

η̇EI = JCG
BI I
(η2CG )vEB (10)

7
 Let’s define three vectors to represent variables to be used in the control:
• ha a vector with one (true) and zero (false) to represent the velocities
degree of freedom used to actuate in the center of mass CG.
• hu to be the negation of vector ha , in other words, to represent the ve-
locities degree of freedom not used to actuate (unactuated) in
the center of mass CG.
• he with one (true) and zero (false) to represent the position degrees of
freedom of the reference point E chosen to be controlled by the
torque imposed on the center of mass CG.
The idea behind these vector is to allow us to work with both, fully or
any kind of under actuated AUVs. Using these vectors is possible to select
appropriate lines and columns of a full matrix for the necessary calculations
to work in a reduced space.

For example, imagine that we have a fully actuated AUV with six thrusters,
two on the left and right pointing backwards (P1 ,P2 ), two on the top and
botton pointing backwards (P3 ,P4 ) and two on the left and right pointing
down (P5 ,P6 ) as shown on figure 5.

P3
P5
CGP2
P1
P6 P4

Figure 5: Fullyactuated AUV with Six Fixed Thrusters

The resulting vectors ha , hu and he of a full actuated system can be written

as follows:      

 1   
 0 
 
 1 

1 0 1 

 
 
 
 
 

 
 
 
 
 
 
1 0 1
    
ha = hu = he = (11)

 1   
 0 
 
 1 

 1 



 

 0 




 1 


    
 
1 0 1
    

8
 Imagine now that we have an underactuated AUV with only four thrusters,
two in the sides left and right pointing backwards (P1 ,P2 ) and two on the
sides top and botton pointing backwards (P3 ,P4 ) as shown on figure 6.

-ψ ψ
P3
CGP2
x -x x
P1
P4
-θ y θ
z

Figure 6: Underactuated AUV with Four Fixed Thrusters

With these thrusters it is possible to actuate torque in three degrees of free-

dom of the AUV x, θ, ψ, in which with all four side thrusters (P1 , P2 , P3 , P4 )
it is possible to rotate all of them in the same direction to go forward or
backward (x), or two side thrusters left and right (P1 , P2 ) rotating in op-
posite direction to spin around the z axis (ψ) in clockwise or anti-clockwise,
or two side thrusters top and botton (P3 , P4 ) rotating in opposite direc-
tion to spin around the y axis (θ) in clockwise or anti-clockwise.
With the same numbers of actuators but located in different positions
along the AUV, different DOFs can be controlled. With these previous
actuators configuration it is possible to control position x, y, z of the AUV.
The resulting vectors ha , hu and he that represents the variables to be used
in the control of an under actuated AUV system can be written as follows:
     

 1  
 0 
 
 1 
0 1 1 

 
 
 
 
 

 
 
 
 
 
 
0 1 1
    
ha = hu = he = (12)

 0  
 1 
 
 0 
1  0  0 

  
  
 

 
 
 
 
 
 
1 0 0
    

Therefore, the resulting vectors ha , hu and he will be used to demonstrate

the equations that represents the variables to be used in the control of an under
actuated AUV system.

9
2.3 Linearization of Kinematic Model
Looking at the kinematic model, equation (10)

η̇EI = JCG (η2CG )vEB ,

BI I

where:
 cos ψcosθ −sinψcosθ + cosψsinθsinφ sinψsinφ + cosψsinθcosφ 0 0 0 
 sin ψcosθ cosψcosθ + sinψsinθsinφ −cosψsinφ + sinψsinθcosφ 0 0 0 
sin θ cosθsinφ cosθcosφ 0 0 0
 
J BI = 
 
0 0 0 1 sinθtanθ cosθtanθ

CG  
 0 0 0 0 cosψ −sinφ 
0 0 0 0 sinφ/cosθ cosφ/cosθ

it is possible to notice that the equation is non linear only on the input.
Therefore to apply the linear control technique it is necessary to linearize
the equation. Applying the technique of Feedback Linearization, adopting
the right hand side of the equation as the control law force u to be added
to the system we obtain
BI I
u = JCG (η2CG )vEB
Isolating now the input vEB ,
−1
vEB = JCG
BI
(η2ICG )u (13)

we obtain the appropriate velocity vEB that will depend of θ and linearizes
the nonlinear system equation (10).
Substituting the input equation (13) into the nonlinear differential equa-
tion (10)

η̇EI = JCG
BI I
(η2CG )vEB

results:
−1
η̇EI = JCG
BI I BI
(η2CG )(JCG (η2ICG )u (14)
: [I]

I BI I BI −1 
(η2ICG )u

η̇E = JCG (η
2 CG
)J
CG
(15)

η̇EI = u (16)

It is possible to see that the result is a linear system.

The block diagram of the result linear system can be seen as:

u −1
vEB ηEI
BI
JCG (η2ICG ) η̇EI = JCG
BI I
(η2CG )vEB

Figure 7: Linearized System

10
3 Kinematic Control Law
The design problem can be formulated as follows:

Given the desired bounded and continuous trajectory:

wC

1
I I
ηEd (t), η̇Ed (t),

z
0

−1

−2
2 2
1 1
0 0
−1 −1
y −2−2 x
vC uC

derive a control law vEB that forces output ηEI (t) of the model

η̇EI = JCG
BI I
(η2CG )vEB
I I
to track asymptotically the desired trajectory ηEd (t),η̇Ed (t), despite of the drift
terms that might have.
Let’s consider that we want to start from a position pc, follow a spiral tra-
jectory of radius r with a angular velocity wmin looking at a fixed center point
of the spiral pa. The reference trajectory components can be given as:
I
ηEd (t) = {xr, yr, zr, phir, thetar, psir}
I
η̇Ed (t) = {dxr, dyr, dzr, dphir, dthetar, dpsir}
and can be written in MatLab format as follows:
wmin = 0.1;
pa = [0;0;0]; %center of observation point
pc = [0;0;-3]; %center of the begining of the trajectory
r = 2; ang=pi/2;
% Spiral reference trajectory controlling all dofs x,y,z,ox,oy,oz
% looking to the center of point pa
xr = (pc(1) + r*sin(wmin*t)).*(t>=0);
yr = (pc(2) - r*cos(wmin*t)).*(t>=0);
zr = (pc(3) + 0.25*wmin*t).*(t>=0);
phir = atan2(sqrt((xr-pa(1)).^2+(yr-pa(2)).^2),zr-pa(3)).*(t>=0);
thetar = 0.*(t>=0);
psir = wmin*t.*(t>=0);
dxr = r*cos(wmin*t).*(t>=0);
dyr = r*sin(wmin*t).*(t>=0);
dzr = 0.25*wmin.*(t>=0);
dphir = (1./(1+(phir).^2)).*(t>=0);
dthetar = 0.*(t>=0);
dpsir=wmin;

11
3.1 Feedfoward and Feedback Control Model
The linearized transfer function P (s) relating the output ηEI with the input
u calculated from the time domain equation,

η̇EI = u

can be written in Laplace domain as:

1
P (s) = (17)
s
The inverse model Q(s) to be used in the Feedforward control can be
obtained as:
Q(s) = s (18)
Applying a Proportional Feedback together with a Feedforward con-
troller, the control loop strategy desired to achieve the solution of the problem
formulated can be drawn in the following closed loop block diagram:
I
η̇Ed (t)
d
dt

uf f
I
ηEd (t) ep uf b + u −1
vEB ηEI (t)
+ Kp + BI
JCG (η2ICG ) η̇EI = JCG
BI I
(η2CG )vEB
−

Figure 8: Kinematics Position Control Loop Diagram

12
3.2 Dynamics Error Equation of Closed-Loop Kinematic
Control System
From Figure 8
I
η̇Ed (t)
d
dt

uf f
I
ηEd (t) ep uf b + u −1
vEB ηEI (t)
+ Kp + BI
JCG (η2ICG ) η̇EI = JCG (η2CG )vEB
BI I
−

the feedback proportional control law equation is defined as follows:

uf b = Kp ep (t) (19)
Also from Figure 8 the feedforward control law equation is defined as
follows:
I
uf f = η̇Ed (t) (20)
The same way from Figure 8, the resulted velocity vEB necessary to follow
the trajectory is written as follows:
−1
vEB = JCG
BI
(η2ICG )(Kp ep (t) + η̇Ed
I
(t)) (21)
Finally from Figure 8 the position error is also defined as follows:
I
ep = ηEd − ηEI (22)
Substituting equation (21) into equation (10),
η̇EI = JCG
BI I
(η2CG )vEB
results:
 : [I]
BI I BI −1
 (23)
η̇EI = JCG (η2ICG )(Kp ep (t) + η̇Ed
I

(η2
CG
)J
CG
(t))

Deriving equation (22) results:
I
ėp = η̇Ed − η̇EI (24)
I
Isolating η̇Ed from equation (24) and substituting into equation (23) results:
ėp = −Kp ep (25)
Equation (25) can be rewritten as the closed-loop dynamics error equa-
tion as follows:
ėp + λep = 0 (26)
in which λ is a diagonal gain matrix.
The solution of equation (26) shows that the error converges expo-
nentially to zero. Now, the steady state error position condition ep = 0
only implies that the head of the vehicle tracks the reference trajectory
together maybe with others variables if they were also chosen to control.

13
3.3 Dynamic Control Law Reference Velocities to Track
the Trajectory
B
Let’s define velocities VCG as the truth desired actuated velocities on
adt
the CG which satisfies the dynamics error equation (26), which also are
B
the components of the velocity VCG of the AUV.
B
To obtain these velocities VCG , let’s start substituting equation (22)
adt

I
ep = ηEd − ηEI

and its derivative

I
ėp = η̇Ed − η̇EI
into the dynamics error equation (26)

ėp + λep = 0

which results:
I
(η̇Ed − η̇EI ) + λ(ηEd
I
− ηEI ) = 0 (27)
I
Isolating η̇E from equation (27) results the closed-loop kinematic equa-
tion:

η̇EI = η̇Ed
I I
+ λ(ηEd − ηEI ) (28)
Substituting equation (10)

η̇EI = JCG
BI I
(η2CG )vEB

into the closed-loop kinematic equation (28) results:

BI I
JCG (η2CG )vEB = η̇Ed
I I
+ λ(ηEd − ηEI ) (29)

Isolating the velocity vEB from equation (29) results:

−
vEB = JCG
BI 1
(η2ICG )(η̇Ed
I I
+ λ(ηEd − ηEI )) (30)

Using the velocity equations of two points fixed on a rigid body (6)

v1BE = v1BCG + v2BCG × r1BE

and (7)
v2BE = v2BCG ,
the velocity vector vEB can be written as:
 B   B
v1CG + v2BCG × r1BE

v1E
vEB = = (31)
v2BE v2BCG

14
 Using the angular velocities equation (2),

v2BCG = {p q r}T

and the head position equation (5),

r1BE = {x y z }T

the calculus of the vectorial product v2BCG × r1BE of equation (31) can be written
as:
 
 p q r p q 
v2BCG × r1BE =   = (qz − ry )~i + (rx − pz )~j + (py − qx )~k
 
x y z x y
~k

 ~i ~j ~i ~j 
(32)
Expanding the terms of equation (31)
 B
v1CG + v2BCG × r1BE

vEB =
v2BCG

by using equation (2),

v1BCG = {u + uC v + vC w + wC }T v2BCG = {p q r}T

and equation (32)

v2BCG × r1BE = (qz − ry )~i + (rx − pz )~j + (py − qx )~k

the velocity vEB can be rewritten as:

     

 u + uC 
 
 qz − ry 
 
 u + qz − ry + uC 

v + vC rx − pz v + rx − pz + vC

 
 
 
 
 


 
 
 
 
 

w + wC py − qx w + py − qx + wC
     
vEB = + = (33)

 p 
 
 0 
 
 p 

q 0 q

 
 
 
 
 


 
    
   
 
r 0 r
   

Taking into account the degrees of freedom he choosen to be controled from

equation (11),  

 1 

1 

 

 

1
 
he =

 0 

0 

 

 

0
 

15
the velocity necessary vEBn to track the choosen trajectory can be rewritten as
only:
  
 1   u + qz − ry + uC 
  
 1  v + rx − pz + vC 
  
  u + qz − ry + uC 
  

1   w + py − qx + wC 
(34)
B

vEn =   = v + rx − pz + vC
0  p
w + py − qx + wC
  
  
0  q
  

  

0  r
 

Now, taking into account the actuated degrees of freedom ha from equation
(11),  
 1 
 
 0 

 

 
0
 
ha =

 0 

1 

 

 
 
1


the velocity necessary vEBn can be rewritten as:

     
1 z −y  u + uC  0 0 0  v + vC 
vEBn =  0 0 +x  q +  1 0 −z  w + wC (35)
0 −x 0 r 0 1 y p
   
| {z }| {z } | {z }| {z }
Sh VB Shn VB
CGadt CGuat
| {z }
B
Vdrif t

where:

1 00 0 1 1 0 1
1 1 0 0
  z }| {   z }| {

 1 
 1 0 0 0 z −y 
 1 
 1 0 0 0 z −y

 1 
 0 1 0 −z 0 +x 


 1 
 0 1 0 −z 0 +x 

 1  0 0 1 +y −x 0   1  0 0 1 +y −x 0 
Sh =    Shn =   
 0 
 0 0 0 1 0 0 
  0 
 0 0 0 1 0 0 

 0 
 0 0 0 0 1 0  
 0 
 0 0 0 0 1 0 
 0  0 0 0 0 0 1  0  0 0 0 0 0 1

       

 1   u + uC
 
 
 uat 


 0   u + uC
 

0  v + vC vat 1  v + vC
 
 
 
   

  
 
 
   

0   w + wC wat 1   w + wC
     
B B
VCG =   = VCG =  
adt
 0 
 p 
  pat
 

uat
 1 
 p 

1  q  qat 0 q
  
    
   


 

 

     
1  r rat 0 r
     

16
 therefore, vEBn equation (34) can be written in a simplified way as:

B
vEBn = Sh VCG B
+ Vdrif t (36)
adt

Substituting previous equation (36) into the closed-loop kinematic equa-

tion (30) below
−
vEBn = JCG
BI 1
(η2ICG )(η̇Ed
I I
+ λ(ηEd − ηEI ))

B
and isolating the truth desired actuated velocities VCG results the kine-
adt
B
matics CG control velocities VCG that guarantees the necessary head
adt
velocities vEBn to achieve the position error ep null:
−
B
VCG = Sh−1 (JCG
BI 1
(η2ICG )(η̇Ed
I I
+ λ(ηEd − ηEI )) − Vdrif
B
t) (37)
adt

It is important to notice that matrix Sh must be invertible, which means

that if you end up with a matrix Sh non invertible, the underactuated
AUV degrees of freedom you are trying to control is not realizable with
the degrees of freedom chosen as actuated.
B
As the truth desired actuated velocities VCG is computed with the
adt
B
velocity of the sea current v1C , if you are going to compute torques that
will generate the respectively truth desired actuated velocities, it is important
B
to define a new reference desired actuated velocity VCG for the AUV
adr
without the velocities of the current. Therefore, it is possible to write the
B
resulting reference desired actuated velocity vector VCG as follows:
adr

   
 uadt   uC 
B
VCG = qadt − 0 (38)
adr
radt 0
   
| {z } | {z }
VB B
Vcurr
CGadt

where:
  
1 uC
  
 1  uat 



 

0 vC
    
0  vat
  
   
    

0 wC
    
0  wat
 
B
Vcurr = 
B

VCG =  
0

0
adt 0  pat  

 

1 0
    
1  qat
  
   
    

1 0
    
1  rat
  

B
The resultant vector VCG is the reference desired actuated velocity to
adr
be used in the Dynamic control law (next class) to calculate the necessary
torque of the AUV to track the desired trajectory in presence of sea current.

17
 In MatLab, the previous calculation can be done in the following form:

ha = [1 0 0 0 1 1]; % entries used in torque input %ha = [1 0 0 0 1 1];

hu = 1 - h; % entries not used in torque input %hu = [0 1 1 1 0 0];
he = [1 1 1 0 0 0]; % entries used in desired positions %he = [1 1 1 0 0 0];
nvara = find(ha); % find indices of nonzero elements nvara=[1 5 6]
nvare = find(he); % find indices of nonzero elements nvare=[1 2 3]
nvarn = find(hn); % find indices of nonzero elements nvarn=[2 3 4]
S = [1 0 0 0 epsilonz -epsilony
0 1 0 -epsilonz 0 epsilonx %Sh=[1 epsilonz -epsilony
0 0 1 epsilony -epsilonx 0 % 0 0 epsilonx
0 0 0 1 0 0 % 0 -epsilonx 0 ]
0 0 0 0 1 0
0 0 0 0 0 1];
Sh = S(nvare,nvara); %space reduction
Shu = S(nvare,nvaru); %space reduction
eta1ed = [xr;yr;zr;phir;thetar;psir];
eta1ed = eta1ed(nvare); %space reduction eta1ed = [xr;yr;zr];
deta1ed = [dxr;dyr;dzr;dphir;dthetar;dpsir];
deta1ed = deta1ed(nvare); %space reduction deta1ed = [dxr;dyr;dzr];
eta12e = [xe;ye;ze;phie;thetae;psie];
eta12e = eta12e(nvare); %space reduction eta12e = [xe;ye;ze];
J1 = [ cos(theta).*cos(psi),...
-cos(phi).*sin(psi)+sin(phi).*sin(theta).*cos(psi),...
sin(phi).*sin(psi)+cos(phi).*sin(theta).*cos(psi);...
cos(theta).*sin(psi),...
cos(phi).*cos(psi)+sin(phi).*sin(theta).*sin(psi),...
-sin(phi).*cos(psi)+cos(phi).*sin(theta).*sin(psi);...
-sin(theta),...
sin(phi).*cos(theta),...
cos(phi).*cos(theta)];
J2 = [1,sin(phi)*sin(theta)/cos(theta),cos(phi)*sin(theta)/cos(theta);
0,cos(phi),-sin(phi);
0,sin(phi)/cos(theta),cos(phi)/cos(theta)];
J = [J1 zeros(3);
zeros(3) J2];
J = J(nvare,nvare); %space reduction % J = J1
vdrift = hu’.*[u+uc;v+vc;w+wc;p;q;r]; % vdrift = [0;v+vc;w+wc;p;0;0];
vdrift = Shu*vdrift(nvare); %space reduction % vdrift = Shu*[0;v+vc;w+wc];
vadt = Sh\(J\(deta1ed + Lambda*(eta1ed-eta12e))-(vdrift)); % vad = [u+uc;q;r]=[uad,qad,rad]
vcurr = ha’.*[uc;vc;wc;0;0;0]; % vcurr = [uc;0;0;0;0;0];
vcurr = vcurr(nvar); %space reduction % vcurr = [uc;0;0];
vadr = vadt - vcurr; % vadr = [uad-uc;qad;rad]

18
4 Exercises
4.1 Exercise 1
Given the desired bounded and continuous spiral 3D trajectory:
wC

1
I I
ηEd (t), η̇Ed (t),

z
0

−1

−2
2 2
1 1
0 0
−1 −1
y −2−2 x
vC uC

Considering the kinematics model:

η̇EI = JCG
BI I
(η2CG )vEB

Using the attached program, simulate numerically a trajectory track con-

trol ηEI (t) to track a desired 3D space reference position trajectory ηEd
I
(t) by a
fullyactuated AUV,

P3
P5
CGP2
P1
P6 P4

starting from an arbitrary initial position, by calculating the truth desired

B
velocity VCG from the closed loop kinematics position diagram below that
adt
has both, a Proportional Feedback controller together with a Feedforward con-
troller.
I
η̇Ed (t)
d
dt

uf f
I
ηEd (t) ep uf b + u −1
vEB ηEI (t)
+ Kp + BI
JCG (η2ICG ) η̇EI = JCG
BI I
(η2CG )vEB
−

19
4.2 Exercise 2
Make the necessary modifications to the ref_traj(t) function to simulate a
trajectory track control ηEI (t) to track a desired 3D space reference position
I
trajectory ηEd (t) by a fullyactuated AUV always looking straight ahead.

4.3 Exercise 3
Make the necessary modifications to the code attached to simulate a trajectory
track control ηEI (t) to track a desired 3D space reference position trajectory
I
ηEd (t) subject to sea current v1BC by an underactuated AUV with only four
thrusters, two in the sides left and right pointing backwards (P1 ,P2 ) and
two on the sides top and botton pointing backwards (P3 ,P4 ) as shown
on figure below. With these thrusters you are going to actuate torque in three
degrees of freedom of the AUV x, θ, ψ and you want to control control position
x, y, z of the head E of the AUV.

-ψ ψ
P3
CGP2 E
x -x x
P1
P4
-θ y θ
z

Email exercises to:janito@fem.unicamp.br

20
 Full Actuated Base Program

% ======================================================================= %
% Kinematic AUV
% ======================================================================= %

clear all
close all
clc

% =============================Simulation Datas========================== %

fontlabel = 22; % size fonts label

fontlegend = 22; % size fonts label

epsilonx = 1/2;
epsilony = 0;
epsilonz = 0;
Lambdap = 10*eye(6); % proportional control gain of kinematic part
re = [epsilonx;epsilony;epsilonz]; % distance vector between G and E

% Datas
data = struct(’re’,re,’Lambdap’,Lambdap);

% ================================Simulation============================= %

% Initial conditions
x0 =-epsilonx;
y0 = 0;
z0 = 0;
phi0 = 0;
theta0 = 0;
psi0 = 0;
xe0 = 0;
ye0 = 0;
ze0 = 0;
s0 = [x0 y0 z0 phi0 theta0 psi0 xe0 ye0 ze0];

% Integration time interval

ti = 0; % initial time
tfn = 200; % final time
tspan = [ti tfn];

% Solver precision
options = odeset(’RelTol’,1e-3,’AbsTol’,1e-6);

21
% Integration
[t,s] = ode45(@(t,s) kinematic_auv(t,s,data),tspan,s0,options);

x = s(:,1);
y = s(:,2);
z = s(:,3);
phi = s(:,4);
theta = s(:,5);
psi = s(:,6);

xe = s(:,7);
ye = s(:,8);
ze = s(:,9);
pe = phi;
qe = theta;
re = psi;

[xr,yr,zr,pr,qr,rr,dxr,dyr,dzr,dpr,dqr,drr,ucI,vcI,wcI] = ref_traj(t);

% =============================Plots===================================== %
% To see the position and orientation of point E of the AUV

for k = 1:length(phi)

J1 = rotxyz(phi(k),theta(k),psi(k)); %transform from Body frame to Inertial frame

posxyzp1 = J1*[1;0;0];
posxyzp2 = J1*[0;1;0];
posxyzp3 = J1*[0;0;1];

xyzp1(:,k) = [xe(k);ye(k);ze(k)] + posxyzp1;

xyzp2(:,k) = [xe(k);ye(k);ze(k)] + posxyzp2;
xyzp3(:,k) = [xe(k);ye(k);ze(k)] + posxyzp3;

end

% Reference trajectory
for i = 1:length(t)
[xr(i,1),yr(i,1),zr(i,1),phir(i,1),thetar(i,1),psir(i,1),dxr(i,1),dyr(i,1),dzr(i,1),...
dphir(i,1),dthetar(i,1),dpsir(i,1),ucg(i,1),vcg(i,1),wcg(i,1)] = ref_traj(t(i));
end

video_auv(xe,ye,ze,xyzp1,xyzp2,xyzp3,xr,yr,zr,epsilonx,t)

22
function dsdt = kinematic_auv(t,s,data)
% ----------------------------Diferential Equations------------------------
% Initialize state vector
dsdt = zeros(6,1);

% Define states
x = s(1);
y = s(2);
z = s(3);
phi = s(4);
theta = s(5);
psi = s(6);
xe = s(7);
ye = s(8);
ze = s(9);

% Parameters
epsilonx = data.re(1);
epsilony = data.re(2);
epsilonz = data.re(3);
Lambdap = data.Lambdap;

% ================================Parameters===============================
J1 = [ cos(theta)*cos(psi),...
-cos(phi)*sin(psi)+sin(phi)*sin(theta)*cos(psi),...
sin(phi)*sin(psi)+cos(phi)*sin(theta)*cos(psi);...
cos(theta)*sin(psi),...
cos(phi)*cos(psi)+sin(phi)*sin(theta)*sin(psi),...
-sin(phi)*cos(psi)+cos(phi)*sin(theta)*sin(psi);...
-sin(theta),...
sin(phi)*cos(theta),...
cos(phi)*cos(theta) ];

J2 = [1,sin(phi)*sin(theta)/cos(theta),...
cos(phi)*sin(theta)/cos(theta);...
0,cos(phi),-sin(phi);
0,sin(phi)/cos(theta),cos(phi)/cos(theta)];

% ==============================Controller=================================
S = [1 0 0 0 epsilonz -epsilony
0 1 0 -epsilonz 0 epsilonx
0 0 1 epsilony -epsilonx 0
0 0 0 1 0 0
0 0 0 0 1 0
0 0 0 0 0 1];

23
[xr,yr,zr,pr,qr,rr,dxr,dyr,dzr,dpr,dqr,drr,ucI,vcI,wcI] = ref_traj(t);

eta1ed = [xr;yr;zr;pr;qr;rr];
deta1ed = [dxr;dyr;dzr;dpr;dqr;drr];

pe = phi;
qe = theta;
re = psi;
eta12e = [xe;ye;ze;pe;qe;re];

J = [J1 zeros(3);
zeros(3) J2];

vadt = S\(J’*(deta1ed + Lambdap*(eta1ed-eta12e)));

p = vadt(4);
q = vadt(5);
r = vadt(6);

ve = S*vadt - [q*epsilonz-r*epsilony;r*epsilonx-p*epsilonz;...
p*epsilony-q*epsilonx;0;0;0];

u = ve(1);
v = ve(2);
w = ve(3);
p = ve(4);
q = ve(5);
r = ve(6);

% ==============================Dynamic model==============================

dsdt(1) = J1(1,:)*[u;v;w];
dsdt(2) = J1(2,:)*[u;v;w];
dsdt(3) = J1(3,:)*[u;v;w];
dsdt(4) = J2(1,:)*[p;q;r];
dsdt(5) = J2(2,:)*[p;q;r];
dsdt(6) = J2(3,:)*[p;q;r];

dsdt(7) = J1(1,:)*([u;v;w] + [q*epsilonz-r*epsilony;r*epsilonx-p*epsilonz;...

p*epsilony-q*epsilonx]);
dsdt(8) = J1(2,:)*([u;v;w] + [q*epsilonz-r*epsilony;r*epsilonx-p*epsilonz;...
p*epsilony-q*epsilonx]);
dsdt(9) = J1(3,:)*([u;v;w] + [q*epsilonz-r*epsilony;r*epsilonx-p*epsilonz;...
p*epsilony-q*epsilonx]);

return

24
%function ref_traj(t)
function [xr,yr,zr,phir,thetar,psir,dxr,dyr,dzr,...
dphir,dthetar,dpsir,ucI,vcI,wcI] = ref_traj(t)

wmin = 0.1;

pa = [0;0;0]; %center of observation point

pc = [0;0;-3]; %center of the begining of the trajectory

r = 2;
ang=pi/2;

% Reference trajectory spiral controlling x,y,z,ox,oy,oz looking to the center of point pa.
xr = (pc(1) + r*sin(wmin*t)).*(t>=0);
yr = (pc(2) - r*cos(wmin*t)).*(t>=0);
zr = (pc(3) + 0.25*wmin*t).*(t>=0);
phir = atan2(sqrt((xr-pa(1)).^2+(yr-pa(2)).^2),zr-pa(3)).*(t>=0);
% phir = atan2(sqrt((xr-pa(1))^2+(yr-pa(2))^2),pa(3)).*(t>=0);
thetar = 0.*(t>=0);
psir = wmin*t.*(t>=0);

dxr = r*cos(wmin*t).*(t>=0);
dyr = r*sin(wmin*t).*(t>=0);
dzr = 0.25*wmin.*(t>=0);
dphir = (1./(1+(phir).^2)).*(t>=0);
dthetar = 0.*(t>=0);
dpsir=wmin;

% Maritime current in the inertial frame

ucI = 0.*(t>=0);
vcI = 0.*(t>=0);
wcI = 0.*(t>=0);

return

25
function J1 = rotxyz(phi,theta,psi);

J1 = [ cos(theta).*cos(psi),...
-cos(phi).*sin(psi)+sin(phi).*sin(theta).*cos(psi),...
sin(phi).*sin(psi)+cos(phi).*sin(theta).*cos(psi);...
cos(theta).*sin(psi),...
cos(phi).*cos(psi)+sin(phi).*sin(theta).*sin(psi),...
-sin(phi).*cos(psi)+cos(phi).*sin(theta).*sin(psi);...
-sin(theta),...
sin(phi).*cos(theta),...
cos(phi).*cos(theta)];

return

26
%function video_auv(xe,ye,ze,xr,yr,zr,x,y,z,phi,theta,psi,epsilon,t)
function [F] = video_auv(xe,ye,ze,xyzp1,xyzp2,xyzp3,xr,yr,zr,epsilon,t)
dt = t(2)-t(1);
fig = figure;
mov = VideoWriter(’movieauv.avi’);
open(mov);
plot3(xr,yr,zr,’r--’)
xlabel(’X (m)’);
ylabel(’Y (m)’);
zlabel(’Z (m)’);
grid on
hold on
axis([-3 3 -3 3 -3 3])
xl=abs(min(xr)*2)+abs(max(xr)*2);
yl=abs(min(yr)*2)+abs(max(yr)*2);
zl=abs(min(zr)*2)+abs(max(zr)*2);
% axis([-2 2 -2 2 0 3])
% axis([min(xr)*2 max(xr)*2 min(yr)*2 max(yr)*2 min(zr)*2 max(zr)*2])

k=0;
for i=1:10:size(xe,1)
k=k+1;
if i==1,
h1r = line([xe(i) xyzp1(1,i)],[ye(i) xyzp1(2,i)],[ze(i) xyzp1(3,i)],...
’Marker’,’.’,’LineStyle’,’-’,’Color’,’b’);
h2r = line([xe(i) xyzp2(1,i)],[ye(i) xyzp2(2,i)],[ze(i) xyzp2(3,i)],...
’Marker’,’.’,’LineStyle’,’-’,’Color’,’k’);
h3r = line([xe(i) xyzp3(1,i)],[ye(i) xyzp3(2,i)],[ze(i) xyzp3(3,i)],...
’Marker’,’.’,’LineStyle’,’-’,’Color’,’g’);
else
set(h1r,’XData’, [xe(i) xyzp1(1,i)],’YData’,[ye(i) xyzp1(2,i)],...
’ZData’,[ze(i) xyzp1(3,i)])
set(h2r,’XData’, [xe(i) xyzp2(1,i)],’YData’,[ye(i) xyzp2(2,i)],...
’ZData’,[ze(i) xyzp2(3,i)])
set(h3r,’XData’, [xe(i) xyzp3(1,i)],’YData’,[ye(i) xyzp3(2,i)],...
’ZData’,[ze(i) xyzp3(3,i)])
end
F(k) = getframe;
writeVideo(mov,F(k))
end
close(mov);
return

27