Ocean Engineering 159 (2018) 56–65

Contents lists available at ScienceDirect

Ocean Engineering
journal homepage: www.elsevier.com/locate/oceaneng

Developing a navigation, guidance and obstacle avoidance algorithm for an

Unmanned Surface Vehicle (USV) by algorithms fusion
Hossein Mousazadeh a, *, Hamid Jafarbiglu a, Hamid Abdolmaleki a, Elham Omrani a,
Farshid Monhaseri a, Mohammad-reza Abdollahzadeh a, Aref Mohammadi-Aghdam a, Ali Kiapei b,
Yousef Salmani-Zakaria a, Ashkan Makhsoos a
a
Mechanical Engineering of Biosystems, University of Tehran, Iran
b
Engineer in Mechanical of Biosystems and Expert of Ports and Maritime Organization, Iran

A R T I C L E I N F O A B S T R A C T

Keywords: Many USVs are equipped with navigation, guidance and control (NGC) algorithms due to human errors that is
Navigation present in manual navigation of surface vehicles. The Morvarid, a catamaran shaped boat is designed and
Obstacle avoidance developed for plotting hydrography map of harbours. To control it autonomously, a fusion algorithm based on
Kalman filter EKF, search ball and potential field concept is developed. Some main parameters of dynamic model is derived
Search ball based on experimental tests and the algorithm is evaluated using hardware-in-the-loop concept. The obstacle
Thrust
avoidance algorithm is assessed for different obstacles in size and position. It is concluded that although the
developed algorithm is robust and precise even in the presence of environmental disturbances, however installing
powerful thrusters would boost maneuverability more than that is.

1. Introduction step of work. Considering potential candidates, classical hull shape

Many researches have been performed and some are under study for
surface vehicles automation, since manual navigation system for vessels
in supervising, data acquisition, and processing are always subject to
human errors (Eydgahi et al., 2007). In this text automatically controlled
surface vehicles are called Unmanned Surface Vehicle (USV) as it is
common in many references, while Autonomous Surface Vehicle (ASV) is
used in another texts. USV technology is rather new in contrast to
autonomous underwater vehicles (AUVs) which has already reached its
maturity. The accuracy of USVs is far better than that of the AUVs since
the GPS fixes are generally available at all time in open waters (Naeem
et al., 2007). USVs are robots for operation at lakes, canals, harbors and
even open sea that are characterized by small size, good hiding capa-
bility, high mobility, and low price (Tang et al., 2015). This surface ve-
hicles are subjected to many objectives, either alone or as part of a sensor
network. Some well-known applications for USVs include marine envi-
ronment monitoring, bathymetry, inspection, mine countermeasure and
port protection, oil and gas exploration, deep sea pipeline monitoring,
coastal security, search-and-rescue (Campbell et al., 2012), (P^etres et al.,
2012).
Selection of a suitable marine craft platform for automation is the first
(single hull or twin hull catamaran shaped vessels) exploits favourable
hydrodynamic properties to obtain long range operation with low energy
consumption (Nađ et al., 2015). Between small boats the catamaran is for
stability with respect to roll, capability of payload transport with respect
to the hydrodynamic drag, and redundancy in hull buoyancy (Caccia
et al., 2005). From view of reaction force applied to surface vehicles,
Marine crafts can be classified according to their maximum operating
speed (U) and length of the submerged portion (L) using the Froude
number: Fr ¼ U/√(L.g). Where g is the acceleration due to gravity. The
weight of a vessel operating with a Froude number less than about
0.4–0.5 is almost completely supported by the hydrostatic force of
buoyancy that is proportional to the volume of fluid displaced
(displacement mode). In Froude numbers greater than about 1.0–1.2 the
weight is almost completely supported by a hydrodynamic force that is
roughly proportional to the square of the speed (planning mode). For
intermediate values of the Froude number, the vessel's weight is sup-
ported by both the hydrostatic and hydrodynamic force (semi-displace-
ment vessel) (Fossen, 2011).
In USVs the Navigation, Guidance and Control (NGC) system intelli-
gently controls the vehicle to go from origin to destination, avoiding the
obstacles in its path. Depending on the anticipated danger of the

* Corresponding author.
E-mail address: hmousazade@ut.ac.ir (H. Mousazadeh).

