# Development of a Low-Level Control System for the ROV Visor3

Authors: Santiago Rúa; Rafael E. Vásquez
Journal: International Journal of Navigation and Observation
 (2016)

---

## Abstract

This paper addresses the development of the simulation of the low-level control system for the underwater remotely operated vehicle Visor3. The 6-DOF mathematical model of Visor3 is presented using two coordinated systems: Earth-fixed and body-fixed frames. The navigation, guidance, and control (NGC) structure is divided into three layers: the high level or the mission planner; the mid-level or the path planner; and the low level formed by the navigation and control systems. The nonlinear model-based observer is developed using the extended Kalman filter (EKF) which uses the linearization of the model to estimate the current state. The behavior of the observer is verified through simulations using Simulink®. An experiment was conducted with a trajectory that describes changes in thex and y and yaw components. To accomplish this task, two algorithms are compared: a multiloop PID and PID with gravity compensation. These controllers and the nonlinear observer are tested using the 6-DOF mathematical model of Visor3. The control and navigation systems are a fundamental part of the low-level control system that will allow Visor3’s operators to take advantage of more advanced vehicle’s capabilities during inspection tasks of port facilities, hydroelectric dams, and oceanographic research.


---

## Body

## 1. Introduction

Due to the growing interest around the world to perform offshore and underwater operations, several researchers have focused their interests on the construction of underwater vehicles that allow one to explore the ocean from a surface station. Underwater vehicles are used to perform different tasks such as observation, sampling, and surveillance. Regardless of whether they are operated by cable (ROVs) or are autonomous (AUVs), it is necessary to develop control strategies to achieve the desired vehicle movements [1, 2].Several control schemes are based on the mathematical model of the system. Hence, having accurate models for prediction and control is desirable; however, this is not a simple task due to the highly nonlinear behavior that appears with the fluid-vehicle interaction [3]. The navigation, guidance, and control (NGC) system for an underwater vehicle can have different degrees of sophistication, depending on the type of operation that is to be performed and the autonomy levels that need to be achieved [1, 2, 4, 5].The desired level of autonomy will determine what kinds of algorithms are necessary to control the variables of interest, which are normally given by the position, attitude (orientation), and velocity of the vehicle with respect to an inertial reference system located at the surface [6]. Figure 1 shows a three-level hierarchical NGC structure for an underwater vehicle; this kind of structure is useful to control and stabilize the vehicle [7].Figure 1
Control structure for an underwater vehicle.One of the main components in the control structure’s lower level is the navigation system. This system provides an estimate of the position, velocity, and attitude of the vehicle with respect to an inertial system located at the surface control station at the harbor, from measurements made with different sensors (IMU, magnetometer, depth, DVL, and SSBL, among others). Given the characteristics of water, the development of underwater localization systems is not trivial and presents a number of challenges [8, 9].The most common algorithm to achieve this task is the Kalman filter (KF). It is an estimator, statistically optimal with respect to a quadratic error function, which allows one to estimate the state of the vehicle [10]. Several Kalman-filter-based navigation systems have been developed for years, and all of them depend on the instrumentation used in the vehicle. Caccia et al. [11] developed a Kalman-filter-based acoustic navigation module to control the UUV Roby2. Caccia and Veruggio [12] used Kalman-filter techniques to estimate the state using different sampling rates of a depth-meter and altimeter. Drolet et al. [13] presented an integrated sensor fusion strategy using multiple Kalman filters allowing different combination of sensors. Blain et al. [14] developed and tested a Kalman filter to merge data from an acoustic positioning system, a bathymeter, and a DVL. Loebis et al. [15] implemented an intelligent navigation system, based on the integrated use of the global positioning system (GPS) and several inertial navigation system (INS) sensors for AUV. Kinsey et al. [16] presented a survey with advances in underwater vehicle navigation and identified future research challenges. Lee and Jun [17] presented a pseudo long baseline (LBL) navigation algorithm using the EKF. Watanabe et al. [18] proposed an accurate tracking method to estimate AUV position by using a super-short baseline (SSBL) from the ship. Geng and Sousa [19] presented a hybrid derivative-free extended Kalman filter, taking advantage of both the linear time propagation of the Kalman filter and nonlinear measurement propagation of the derivative-free extended Kalman filter.The control system is another component of the lower level of the control structure. It contains a set of algorithms that stabilize the state of the vehicle, so it can follow the commands generated in the path planning system. The control of an underwater vehicle is complex because there are highly nonlinear hydrodynamic effects resulting from the interaction with the environment that cannot be quantified [20]. Cohan [21] states that the development of control systems for ROVs is a current and promising topic for future developments; this can be verified with the number of papers that can be found in literature.Caccia and Veruggio [12] implemented and tested a guidance and control system for underwater vehicles using programmed controllers to regulate speed at the low level in a hierarchical three-level structure. Do et al. [22] developed a robust adaptive control strategy to ensure that a six-degree-of-freedom vehicle follows a prescribed path using four actuators. Van de Ven et al. [23] presented a qualitative assessment of the performance of control strategies using neural networks, indicating the advantages, disadvantages, and application recommendations. Hoang and Kreuzer [24] designed an adaptive PD controller for dynamic positioning of ROVs when the mission is executed in places near submerged structures and requires great execution precision. Bessa et al. [25] used sliding mode controllers, combined with fuzzy adaptive algorithms for controlling depth in ROVs. Alvarez et al. [26] developed a robust PID controller for controlling AUV used in oceanographic sampling work. Subudhi et al. [27] presented the design of a feedback controller for tracking paths in vertical planes. Ishaque et al. [28] presented a simplification of the conventional fuzzy controller for an underwater vehicle. Herman [29] presented a decoupled PD setpoint controller which is expressed in terms of quasi-velocities for underwater vehicles. Petrich and Stilwell [30] presented a robust control for an autonomous underwater vehicle that suppresses pitch and yaw coupling.Gutiérrez et al. [31] developed the underwater remotely operated vehicle Visor3 for surveillance and maintenance of ship hulls and underwater structures of Colombian port facilities and oceanographic research. Visor3 has a three-layer hardware architecture: instrumentation, communications, and control [32]. Although much work has been done in mechanics and electronics, a closed-loop control system was not developed for the ROV Visor3, so the capabilities of the vehicle are still completely dependent on the pilot skills.This work addresses the first approach to develop the low-level control system for the ROV Visor3 that will help operators to maneuver and determine the position and attitude of the vehicle under certain scenarios such as in ports or dams inspection tasks. The second section presents the mathematical model of the vehicle, the third section shows the development of the nonlinear model-based observer, and the fourth section presents the implemented control algorithms; then, the simulation results are shown, and some conclusions are presented.


