IEEE Robotics & Automation Magazine - September 2015 - 78

where o ) = 6u ) v ) r )@T is the desired linear and angular
speeds of the platform, K P = diag ^K Pu, K Pv, K Pr h and
K I = diag ^K Iu, K Iv, K Ir h are diagonal matrices with the PI
gains for individual degrees of freedom, respectively. The x F
term represents additional action introduced in the controller
to improve the closed-loop behavior. This action can be in the
form x F = D (o) o, which results in the feedback linearization procedure, where measured or estimated speeds are used
to compensate for the nonlinearity in the process.
Controller parameters
K P and K I can be calculated based on the desired
The diving supervisor at
closed-loop characteristic
equation, as shown in
the surface has reliable
[13]. These parameters
will naturally depend on
data about the diver 's
the parameters of the dynamic model that have to
position, and reliable
be identified. The dynamic model parameters
communication.
of the platform that is addressed in this article
have been identified
using the identification method based on self-oscillations reported in [14].
Guidance Controller Design
Since the platform is overactuated, it can move in a horizontal
plane while keeping an arbitrary heading. For this reason, the
high-level guidance controller is divided into the heading
controller and the tracking controller design.
Heading Controller
For the heading controller, a PI structure is chosen since it
compensates for all environmental disturbances in the yaw degree of freedom. In addition, the integral action will compensate for all the unmodeled dynamics and ensure convergence
of the heading to the desired value } ). The controller can be
written in the form

GPS at 1 Hz
IMU at 10 Hz
x at 10 Hz

USBL at
(0.2-1.2) Hz

PlaDyPos
State Estimator
(with Dynamic Model)

Diver
State Estimator
(with Dynamic Model)

~ x,
~ y,
~ }
~
v,
at 10 Hz
Control
and
~ ~ ~ ~ Tracking
dD, uD, rD, }D System
at 10 Hz

Figure 8. A schematic description of estimator inputs and
outputs. Since measurements are available at different update
rates, the state estimators are used to ensure the update rate
of 10 Hz required for the control and tracking system. The diver
estimator is also required since diver position measurements
are intermittent due to possible occlusions of the acoustic link
caused by air bubbles.

78

*

IEEE ROBOTICS & AUTOMATION MAGAZINE

*

September 2015

r ) = K P} (} ) - }) + K I}

# (} ) - }) dt,

(10)

where K P} and K I} are controller parameters chosen so that
the desired heading closed-loop dynamics are achieved.
Tracking Controller
With the tracking model given with (8), the PI control action
in the form
u)
; )E = R T (}) ` -K P, d d - K I, d
v

# d dt j + o F,

(11)

where K P, d = diag ^K P, dx, K P, dy h and K I, d = diag ^K I, dx,
K I, dy h are PI gain matrices, respectively, will ensure convergence of the distance d to the desired value d ) = 60 0@T.
The o F is the feedforward action that can improve the behavior of the tracking closed loop. The proposed PI controller will ensure convergence even without the feedforward
action, i.e., o F = 60 0@T , 610@ . However, tracking may be improved if feedforward action in the form (12) is introduced
oF

= R T (}) R D (} D) v D .

(12)

The proposed feedforward action requires the estimation of
the diver surge speed and heading since they cannot be directly measured.
Sensor Fusion
Two extended Kalman filters are implemented in the system,
as shown in Figure 8. Their main purpose is to fuse measurements available at different update rates to ensure state estimations at 10 Hz, as required by the control and tracking
system. The PlaDyPos state estimator uses the kinematic (2)
and dynamic model (1) of the vehicle to provide speed and
position estimates for the control and tracking system based
on the input GPS and inertial measurement unit (IMU) measurements as well as the commanded thrust vector x.
The diver state estimator uses intermittent USBL measurements, PlaDyPos states, and the simplified diver model given
with (5) and (7) to estimate tracking distance and speed and
orientation of the diver. Since USBL measurements are often
not available due to presence of air bubbles exhaled by the
diver, this estimator ensures continuous estimates required for
the diver-tracking algorithms.
Benchmark Scenario for Diver Tracking
Performing real-life experiments that include humans and
robots is always a complex task. The unpredictability of
human nature does not allow replicability of experiments,
which is why careful planning and preparation is always required. To validate and replicate diver-tracking experiments
under different environmental conditions, we define a
benchmark scenario that includes tracking a predefined, georeferenced, and underwater transect. A 50-m rope was laid
on seabed at the test site and georeferenced using precise
GPS and USBL measurements. During the experiments, the
Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - September 2015