https://doi.org/10.1016/j.oceaneng.2018.04.018
Received 14 February 2016; Received in revised form 1 January 2017; Accepted 7 April 2018
Available online 16 April 2018
0029-8018/© 2018 Elsevier Ltd. All rights reserved.
H. Mousazadeh et al.
obstacles, the vehicle chooses an angle of divergence which avoids the
obstacles and keeps the speed of the vehicle (Eydgahi et al., 2007).
During navigation, the sensors for detecting the obstacles would be a
radar, Lidar, binocular vision, stereo vision, monocular vision, infrared
cameras, laser range finders and ultrasonic sensors or combination some
of these sensors. In this field, usually detecting distance is limited to a few
hundred meters. The maximum detecting distance of sensors would be
under 200 m e.g. for Lidar (Tang et al., 2015). While ultrasonic sensors
difficultly could detect up to 10 m. Some researches use high accuracy
and expensive positioning systems such as RTK-DGPS. Despite to high
accuracy and expensive positioning systems, some experiments demon-
strated the effectiveness of extended Kalman filter (EKF) and simple PID
guidance and control laws to perform basic control tasks such as
auto-heading, auto-speed and straight line following with a USV equip-
ped only with GPS and compass (Caccia et al., 2008).
Different algorithms have been developed and were used for NGC as
well as obstacle avoidance in reviewed texts, each with some pros and
cons. Some main methods that have been proposed for navigation and
obstacle avoidance in robot boats are; Raytracing techniques, the use of a
state machine, dynamic programming, modified A* algorithm, fuzzy
logic, interval analysis, Voronoi diagrams, optimization of the time-
derivative of the distance between boat and target and potential field
framework (P^etres et al., 2012). Generally speaking, the problems related
to motion control and Guidance of USVs are classified in the literature
into three basic groups: a) Point stabilization: the goal is to stabilize the
vehicle zeroing the position and orientation error with respect to a given
target point, with a desired orientation (Bibuli et al., 2009). b) Path
following: More specifically, the full path might be known to the
experimenter prior starting the experiment and communicated to the
vehicle in full or just a piece at the time, i.e. the vehicle has to follow a
planar path without temporal constraints. This control compute and
reduce to zero the distance between the vehicle and the path as well as
the angle between the vessel speed and the tangent to the path (Bibuli
et al., 2009), (Caccia, 2006). c) Path-tracking (Trajectory tracking): the
vehicle might need to learn segments of the path to track sequentially just
seconds ahead of setting on each one. Requiring the vessel to follow a
time parameterised reference curve, i.e. to be in specific points at specific
instants of time (Bibuli et al., 2009), (Sagginia et al., 2015), (Caccia,
2006).
Except to these three main groups, two different methods are added in
some texts: d) Control in the horizontal plane: where a simple P(I)D
heading controller is sufficient for guaranteeing satisfactory perfor-
mance. And e) Cooperative motion control: a leader and a follower
vehicle following two parallel paths (Master-Slave) (Caccia, 2006).
1.1. Literature review
Some papers about guidance, navigation and obstacle avoidance in
USVs are reviewed and finally some useful information from different
articles are given in a table for comparison.
Praczyk (2015) presented application of evolutionary neural net-
works to the collision avoidance task and the navigation problem in a
complex, multi-object, rapidly changing environment, which do not
behave according to any widely accepted regulations (Praczyk, 2015).
Liu et al. (2015) introduced a path planning algorithm for the USV for-
mation navigation based on the fast marching (FM) method, which has
features of fast computation speed and low computation complexity. This
algorithm covers two areas, i.e. ship domain area and collision avoidance
area, to ensure the planned trajectory to not violate any forbidden area
(Liu and Bucknall, 2015).
Zereik et al. (2015) simulated three different control tasks, namely
path following, obstacle avoidance and speed regulation as an algorithm
named Priority Task Approach. The main objective of this approach was
to correctly execute a path following task, while guaranteeing also
obstacle avoidance and vehicle speed regulation (Zereik et al., 2015). Ma
(2015) developed a Modified Unscented Kalman Filter as an online
57
Ocean Engineering 159 (2018) 56–65
estimation algorithm with higher precision and lower calculation
complexity for USV state estimation. It is concluded that the proposed
algorithms to be superiority to the general UKF. Motwani et al. (2013)
evaluated potential application of Interval Kalman Filter (IKF) algorithms
for estimating the heading angle of the vessel under erroneous modeling
assumptions. The simulations reveal several characteristics of the IKF
(Motwani et al., 2013).
Onunka et al. (2013) combined the ultrasonic sensor and radar to
reduce the uncertainties present in the guidance and control of the USV
while improving the obstacle detection and avoidance capacity of the
USV especially in the near field-region up to 5 m from the USV (Onunka
et al., 2013). Finally for straightforward comparison, some useful infor-
mation from reviewed texts are given in Table 1. Some design charac-
teristics, Thrust force, maximum speed, application field and navigation
algorithms are selected information that is extracted from some pub-
lished papers.
Considering different algorithms are equipped to different USVs each
with some drawbacks, and since for faultless navigation and obstacle
avoidance, each platform would have its own correct and effective pa-
rameters, the main objectives of this research is as: a) determining the
dynamic model parameters based on experimental tests for the USV that
is designed and constructed in this project. b) State estimation using EKF
predictor. c) Giving a complex path following algorithm for navigation,
control and obstacle avoidance, based on research ball and potential field
methods.
2. Materials and methods
This section first considers the designed and constructed USV plat-
form. The derived rigid body model based on experimental results are
described next. Then it is followed by Extended Kalman Filter (EKF)
application and developing a complex algorithm for NGC. Finally it is
terminated with explanation of obstacle avoidance algorithm.
2.1. The evaluated platform
This manuscript explains a part of a complete scientific project
(Morvarid1) that is pending for tracing the hydrography map of a harbour
in the bureau using an USV. Considering common platforms, the twin-
hulled catamaran design is selected due to its stability and simplicity of
steering mechanism which is steered using differential thrust. The Mor-
varid is a plug-in hybrid solar powered USV, equipped with two thrusting
motors each with 445 N (100 lbs) thrust force. This 24 V motors are
controlled as variable speed in forward and backward. The boat length,
width and height approximately are 3.8, 2.4 and 1.5 m respectively. It's
overall weight is about 700 kg. An 8 kWh high technology Li-ion battery
pack can extend its stability for more than three continues cloudy days.
To perform the defined duty, it is equipped with some local perception
and global monitoring sensors as; a simple GPS, pack of state sensor
(IMU, compass and air pressure), range finder Lidar, a set of ultrasonic
sensors, stereoscopic vision system and three single beam echo-sounders.
An outboard waterproof IPC is a central processing and decision making
unit that receives all data and information, analysis data and activate the
port and starboard motors in the proper manner. Communication be-
tween Morvarid and bureau is based on an Ethernet network that capable
for data sending to maximum 300 Mbps in distances to 10 km. The 3D
CAD model of the Morvarid is shown in Fig. 1-a and its first test in the
Persian Gulf Lack, suburban of Tehran is illustrated in Fig. 1-b.
This USV can operate in four modes by a priority as: a) manual
operation by a joystick on the vehicle, b) remote control by a handheld
unit c) control from office using a GUI and d) fully automatic path
following.
1
eMorvarid.ut.ac.ir.
H. Mousazadeh et al. Ocean Engineering 159 (2018) 56–65

Table 1
Comparison between some specifications of evaluated USVs.
USV Design Features Thrust Turning Speedmax Application Navigation Ref.
diameter (m/s) algorithm
type Length Width Height Weight
(m)
(m) (m) (m) (kg)