## 2. Mathematical Model

To analyze the motion of Visor3 in a three-dimensional space, two coordinate frames are defined: an inertial Earth-fixed frame (this reference frame is considered as the North-East-Down (NED) frame, attached to a port facility), where the motion of the vehicle is described, and a body-fixed frame, which is conveniently fixed to the vehicle and moves with it, Figure2. The acceleration of the Earth due to rotation is neglected for this work. The position and orientation of the vehicle are described relative to the Earth-fixed frame as(1)
η
=
x
y
z
ϕ
θ
ψ
T
,where η
1
=
x
y
z
T is the position and η
2
=
ϕ
θ
ψ
T is the orientation. The linear and angular velocities of the vehicle, relative to the body-fixed frame, are given by(2)
ν
=
u
v
w
p
q
r
T
,where ν
1
=
u
v
w
T and ν
2
=
p
q
r
T are the linear and angular velocities, respectively.Figure 2
Coordinate frames and velocities (linear and angular components).The mathematical model that describes the 6-DOF differential nonlinear equation of motion for an underwater vehicle, stated in [6], is given by(3)
M
ν
˙
+
C
ν
ν
+
D
ν
ν
+
g
η
=
τ
,
η
˙
=
J
η
ν
,where M
∈
R
6
×
6 is the inertia matrix, which comprises the mass of the rigid body and the added mass, M
=
M
R
B
+
M
A; C
∈
R
6
×
6 is the Coriolis and centripetal matrix, which includes a term due to rigid body and a term due to the added mass, C
=
C
R
B
+
C
A; D
∈
R
6
×
6 is the damping matrix; g
∈
R
6
×
1 is the gravitational and moments vector; τ
∈
R
6
×
1 is the force vector; and J
∈
R
6
×
6 is the rotation matrix from the body-fixed frame to the Earth-fixed frame. For this work, it is assumed that the origin of the body-fixed frame is located at the same point of the center of gravity of the vehicle.Applying Newtonian laws of motion, the rigid body equation of motion for the vehicle is given by(4)
M
R
B
v
˙
+
C
R
B
v
v
=
τ
R
B
.In (4), the rigid body inertia matrix M
R
B can be expressed as(5)
M
R
B
=
m
I
3
×
3
-
m
S
r
G
m
S
r
G
I
0
,where m is the mass of the vehicle, I
3
×
3 is the identity matrix, I
0 is the inertia tensor with respect to the center of gravity, r
G is the gravity vector in the body-fixed frame, and S
(
·
) is a skew symmetric matrix. The centripetal and Coriolis matrix can be parametrized in the form of a skew symmetric matrix as follows:(6)
C
R
B
=
C
R
B
11
C
R
B
12
C
R
B
21
C
R
B
22
,where(7)
C
R
B
11
=
0
3
×
3
,
C
R
B
12
=
-
m
S
ν
1
-
m
S
ν
2
S
r
G
,
C
R
B
21
=
-
m
S
ν
1
+
m
S
ν
2
S
r
G
,
C
R
B
22
=
-
S
I
0
ν
2
.The added mass due to the inertia of the surrounding fluid is given by(8)
M
A
=
A
11
3
×
3
A
12
3
×
3
A
21
3
×
3
A
22
3
×
3
=
-
∂
τ
∂
u
˙
∂
τ
∂
v
˙
∂
τ
∂
w
˙
∂
τ
∂
p
˙
∂
τ
∂
q
˙
∂
τ
∂
r
˙
,where τ
=
X
Y
Z
K
M
N are the hydrodynamics added-mass forces and moments in each direction. Finding the 36 entries of M
A is a difficult task, but this can be simplified by using symmetry properties of the vehicle. For this work, Visor3 is assumed to be symmetric with respect to x
y, x
z, and y
z planes. Therefore, the added-mass matrix can be computed as(9)
M
A
=
-
diag
⁡
X
u
˙
,
Y
v
˙
,
Z
w
˙
,
K
p
˙
,
M
q
˙
,
N
r
˙
.The hydrodynamic centripetal and Coriolis matrix can also be parametrized as(10)
C
A
ν
=
0
3
×
3
-
S
A
11
ν
1
+
A
12
ν
2
-
S
A
11
ν
1
+
A
12
ν
2
-
S
A
21
ν
1
+
A
22
ν
2
.For underwater vehicles, damping is mainly caused by skin friction and vortex shedding. One approximation commonly used [6, 33, 34] is a term with linear and quadratic components, given by(11)
D
ν
=
-
diag
⁡
X
u
,
Y
v
,
Z
w
,
K
p
,
M
q
,
N
r
-
diag
⁡
X
u
u
u
,
Y
v
v
v
,
Z
w
w
w
,
K
p
p
p
,
M
q
q
q
,
N
r
r
r
.Restoring forces and moments, calculated from center of gravity, are given by(12)
g
η
=
W
-
B
sin
⁡
θ
-
W
-
B
cos
⁡
θ
sin
⁡
ϕ
-
W
-
B
cos
⁡
θ
cos
⁡
ϕ
y
b
B
cos
⁡
θ
cos
⁡
ϕ
-
z
b
B
cos
⁡
θ
sin
⁡
ϕ
-
z
b
B
sin
⁡
θ
-
x
b
B
cos
⁡
θ
cos
⁡
ϕ
x
b
B
cos
⁡
θ
sin
⁡
ϕ
+
y
b
B
sin
⁡
θ
,where W
=
m
g is the weight of the vehicle and B
=
ρ
g
∇ is the buoyancy, ρ is the density of the fluid, g is the acceleration of the gravity, and ∇ is the volume of fluid displaced by the vehicle.


