Developing a navigation, guidance and obstacle avoidance algorithm for an Unmanned Surface Vehicle (USV) by algorithms fusion

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 b

Mechanical Engineering of Biosystems, University of Tehran, Iran 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: Navigation Obstacle avoidance Kalman filter Search ball Thrust

Many USVs are equipped with navigation, guidance and control (NGC) algorithms due to human errors that is present in manual navigation of surface vehicles. The Morvarid, a catamaran shaped boat is designed and developed for plotting hydrography map of harbours. To control it autonomously, a fusion algorithm based on EKF, search ball and potential field concept is developed. Some main parameters of dynamic model is derived based on experimental tests and the algorithm is evaluated using hardware-in-the-loop concept. The obstacle 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 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 capability, high mobility, and low price (Tang et al., 2015). This surface vehicles are subjected to many objectives, either alone or as part of a sensor network. Some well-known applications for USVs include marine environment 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

step of work. Considering potential candidates, classical hull shape (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 supported by both the hydrostatic and hydrodynamic force (semi-displacement vessel) (Fossen, 2011). In USVs the Navigation, Guidance and Control (NGC) system intelligently 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: [email protected] (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.

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 information from reviewed texts are given in Table 1. Some design characteristics, Thrust force, maximum speed, application field and navigation algorithms are selected information that is extracted from some published 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 parameters, 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.

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 demonstrated 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 equipped 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 timederivative 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 performance. And e) Cooperative motion control: a leader and a follower vehicle following two parallel paths (Master-Slave) (Caccia, 2006).

2. Materials and methods This section first considers the designed and constructed USV platform. 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 twinhulled catamaran design is selected due to its stability and simplicity of steering mechanism which is steered using differential thrust. The Morvarid 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 between 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.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 networks 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 formation 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

1

57

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 Height (m)

Turning diameter (m)

Speedmax (m/s)

Weight (kg)

Application

Navigation algorithm

Ref.

(Bertaska et al., 2015) (Bertaska et al., 2015) (Bertaska et al., 2015) (Naeem et al., 2012)

type

Length (m)

DUKWLing USV14

twin hull

2.7

266 N

10

1

Path planning

twin hull

4.3

205 N

12

2.8

Path planning

USV12

twin hull

3.7

200 N

3.6

Path planning

Springer

catamaran

4

2.3

653 N

1.5

catamaran

1.5

0.8

266 N

1

Charlie

catamaran

2.4

1.7

Ribcraft 4.8 SeaFox

Hull

4.5

Hull

4.9

Pontoon

pontoon

3

Hull with two motors

0.5

0.14

Swath

3.4

1.3

1.3

sailboat

1.38

0.36

2.2

Catamaran

3

KAIST

Width (m)

Thrust

600

0.4

300

1.5

600 W

50 hp

10.3

250 hp 353 N

20.6 2

1.6

360

485 N

0.78

Pollutant tracking environmental monitoring Survey shallow costal area and support works of underwater vehicles Sampling sea surface microlayer Research in autonomous Military Investigate motion control Surveillance, search and rescue Bathymetry

Fuzzy LQG and GA-based MPC PD controller Linear Quadratic (LQ)

(Park et al., 2010)

PID - EKF

(Bibuli et al., 2009). (Caccia et al., 2005) (Sonnenburg et al., 2010) (Sonnenburg et al., 2010) (Sonnenburg et al., 2010) (Muske and Ashrafiuon, 2008) (Rasal, 2013)

Waypoint following Remote control and LSE

Trajectory planning

Trial

Path-following and trajectory controller Potential fields

RobotX Challenge

A*

(P^etres et al., 2012) (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.

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 position and φ, θ and ψ are orientation in earth-fixed frame. Also u, v and w, are velocity respectively in surge, sway and heave and p, q, and r, are angular velocity respectively around roll, pitch and yaw. For surface vehicles the depth z and pitch θ variables are not applicable. Also the roll φ variation is found to be negligible during experimental evaluations (Naeem et al., 2012). For simplicity ignoring the swing (z  cons. and we0), pitch (θe0 and qe0) and roll (φe0 and pe0), the kinematic model in Eq. (1), which describes the geometrical relationship between the earth-fixed and the vehicle fixed motion is given as:

2.2. Rigid body model This section discuss deriving the dynamic model of the marine vehicle in six degrees of freedom (DOF) since six independent coordinates are necessary to determine the position and orientation of a rigid body. According to Fossen (1996) (Fossen, 1994) It is assumed two coordinate frames; the moving coordinate frame that is fixed to the vehicle and is called body-fixed reference and an earth fixed reference frame. As shown in Fig. 1, the origin of the body-fixed frame is usually chosen to coincide with the center of gravity (CG). The kinematic equation for transforming body-fixed reference to earth-fixed one can be expressed as (Fossen, 1994);

η_ ¼ JðηÞ:ν

2

2 3 x_ cosψ 4 y_ 5 ¼ 4 sinψ 0 ψ_

(1)

Where η ¼ [x, y, z, φ, θ, ψ] denotes the position and orientation vector with coordinates in the earth-fixed frame, ν ¼ [u, v, w, p, q, r]T is the generalized linear and angular velocity vector in body-frame T

sinψ cosψ 0

32 3 u 0 0 5:4 υ 5 1 r

(2)

According to Newtonian and Lagrangian mechanics, nonlinear dynamic equation of motion can be conventionally expressed as (Fossen, 58

H. Mousazadeh et al.

Ocean Engineering 159 (2018) 56–65

2008), (Doa et al., 2004) and (Reyhanoglu, 1996). The time-varying terms, τwu(t), τwv(t) and τwr(t), are the environmental disturbances induced by wave, wind and ocean-current with jτwu(t)j  τwu max < ∞, jτwv(t)j  τwv max < ∞ and jτwr(t)j  τwr max < ∞. Finally, the available controls are the surge force τu and the yaw moment τr that are calculated as Equ. (5). The parameters; added mass and damping coefficients are the unknown variables that would be derived. Mass parameters include added mass contributions that represent hydraulic pressure forces and torque due to forced harmonic motion of the vessel which are proportional to acceleration. Using the estimate of the added mass terms presented in (Fossen, 1994) for the experimental model boat it would be derived as (Muske and Ashrafiuon, 2008):

1994): MRB :_υ þ CRB ðυÞ:υ þ DRB ðυÞ:υ þ gðηÞ ¼ τ

(3)

Where MRB denotes the rigid-body inertia matrix (including added mass) and MRB ¼ MTRB>0, and M_ RB ¼ 0; CRB(ν) is denotes the matrix of Coriolis and centripetal terms (including added mass) and CRB(ν) ¼ CRB(ν)T, DRB(ν) is damping matrix, g(η) is vector of gravitational forces and moments and τ ¼ [X, Y, Z, K, M, N]T is the vector of control input including force and moments in three directions. In this case since the inputs are only from two port and starboard motors the USV is deal as underactuated surface ship (fewer numbers of actuators than the to-becontrolled degrees of freedom). Considering Tport as portside thrusting force and Tstbd as starboard thrusting force each in surge direction, the control input vector become as:

τ ¼ ½τ u ; τ ν ; τ r T ¼

 





Tport þ Tstbd ; 0; Tport

 B T þ Tstbd : 2

8 > > > <

(5)

> > > :

Where B is the distance between two thrusting power sources (1.2 m 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; primarily, the turning radius for the vehicle is substantially reduced, as well as the additional capability to steer without the requirement of a forward speed (Bertaska et al., 2015). The mathematical model of an underactuated ship moving in surge, sway and yaw is obtained from the motion equation of the ship moving in six degrees of freedom under disturbances induced by wave, wind and ocean current. Furthermore it is assumed that the inertia, added mass and damping matrices are diagonal. This assumption holds when the vessels have three planes of symmetry, for which the axes of the body-fixed reference frame are chosen to be parallel to the principal axis of the displaced fluid, which are equal to the principal axis of the vessel. Most ships have port/starboard symmetry. Moreover, bottom/top symmetry is not required for horizontal motion. Ship fore/aft nonsymmetry implies that the off-diagonal terms of the inertia and damping matrices are nonzero. However, these terms are small compared to the main diagonal terms. So the underactuated ship moving in surge, sway and yaw can be described as (Doa et al., 2004):

m11  m þ 0:05m   m22  m þ 0:5 ρ:π :D2 :L m33 

mðL2 þ W 2 Þ þ

 1 0:1m:B2 þ ρ:π :D2 :L3 2 12

(7)

Where m ¼ 700 kg is the actual mass of the USV, L ¼ 3 m is the effective length, W ¼ 0.8 m is the width, D ¼ 0.5 m is the mean submerged depth, and ρ is the density of water. About damping coefficients, according to (Ma, 2015): 

du2 ¼ 0:2du ; du3 ¼ 0:1du ;

dv2 ¼ 0:2 dv ; dv3 ¼ 0:1 dv ;

dr2 ¼ 0:2 dr dr3 ¼ 0:1 dr

(8)

For defining du, dv and dr, combination of experimental results, simulation and conformance with some references are used. Considering that the damping parameters are dependent on boat shape and dimension, main related parameters of some USVs as well as Morvarid are given as Table 2; According to Table 2 the ratio of m11/du, m22/dv and m33/dr for all given USVs are in specific range. Inserting maximum values of these ratios in the simulation model and tuning their values till the surge and yaw speeds for predefined thrusting force and moments become stable, is continued. The predefined thrusting force is achieved from experimental results and is the minimum thrust force that could move the boat in the steady state (for this case is 0.2 of maximum thrust). Finally these ratios for Morvarid is defined as m11/du ¼ 10, m22/dv ¼ 5 and m33/dr ¼ 2.

8 x_ ¼ u:cos ψ  v:sin ψ > > > > > > > > y_ ¼ u:sin ψ þ v:cos ψ > > > > > ψ_ ¼ r > > > > X3 dui i1 < m22 du 1 1 ⋅v:r  :u  :τ u þ :τwu ðtÞ u_ ¼ juj u þ i¼2 m m11 m11 m11 m11 11 > > > > X3 dυi i1 > 1 > > v_ ¼ m11 :u:r  dυ :v  jvj v þ :τwυ ðtÞ > > i¼2 m > m22 m m 22 22 22 > > > > X3 dri i1 > ðm11  m22 Þ dυ 1 1 > > :u:r  :v  :τ r þ :τwr ðtÞ jrj :r þ : r_ ¼ i¼2 m m33 m33 m33 m22 33

2.3. EKF Algorithm and its parameters To best estimate the statues of the vehicle, application an estimator filter is common. In estimation theory, the extended Kalman filter (EKF) is the nonlinear version of the Kalman filter which linearizes about an estimate of the current mean and covariance. As it is the case in this study, the state transition and observation models do not need to be linear functions of the state, but may instead be differentiable functions. The EKF can be formulated as: Model:

(6) The positive constant terms mjj, where 1  j  3 denote the ship 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 the hydrodynamic damping in surge, sway and yaw. For simplicity, the higher nonlinear damping terms are ignored (Muske and Ashrafiuon,

(

xk ¼ f ðxk1 ; uk Þ þ wk ⇒ xk ¼ xk1 þ T:f ðxk1 ; uk Þ þ wk zk ¼ hðxk Þ þ vk ⇒ zk ¼ H:xk þ vk

Table 2 Mass parameters and damping coefficient of some USVs. m [kg] 118*10 1.61 – – 700

L [m] 3

38 0.4 – – 3

m11 [kg] 120*10 1.69 25.8 200 735

3

m33 [kgm2]

m22 [kg] 177.9*10 1.8 33.8 250 1453

3

636*10 0.027 2.7 80 1150

5

du [kg/s] 2

215*10 2.72 2 70 ?

59

dv [kg/s] 147*10 13.4 7 100 ?

3

dr [kgm2/s]

Ref.

802*104 0.05 0.5 50 ?

(Doa et al., 2004) (Muske and Ashrafiuon, 2008) (Kjerstad, 2009) (Reyhanoglu, 1996), (Dong and Guo, 2005) Morvarid

H. Mousazadeh et al.

Ocean Engineering 159 (2018) 56–65

the most well-known algorithm that operates according to magnetic potential field (i.e. attractive or repulsive force are inversely proportional to distance from destination point). This algorithm have been widely and successfully employed for control and motion planning of mobile robots for decades. The potential field based algorithms are fast, reliable and easy to tune once implemented on real platforms (P^etres et al., 2012). In artificial potential field method, movements of the robot (represented as a particle) are governed by a field, which is usually composed of two components; an attractive potential drawing the robot towards the goal and a repulsive potential pushing the robot away from obstacles. Approximately in all developed algorithms the destination point is a single point that attract the vehicle. Due to this, the main drawback with potential field techniques is their susceptibility to local minima. To dissolve this problem, an algorithm is developed based on the combination of potential field and research ball algorithms (Mas et al., 2010). This concept, is illustrated in Fig. 2, as NGC algorithm. For each point on the navigation course a search is conducted to find two nearest goal points on the reference path (The algorithm first finds the nearest path line in whole area, and then searches on that line to complete the search ball). Once determined, the distance of these points from the USV represents the greatness of the attractive force. The process continues for all of the components in the examined trajectory. The search is spatially bounded by the user-defined search ball, that here it is selected as equal as 2*L (L is boat length) (Fossen, 1994), (Naeem et al., 2008). Actually, a Line-Of-Sight (LOS) maneuvering system was used to control USV position and transition between waypoints in a trajectory as in Fossen (2011). This system used the vehicle's current position and desired positions to determine a desired heading. The radius of the search ball, Br, allows a certain level of flexibility on the user side; however, a well-adjusted magnitude makes the algorithm more computationally efficient. The search ball algorithm can be mathematically 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 reference path points inside the search ball. If E and N represent the east and north coordinates respectively, for each j, a new set Bj ¼ {i ε T/ dij < Br} can be defined, where distances dij are calculated with Equ. (12).

Predict: (

bx k ¼ f ðbx k1 ; uk Þ⇒ bx k ¼ bx k1 þ f ðbx k1 ; uk Þ T Pk ¼ Fk1 Pk1 Fk1 þ Qk1

(9)

Update: 8 1  < Kk ¼ Pk HkT Hk Pk HkT þ R bx ¼ bx k þ Kk ðzk  hðbx k ÞÞ : k Pk ¼ ðI  Kk Hk ÞPk Since three degree-of- freedom (DOF) motion composed of surge, sway, and yaw was considered for the filter dynamics, the USV's motion was estimated using position and attitude measurements provided by the GPS, compass and IMU. Here, xk ¼ [x, y, ψ, u, ν, r]T is state vector in the _ ν_ , r]T is measurement vector that the first and step of kth, zk ¼ [x, y, ψ, u, second parameters are measured bys GPS, the 3rd parameter is measured by compass and the last three are measured by IMU. The state estimation function, f (xk-1, uk) is a function of state parameters and control signal, uk and is given according to Equ. (6). T is time step and H is jacobian of h (xk) that here would be a function of T (as given below) also Fk-1 is jacobian of f (xk-1, uk). The jacobian matrix, J for an arbitrary function, f is an m*n matrix that is given as Equ. (10). 2

∂ f1 ∂   6 6 x1 df ∂f ∂f 6 J¼ ¼ ¼6 ⋮ … 6 dx ∂x1 ∂xn 4 ∂fm ∂x1

3

⋯

∂f1 ∂xn 7 7

7 ⋮ 7 7 ∂fm 5 ⋯ ∂x1

⋱

(10)

The wk and vk in Equ. (9) are white noise for process and measurement respectively which are the characterization of system modeling errors and sensors' uncertainty. Furthermore the following conditions are simultaneously satisfied (Rodríguez and G omez, 2009): E½wk  ¼ 0;

    E½vk  ¼ 0; E wk :wTi ¼ δðk  iÞ:Qk ; υk :υTi ¼ δðk  iÞ:Rk (11)



1; n¼0 is the Kronecker delta function, Qk is 0; n 6¼ 0 the covariance matrix of the system noise, and Rk is the covariance matrix of the observation noise that all of these are positive parameters. The adjustment of Qk and Rk is critical in the development of Kalman filter. Although Estimating of Rk is not very sophisticated, about Qk there are some methods for these parameters definition, e.g. Monte Carlo (Gomez-Gil et al., 2013). However, in this research Rk estimated as: Where δðnÞ ¼

2 6 6 6 6 R¼6 6 6 4 2

σ 2xðGPSÞ

σ

0 0 0 0 0

1 6 60 6 6 60 6 6 ¼ 60 6 6 6 6 60 4

0

0 0

0 0 0 0

σ 2ψ ðcomp:Þ

2 yðGPSÞ

0

0

0

0

1

0

0

0

0

1

0

0

0

0 1 T

0

0

0

0 0

0

0

0 1 T 0

0

0 0 0 3

0 0 0

σ

2 _ uðIMUÞ

0 0

0 0 0 0

σ 2υ_ ðIMUÞ 0

0 0 0 0 0

dij ¼

qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ½EðiÞ  EðjÞ2 þ ½NðiÞ  NðjÞ2

(12)

The members in the set of Bj is sorted from min to max, to obtain the minimum distance and its number. Actually the algorithm determines two nearest goal points to USV. It is necessary to mention that, as shown 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 point is calculated using Equ. (13).

3 7 7 7 7 7; H 7 7 5

αi ¼ Arctan

½NðiÞ  NðjÞ ½EðiÞ  EðjÞ

(13)

To attract the USV toward the goals, a linear potential, pot is built for every path points as follows (P^etres et al., 2012):

σ 2rðIMUÞ

poti ¼ k2 ⋅*⋅dij ⋅ðhere⋅i ¼ 1⋅to⋅4Þ

7 07 7 7 07 7 7 07 7 7 7 7 07 5

(14)

where k2 is the desired attractive gradient and dij is the Euclidean distance between P (xj, yj) and ith goal. Finally, the real destination point and the required orientation of USV that is shown by red vector in Fig. 3, is calculated from the result of calculated vectors using the Equ. (15). P poti : sinαi δ ¼ Arctan Pi i poti :cosαi

1

(15)

To set the motors thrusting force, the algorithm, first defines if the 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

2.4. Path following and obstacle Avoidance Algorithm 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Þ In left : Tstbd ¼ primforce  z1  jδj  3ðψ  path angleÞ; > > : Tport ¼ primforce þ z1 þ jδj þ 3ðψ  path angleÞ

(16)

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)).  prim force ¼ k1  ynear

goal1 goal2

 5⋅z1

Fig. 4. Obstacle avoidance senores and cover area of each system.

speed vector (value and direction). If the detected obstacle was static, considering the near goal point to USV and distance between obstacle and USV, the two near goal points to the obstacle is defined according to Fig. 3. If the obstacle is located in the right-hand of the path, the algorithm steers to left and vice versa, using Equ. (18), with some alteration from (P^etres et al., 2012).

(17)

where k1 is a constant number for example; 30% of motors thrust. It is assumed that thrust of motors, T ¼ Tmax * rpm/rpmmax (Bertaska et al., 2015). Variables ynear_goal1 and ynear_goal2 are component of goal positions in y direction for two nearest goals and difference of these two cause to reduce the speed at curved paths (e.g. at head of the path). Also drift is causes to reduce speed. Obstacle avoidance: To detect obstacles, three set of sensors were used. According to Fig. 4, a Lidar (Hokuyo's UXM series) was used in front of the boat that covers 190 . To cover the rear area, a stereoscopic system is under development using two CCD cameras and finally a set of ultrasonic sensors using 10 waterproof modules (JSN-SR04T) were used that detect near obstacles approximately all-around the vehicle. Considering the operation environment of USV, two type of obstacles would be categorized as; static obstacles and dynamic obstacles. When an obstacle is detected inside the sight of any three mentioned sensors, the obstacle function is activated and using clustering methods, four parameters would be extracted include; width of obstacle, central point,

8 > > > <

kobs kobs obstacle in right : Tport ¼ Tport  pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ; pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi distobs þ 1 distobs þ 1

> kobs kobs > > : Obstacle In left : Tport ¼ Tport þ pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi; Tstbd ¼ Tstbd  pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi distobs þ 1 distobs þ 1 (18) Where kobs ¼ k1 ⋅*⋅obs width þ k2

(19)

Here k1 and k2 are tunable scalars that are defined by trial and error. In this case k2 is set as 50. The k1 is a variable that changes between 0 and 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.

trajectories are intersect inside the sight of the sensor, some actions 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 .

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), distance 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

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.

Ocean Engineering 159 (2018) 56–65

Fig. 7. Obstacle avoidance with two obstacles.

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 experimental 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 navigation 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.

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

4. Conclusion It has been proven that multiple motion sensors play a vital role in

63

H. Mousazadeh et al.

Ocean Engineering 159 (2018) 56–65

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 equipped 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.

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 Offroad 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.

Acknowledgment Authors would like to acknowledge the Ports and Maritime Organization 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

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 reactive navigation of autonomous sailboats. Robotics Aut. Syst. 60, 1520–1527. Praczyk, T., 2015. Neural anti-collision system for autonomous surface vehicle. Neurocomputing 149, 559–572. Rasal, K., 2013. Navigation & Control of an Automated SWATH Surface Vessel for Bathymetric Mapping. Mechanical Engineering Masters Theses. Santa Clara University. Reyhanoglu, M., 1996. Control and stabilization of an underactuated surface vessel. In: Proceedings of the 35th Conference on Decision and Control Kobe, Japan. Rodríguez, M., G omez, J., 2009. Analysis of three different Kalman filter implementations for agricultural vehicle positioning. Agric. J. 3, 13–19. Sagginia, E., Zereika, E., Bibulia, M., Ranieria, A., Bruzzonea, G., Cacciaa, M., Riccomagno, E., 2015. Evaluation and comparison of navigation guidance and control system for 2D surface path-following. Annu. Rev. control 40, 182–190. Sonnenburg, C., Gadre, A., Horner, D., Krageland, S., Marcus, A., Stilwell, D.J., Woolsey, C.A., 2010. Control-oriented Planar Motion Modeling of Unmanned Surface Vehicles. Virginia Center for Autonomous Systems. Technical Report No. VaCAS2010-01. Tang, P., Zhang, R., Liu, D., Huang, L., Liu, G., Deng, T., 2015. Local reactive obstacle avoidance approach for high-speed unmanned surface vehicle. Ocean. Eng. 106, 128–140. Zereik, E., Sorbara, A., Bibuli, M., Bruzzone, G., Caccia, M., 2015. Priority task approach for usvs' path following missions with obstacle avoidance and speed regulation. IFACPapers Line 48 (16), 25–30.

L: Length of the submerged portion of boat m: Actual mass mjj: Inertia including added mass MRB: Rigid-body inertia matrix p: Angular velocity around roll q: Angular velocity around pitch Qk: Covariance matrix of the system noise r: Angular velocity around yaw Rk: Covariance matrix of observation noise T: Time step Tport: Portside thrusting force Tstbd: Starboard thrusting force u: Velocity in surge U: Maximum operating speed v: Velocity in sway vk: White noise measurement w: Velocity in heave wk: White noise process W: Width of boat xk: State vector z1: Minimum distance between USV and path zk: Measurement vector α: Deflection angle of USV from each point δ: Required orientation of USV η: Position and orientation vector θ: Orientation in pitch earth-fixed frame ν: velocity vector in body-frame ρ: Density of water τ: Vector of control input τw: Environmental disturbances φ: Orientation in roll earth-fixed frame ψ : Orientation in yaw earth-fixed frameAbbreviation ASV: Autonomous Surface Vehicle AUV: Autonomous Underwater Vehicles EKF: Extended Kalman Filter GPS: Global Positioning System IMU: Inrtial Measurement Unit Lidar: Light detection and Ranging NGC: Navigation, Guidance and Control PID: Proportional, Integral, Differential USV: Unmanned Surface Vehicle

Glossary B: Distance between two thrusters Br: Search ball radius CRB(ν): Matrix of Coriolis and centripetal terms dij: Euclidean distance from USV to goal points dr: Hydrodynamic damping in yaw du: Hydrodynamic damping in surge dv: Hydrodynamic damping in sway D: Mean submerged depth of boat DRB(ν): Damping matrix Fr: Froude number g(η): Vector of gravitational forces and moments H: Coafficent matrix J(η): Transformation matrix

65