DUKW- twin hull 2.7 266 N 10 1 Path planning (Bertaska et al.,

Ling 2015)
USV14 twin hull 4.3 205 N 12 2.8 Path planning (Bertaska et al.,
2015)
USV12 twin hull 3.7 200 N 3.6 Path planning (Bertaska et al.,
2015)
Springer catamaran 4 2.3 600 653 N 1.5 Pollutant tracking Fuzzy LQG and (Naeem et al.,
environmental GA-based MPC 2012)
monitoring
catamaran 1.5 0.8 0.4 266 N 1 Survey shallow costal PD controller - (Park et al.,
area and support Linear 2010)
works of underwater Quadratic (LQ)
vehicles
Charlie catamaran 2.4 1.7 300 600 W Sampling sea surface PID - EKF (Bibuli et al.,
microlayer 2009). (Caccia
et al., 2005)
Ribcraft Hull 4.5 50 hp 10.3 Research in Waypoint (Sonnenburg
4.8 autonomous following et al., 2010)
SeaFox Hull 4.9 1.5 250 20.6 Military Remote control (Sonnenburg
hp and LSE et al., 2010)
Pontoon pontoon 3 353 N 2 Investigate motion (Sonnenburg
control et al., 2010)
Hull with 0.5 0.14 1.6 Surveillance, search Trajectory (Muske and
two motors and rescue planning Ashrafiuon,
2008)
Swath 3.4 1.3 1.3 360 485 N 0.78 Bathymetry Path-following (Rasal, 2013)
and trajectory
controller
sailboat 1.38 0.36 2.2 Trial Potential fields (P^etres et al.,
2012)
KAIST Catamaran 3 RobotX Challenge A* (Kang et al.,
2015)

Fig. 1. A) The Morvarid 3D CAD model b) The first test of the Morvarid in the Persian Gulf Lake, Tehran.

2.2. Rigid body model coordinate, and J(η) is transformation matrix that for details the reader is
referred to Fossen (1994) (Fossen, 1994). Here x, y and z are vehicle
This section discuss deriving the dynamic model of the marine vehicle position and φ, θ and ψ are orientation in earth-fixed frame. Also u, v and
in six degrees of freedom (DOF) since six independent coordinates are w, are velocity respectively in surge, sway and heave and p, q, and r, are
necessary to determine the position and orientation of a rigid body. Ac- angular velocity respectively around roll, pitch and yaw. For surface
cording to Fossen (1996) (Fossen, 1994) It is assumed two coordinate vehicles the depth z and pitch θ variables are not applicable. Also the roll
frames; the moving coordinate frame that is fixed to the vehicle and is φ variation is found to be negligible during experimental evaluations
called body-fixed reference and an earth fixed reference frame. As shown (Naeem et al., 2012). For simplicity ignoring the swing (z  cons. and
in Fig. 1, the origin of the body-fixed frame is usually chosen to coincide we0), pitch (θe0 and qe0) and roll (φe0 and pe0), the kinematic model in
with the center of gravity (CG). The kinematic equation for transforming Eq. (1), which describes the geometrical relationship between the
body-fixed reference to earth-fixed one can be expressed as (Fossen, earth-fixed and the vehicle fixed motion is given as:
1994); 2 3 2 32 3
x_ cosψ sinψ 0 u
η_ ¼ JðηÞ:ν (1) 4 y_ 5 ¼ 4 sinψ cosψ 0 5:4 υ 5 (2)
ψ_ 0 0 1 r
Where η ¼ [x, y, z, φ, θ, ψ] denotes the position and orientation
T

vector with coordinates in the earth-fixed frame, ν ¼ [u, v, w, p, q, r]T is According to Newtonian and Lagrangian mechanics, nonlinear dy-
the generalized linear and angular velocity vector in body-frame namic equation of motion can be conventionally expressed as (Fossen,

58
H. Mousazadeh et al. Ocean Engineering 159 (2018) 56–65

1994): 2008), (Doa et al., 2004) and (Reyhanoglu, 1996). The time-varying
terms, τwu(t), τwv(t) and τwr(t), are the environmental disturbances
MRB :_υ þ CRB ðυÞ:υ þ DRB ðυÞ:υ þ gðηÞ ¼ τ (3) induced by wave, wind and ocean-current with jτwu(t)j  τwu max < ∞,
Where MRB denotes the rigid-body inertia matrix (including added jτwv(t)j  τwv max < ∞ and jτwr(t)j  τwr max < ∞. Finally, the available
mass) and MRB ¼ MTRB>0, and M_ RB ¼ 0; CRB(ν) is denotes the matrix of controls are the surge force τu and the yaw moment τr that are calculated
Coriolis and centripetal terms (including added mass) and CRB(ν) ¼ - as Equ. (5).
CRB(ν)T, DRB(ν) is damping matrix, g(η) is vector of gravitational forces The parameters; added mass and damping coefficients are the un-
and moments and τ ¼ [X, Y, Z, K, M, N]T is the vector of control input known variables that would be derived. Mass parameters include added
including force and moments in three directions. In this case since the mass contributions that represent hydraulic pressure forces and torque
inputs are only from two port and starboard motors the USV is deal as due to forced harmonic motion of the vessel which are proportional to
underactuated surface ship (fewer numbers of actuators than the to-be- acceleration. Using the estimate of the added mass terms presented in
controlled degrees of freedom). Considering Tport as portside thrusting (Fossen, 1994) for the experimental model boat it would be derived as
force and Tstbd as starboard thrusting force each in surge direction, the (Muske and Ashrafiuon, 2008):
control input vector become as: 8 m11  m þ 0:05m
  >
>  
   B T >