### 2.1. Transformations

To obtain the linear velocity in the Earth-fixed frame from the linear velocity in the body-fixed frame, a linear transformation must be applied. The transformation matrix is given by(13)
J
1
η
2
=
c
ψ
c
θ
-
s
ψ
c
ϕ
+
c
ψ
s
θ
s
ϕ
s
ψ
s
ϕ
+
c
ψ
s
θ
c
ϕ
s
ψ
c
θ
c
ψ
c
ϕ
+
s
ψ
s
θ
s
ϕ
-
c
ψ
s
ϕ
+
s
ψ
s
θ
c
ϕ
-
s
θ
c
θ
s
ϕ
c
θ
c
ϕ
,where s
·
=
s
i
n
(
·
) and c
·
=
c
o
s
(
·
). The angular velocity transformation matrix is given by(14)
J
2
η
2
=
1
s
ϕ
t
θ
c
ϕ
t
θ
0
c
ϕ
-
s
ϕ
0
s
ϕ
c
θ
c
ϕ
c
θ
,where t
·
=
t
a
n
(
·
).Finally, the kinematic equations of the vehicle can be expressed as(15)
η
˙
1
η
˙
2
=
J
1
η
2
0
3
×
3
0
3
×
3
J
2
η
2
ν
1
ν
2
.


## 2.1. Transformations

To obtain the linear velocity in the Earth-fixed frame from the linear velocity in the body-fixed frame, a linear transformation must be applied. The transformation matrix is given by(13)
J
1
η
2
=
c
ψ
c
θ
-
s
ψ
c
ϕ
+
c
ψ
s
θ
s
ϕ
s
ψ
s
ϕ
+
c
ψ
s
θ
c
ϕ
s
ψ
c
θ
c
ψ
c
ϕ
+
s
ψ
s
θ
s
ϕ
-
c
ψ
s
ϕ
+
s
ψ
s
θ
c
ϕ
-
s
θ
c
θ
s
ϕ
c
θ
c
ϕ
,where s
·
=
s
i
n
(
·
) and c
·
=
c
o
s
(
·
). The angular velocity transformation matrix is given by(14)
J
2
η
2
=
1
s
ϕ
t
θ
c
ϕ
t
θ
0
c
ϕ
-
s
ϕ
0
s
ϕ
c
θ
c
ϕ
c
θ
,where t
·
=
t
a
n
(
·
).Finally, the kinematic equations of the vehicle can be expressed as(15)
η
˙
1
η
˙
2
=
J
1
η
2
0
3
×
3
0
3
×
3
J
2
η
2
ν
1
ν
2
.


## 3. Nonlinear Model-Based Observer

The development of the observer needs to account for the ROV high nonlinearities and coupled dynamics. Therefore, an extended 6-DOF Kalman filter (EKF) that considers Coriolis, damping, and restoring forces has been developed. Equations (3) can be written as a state-space realization, as follows:(16)
x
˙
=
f
x
,
t
+
B
k
t
+
m
t
,
(17)
z
=
h
x
,
t
+
n
t
,where f
(
x
,
t
) is a function of the state vector x
=
ν
η
T and is given by(18)
f
x
,
t
=
M
-
1
-
C
ν
ν
-
D
ν
ν
-
g
η
J
η
νand B is the input coupling matrix given by(19)
B
=
M
-
1
0
6
×
6
.In (16), k
(
t
) is the input vector given by thrusters’ forces; h
(
x
,
t
) is the measurement sensitivity matrix, which depends on the ROV sensors; and m
t and n
t are plant and measurement noises, respectively; they are assumed to be zero-mean Gaussian white noise processes with covariance matrix Q
(
t
) and R
(
t
) given by(20)
E
m
t
=
0
,
E
m
t
m
T
s
=
δ
t
-
s
Q
t
,
E
n
t
=
0
,
E
n
t
n
T
s
=
δ
t
-
s
R
t
.The continuous-time model in (16) and (17) is discretized using a 1st-order approximation Euler method as follows:(21)
x
k
=
g
k
-
1
x
k
-
1
+
Δ
k
k
-
1
+
m
k
-
1
,
z
k
=
h
k
x
k
+
n
k
,where(22)
g
k
-
1
x
k
-
1
=
x
k
-
1
+
h
f
x
k
-
1
,
t
k
-
1
,
Δ
=
h
B
,and h is the step time. The discretized system is given by(23)
ν
k
=
ν
k
-
1
+
h
M
-
1
τ
k
-
1
-
C
ν
k
-
1
ν
k
-
1
-
D
ν
k
-
1
ν
k
-
1
-
g
η
k
-
1
,
η
k
=
η
k
-
1
+
h
J
η
k
-
1
ν
k
.The objective of the EKF is to estimate the current state by using a linearized version of the system’s model. The EKF is executed in two steps: the predictor which calculates an approximation of the state and covariance matrix and the corrector which improves the initial approximation. The predictor equations for the EKF are(24)
x
^
k
-
=
g
k
-
1
x
^
k
-
1
+
+
Δ
k
k
-
1
,
z
^
k
=
h
k
x
^
k
-
,
P
k
-
=
Φ
k
-
1
P
k
-
1
+
Φ
T
k
-
1
+
Q
k
-
1
.
x
^
k
- is the a priori estimate of the state, x
^
k
+ is the a posteriori estimate of the state, z
^
k is the predicted measurement, P
k
- is the a priori covariance matrix, P
k
+ is the a posteriori covariance matrix, and Φ
k
-
1 is the state transition matrix defined by the following Jacobian matrix:(25)
Φ
k
-
1
≈
∂
g
k
∂
x
x
=
x
^
k
-
1
-
.The corrector equations for the EKF algorithm are(26)
x
^
k
+
=
x
^
k
-
+
K
¯
k
z
k
-
z
^
k
,
P
k
+
=
I
-
K
¯
k
H
k
P
k
-
,where K
¯
k is the Kalman-filter gain calculated as(27)
K
¯
k
=
P
k
-
H
T
k
H
k
P
k
-
H
T
k
+
R
k
-
1
.The observation matrix is defined by the following Jacobian matrix:(28)
H
k
≈
∂
h
k
∂
x
x
=
x
^
k
-
.It is important to state that the EKF is not an optimal filter, due to the linearization process of the system. Furthermore, the matrices Φ
k
-
1 and H
k depend on the previous state estimation and the measurement noise. Therefore, the EKF may diverge if consecutive linearizations are not a good approximation of the linear model in the whole domain. However, with good knowledge of the system’s dynamics, bias and noise compensation, and an appropriate configuration sampling time, this algorithm is good enough for application in control systems in marine vehicles.