< m22  m þ 0:5 ρ:π :D2 :L
τ ¼ ½τ u ; τ ν ; τ r T ¼ Tport þ Tstbd ; 0; Tport þ Tstbd : (5) (7)
2 >
> 1 
>
: mðL2 þ W 2 Þ þ 0:1m:B2 þ ρ:π :D2 :L3
Where B is the distance between two thrusting power sources (1.2 m m33  2
12
in this case). Differential steering imposes a moment about the vehicle
that causes it to turn. There are distinct advantages to this type of system; Where m ¼ 700 kg is the actual mass of the USV, L ¼ 3 m is the
primarily, the turning radius for the vehicle is substantially reduced, as effective length, W ¼ 0.8 m is the width, D ¼ 0.5 m is the mean sub-
well as the additional capability to steer without the requirement of a merged depth, and ρ is the density of water.
forward speed (Bertaska et al., 2015). About damping coefficients, according to (Ma, 2015):
The mathematical model of an underactuated ship moving in surge, 
du2 ¼ 0:2du ; dv2 ¼ 0:2 dv ; dr2 ¼ 0:2 dr
sway and yaw is obtained from the motion equation of the ship moving in (8)
du3 ¼ 0:1du ; dv3 ¼ 0:1 dv ; dr3 ¼ 0:1 dr
six degrees of freedom under disturbances induced by wave, wind and
ocean current. Furthermore it is assumed that the inertia, added mass and For defining du, dv and dr, combination of experimental results,
damping matrices are diagonal. This assumption holds when the vessels simulation and conformance with some references are used. Considering
have three planes of symmetry, for which the axes of the body-fixed that the damping parameters are dependent on boat shape and dimen-
reference frame are chosen to be parallel to the principal axis of the sion, main related parameters of some USVs as well as Morvarid are given
displaced fluid, which are equal to the principal axis of the vessel. Most as Table 2;
ships have port/starboard symmetry. Moreover, bottom/top symmetry is According to Table 2 the ratio of m11/du, m22/dv and m33/dr for all
not required for horizontal motion. Ship fore/aft nonsymmetry implies given USVs are in specific range. Inserting maximum values of these
that the off-diagonal terms of the inertia and damping matrices are ratios in the simulation model and tuning their values till the surge and
nonzero. However, these terms are small compared to the main diagonal yaw speeds for predefined thrusting force and moments become stable, is
terms. So the underactuated ship moving in surge, sway and yaw can be continued. The predefined thrusting force is achieved from experimental
described as (Doa et al., 2004): results and is the minimum thrust force that could move the boat in the
8 steady state (for this case is 0.2 of maximum thrust). Finally these ratios
>
> x_ ¼ u:cos ψ  v:sin ψ for Morvarid is defined as m11/du ¼ 10, m22/dv ¼ 5 and m33/dr ¼ 2.
>
>
>
>
> y_ ¼ u:sin ψ þ v:cos ψ
>
>
>
>
>
> ψ_ ¼ r
>
> 2.3. EKF Algorithm and its parameters
>
> X3 dui i1
< m22 du 1 1
u_ ¼ ⋅v:r  :u  juj u þ :τ u þ :τwu ðtÞ
> m11 m11 i¼2 m
11 m11 m11 To best estimate the statues of the vehicle, application an estimator
>
>
>
> X3 dυi i1 1 filter is common. In estimation theory, the extended Kalman filter (EKF)
>
> v_ ¼ m11 :u:r  dυ :v 
>
> jvj v þ :τwυ ðtÞ is the nonlinear version of the Kalman filter which linearizes about an
>
> m 22 m 22
i¼2 m
22 m22
>
>
>
> ðm11  m22 Þ X3 dri i1 1 1
estimate of the current mean and covariance. As it is the case in this
>
> dυ
: r_ ¼ :u:r  :v  jrj :r þ :τ r þ :τwr ðtÞ study, the state transition and observation models do not need to be
m33 m22 i¼2 m
33 m33 m33
linear functions of the state, but may instead be differentiable functions.
(6)
The EKF can be formulated as:
The positive constant terms mjj, where 1  j  3 denote the ship Model:
inertia including added mass and is derived according to Equ. (7). The (
positive constant terms du, dv, dr, dui, dvi, and dri, where i ¼ 2, 3 represent xk ¼ f ðxk1 ; uk Þ þ wk ⇒ xk ¼ xk1 þ T:f ðxk1 ; uk Þ þ wk
the hydrodynamic damping in surge, sway and yaw. For simplicity, the zk ¼ hðxk Þ þ vk ⇒ zk ¼ H:xk þ vk
higher nonlinear damping terms are ignored (Muske and Ashrafiuon,

Table 2
Mass parameters and damping coefficient of some USVs.
m [kg] L [m] m11 [kg] m22 [kg] m33 [kgm2] du [kg/s] dv [kg/s] dr [kgm2/s] Ref.
3 3 3 5 2 3
118*10 38 120*10 177.9*10 636*10 215*10 147*10 802*104 (Doa et al., 2004)
1.61 0.4 1.69 1.8 0.027 2.72 13.4 0.05 (Muske and Ashrafiuon, 2008)
– – 25.8 33.8 2.7 2 7 0.5 (Kjerstad, 2009)
– – 200 250 80 70 100 50 (Reyhanoglu, 1996), (Dong and Guo, 2005)
700 3 735 1453 1150 ? ? ? Morvarid

59
H. Mousazadeh et al. Ocean Engineering 159 (2018) 56–65

Predict: the most well-known algorithm that operates according to magnetic

( potential field (i.e. attractive or repulsive force are inversely propor-
bx k ¼ f ðbx k1 ; uk Þ⇒ bx k ¼ bx k1 þ f ðbx k1 ; uk Þ tional to distance from destination point). This algorithm have been
(9)
Pk ¼ Fk1 Pk1 Fk1 T
þ Qk1 widely and successfully employed for control and motion planning of
mobile robots for decades. The potential field based algorithms are fast,
Update: reliable and easy to tune once implemented on real platforms (P^etres
8  1 et al., 2012). In artificial potential field method, movements of the
< Kk ¼ Pk HkT Hk Pk HkT þ R robot (represented as a particle) are governed by a field, which is
bx ¼ bx k þ Kk ðzk  hðbx k ÞÞ
: k usually composed of two components; an attractive potential drawing
Pk ¼ ðI  Kk Hk ÞPk
the robot towards the goal and a repulsive potential pushing the robot
Since three degree-of- freedom (DOF) motion composed of surge, away from obstacles. Approximately in all developed algorithms the
sway, and yaw was considered for the filter dynamics, the USV's motion destination point is a single point that attract the vehicle. Due to this,
was estimated using position and attitude measurements provided by the the main drawback with potential field techniques is their susceptibility
GPS, compass and IMU. Here, xk ¼ [x, y, ψ, u, ν, r]T is state vector in the to local minima. To dissolve this problem, an algorithm is developed
step of kth, zk ¼ [x, y, ψ, u,_ ν_ , r]T is measurement vector that the first and based on the combination of potential field and research ball algorithms
second parameters are measured bys GPS, the 3rd parameter is measured (Mas et al., 2010). This concept, is illustrated in Fig. 2, as NGC
by compass and the last three are measured by IMU. The state estimation algorithm.
function, f (xk-1, uk) is a function of state parameters and control signal, uk For each point on the navigation course a search is conducted to find
and is given according to Equ. (6). T is time step and H is jacobian of h two nearest goal points on the reference path (The algorithm first finds
(xk) that here would be a function of T (as given below) also Fk-1 is ja- the nearest path line in whole area, and then searches on that line to
cobian of f (xk-1, uk). The jacobian matrix, J for an arbitrary function, f is complete the search ball). Once determined, the distance of these points
an m*n matrix that is given as Equ. (10). from the USV represents the greatness of the attractive force. The process
2 3 continues for all of the components in the examined trajectory. The
∂ f1 ∂f1 search is spatially bounded by the user-defined search ball, that here it is
⋯
  6 ∂
6 x1 ∂xn 7
7 selected as equal as 2*L (L is boat length) (Fossen, 1994), (Naeem et al.,
df ∂f ∂f 6 7
J¼ ¼ … ¼6 ⋮ ⋱ ⋮ 7 (10) 2008). Actually, a Line-Of-Sight (LOS) maneuvering system was used to
dx ∂x1 ∂xn 6 7
4 ∂fm ∂fm 5 control USV position and transition between waypoints in a trajectory as
⋯
∂x1 ∂x1 in Fossen (2011). This system used the vehicle's current position and
desired positions to determine a desired heading.
The wk and vk in Equ. (9) are white noise for process and measure-
The radius of the search ball, Br, allows a certain level of flexibility on
ment respectively which are the characterization of system modeling
the user side; however, a well-adjusted magnitude makes the algorithm
errors and sensors' uncertainty. Furthermore the following conditions are
more computationally efficient. The search ball algorithm can be math-
simultaneously satisfied (Rodríguez and G omez, 2009):
ematically formulated as follows. Assuming points P (xj, yj) to be the
    position of USV estimated by EKF, and T ¼ {1, …, n} as a set of indices for
E½wk  ¼ 0; E½vk  ¼ 0; E wk :wTi ¼ δðk  iÞ:Qk ; υk :υTi ¼ δðk  iÞ:Rk
reference path points inside the search ball. If E and N represent the east
(11)
and north coordinates respectively, for each j, a new set Bj ¼ {i ε T/

1; n¼0 dij < Br} can be defined, where distances dij are calculated with Equ.
Where δðnÞ ¼ is the Kronecker delta function, Qk is
0; n 6¼ 0 (12).
the covariance matrix of the system noise, and Rk is the covariance qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi
matrix of the observation noise that all of these are positive parameters. dij ¼ ½EðiÞ  EðjÞ2 þ ½NðiÞ  NðjÞ2 (12)
The adjustment of Qk and Rk is critical in the development of Kalman
filter. Although Estimating of Rk is not very sophisticated, about Qk The members in the set of Bj is sorted from min to max, to obtain the
there are some methods for these parameters definition, e.g. Monte minimum distance and its number. Actually the algorithm determines
Carlo (Gomez-Gil et al., 2013). However, in this research Rk estimated two nearest goal points to USV. It is necessary to mention that, as shown
as: in Fig. 3, searching is done only inside a semicircle, i.e. the algorithm
seeks only ahead of the boat. Also the deflection angle of USV from each
2 3
σ 2xðGPSÞ 0 0 0 0 0 point is calculated using Equ. (13).
6 0 σ 2
0 0 0 0 7
6 yðGPSÞ 7
6 0 0 σ 2ψ ðcomp:Þ 0 0 0 7 ½NðiÞ  NðjÞ
6 7 αi ¼ Arctan (13)
R¼6 2 7; H ½EðiÞ  EðjÞ
6 0 0 0 σ 0 0 7
6 _
uðIMUÞ 7
4 0 0 0 0 σ 2υ_ ðIMUÞ 0 5 To attract the USV toward the goals, a linear potential, pot is built for
0 0 0 0 0 σ 2rðIMUÞ every path points as follows (P^etres et al., 2012):
2 3
1 0 0 0 0 0
6 7 poti ¼ k2 ⋅*⋅dij ⋅ðhere⋅i ¼ 1⋅to⋅4Þ (14)
60 1 0 0 0 07
6 7
6 7 where k2 is the desired attractive gradient and dij is the Euclidean dis-
60 0 1 0 0 07
6 7
6 1 7 tance between P (xj, yj) and ith goal. Finally, the real destination point
¼ 60 0 0 0 07
6 7 and the required orientation of USV that is shown by red vector in Fig. 3,
6 T 7
6 7 is calculated from the result of calculated vectors using the Equ. (15).
6 1 7
60 0 0 0 07 P
4 T 5
poti : sinαi
0 0 0 0 0 1 δ ¼ Arctan Pi (15)
i poti :cosαi

To set the motors thrusting force, the algorithm, first defines if the
2.4. Path following and obstacle Avoidance Algorithm boat is in the right-hand or left-hand of the pass by comparing latitude of
the boat and desired path. Then it sets the desired thrusting force
Between path following algorithms, potential field method is one of

60
H. Mousazadeh et al. Ocean Engineering 159 (2018) 56–65

Fig. 2. The architecture of navigation, guidance and control.

Fig. 3. Morvarid navigation based on the combination of search ball and potential field.

according to Equ. (16).

8
>
> in right : Tstbd ¼ primforce þ z1 þ jδj þ 3ðψ  path angleÞ;
<
Tport ¼ primforce  z1  jδj  3ðψ  path angleÞ
(16)
>
> In left : Tstbd ¼ primforce  z1  jδj  3ðψ  path angleÞ;
:
Tport ¼ primforce þ z1 þ jδj þ 3ðψ  path angleÞ

According to Fig. 3, the z1 is minimum distance between USV and

path that is calculated using triangulation relations. Actually increasing
distance between the boat and the desired path, as well as increasing δ
causes more differences between two motors (more turning torque). Also
according to this equation, the boat forced in a way that differences of
heading and path angle is decreased. The prime_force is a variable,
defining the thrusting force when the USV is on the non-curved path
without any bias (Equ. (17)). Fig. 4. Obstacle avoidance senores and cover area of each system.

prim force ¼ k1  ynear goal1 goal2  5⋅z1 (17) speed vector (value and direction). If the detected obstacle was static,
considering the near goal point to USV and distance between obstacle
where k1 is a constant number for example; 30% of motors thrust. It is and USV, the two near goal points to the obstacle is defined according to
assumed that thrust of motors, T ¼ Tmax * rpm/rpmmax (Bertaska et al., Fig. 3. If the obstacle is located in the right-hand of the path, the algo-
2015). Variables ynear_goal1 and ynear_goal2 are component of goal positions rithm steers to left and vice versa, using Equ. (18), with some alteration
in y direction for two nearest goals and difference of these two cause to from (P^etres et al., 2012).
reduce the speed at curved paths (e.g. at head of the path). Also drift is 8
>
> kobs kobs
obstacle in right : Tport ¼ Tport  pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ; pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi
causes to reduce speed. >
<
Obstacle avoidance: To detect obstacles, three set of sensors were distobs þ 1 distobs þ 1
used. According to Fig. 4, a Lidar (Hokuyo's UXM series) was used in front >
> kobs kobs
>
: Obstacle In left : Tport ¼ Tport þ pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi; Tstbd ¼ Tstbd  pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi
of the boat that covers 190 . To cover the rear area, a stereoscopic system distobs þ 1 distobs þ 1
is under development using two CCD cameras and finally a set of ultra- (18)
sonic sensors using 10 waterproof modules (JSN-SR04T) were used that
detect near obstacles approximately all-around the vehicle. Where
Considering the operation environment of USV, two type of obstacles
kobs ¼ k1 ⋅*⋅obs width þ k2 (19)
would be categorized as; static obstacles and dynamic obstacles. When an
obstacle is detected inside the sight of any three mentioned sensors, the Here k1 and k2 are tunable scalars that are defined by trial and error.
obstacle function is activated and using clustering methods, four pa- In this case k2 is set as 50. The k1 is a variable that changes between 0 and
rameters would be extracted include; width of obstacle, central point, 35. If distance between obstacle and USV decrease in each step, k1

61
H. Mousazadeh et al. Ocean Engineering 159 (2018) 56–65

Fig. 5. Path following by EKF estimator and combination of search ball and potential field algorithm.

increases and vice versa. The variable obs_width is the width of detected
obstacle and distobs is distance between USV and obstacle. According to
Equ. (18), when the boat gets closer to an obstacle, it steers with more
differential force. Although when an obstacle is very near, the motors'
thrust is going to be a very large number, but it is limited to 445 N. All
calculations for obstacle position was based on its CG point. However,
considering the dimension of obstacle and according to Equ. (19), dis-
tance from obstacle is kept based on k1 and k2 values.
About the dynamic obstacles, some potential scenarios would be
considered;
i) The obstacle is detected by Lidar; considering the speed vectors of
USV and obstacle and extension of these vectors, if these two
trajectories are intersect inside the sight of the sensor, some ac-
tions are required. First, the algorithm calculates all mentioned
steps above to reduce the speed (one gear below) of the USV, and
this is repeated until the forward speed of USV becomes zero. If
the algorithm predicts a collision even with stopping the USV, this
means that the obstacle is coming directly toward the USV, so it
goes backward in 45 to right or left. However, if trajectories of
two speed vectors are not intersect in special speed, the USV runs
with that speed.
ii) The obstacle is detected by 3D vision: in this case only obstacles
are required attention that come toward and faster than USV.
However, if it is estimated that collision is probable, the USV steer
to right or left with 45 .

Fig. 6. (A) Surge speed, (b) yaw angular velocity, (c) orientation angle, (d) thrust force from starboard propeller, (e) thrust force from port propeller and (f)
turning torque.

62
H. Mousazadeh et al.
Fig. 7. Obstacle avoidance with two obstacles.
iii) The obstacle is detected by ultrasonic: this means that USV is very
close to obstacle. The boat activates emergency cut-off switch and
all warning lights and signals are activated.
For Morvarid USV the first test is performed in real situation and
useful experimental data is obtained from some sensors. However, to
evaluate the robustness and confidantes of the developed NGC algorithm,
before second test, it was simulated as hardware-in-the-loop by using
Lidar, GPS, IMU/compass and ultrasonic sensors as a real data acquisition
sources. The algorithm was programmed and developed using visual C#
in Microsoft Visual Studio software.
3. Results and discussions
To plot the hydrography map of the purposed port, all area was
divided to straight lines that were connected by U turns at the ends. Many
points on the lines were regarded and 2D position of points were defined
in earth fixed frame. Fig. 5 illustrates three of these lines with two end
turns. As shown in the figure, the lines were 800 m in length and 150 m
distance between them. The predefined path was drawn by blue line and
GPS data was pointed by green, while the estimated positions were
shown by red line. For careful illustration, the GPS data was drawn by
multiplying data to 10 in y direction. For border conditions in starting of
the algorithm, the state vector x0 was assumed as x0 ¼ P0 ¼ {0, 10, 0, 1, 0,
0}.
According to Fig. 5, the developed algorithm followed the straight
paths very precisely, while some errors were present for U turns. At the
end of the path, since there was no more goal point, the USV started to
fluctuate. Inserting some disturbing forces in the algorithm had no effect
on the navigation. However, if this force increased very much the USV
lost its paths. According to the simulation results, disturbance forces in
sway was acceptable, when its approximated value was less than the
thrusting force in surge.
Fig. 6 shows some main evaluated parameters versus time steps. In
this figure (a) is the surge velocity curve versus time. This speed set to be
1 m/s in the start-up. In the straight path, speed was increased a little and
continued approximately constantly. As it was expected, the speed
decreased at the turning so the boat could navigate precisely. Since in
Fig. 6, evaluated parameters were traced versus time steps and because of
decreasing speed in the turning, according to (a) the time that USV spend
in the straight path seems to be equal to that of turning. Fig. 6-b shows
the yaw angle rate of USV. The maximum value of r was approximately
63
Ocean Engineering 159 (2018) 56–65
12 /s. Fig. 6-c illustrates the orientation of boat based on east direction.
According to this chart, in the first straight line, the ψ is zero. In the first U
turning it was increased till to 180 and continued to second turning,
then it was decreased to zero for third straight path. Fig. 6 –d and e are
propellers thrusting force for starboard and port respectively. For straight
path, they were adjusted equally to 60 N that was approximately fit to
second gear of motors. This result approximately confirms the experi-
mental results that forward velocity of 1 m/s was obtained from second
gear. At the first turning, since the boat turns to left side, the starboard
thrust was increased, while the port was decreased and in the second
turning this occurred vice versa. According to these figures the maximum
value of thrusting force was limited to 150 N. Finally Fig. 6-f shows
turning torque that was derived from Equ. (5).
Result of obstacle avoidance algorithm is shown in Fig. 7. Two
different obstacles in two different positions were regarded. The first
obstacle's width was 70 m and the second was 100 m. The algorithm was
evaluated by changing the position and size of the obstacle in different
cases. Distance of USV from obstacle was simply controlled with variable
kobs (Equ. (19)). In the algorithm, if distance between USV and obstacle
was incremental, the kobs was decreased and vice versa. During naviga-
tion between lines when the USV lost its former path for any reason (such
as a wide obstacle) and reached to a new line it started a new navigation
and kept going forward.
Fig. 8 shows surge velocity, angular velocity about yaw, orientation,
thrust force in port, thrust force in starboard and turning torque
respectively in (a) to (f) in presence of obstacles illustrated in Fig. 7.
According to (a) the surge velocity was decreased when the boat got close
to turnings and obstacles. The angular velocity about yaw was maximum
to 15 and depends on the boat body characteristics as well as thrusting
motors capabilities. In the (c), orientation angle is illustrated while its
maximum value goes approximately up to 200 . Figure (8- d and e) shows
the thrusting force from starboard and port motors. It was desirable to
obtain even changes in motors speed, since uneven changes in revolution
of motors is not favorite and sometimes impossible. Finally (f) shows the
thrusting force that causes steering. According to Fig. 6, the developed
algorithm navigated and controlled the USV in a reasonable manner.
Although using powerful propellers would increase maneuverability and
provide more precise navigation especially in presence of obstacles.
4. Conclusion
It has been proven that multiple motion sensors play a vital role in
H. Mousazadeh et al.
Fig. 8. (A) Surge velocity, (b) angular velocity about yaw, (c) orientation, (d) thrust force in starboard, (e) thrust force in port and (f) turning torque.
autonomous navigation. The Morvarid an autonomous USV was equip-
ped with multiple sensors to plot the hydrography map in shallow waters.
Dynamic model of the boat was derived and some main parameters of
rigid body model was defined based on experimental evaluations. For the
best estimation of position and states, the EKF estimator was used in
navigation algorithm while for navigation and obstacle avoidance in path
following algorithm, combination of search ball and potential field
methods were applied. Real data was provided for algorithm simulation
based on hardware-in-the-loop concept, connecting GPS, IMU/compass,
Lidar and ultrasonic sensors to a developed application in visual C#. The
algorithm was evaluated in different circumstances with different values
of environmental disturbances (wind, wave and current) as well as
different obstacles. The traced curves, from main evaluated parameters,
were illustrated the robustness and precision of the developed algorithm.
However, installing powerful thrusters would boost maneuverability
which in turn would smooth the curves.
Acknowledgment
Authors would like to acknowledge the Ports and Maritime Organi-
zation for funding the Morvarid Project No. 20S/7509. 2015.
References

Bertaska, I.R., Shah, B., Ellenrieder, K., Svec, P., Klinger, W., Sinisterra, A.J., Dhanak, M.,
Gupta, S.K., 2015. Experimental evaluation of automatically-generated behaviors.
Ocean. Eng. 106, 496–514.
Bibuli, M., Bruzzone, G., Caccia, M., Lapierre, L., 2009. Path-following algorithms and
experiments for an unmanned surface vehicle. J. Field Robotics 26, 669–688.
Caccia, M., 2006. Autonomous surface craft prototypes and basic research issues. In:
Control and Automation. MED '06. 14th Mediterranean Conference. IEEE, Pp,
pp. 1–6.
Caccia, M., Bono, R., Bruzzone, G., Bruzzone, G., Spirandelli, e., Veruggio, G., Maria
Stortini, A., Capodaglio, G., 2005. An autonomous craft for the study of sea-air
interactions. IEEE Robotics Automation Mag. 95–105.
Caccia, M., Bibuli, M., Bono, R., Bruzzone, G., 2008. Basic navigation, guidance and
control of an unmanned surface vehicle. Auton. Robot. 25, 349–365.
Campbell, S., Naeem, W., Irwin, G.W., 2012. A review on improving the autonomy of
unmanned surface vehicles through intelligent collision avoidance manoeuvres.
Annu. Rev. Control 36, 267–283.
64
Ocean Engineering 159 (2018) 56–65
Doa, K.D., Jiang, Z.P., Pana, J., 2004. Robust adaptive path following of underactuated
ships. Automatica 40, 929–944.
Dong, W., Guo, Y., 2005. Nonlinear tracking control of underactuated surface vessel. In:
American Control Conference June 8-10. Portland, OR, USA.
Eydgahi, A., Godaliyadda, D., Falase, S., 2007. A simulation software for autonomous
navigation of unmanned surface vehicles using MATLAB environment. Int. J. Mod.
Eng. 7 (2).
Fossen, T.I., 1994. Guidance and Control of Ocean Vehicles. John Wiley and sons.
Fossen, T.I., 2011. Handbook of marine Craft Hydrodynamics and Motion Control, first
ed. John Wiley & Sons Ltd.
Gomez-Gil, J., Ruiz-Gonzalez, R., Alonso-Garcia, S., Gomez-Gil, F.J., 2013. A Kalman
Filter implementation for precision improvement in low-cost GPS positioning of
tractors. Sensors 13, 15307–15323.
Kang, M., Kwon, S., Park, J., Kim, T., Han, J., Wang, J., Hong, S., Shim, Y., Yoon, S.,
Yoo, B., Kim, J., 2015. Development of USV autonomy for the 2014 maritime RobotX
challenge. IFAC-Papers Line 48 (16), 13–18.
Kjerstad, Q.K., 2009. Dynamic Positioning Concepts for Unmanned Surface Vehicles.
Master Thesis in Department of Engineering Cybernetics. Norwegian University of
Science and Technology.
Liu, Y., Bucknall, R., 2015. Path planning algorithm for unmanned surface vehicle
formations in a practical maritime environment. Ocean. Eng. 97, 126–144.
Ma, E., 2015. Two modified unscented Kalman filter and acceleration information in
unmanned surface vehicle estimation. IFAC-Papers Line 48 (28), 1450–1455.
Mas, F.R., Zhang, Q., Hansen, A.C., 2010. Mechatronics and Intelligent Systems for Off-
road Vehicles. Springer, pp. 62–65.
Motwani, A., Sharma, S.K., Sutton, R., Culverhouse, P., 2013. Interval Kalman Filter in
navigation system design for an uninhabited surface vehicle. J. navigation 66,
639–652.
Muske, K.R., Ashrafiuon, H., 2008. Identification of a control oriented nonlinear dynamic
USV model. In: American Control Conference. Westin Seattle Hotel, Seattle,
Washington, USA.
Naeem, W., Xu, T., Sutton, R., Chudley, J., 2007. Design of an unmanned catamaran with
pollutant tracking and surveying capabilities. Publ. by Institution Eng. Technol.
99–113.
Naeem, W., Xu1, T., Sutton, R., Tiano, A., 2008. The design of a navigation, guidance, and
control system for an unmanned surface vehicle for environmental monitoring.
J. Eng. Marit. Environ. 222 (Part M), 67–79.
Naeem, W., Sutton, R., Xu, T., 2012. An integrated multi-sensor data fusion algorithm and
autopilot implementation in an uninhabited surface craft. Ocean. Eng. 39, 43–52.
Nađ, D., Miskovi, N., Mandi, F., 2015. Navigation, guidance and control of an
overactuated marine surface vehicle. Annu. Rev. Control 40, 172–181.
Onunka, C., Bright, G., Stopforth, R., 2013. Probabilistic uncertainty identification
modelling in USV obstacle detection. R D J. South Afr. Institution Mech. Eng. 29,
36–43.
Park, J.H., Shim, H.W., Jun, B.H., Kim, S.M., Lee, P.M., Lim, Y.K., 2010. A Model
Estimation and Multi-variable Control of an Unmanned Surface Vehicle with Two
Fixed Thrusters, 978-1-4244-5222-4/10/$26.00 ©IEEE.
H. Mousazadeh et al. Ocean Engineering 159 (2018) 56–65

P^etres, C., Romero-Ramirez, M.A., Plumet, F., 2012. A potential field approach for L: Length of the submerged portion of boat
reactive navigation of autonomous sailboats. Robotics Aut. Syst. 60, 1520–1527. m: Actual mass
Praczyk, T., 2015. Neural anti-collision system for autonomous surface vehicle. mjj: Inertia including added mass
Neurocomputing 149, 559–572. MRB: Rigid-body inertia matrix
Rasal, K., 2013. Navigation & Control of an Automated SWATH Surface Vessel for p: Angular velocity around roll
Bathymetric Mapping. Mechanical Engineering Masters Theses. Santa Clara q: Angular velocity around pitch
University. Qk: Covariance matrix of the system noise
Reyhanoglu, M., 1996. Control and stabilization of an underactuated surface vessel. In: r: Angular velocity around yaw
Proceedings of the 35th Conference on Decision and Control Kobe, Japan. Rk: Covariance matrix of observation noise
Rodríguez, M., G omez, J., 2009. Analysis of three different Kalman filter implementations T: Time step
for agricultural vehicle positioning. Agric. J. 3, 13–19. Tport: Portside thrusting force
Sagginia, E., Zereika, E., Bibulia, M., Ranieria, A., Bruzzonea, G., Cacciaa, M., Tstbd: Starboard thrusting force
Riccomagno, E., 2015. Evaluation and comparison of navigation guidance and u: Velocity in surge
control system for 2D surface path-following. Annu. Rev. control 40, 182–190. U: Maximum operating speed
Sonnenburg, C., Gadre, A., Horner, D., Krageland, S., Marcus, A., Stilwell, D.J., v: Velocity in sway
Woolsey, C.A., 2010. Control-oriented Planar Motion Modeling of Unmanned Surface vk: White noise measurement
Vehicles. Virginia Center for Autonomous Systems. Technical Report No. VaCAS- w: Velocity in heave
2010-01. wk: White noise process
Tang, P., Zhang, R., Liu, D., Huang, L., Liu, G., Deng, T., 2015. Local reactive obstacle W: Width of boat
avoidance approach for high-speed unmanned surface vehicle. Ocean. Eng. 106, xk: State vector
128–140. z1: Minimum distance between USV and path
Zereik, E., Sorbara, A., Bibuli, M., Bruzzone, G., Caccia, M., 2015. Priority task approach zk: Measurement vector
for usvs' path following missions with obstacle avoidance and speed regulation. IFAC- α: Deflection angle of USV from each point
Papers Line 48 (16), 25–30. δ: Required orientation of USV
η: Position and orientation vector
θ: Orientation in pitch earth-fixed frame
Glossary ν: velocity vector in body-frame
ρ: Density of water
τ: Vector of control input
B: Distance between two thrusters
τw: Environmental disturbances
Br: Search ball radius
φ: Orientation in roll earth-fixed frame
CRB(ν): Matrix of Coriolis and centripetal terms
ψ : Orientation in yaw earth-fixed frameAbbreviation
dij: Euclidean distance from USV to goal points
ASV: Autonomous Surface Vehicle
dr: Hydrodynamic damping in yaw
AUV: Autonomous Underwater Vehicles
du: Hydrodynamic damping in surge
EKF: Extended Kalman Filter
dv: Hydrodynamic damping in sway
GPS: Global Positioning System
D: Mean submerged depth of boat
IMU: Inrtial Measurement Unit
DRB(ν): Damping matrix
Lidar: Light detection and Ranging
Fr: Froude number
NGC: Navigation, Guidance and Control
g(η): Vector of gravitational forces and moments
PID: Proportional, Integral, Differential
H: Coafficent matrix
USV: Unmanned Surface Vehicle
J(η): Transformation matrix

65