## 4. Control Algorithms

The control of an underwater vehicle is complex because there are highly nonlinear hydrodynamic effects resulting from the interaction with the environment that cannot be quantified [20]. Additionally, disturbances from the environment may appear. Problems such as obtaining all the state variables for the vehicle can limit the design of such control algorithms.Currently, the ROV Visor3 is driven through a joystick that sends power commands to a main-board installed in the vehicle; this board translates commands into input signals for each motor. Figure3 shows the current open-loop control system implemented in Visor3.Figure 3
Open-loop control system implemented in Visor3.


### 4.1. Thruster Allocation

The task which consists in generating a particular command to be sent to each individual actuator according to the control law and thrusters configuration is called control allocation [33].The thrust vector that describes the forcef
i generated by the thruster i is given by(29)
f
=
f
1
f
2
⋯
f
r
T
,where r is the total number of thrusters. The total force and moment generated by the thrusters will be(30)
τ
=
T
f
,where T is the thruster configuration matrix, which is a function of the thrusters position r
t
i
/
b
b, as well as the azimuth and elevation angles, α and β, respectively. This vector is referenced to the body-fixed frame. The thruster configuration matrix describes how the thrust of each motor contributes to the force or moment in each direction. The matrix T is given by(31)
T
=
t
1
t
2
⋯
t
r
,where t
i is the column vector of the ith thruster and is computed as(32)
t
i
=
I
3
×
3
-
S
T
r
t
i
/
b
b
R
α
,
β
1
0
0
f
i
.The rotation matrix R
α
,
β is defined by the product of two rotation matrices:(33)
R
α
,
β
=
R
z
,
α
R
y
,
β
,where R
z
,
α and R
y
,
β are described by(34)
R
z
,
α
=
c
α
-
s
α
0
s
α
c
α
0
0
0
1
,
(35)
R
y
,
β
=
c
β
0
s
β
0
1
0
-
s
β
0
c
β
,respectively; Visor3 has fixed heading and pitch angles for each thruster.Now that the thrust provided by each thruster is known using (30), the input that must be sent to each motor has to be computed. The input could be the revolution speed of the motor or a voltage signal for the driver. Equation (30) is then rewritten as(36)
τ
=
T
K
u
,where K
=
diag
⁡
K
1
K
2
⋯
K
r is a diagonal matrix with the thrust coefficients described by the equation(37)
K
i
=
K
T
J
ρ
D
4
.
u is a column vector with elements u
i
=
|
n
|
n, where n is the propeller velocity rate, ρ is the water density, and D is the propeller diameter. The thruster allocation problem is solved finding u as(38)
u
=
K
-
1
T
-
1
τ
.Although T may not be square or invertible, it can be solved using the Moore-Penrose pseudo inverse given by(39)
T
†
=
T
T
T
T
T
-
1
.Substituting (38) into (39) yields(40)
u
=
K
-
1
T
†
τ
.The voltage for each driver calculated from u is given by(41)
v
i
=
1
G
m
i
1
G
d
i
sgn
⁡
u
i
u
i
,where G
m
i and G
d
i are the gain of the ith motor and driver, respectively. This last equation fails when saturation occurs in the actuators. Figure 4 shows how to change v
i according to u
i, with different values from multiplication M
g
=
1
/
G
m
i
1
/
G
d
i.Figure 4
Change of voltage according tou
i in the thruster allocation problem.


### 4.2. Multivariable PID Control

The parallel noninteracting structure of the PID is given by(42)
τ
P
I
D
=
K
p
e
t
+
K
d
e
˙
t
+
K
i
∫
0
t
e
τ
d
τ
,where K
p is the proportional gain, K
d is the derivative gain, K
i is the integral gain, and e
t is the error defined by(43)
e
=
η
d
-
η
.For Visor3, each controller is designed for the control of one DOF; this implies that K
p, K
i, and K
d are diagonal and positive matrices. In this work, heuristic methods were used to tune the gains of the PID controller. A high proportional gain acts rapidly to correct changes in the references. Due to the vehicle’s dynamics and the interaction with the fluid, a high derivative action is used to provide damping to the motion of the vehicle, when it is reaching the setpoint. Finally, it is decided that a small integral action can correct the steady-state error. This PID control can be improved, according to Fossen [6], by using gravity compensation and vehicle’s kinematics. The PID control signal is transformed by(44)
τ
=
J
T
η
τ
P
I
D
+
g
η
.Figure 5 shows the implementation of (44). The controller needs the error and the position estimation to calculate the output.Figure 5
Closed-loop control system implemented in Visor3.


## 4.1. Thruster Allocation

The task which consists in generating a particular command to be sent to each individual actuator according to the control law and thrusters configuration is called control allocation [33].The thrust vector that describes the forcef
i generated by the thruster i is given by(29)
f
=
f
1
f
2
⋯
f
r
T
,where r is the total number of thrusters. The total force and moment generated by the thrusters will be(30)
τ
=
T
f
,where T is the thruster configuration matrix, which is a function of the thrusters position r
t
i
/
b
b, as well as the azimuth and elevation angles, α and β, respectively. This vector is referenced to the body-fixed frame. The thruster configuration matrix describes how the thrust of each motor contributes to the force or moment in each direction. The matrix T is given by(31)
T
=
t
1
t
2
⋯
t
r
,where t
i is the column vector of the ith thruster and is computed as(32)
t
i
=
I
3
×
3
-
S
T
r
t
i
/
b
b
R
α
,
β
1
0
0
f
i
.The rotation matrix R
α
,
β is defined by the product of two rotation matrices:(33)
R
α
,
β
=
R
z
,
α
R
y
,
β
,where R
z
,
α and R
y
,
β are described by(34)
R
z
,
α
=
c
α
-
s
α
0
s
α
c
α
0
0
0
1
,
(35)
R
y
,
β
=
c
β
0
s
β
0
1
0
-
s
β
0
c
β
,respectively; Visor3 has fixed heading and pitch angles for each thruster.Now that the thrust provided by each thruster is known using (30), the input that must be sent to each motor has to be computed. The input could be the revolution speed of the motor or a voltage signal for the driver. Equation (30) is then rewritten as(36)
τ
=
T
K
u
,where K
=
diag
⁡
K
1
K
2
⋯
K
r is a diagonal matrix with the thrust coefficients described by the equation(37)
K
i
=
K
T
J
ρ
D
4
.
u is a column vector with elements u
i
=
|
n
|
n, where n is the propeller velocity rate, ρ is the water density, and D is the propeller diameter. The thruster allocation problem is solved finding u as(38)
u
=
K
-
1
T
-
1
τ
.Although T may not be square or invertible, it can be solved using the Moore-Penrose pseudo inverse given by(39)
T
†
=
T
T
T
T
T
-
1
.Substituting (38) into (39) yields(40)
u
=
K
-
1
T
†
τ
.The voltage for each driver calculated from u is given by(41)
v
i
=
1
G
m
i
1
G
d
i
sgn
⁡
u
i
u
i
,where G
m
i and G
d
i are the gain of the ith motor and driver, respectively. This last equation fails when saturation occurs in the actuators. Figure 4 shows how to change v
i according to u
i, with different values from multiplication M
g
=
1
/
G
m
i
1
/
G
d
i.Figure 4
Change of voltage according tou
i in the thruster allocation problem.


## 4.2. Multivariable PID Control

The parallel noninteracting structure of the PID is given by(42)
τ
P
I
D
=
K
p
e
t
+
K
d
e
˙
t
+
K
i
∫
0
t
e
τ
d
τ
,where K
p is the proportional gain, K
d is the derivative gain, K
i is the integral gain, and e
t is the error defined by(43)
e
=
η
d
-
η
.For Visor3, each controller is designed for the control of one DOF; this implies that K
p, K
i, and K
d are diagonal and positive matrices. In this work, heuristic methods were used to tune the gains of the PID controller. A high proportional gain acts rapidly to correct changes in the references. Due to the vehicle’s dynamics and the interaction with the fluid, a high derivative action is used to provide damping to the motion of the vehicle, when it is reaching the setpoint. Finally, it is decided that a small integral action can correct the steady-state error. This PID control can be improved, according to Fossen [6], by using gravity compensation and vehicle’s kinematics. The PID control signal is transformed by(44)
τ
=
J
T
η
τ
P
I
D
+
g
η
.Figure 5 shows the implementation of (44). The controller needs the error and the position estimation to calculate the output.Figure 5
Closed-loop control system implemented in Visor3.


## 5. Simulation and Results

The simulation of the ROV dynamics, the observer algorithm, and the controller were implemented in Simulink. Additionally, several functions can be constructed using conventional MATLAB code, providing great flexibility for high-level programming.Visor3 parameters were obtained using CAD models (Solid-Edge® software) and CFD simulation (ANSYS® software). Table1 contains all the model parameters used in the simulation to represent the dynamics of the vehicle.Table 1
ROV Visor3 parameters for simulation.
Parameter
Value
m
64.5 kg
I
x
x
2.9 kg m
2
I
y
y
2.5 kg m
2
I
z
z
3.0 kg m
2
I
x
y
-
7.0
×
1
0
-
3 kg m
2
I
x
z
-
2.1
×
1
0
-
3 kg m
2
I
y
z
-
7.2
×
1
0
-
3 kg m
2
∇
1.8
×
1
0
-
2 m
3
[
x
g
,
y
g
,
z
g
]
[
0,0
,
0
] m
[
x
b
,
y
b
,
z
b
]
[
1.7,1.8,68
]
×
1
0
-
3 m
X
u
˙
6.5 kg
Y
v
˙
59.8 kg
Z
w
˙
59.8 kg
K
p
˙
0 kg m
2
M
q
˙
2.2 kg m
2
N
r
˙
2.2 kg m
2
X
u
u
-
10.3 kg/m
Y
v
v
-
100.8 kg/m
Z
w
w
-
100.8 kg/m
K
p
p
-
400.3 kg m
2
M
q
q
-
100.8 kg m
2
N
r
|
r
|
-
100.8 kg m
2The thruster simulation is divided into three steps: the driver, the motor dynamics, and the propeller dynamics. The low-level control of the system is managed by the driver. The driver regulates the torque by increasing or decreasing the current in order to maintain the velocity of the motor shaft. For this work, it is assumed that load changes generated for the propellers are regulated by the driver. In that order, only thrust is considered.For this work, motors’ dynamics are not considered, since they present a fast time response in comparison with the dynamics of the vehicle. Figure6 shows the dynamic response of the MAXON DC motor. The steady-state value is reached in less than 40 ms. The simulation model is represented only by a constant gain G
m, which is the relationship between the maximum velocity V
e
l
m
a
x and the nominal voltage V
n
o
m and is described by(45)
G
m
=
V
e
l
m
a
x
V
n
o
m
.The motor used is a MAXON EC motor 136201 with a planetary gearhead GP 42C-203113. The nominal speed, reduction, and the nominal voltage are taken from [35, 36] and used in (45) to obtain G
m.Figure 6
Time response for a MAXON DC motor.The driver’s function is to regulate the velocity when there are changes in the load. The driver receives an input voltage between 0 and 5 V (saturation) and transforms it to 0–48 V. Additionally, the driver can be configured to follow a prescribed acceleration curve, in order to decrease the current consumed by the motor in the initial operation.The propellers used in Visor3 are the Harbor Models 3540 from the Wageningen B-series with four blades. The coefficientsK
T and K
Q can be approximated using polynomials [37], given by(46)
K
T
=
∑
s
,
t
,
u
,
v
C
T
s
,
t
,
u
,
v
J
s
P
D
t
A
E
A
o
z
v
,
(47)
K
Q
=
∑
s
,
t
,
u
,
v
C
Q
s
,
t
,
u
,
v
J
s
P
D
t
A
E
A
o
z
v
.Figure 7 shows K
T and K
Q curves for Visor3’s propellers. In order to obtain the thrust and torque coefficients, a nominal advance speed of 1.5 m/s for the vehicle was assumed. The parameters for a thruster are shown in Table 2.Table 2
Visor3’s thruster parameters.
Parameter
Value
Motor and driver
High/low voltage driver saturation
±
5 V
Slew rate
1200 V s
Driver gain
9.6
Motor gain
22.48 rpm/V
Seawater density
1027 kg/m
3
Propeller
Diameter
8.8 cm
Pitch
10.16 cm
Blade area ratio
0.7
Pitch ratio
1.1
Thrust coefficients
0.5
Torque coefficients
0.08Figure 7
CoefficientsK
T and K
Q for Visor3’s propellers.Visor3 has four controllable DOFs: surge, sway, heave, and yaw. Figure8 shows the thruster location in Visor3 and Table 3 summarizes the parameters from thrusters position and orientation in Visor3.Table 3
Thruster allocation.
Thruster
Vector position
α
β
T
1
[
-
0.31
,
-
0.22,0.025
] m
0
0
T
2
[
-
0.31,0.22,0.025
] m
0
0
T
3
[
0,0
,
0
] m
0
π
/
2
T
4
[
0.11,0
,
0
] m
π
/
2
0Figure 8
Thrusters position. Note that all units are in meters.The ROV dynamics were implemented as a continuous-time model, and the EKF runs in discrete time with a 0.05 s fixed sample time. The implementation was done using four main functions; see Figure9:(i)
ROV_Nolinear_Discrete: it calculates the next state of the ROV described by (23).
(ii)
ROVSal_Nolinear_Discrete: it calculates the output of the ROV according to the sensors.
(iii)
ROV_linear_Discrete: it evaluates the Jacobian in each state estimation described by (25).
(iv)
ROVSal_linear_Discrete: it evaluates the output of the ROV given the Jacobian described by (28).For the measurement matrix, it was considered that Visor3 has a MEMSENSE® IM05-0300C050A35 triaxial micro inertial measurement unit (μIMU) that provides three linear acceleration measurements and three angular velocities, with noise of 5.0 mg and 0.5°/s. In this work, all measurements are assumed to be made in the body-fixed frame and a standard INS solution is proposed; that is, attitude and position are obtained through integration of signals from the IMU. The measurement matrix is given by(48)
H
k
=
I
6
×
6
0
6
×
6
.To test the performance of the EKF algorithm and its implementation, two different position references were commanded to the ROV: in the surge direction a position of 2 m (see Figure 10) and in sway 1 m (see Figure 10). Figure 10 shows the estimation of the position in the Earth-fixed frame.Figure 9
EKF Simulink implementation.Figure 10
Position estimation in Earth-fixed frame.A simulation experiment was conducted with a trajectory that describes changes in thex and y and yaw directions. Figure 11 shows the result of the controller. The experiment was conducted with the following gain matrices:(49)
K
p
=
diag
⁡
10,10,10,0
,
0,10
,
K
i
=
diag
⁡
0.01,0.01,0.01,0
,
0,0.01
,
K
d
=
diag
⁡
50,50,50,0
,
0,50
.PID gains were obtained by trial and error; a high proportional gain generates fast but aggressive response; hence, a high derivative gain was added to increase damping and decrease oscillations in spite of having a slower time response. A small integral gain was used to eliminate the steady-state error. The PID with gravity compensation cancels the effects of restoring forces while the vehicle is moving. This PID with compensation has a better performance, but it requires a good estimation of the vehicle’s attitude. Additionally, Figure 12 shows that the regular PID is less energy-efficient, due to the high changes in the control signal. This change in the control signal can reduce duty life of the thrusters. Finally, the regular PID structure is unstable when the vehicle moves far away from the origin, due to restoring forces and propagation error.Figure 11
PID controllers with and without compensation—planar motion control.Figure 12
Control signal for each thruster—planar motion control.


## 6. Conclusions

The dynamic model of the underwater remotely operated vehicle Visor3 has been presented. This model considers forces and moments generated by the movement of the vehicle within the fluid, damping, and restoring forces. The model was defined using body-fixed and Earth-fixed coordinate systems, and Visor3’s parameters were obtained using CAD models and CFD simulations.The full ROV system and the EKF were simulated to compare the performance of the navigation system with the use of a PID + gravity compensation control algorithm that depends on a good estimation of the state. It is important to state that the estimation in large periods of time can diverge due to integration of the noise present in acceleration measurements to obtain the velocity of the vehicle in the body-fixed frame. More sensors (such as SSBL and DVL) can be used in Visor3 to overcome such a problem; this will allow the navigation system to get the position of the vehicle in a more precise way, which is required for its regular operations: maintenance purposes can be performed in ships hulls and underwater structures within Colombian ports facilities.As it was shown, the EKF-based navigation system is capable of filtering the noise in measurements and accurately estimates the state of the vehicle; this is important since a noisy signal that enters into the feedback system can cause greater efforts in the thrusters and more energy consumption. However, more simulations have to be carried out in order to determine how critical is the divergence in the position estimation caused by phenomena such as biases and losses in the communications.Two different control algorithms were tested with the simulation of the ROV: PID and PID + gravity compensation. The PID with gravity compensation is capable of stabilizing the system in less time compared to the PID and decreases the energy consumption. On the other hand, the PID without gravity compensation loses tracking at different times; this may be caused by the force exerted from thrusters in order to compensate oscillations from the vehicle. In Visor3, the forward/backward thrusters are nonaligned with the body frame; therefore, they can cause pitch and roll oscillations. Finally, the simple PID goes unstable after some time period. However, the PID with gravity compensation needs a good estimation of the position and attitude. This PID strategy is a first approach to the motion control of the vehicle.Implementing the proposed navigation system and the controller in Visor3’s digital system requires the knowledge of the dynamic response of the vehicle and appropriate selection of the sample time since it affects the EKF algorithm’s convergence. Moreover, many operations are in matrix form and with floating-point format, so the implementation of such navigation system and controllers requires a high computation capacity of the on-board processor. These algorithms are the first approximation to the real closed-loop control system that will be implemented in Visor3.



---

*Source: 8029124-2016-07-20.xml*