Get 20M+ Full-Text Papers For Less Than $1.50/day. Start a 14-Day Trial for You or Your Team.

Learn More →

A Real-Time Reaction Obstacle Avoidance Algorithm for Autonomous Underwater Vehicles in Unknown Environments

A Real-Time Reaction Obstacle Avoidance Algorithm for Autonomous Underwater Vehicles in Unknown... sensors Article A Real-Time Reaction Obstacle Avoidance Algorithm for Autonomous Underwater Vehicles in Unknown Environments Zheping Yan, Jiyun Li *, Gengshi Zhang and Yi Wu Marine Assembly and Automatic Technology Institute, College of Automation, Harbin Engineering University, Harbin 150001, China; yanzheping@hrbeu.edu.cn (Z.Y.); zhanggengshi@hrbeu.edu.cn (G.Z.); wuyi_ivy@hrbeu.edu.cn (Y.W.) * Correspondence: jiyun5409@hrbeu.edu.cn; Tel.: +86-188-4642-2618 Received: 22 December 2017; Accepted: 29 January 2018; Published: 2 February 2018 Abstract: A novel real-time reaction obstacle avoidance algorithm (RRA) is proposed for autonomous underwater vehicles (AUVs) that must adapt to unknown complex terrains, based on forward looking sonar (FLS). To accomplish this algorithm, obstacle avoidance rules are planned, and the RRA processes are split into five steps Introduction only lists 4 so AUVs can rapidly respond to various environment obstacles. The largest polar angle algorithm (LPAA) is designed to change detected obstacle’s irregular outline into a convex polygon, which simplifies the obstacle avoidance process. A solution is designed to solve the trapping problem existing in U-shape obstacle avoidance by an outline memory algorithm. Finally, simulations in three unknown obstacle scenes are carried out to demonstrate the performance of this algorithm, where the obtained obstacle avoidance trajectories are safety, smooth and near-optimal. Keywords: autonomous underwater vehicle; forward looking sonar; obstacle avoidance; reaction obstacle avoidance algorithm 1. Introduction Recently, the question of collision avoidance for AUVs has attracted much attention from the ocean engineering and control community due to the wide commercial and military applications of AUVs, such as submarine cable inspection, submarine petroleum pipeline checking, underwater topographic surveying, ocean resource detection and oil field exploitation [1–5]. Currently, plenty of approaches were proposed for the obstacle avoidance of AUVs, such as artificial potential field algorithm (APF) [6,7], Dijkstra’s algorithm, A* algorithm, grid method, particle swarm optimization (PSO) [8,9], fuzzy algorithm [10–12], neuro-fuzzy algorithm [13,14], bio-inspired algorithm [15], etc. These methods each have their own advantages and special features, while they also have some inherent deficiencies. The artificial potential field method is characterized by a simple structure and easiness to implement, however, it is apt to be trapped in a local minimum when AUVs run into clustered obstacles environments or narrow channels [16,17], the same drawbacks can also be found in genetic algorithm (GA) [18]. Different from the aforementioned approaches, the grid method is especially suitable for terrains with known obstacles due to the capability of searching for the optimal collision avoidance path, however, the storage expense and calculation burden are disadvantages limiting its broad implementation. Motivated by the aforementioned drawbacks in the traditional algorithms, hybrid approaches have been widely presented in the literature. In [19,20], a particle swarm optimization (PSO) with a differentially perturbed velocity hybrid algorithm was proposed for collision avoidance. In [21], a method that combines adaptive fuzzy control and image Sensors 2018, 18, 438; doi:10.3390/s18020438 www.mdpi.com/journal/sensors Sensors 2018, 18, 438 2 of 19 Sensors 2018, 18, x FOR PEER REVIEW   2 of 21  detection was presented. Other classical methods such as the neural network merging fuzzy inference system can be found in [14]. combines adaptive fuzzy control and image detection was presented. Other classical methods such  Most of the above obstacle avoidance algorithms are based on hypothesis that the obstacles are as the neural network merging fuzzy inference system can be found in [14].  circle-shaped or spherical, however, real obstacles are irregular and in fact they can’t be disposed of Most of the above obstacle avoidance algorithms are based on hypothesis that the obstacles are  simply as circle-shaped or spherical, so those algorithms lack a rapid response to irregular obstacles in circle‐shaped or spherical, however, real obstacles are irregular and in fact they can’t be disposed of  unknown simply situations as circle‐shaped and ar ore spheric easilyatrapped l, so those in alU-shape gorithms obstacles. lack a rapidConsidering  response to irre thegular above  obstacles problems,   in unknown situations and are easily trapped in U‐shape obstacles. Considering the above problems,  a novel real-time reaction obstacle avoidance method is proposed based on FLS. In this strategy, a novel real‐time reaction obstacle avoidance method is proposed based on FLS. In this strategy, the  the whole obstacle avoidance process is divided into several steps: directly drive to the target, whole obstacle avoidance process is divided into several steps: directly drive to the target, keep a  keep a safe distance, obstacle avoidance, and walk along wall. The algorithm has good flexibility in safe  distance,  obstacle  avoidance,  and  walk  along  wall.  The  algorithm  has  good  flexibility  in  unknown environments. unknown environments.   The rest of the paper is organized as follows: Section 2 presents and formulates the The  rest  of  the  paper  is  organized  as  follows:  Section  2  presents  and  formulates  the   obstacle-avoidance problem description. Section 3 presents obstacle avoidance algorithms for different obstacle‐avoidance  problem  description.  Section  3  presents  obstacle  avoidance  algorithms  for  types of obstacles. In Section 4, simulation results are provided to illustrate the performance of the different  types  of  obstacles.  In  Section  4,  simulation  results  are  provided  to  illustrate  the  presented algorithm in all sorts of unknown environments. Finally, conclusions are given in Section 5. performance of the presented algorithm in all sorts of unknown environments. Finally, conclusions  are given in Section 5.  2. Problem Description and Formulation 2. Problem Description and Formulation  2.1. Kinematics Model 2.1. Kinematics Model  In this paper, the AUV is equipped with two main propellers mounted astern, which providing navigation power (surge), The AUV is also equipped with four auxiliary thrusters, two auxiliary In this paper, the AUV is equipped with two main propellers mounted astern, which providing  thrusters navigation are transverse  power (surg layout e), The pr AU oviding V is al sway so eqpower uipped and withyaw  fourmomentums,  auxiliary thrusters, the other  two two auxiauxiliary liary  thrusters are transverse layout providing sway power and yaw momentums, the other two auxiliary  thrusters are vertical layout providing power for heave and moments for pitching. In addition, thrusters are vertical layout providing power for heave and moments for pitching. In addition, a  a horizontal rudder and vertical rudder are mounted onboard the AUV to change the heading angle of horizontal rudder and vertical rudder are mounted onboard the AUV to change the heading angle of  the AUV in the horizontal and vertical directions, respectively. Accordingly, except for roll, the other the AUV in the horizontal and vertical directions, respectively. Accordingly, except for roll, the other  five degrees of freedoms such as surge, sway, heave, pitch and yaw are controllable. According to the five degrees of freedoms such as surge, sway, heave, pitch and yaw are controllable. According to  definition in [22], underactuated AUVs are those where the number of their independent actuators is the  definition  in  [22],  underactuated  AUVs  are  those  where  the  number  of  their  independent  fewer than their degrees of freedom, so the AUV in the paper belongs to this type. actuators is fewer than their degrees of freedom, so the AUV in the paper belongs to this type.  Two reference coordinates are adapted in the paper, they are North-East-Down (NED) coordinate Two  reference  coordinates  are  adapted  in  the  paper,  they  are  North‐East‐Down  (NED)  and body-fixed coordinate, as shown in Figure 1, where the linear velocity V = [u, v, w] , angular coordinate  and  body‐fixed  coordinate,  as  shown  in  Figure  1,  where  the  linear  velocity   T T velocity ! = [pT, q, r] , and attitude  = [f, Tq, y] . T V = [u, v, w] , angular velocity ω = [p, q, r] , and attitude η = [φ, θ, ψ] , .   NED coordinate BODY coordinate p (roll) u (Surge) q (pitch) r (yaw) v (sway) w(heave) Figure 1. Earth‐ inertial frames and body‐fixed reference frames.  Figure 1. Earth- inertial frames and body-fixed reference frames. Sensors 2018, 18, 438 3 of 19 According to the standard notation motion equations of an AUV, a six degrees of freedom kinematics model for an AUV is described in [23]: 2 3 2 3 x u 6 7 6 7 = J () (1) 4 y 5 4 v 5 z w 2 3 2 3 6 7 6 7 4 q 5 = J ()4 q 5 (2) 2 3 cos y cos q cos y sin q sin f sin y cos f cos y sin q cos f + sin y sin f 6 7 J () = sin y cos q sin y sin q sin f + cos y cos f sin y sin q cos f cos y sin f (3) 4 5 sin q cos q sin f cos q cos f 2 3 1 sin f tan q cos f tan q 6 7 J () = 4 0 cos f sin f 5 (4) 0 sin f/ cos q cos f/ cos q Hypothesis 1: Because the roll movement is uncontrollable for AUV, and the structure of AUV is bilateral symmetrical, define f = 0, so Equations (1) and (2) can be rewritten as: x = u cos y cos q v sin y + w cos y sin q y = u sin y cos q + v cos y + w sin y sin q (5) z = u sin q + w cos q q = q . (6) y = r/ cos q In consideration of the fact the AUV’s additional hydrodynamic resistances in the horizontal and vertical direction are greater than those in the longitudinal one, when the speed over grand (SOG) exceeds 1 knot, the propulsive efficiency of the auxiliary thrusters is very low, so the auxiliary thrusters are idle while the AUV is navigating at normal speed. In general, we take w = 0, v = 0, then Equation (1) is simplified as: x = u cos y cos q y = u sin y cos q (7) z = u sin q 2.2. Obstacle Detection A FLV is utilized for obstacle detection, and the major parameters of the FLS model are designed as follows: the detection range is 150 m; field of view is 120 ; detection frequency is 2 Hz; number of beams is 80 beams. As the pitch angle q is seldom altered during normal navigation, the FLS is installed onboard the AUV in the XOY plate. Beams are numbered 0, 1, . . . , 79 in order, range from (60 , 60 ), and beams’ detection distances are denoted as S , i = 0, 1, . . . , 79. The schematic is shown in Figure 2. In the paper, a real-time obstacle avoidance strategy is proposed based on FLS, all the obstacles are considered unknown and their shapes are irregular, the obstacles’ outlines are generated by the detection datum of FLS, only the multi-beams in the horizon plane in body coordinates are adopted in consideration of the fact the AUV’s pitch angles are seldom changed. The purple lines are the sonar beams in the horizontal plane in body coordinates, the gray body is an obstacle, and the blue curves Sensors 2018, 18, 438 4 of 19 are the detected outline of the obstacle. However, the curves, which are also determined by the shape Sensors 2018, 18, x FOR PEER REVIEW 4 of 21 and the detected position of obstacles, are not predetermined even though the environment is known. l  150 m  120 Figure 2. Obstacle detection diagram. Figure 2. Obstacle detection diagram. 2.3. Desirable Maximum Turning Radius R max In the paper, a real-time obstacle avoidance strategy is proposed based on FLS, all the obstacles are con The main sidere specifications d unknown and their of AUV model shapes ar are e as irreg follows: ular, th length e obstac is les’ o 4 m, u width tlines is are genera 1.2 m, width ted by the is 1.2 m, height detect ision d 0.8 m, atu Max m of speed FLS, on isly 3.0 the m m, rated ulti-beams speed inis th 2.0 e horizon m/s. When plane in the bod AUV y coo nar v din igates ates ar at e 2 a m/s doptin ed an in consideration of the fact the AUV’s pitch angles are seldom changed. The purple lines are the underwater environment without ocean currents, the minimum turning radius is about five times sonar beams in the horizontal plane in body coordinates, the gray body is an obstacle, and the blue the length of the AUV when the rudder angle is set at the largest steering angle of 35 , and it takes curves are the detected outline of the obstacle. However, the curves, which are also determined by about 3.5 s for the rudder angle to vary from 0 to 35 . If the time delay for steering angle transition is the shape and the detected position of obstacles, are not predetermined even though the taken account of, the trajectory deviation distance is 1–1.5 m, which is small in comparison with the environment is known. turning radius, to simplify the problem the deviation distance is ignored, in other words, the trajectory of turning is replaced by an arc with a certain radius. 2.3. Desirable Maximum Turning Radius Rmax In Figure 3, S is an arbitrary obstacle point detected by FLS, locating on the right of AUV and the The main specifications of AUV model are as follows: length is 4 m, width is 1.2 m, width is relative bearing angle is , r is the distance from obstacle point S to FLS. R is the biggest turning i i i i 1.2 m, height is 0.8 m, Max speed is 3.0 m, rated speed is 2.0 m/s. When the AUV navigates at 2 m/s in radius for AUV to detour this obstacle point, point O is the center of turning radius, and co is the an underwater environment without ocean currents, the minimum turning radius is about five times mid-perpendicular of line ab, D is safe distance, and defines: the length of the AUV when the rudder angle is set at the largest steering angle of 35°, and it takes about 3.5 s for the rudder angle to vary from 0° to 35°. If the time delay for steering angle transition is \daS = a , \dab = b , aS = r , S b = D , R = oa. i i i i i i s i taken account of, the trajectory deviation distance is 1–1.5 m, which is small in comparison with the turning radius, to simplify the problem the deviation distance is ignored, in other words, the The desirable maximum turning radius R is described as: trajectory of turning is replaced by an arc with a certain radius. aS S b In Figure 3, Si is an arbitrary obstacle point detected by FLS, locating on the right of AUV and i i = (8) the relative bearing angle is αi, ρi is the distance from obstacle point S to FLS. Ri is the biggest sin(\S ba) sin(\S ab) i i turning radius for AUV to detour this obstacle point, point O is the center of turning radius, and co \S ab = b a is the mid-perpendicular of line ab , Ds is safe distance, and defines: i i i (9) \S ab = p (p/2 b ) = p/2 + b i i i , , , , .  daS   dab  aS   Sb D Ro  a ii i ii is i Unite Equations (8) and (9), yields: The desirable maximum turning radius R is described as: a = j (i 0.5n + 0.5)/n aS S b (10) ii b = sec a + tan a (8) i i i sin( Sba) sin( S ab) ii  Sab   aS oa iii i = (11) (9) sin(\S oa) sin(\oS a)  Sab  i (/2 ) / i2 ii i Substituting Equation (10) into Equation (11), yields: Unite Equations (8) and (9), yields: (0in.50.5)/n R = r [cos a sin a / tan(2b )] (12) is i i i i i  (10)  sec tan ii i  i Sensors 2018, 18, x FOR PEER REVIEW 5 of 21 aS oa (11) sin( Soa) sin( oS a) ii Substituting Equation (10) into Equation (11), yields: Sensors 2018, 18, 438 5 of 19 R  [cos sin / tan(2 )] (12) ii i i i Figure 3. Desirable maximum turning radius for an arbitrary detection point. Figure 3. Desirable maximum turning radius for an arbitrary detection point. Remark: The AUV postures will change following steering, which makes the hydrodynamic resistance on the AUV increase, and produces roll momentum, and the larger the steering angle is, Remark: The AUV postures will change following steering, which makes the hydrodynamic the larger the effect is. If possible, we choose the steering angle as small as possible. resistance on the If the AUV AUV incr detours ease, around the obsta and produces cle frr om the ri oll momentu ght side, i m, is th and e order n theulmb arger er of be the ams, t steering he angle is, obstacle point detected by the i-th beam whose maximum turning is denoted as R , the desirable the larger the effect is. If possible, we choose the steering angle as small as possible. maximum turning radius1: If the AUV detours around the obstacle from the right side, i is the order number of beams, mRR in{ |i 40,41,,79} (13) max i the obstacle point detected by the i-th beam whose maximum turning is denoted as R , the desirable Otherwise, when the AUV detours around the obstacle from the left side, the desirable maximum turning radius1: maximum turning radius is given by: R = minfR ji = 40, 41, , 79g (13) max RR min{ |i 0,1,, 39} (14) max i Otherwise, when the AUV detours around the obstacle from the left side, the desirable maximum turning radius 2.4.is Obstacles Classification given by: R = minfR ji = 0, 1, , 39g (14) For convenient obstacle avoidance, obstacles are divided into four categories: bounded obstacle, max i left unbounded obstacle, right unbounded obstacle and unbounded obstacle. the classification criterion is when obstacles enter sonar’s segment areas and their distances to sonar is less than 80 m, 2.4. Obstacles Classification according to their visible outline (from the spot of FLS) beyond the detection range of sonar or not, which is illustrated in Figure 4. k and l is the left boundary and right boundary detected by FLS, respectively, For convenient obstacle avoidance, obstacles are divided into four categories: bounded obstacle, i, j is serial number of beams, δ, ζ is arbitrary nature number. left unbounded obstacle, right unbounded obstacle and unbounded obstacle. the classification criterion is when obstacles enter sonar ’s segment areas and their distances to sonar is less than 80 m, according to a) If all the visible outline of obstacle is within the segment areas of FLS, it was defined as a bounded obstacle (BO). their visible outline (from the spot of FLS) beyond the detection range of sonar or not, which is  kl, ,, ,kl,i, jN,i[k,l],[ j k,1 k ] [l1,l],st.0 S LS, 0 illustrated in Figure 4. k and l is the left boundary and right boundary detected ie byj FLS, respectively, i, j is serial number of beams, d, z is arbitrary nature number. b) If the left edges of obstacle exceed the FLS detection range, and the right ones are within the FLS detection range, it is defined as left unbounded obstacle (LUBO). (a) If all the visible outline of obstacle is within the segment areas of FLS, it was defined as a bounded obstacle (BO). 9 k, l, d, V 2 Z, k < l, i, j 2 N, i 2 [k, l], j 2 [k V, k 1][ [l + 1, l + d], st. 0 < S  L , S = 0 i e j (b) If the left edges of obstacle exceed the FLS detection range, and the right ones are within the FLS detection range, it is defined as left unbounded obstacle (LUBO). 9 k, d 2 Z, i 2 [0, k], j 2 [k, k + d], st. 0 < S  L , S = 0 i e j (c) If the right edges of obstacle exceed the FLS detection range, and the left one is within the FLS detection range, it is defined as a right unbounded obstacle (RUBO): 9 k, d 2 Z, i 2 [0, k], j 2 [k, k + d], st. 0 < S  L , S = 0 i j (d) If two sides of the obstacle exceed the FLS detection range, we define it as a unbounded obstacle (UBO). 9 i 2 [0, 79], st. 0 < S  L i e Sensors 2018, 18, x FOR PEER REVIEW 6 of 21 ki ,,  [0,k],j[k,k ],st.0SL,S0 ie j c) If the right edges of obstacle exceed the FLS detection range, and the left one is within the FLS detection range, it is defined as a right unbounded obstacle (RUBO): ki ,,  [0,k],j[k,k ],st.0SL,S0 ie j d) If two sides of the obstacle exceed the FLS detection range, we define it as a unbounded obstacle (UBO). is [0, 79],t. 0S L ie Sensors 2018, 18, 438 6 of 19 (a) (b) (c) (d) Figure 4. Obstacle classification. (a) bounded obstacle; (b) left unbounded obstacle; (c) right Figure 4. Obstacle classification. (a) bounded obstacle; (b) left unbounded obstacle; (c) right unbounded unbounded obstacle; (d) unbounded obstacle. obstacle; (d) unbounded obstacle. 2.5. Rules of Obstacle Avoidance 2.5. Rules of Obstacle Avoidance Hypothesis 2: The ocean current is below 0.5 knot. Hypothesis 3: The rudder angle response delay can be ignored. Hypothesis 2: The ocean current is below 0.5 knot. In general, there are two purposes for steering, one is changing of AUV’s heading angle, the other is counteracting the effect of any external disturbance on the AUV. In this paper, only the first Hypothesis 3: The rudder angle response delay can be ignored. one is taken into account, and collision avoidance is classified into two modes: Normal obstacle avoidance, which is implemented by steering to change AUV heading angle, regulating the AUV navigation speed appropriately if necessary. In general, there are two purposes for steering, one is changing of AUV’s heading angle, the other is counteracting the effect of any external disturbance on the AUV. In this paper, only the first one is taken into account, and collision avoidance is classified into two modes: Normal obstacle avoidance, which is implemented by steering to change AUV heading angle, regulating the AUV navigation speed appropriately if necessary. Emergency obstacle avoidance, which is implemented in emergency situations when obstacles are discovered, and part of the obstacle points have entered the smallest safety range of AUV, and it is impossible to avoiding a collision by normal obstacle avoidance; those obstacle points are called emergency collision avoidance points (ECAP) in the paper. The process of emergency obstacle avoidance is composed of the following steps, firstly, the main propellers slow down to zero quickly, then propeller reverse and acceleration, which make the navigation speed of AUV decline rapidly, and auxiliary thrusters will be started once the SOG is below 0.5 m/s. As the emergency obstacle avoidance process is invariable, only normal mode collision avoidance is studied in this paper. 2.5.1. Obstacle Avoidance Rules I If obstacles are BO, and there isn’t any ECAP when the AUV detours the obstacles, we select the side as detour direction from which the angle deviating from target (ADT) is smaller, the obstacle-avoidance rules described as follows: Sensors 2018, 18, 438 7 of 19 (a) If the AUV is currently in line navigation, g is the ADT when detouring around the obstacle from its left side, and is the ADT when detouring from the other side, and the detour direction is as follows: g > g , le f t g  g , right r l (b) If the AUV is currently turning around for obstacle-avoidance, keep the turn direction; (c) If AUV is currently turning around for reducing ADT, and the turning radius is R , the distance to target is L , the detour direction is as follows: L sin(g ) 2sgn(R )R > L sin(g ), le f t v r v d d l else, right where, sgn(.) is signum function, l is constant coefficient. 2.5.2. Obstacle Avoidance Rules II Obstacles are one side or both sides unbounded, and there are no ECAP, so we adopt the following rules: (a) If the obstacle is LUBO, detour around obstacles from the right side. (b) If the obstacle is RUBO, detour around obstacles from the left side. (c) If the obstacle is UBO, and the AUV is turning around, keeping the turn direction, detour around obstacles from the left side; otherwise, detour around obstacles from the right side. 2.5.3. Obstacle Avoidance Rules III There is ECAP when an AUV detours around obstacles, and we use these obstacle avoidance rules: (a) If one side of the obstacle is unbounded and exists in ECAP, there is no ECAP on the other side, detour around the other side; (b) If ECAP only exist on one side of the obstacle and this side is bounded, and the other one is unbounded, select the former as the detour direction; (c) If ECAP exist on both sides of obstacle, choose the direction to detour from which the ADT is smaller. The flowchart for obstacle avoidance rules is shown in Figure 5: Sensors 2018, 18, x FOR PEER REVIEW 8 of 21 start No no ECAP? Rule II Rule III No No LUBO Is OB? ECAP in two sides? Yes Yes Yes No RUBO Smaller ADT Obstacle avoidance Turn right ECAP and UB? Rule I side Yes Yes Yes No No Yes Turn the Current LUBO? Line navigation? Turn left other side direction No Yes Yes No Yes No No Yes Is turning Turn left Turn right Satify condition 1   rl around? Current Turn left Turn right Turn left Turn right Turn right direction Figure 5. The flowchart for obstacle avoidance rules. Figure 5. The flowchart for obstacle avoidance rules. 2.5.4. Path Update Principle If the rudder angle isn’t equal to zero and AUV is detouring obstacles, it is necessary to calculate desirable maximum turning radius Rmax and verify that the current turning radius Rc is smaller than Rmax for guaranteeing obstacle-avoidance safety, if it is insured that AUV won’t collide with obstacles, keeping the current status, otherwise, a new path needs to be redesigned. If the rudder angle is zero, in other words, the AUV is in straight line navigation, when an obstacle is detected dead ahead of the AUV at less than 80 m (Si < 80, i = 37, 38, …, 42), it is necessary to design a new path immediately, however, the distance is decreased to 60 m (S < 40, i = 37, 38, …, 42) in dense obstacles environments. 3. Obstacle Avoidance Algorithms 3.1. Reaction Obstacle Avoidance RRA is a real-time obstacle-avoidance algorithm, which is especially adapted to unknown complex environments. The process is divided into several steps: a) Directly drive to the target: when the AUV is passing a certain position, FLS doesn’t detect any obstacles, or detect obstacles which are on the flank of the AUV and the target (or next waypoint) is in the other side, the heading angle is adjusted to the target (or next waypoint) immediately. b) Keep a safe distance: obstacles are detected on the side of the AUV, so if driving to the target directly, the AUV will collide with obstacles or the safety distance margin isn’t enough, then AUV keeps safe distance from obstacles and decreases the ADT if possible, which makes the AUV drive parallel with the edges of obstacle’s outline. c) Solo obstacle avoidance: if by keeping the current posture, the AUV will collide with obstacles, then the heading angle must be adjusted to avoid colliding, the detour direction is chosen by the aforementioned rules, reducing the navigation speed if necessary. d) Cluttered obstacle avoidance: when one of obstacles has entered the 80 m range of AUV in a cluttered environment, this manner is activated, and the AUV selects an appropriate passage between obstacles considering comprehensive factors such as ADT, the width of the passage, and the unobstructed character from the spot of FLS. The AUV keeps a safe Sensors 2018, 18, 438 8 of 19 2.5.4. Path Update Principle If the rudder angle isn’t equal to zero and AUV is detouring obstacles, it is necessary to calculate desirable maximum turning radius R and verify that the current turning radius R is smaller than max c R for guaranteeing obstacle-avoidance safety, if it is insured that AUV won’t collide with obstacles, max keeping the current status, otherwise, a new path needs to be redesigned. If the rudder angle is zero, in other words, the AUV is in straight line navigation, when an obstacle is detected dead ahead of the AUV at less than 80 m (S < 80, i = 37, 38, . . . , 42), it is necessary to design a new path immediately, however, the distance is decreased to 60 m (S < 40, i = 37, 38, . . . , 42) in dense obstacles environments. 3. Obstacle Avoidance Algorithms 3.1. Reaction Obstacle Avoidance RRA is a real-time obstacle-avoidance algorithm, which is especially adapted to unknown complex environments. The process is divided into several steps: (a) Directly drive to the target: when the AUV is passing a certain position, FLS doesn’t detect any obstacles, or detect obstacles which are on the flank of the AUV and the target (or next waypoint) is in the other side, the heading angle is adjusted to the target (or next waypoint) immediately. (b) Keep a safe distance: obstacles are detected on the side of the AUV, so if driving to the target directly, the AUV will collide with obstacles or the safety distance margin isn’t enough, then AUV keeps safe distance from obstacles and decreases the ADT if possible, which makes the AUV drive parallel with the edges of obstacle’s outline. (c) Solo obstacle avoidance: if by keeping the current posture, the AUV will collide with obstacles, then the heading angle must be adjusted to avoid colliding, the detour direction is chosen by the aforementioned rules, reducing the navigation speed if necessary. (d) Cluttered obstacle avoidance: when one of obstacles has entered the 80 m range of AUV in a cluttered environment, this manner is activated, and the AUV selects an appropriate passage between obstacles considering comprehensive factors such as ADT, the width of the passage, and the unobstructed character from the spot of FLS. The AUV keeps a safe distance with the obstacles on the sides of passages, if the passage is wide enough, decreasing the ADT as far as possible. (e) Walk along the wall: if encountering UBO or gallery terrain, the AUV navigates parallel to the edges of the obstacle’s outline until there is no obstacle hampering the AUV from directly driving to the target. During the whole process of walking along wall, some obstacle points need to be saved at intervals as a judgement criterion for terminating this manner. The details will be described in Section 3.4. 3.2. Obstacles Outline Disposition It’s important to choose the right timing for obstacle avoidance. Too early, the obstacles partially enter the detection range of FLS, they aren’t detected entirely; too late, and the AUV gets so close to the obstacle that the obstacle avoidance time is hasty, which creates unnecessary difficulties for collision avoidance. The best time to take obstacle-avoidance measures is when obstacles enter into the 80 m detection range of FLV, except for dense obstacle environments, so the FLS achieves more accurate detection of the obstacles, and the time set aside for collision-avoidance is enough. By this method, not only rudder angle and the time of steering are decreased, but also a more appropriate obstacle avoidance path is produced. 3.2.1. FLS Detection Data Grouping 803 FLS detection data is saved as an array b 2 R , where each item of the array denotes the detection distance between sonar beams and obstacle points, and if some item is equal to zero it indicates this sonar beam doesn’t detect any obstacle point. Sensors 2018, 18, x FOR PEER REVIEW 9 of 21 distance with the obstacles on the sides of passages, if the passage is wide enough, decreasing the ADT as far as possible. e) Walk along the wall: if encountering UBO or gallery terrain, the AUV navigates parallel to the edges of the obstacle’s outline until there is no obstacle hampering the AUV from directly driving to the target. During the whole process of walking along wall, some obstacle points need to be saved at intervals as a judgement criterion for terminating this manner. The details will be described in Section 3.4. 3.2. Obstacles Outline Disposition It’s important to choose the right timing for obstacle avoidance. Too early, the obstacles partially enter the detection range of FLS, they aren’t detected entirely; too late, and the AUV gets so close to the obstacle that the obstacle avoidance time is hasty, which creates unnecessary difficulties for collision avoidance. The best time to take obstacle-avoidance measures is when obstacles enter into the 80 m detection range of FLV, except for dense obstacle environments, so the FLS achieves more accurate detection of the obstacles, and the time set aside for collision-avoidance is enough. By this method, not only rudder angle and the time of steering are decreased, but also a more appropriate obstacle avoidance path is produced. 3.2.1. FLS Detection Data Grouping Sensors 2018, 18, 438 9 of 19 80 3 FLS detection data is saved as an array   R , where each item of the array denotes the detection distance between sonar beams and obstacle points, and if some item is equal to zero it indicates this sonar beam doesn’t detect any obstacle point. The output datum of FLS needs to be disposed to produce obstacle outline. The first step is The output datum of FLS needs to be disposed to produce obstacle outline. The first step is taking out the second column date from array b and dividing the data into groups according to taking out the second column date from array  and dividing the data into groups according to Equations (15) and (16), and each group data is considered as belonging to an obstacle. The group Equations (15) and (16), and each group data is considered as belonging to an obstacle. The group criterion is as follows: criterion is as follows: kS SSS k < ddS , S,0 S S 6=,i0,[1,i729] [1, 79] (15) (15) i1 i i i1 ii 11 bb i i dl  d = l l (16) (16) bbe e b b where is gap width, is a select coefficient whose value range is [1, 4], is the total length of d  l b b o where d is gap width, l is a select coefficient whose value range is [1, 4], l is the total length of the b b o the AUV. All of the points which satisfy Equations (15) and (16) can be considered as a group AUV. All of the points which satisfy Equations (15) and (16) can be considered as a group datapoint of datapoint of the same obstacle. For example, in Figure 6, the detection data is divided into two the same obstacle. For example, in Figure 6, the detection data is divided into two groups. groups. Figure 6. FLS detecting data grouped. Figure 6. FLS detecting data grouped. 3.2.2. Largest Polar Angle Algorithm 3.2.2. Largest Polar Angle Algorithm The output datum of FLS are disperse points in the coordinate system, and an obstacle outline is produced by aligning those points in the same group one by one, however, the outlines aren’t always regular, which can’t be adopted in obstacle avoidance, hence LPAA is proposed to transform the irregular shape into a polygon. Figure 7 shows a detected obstacle which is LUBO. In order to improve the efficiency of collision avoidance, it is necessary to simplify the detected outline of the obstacle. The concept of LPAA involves using a convex polygon with fewer sides to surround a group of obstacle points, and satisfy that each vertex of those sides belongs to the array b, then the irregular obstacle visible outline is changed into a convex polygon. The polar angle of those sides is decreased gradually according to generated sequence, the specific steps are as follows: Step 1: Take grouped data to generate sonar beams (purple line) and generate obstacle points; Step 2: Line up obstacle points one by one to produce a detected obstacle outline (black line), choose the rightmost border point (point A) as the starting point, connect it with others points on the left of it; Step 3: Find an obstacle point as point B, which satisfies the condition that the polar angle of line AB is larger than those of other points aligned with point A. The polar angle value range is [0 , 360 ), which is taken as positive in the anti-clockwise sense without loss of generality; Step 4: Take the point found in step 3 as the new starting point, repeat step 1 and step 2 to find the next point (point C, D, . . . ) until the leftmost border point is picked out; Step 5: Align points A, B, . . . in sequence. The blue line ABCDE is the disposed result. In practice, once the detour direction is determined, we only need to deal with the points in this direction. Taking Figure 7 as an example, we only need to deal with the obstacle points from S to the rightmost border point when detouring around the right side of the obstacle, which increases the obstacle avoidance response speed by reducing unnecessary computation. Sensors 2018, 18, x FOR PEER REVIEW 10 of 21 The output datum of FLS are disperse points in the coordinate system, and an obstacle outline is produced by aligning those points in the same group one by one, however, the outlines aren’t always regular, which can’t be adopted in obstacle avoidance, hence LPAA is proposed to transform the irregular shape into a polygon. Figure 7 shows a detected obstacle which is LUBO. In order to improve the efficiency of collision avoidance, it is necessary to simplify the detected outline of the obstacle. The concept of LPAA involves using a convex polygon with fewer sides to surround a group of obstacle points, and satisfy that each vertex of those sides belongs to the array , then the irregular obstacle visible outline is changed into a convex polygon. The polar angle of those sides is decreased gradually according to generated sequence, the specific steps are as follows: Step 1: Take grouped data to generate sonar beams (purple line) and generate obstacle points; Step 2: Line up obstacle points one by one to produce a detected obstacle outline (black line), choose the rightmost border point (point A) as the starting point, connect it with others points on the left of it; Step 3: Find an obstacle point as point B, which satisfies the condition that the polar angle of line AB is larger than those of other points aligned with point A. The polar angle value range is [0°, 360°), which is taken as positive in the anti-clockwise sense without loss of generality; Step 4: Take the point found in step 3 as the new starting point, repeat step 1 and step 2 to find the next point (point C, D, …) until the leftmost border point is picked out; Step 5: Align points A, B, … in sequence. The blue line ABCDE is the disposed result. In practice, once the detour direction is determined, we only need to deal with the points in this direction. Taking Figure 7 as an example, we only need to deal with the obstacle points from S40 to Sensors 2018, 18, 438 10 of 19 the rightmost border point when detouring around the right side of the obstacle, which increases the obstacle avoidance response speed by reducing unnecessary computation. Figure 7. Obstacle detected surface disposed by LPAA. Figure 7. Obstacle detected surface disposed by LPAA. 3.3. Path Design for Single Obstacle 3.3. Path Design for Single Obstacle The aim of path design is achieving the shortest and smoothest paths free of collisions using fewer steering corrections and smaller steering rudder angles if possible. We design several The aim of path design is achieving the shortest and smoothest paths free of collisions using fewer waypoints and connect them by a straight or smooth arc (its radius is the turning radius) to construct a safe path. steering corrections and smaller steering rudder angles if possible. We design several waypoints and connect them by a straight or smooth arc (its radius is the turning radius) to construct a safe path. In Figure 8 the obstacle corresponds to LUBO, and the AUV detours around the right side of the obstacle according to the aforementioned obstacle avoidance rules. Extend the line ab, bc, cd, . . . , to intersect AUV track line at point k , k , k , . . . , respectively, and the steps are as follows: 1 2 3 Step 1: Find out the intersection k between line AB and AUV track line, calculating the length of line segment Pk . Step 2: Calculate f (t): f (t) = PK U T R  tan(0.5a) D  csc a (17) t min s 1 1 where, T is the response time of the obstacle-avoidance system, U is the speed of AUV, R min is the smallest turning radius of AUV, D is safe distance, take D  4L s s o Step 3: If f (t) is negative, turn to step 8, otherwise, calculate f (t): 1 2 f (t) = PK U T R  tan(0.5a) D  csc a (18) 2 t max s where, R is the largest turning radius for AUV. max Step 4: If f (t) is non-negative, parallel translation AK , and intersect AUV track line at point K, 2 1 KK = D csc a, design a transition point S as beginning position of steering, which satisfies: 1 s 1 PS = PK R  tan(0.5a) D  csc a (19) 1 1 max s Step 5: If f (t) is negative, the turning radius R and transition point S satisfies: 2 1 R = (PK U T D  csc a)/ tan(0.5a) 1 t s (20) PS = PK R tan(0.5a) D  csc a 1 1 Step 6: Design a transition point S as the steering end position, where the heading angle of the AUV satisfies: y = y + a where, y is the current heading angle of AUV, y is the heading angle when AUV is arriving at S . 0 t 1 Step 7: End Sensors 2018, 18, 438 11 of 19 Step 8: Replace the current line segment with the next line segment (such as: line BC instead of line AB), which is displayed in Figure 8b,c, repeat steps 1–6; Step 9: Taking the transition point S as beginning position of steering, D  2L , D  4L , 3 s1 o s2 o in Figure 8d, one can obtain: B L = D ctg(0.5a) s1 (21) LN = (D D )/ sin a s2 s1 Step 10: If D  4L and g (a) is nonnegative, take D = D , and replacing S with B : s1 o 1 s2 s1 3 g (a) = B L/ cos(0.5a) R (22) 1 min The turning radius R satisfies: R = D / sin(0.5a) (23) s1 Step 11: Else, take turning radius R = R , and point S satisfies: min 3 g (a) = R sin a cos(0.5a) + D cos a < 2 min s1 D = max(g (a), 4L ) (24) s2 2 MS = D / sin a R cos(0.5a) 3 s2 min Sensors 2018, 18, x FOR PEER REVIEW 12 of 21 Step 12: Design a transition point S as steering end position, where the heading angle of the AUV satisfies: Step 12: Design a transition point S as steering end position, where the heading angle of the y = y + a (25) AUV satisfies: where, y is the heading angle befor e the AUV arrives at S . (25) t t 0 3 Step 13: Turn to step 7. where, is the heading angle before the AUV arrives at S .  3 Step 13: Turn to step 7. (a) (b) (c) (d) Figure 8. (a–c) Path design for single obstacle; (d) Local amplification diagram. Figure 8. (a–c) Path design for single obstacle; (d) Local amplification diagram. 3.4. Wall-Form Obstacles Norgren presented the obstacle avoidance method called “iceberg Edge-Following” in [24], however, the wall form of obstacles in this paper is too simple, and this obstacle avoidance method doesn’t adapt to complex U-shape obstacles, or labyrinth obstacles. The next question is when is the optimal time for ending this manner and driving straight to the target for complex wall form obstacles, which hasn’t been proposed in the above paper. To solve this question, the paper presents a novel solution, which is obstacle outline memory algorithm. In Figure 9, the AUV detours around the wall-form obstacle from the right side, and once a wall-form obstacle is detected, the walking along wall manner is activated, and the memory function is started to recorded the rightmost obstacle points S detected by FLV at the transition position where steering is beginning or ending. Nevertheless, the wall-form obstacle outline is not described entirely by these points, so it is necessary to add other supplementary points, to ensure an obstacle point must be recorded that at least every 100 m of navigation distance. On the contrary, if the AUV detours around the wall-form obstacle from the left direction, leftmost obstacle points S are recorded at the transition position. Sensors 2018, 18, 438 12 of 19 3.4. Wall-Form Obstacles Norgren presented the obstacle avoidance method called “iceberg Edge-Following” in [24], however, the wall form of obstacles in this paper is too simple, and this obstacle avoidance method doesn’t adapt to complex U-shape obstacles, or labyrinth obstacles. The next question is when is the optimal time for ending this manner and driving straight to the target for complex wall form obstacles, which hasn’t been proposed in the above paper. To solve this question, the paper presents a novel solution, which is obstacle outline memory algorithm. In Figure 9, the AUV detours around the wall-form obstacle from the right side, and once a wall-form obstacle is detected, the walking along wall manner is activated, and the memory function is started to recorded the rightmost obstacle points S detected by FLV at the transition position where steering is beginning or ending. Nevertheless, the wall-form obstacle outline is not described entirely by these points, so it is necessary to add other supplementary points, to ensure an obstacle point must be recorded that at least every 100 m of navigation distance. On the contrary, if the AUV detours around the wall-form obstacle from the left direction, leftmost obstacle points S are recorded at the Sensors 2018, 18, x FOR PEER REVIEW 13 of 21 transition position. Figure 9. Wall-form obstacle avoidance. 1-wall form obstacle; 2-recorded obstacle point; 3-start Figure 9. Wall-form obstacle avoidance. 1-wall form obstacle; 2-recorded obstacle point; 3-start record record position; 4-supplement record position; 5-transition position; 6-obstacle trajectory; 7-target. position; 4-supplement record position; 5-transition position; 6-obstacle trajectory; 7-target. n2 These obstacle points are kept in a storage array DR  , As a demonstration in Figure 9, when n2 These obstacle points are kept in a storage array D 2 R , As a demonstration in Figure 9, when the wall-form obstacles are identified and it is decided to detour around their right side, the first the wall-form three obstacle obstacles pointsar ar ee re identified corded in and arrit ay D is decided in order,to which detour are t arh ound e leftmost their p right oint S side, , dead theahe first ad three point S and rightmost point S . Then the next obstacle point position is added into array D as 39 79 obstacle points are recorded in array D in order, which are the leftmost point S , dead ahead point S 0 39 mentioned above. This is repeated until this obstacle is passed by, and the simulation process is and rightmost point S . Then the next obstacle point position is added into array D as mentioned explained in the following section. above. This is repeated until this obstacle is passed by, and the simulation process is explained in the The manner of walking along wall is terminated if the following two conditions are satisfied: following section. firstly, ADT is smaller than  (take  = 15°); secondly, any line composed by two arbitrarily  The manner of walking along wall is terminated if the following two conditions are satisfied: adjacent points taken from array D, which doesn’t intersect line PG , where, P(x, y) is the current firstly, ADT is smaller than J (take J = 15 ); secondly, any line composed by two arbitrarily adjacent position of AUV and G(xt, yt) is the position of target. Whenever the first condition is satisfied, an points obtaken stacle p froom int w array ill beD, recwhich orded b doesn’t y the afo intersect remention line ed w PG ay,s wher at cur e, reP n(x, t po y) sitis ion the P(x, curr y) ri en ght t position now, of and the second condition will be verified. Let K1 and K2 be arbitrarily two adjacent points taken from AUV and G(x , y ) is the position of target. Whenever the first condition is satisfied, an obstacle point t t D, the second condition can be described by the following formulas: will be recorded by the aforementioned ways at current position P(x, y) right now, and the second   dPk PG    dPk PG  (26) dd 0   dkP kk 31 1 2   dkG kk  (27) 41 1 2 dd 0 where: × denotes a cross multiplication, and  denotes a scalar multiplication.  If the above Equations (26) and (27) are satisfied, which means segment doesn’t intersect kk  with segment PG , the second condition is satisfied, too. Then “wall-form walk” manner is terminated, and the AUV drives straight to the target. 3.5. Clutter Obstacles Environment When any obstacle enters into the 80 m detection range of the AUV, and the detected obstacles are more than two, the obstacles environment is considered a cluttered obstacle environment. In this Sensors 2018, 18, 438 13 of 19 condition will be verified. Let K and K be arbitrarily two adjacent points taken from D, the second 1 2 condition can be described by the following formulas: ! ! d = Pk  PG < 1 1 ! ! (26) d = Pk  PG 2 2 d  d > 0 1 2 ! ! d = k P k k < 3 1 1 2 ! ! (27) d = k G k k 4 1 1 2 d  d > 0 3 4 where:  denotes a cross multiplication, and  denotes a scalar multiplication. If the above Equations (26) and (27) are satisfied, which means segment k k doesn’t intersect 1 2 with segment PG, the second condition is satisfied, too. Then “wall-form walk” manner is terminated, and the AUV drives straight to the target. 3.5. Clutter Obstacles Environment When any obstacle enters into the 80 m detection range of the AUV, and the detected obstacles are more than two, the obstacles environment is considered a cluttered obstacle environment. In this environment, the speed is slowed down in narrow channels if necessary, and it is unnecessary to limit the steering times in this obstacles scenario for three reasons, firstly, the effect of steering is decreased steeply with the speed decline; and there is not enough space for the AUV turn round freely either; finally, sometimes obstacle avoidance can’t be accomplished without the assistance of auxiliary thrusters. There are several candidate detouring paths in dense obstacles environments, so a weight coefficient l is designed to evaluate these obstacle avoidance paths, and the path whose weight coefficient l is the largest is chosen as the optimal path, and l denoted as: c c l l L o e l = (28) r sin j d d r , first type gap r = (29) 0.5(r + r ), second type gap i i+1 0, D  4L o o 1/2 D 4L o o l = (30) ( ) , 4L < D < 8L o o o 8L 1, D  8L o o where, D is the gap width, j is ADT, r , r are the distances from FLV to the right or left border points o i j of the gap respectively, l , l are the width coefficient of the gap, and type coefficient of the gap. o b Considering the detouring path forming a gap to the target may be obstructed, which can’t be identified directly from current visual angle of FLS, so the type coefficient is introduced as the reliability factor. If there is a detection point S between a gap that satisfies the condition S = 0, i i which means the gap is unobstructed from the current visual of angle FLV, take l = 1; otherwise take l = 0.5. The weight function l is considered a comprehensive factor that includes safety, reliability and efficiency. After the detour direction is decided, a temporary waypoint needs to be designed, which is solved by the following rules: (a) If the width of gap is smaller than 8 L , choosing the midpoint of the gap as waypoint; (b) If the width of gap is larger than 8 L , choosing the point as waypoint where the ADT is the least and the distance from AUV to two border points of the gap is larger than 4 L . o Sensors 2018, 18, 438 14 of 19 When the width of path less than 6 L the AUV should navigate at low speed, In Figure 10, the AUV has five paths, and the weight function and the parameters give the following table: From Table 1, we can get the 4th path is the optimal path and the 2nd path is the worst one. Table 1. The weight factors of path in clutter environment. l D (L ) F ( ) P l l o o o d d b c Path1 1 ¥ 66 38.1 1 4.31 Path2 0.84 9.6 34 47.5 0.5 2.36 Path3 1 14.2 18 51.9 1 9.36 Path4 0.81 9.2 14 46.5 1 10.76 Path5 1 ¥ 43 47.4 1 4.64 Sensors 2018, 18, x FOR PEER REVIEW 15 of 21 Figure 10. Dense obstacles environment voidance. Figure 10. Dense obstacles environment voidance. 4. Simulation Results and Discussion 4. Simulation Results and Discussion Numerical simulations have been performed in three obstacle environments, and three Numerical simulations have been performed in three obstacle environments, and three different different obstacle avoidance algorithms are adapted in order to verify the effectiveness of the obstacle avoidance algorithms are adapted in order to verify the effectiveness of the proposed proposed obstacle-avoidance algorithm in complex environments. and software simulation experiments are carried out in the Matlab 2015 environment. We design three obstacle scenes, and obstacle-avoidance algorithm in complex environments. and software simulation experiments are the obstacles are irregular shape, the time step takes 0.1 s. The other parameters are as follows: carried out in the Matlab 2015 environment. We design three obstacle scenes, and the obstacles are irregular shape, the time step takes 0.1 s. The other parameters are as follows: l = 4 m, U = 2 m/s, Rmax = 30 m, Rmin = 30 m, λ2 = −0.663, λb = 2.0. l = 4 m, U = 2 m/s, R = 30 m, R = 30 m, l = 0.663, l = 2.0. o max min 2 4.1. Simulation 4.1. Simulation Figure 11 depicts a AUV’s obstacle avoidance in a general obstacle environment, initial position (40, 40), heading angle is 0° (north), and initial speed is 2 m/s, the target position is (1200, 1600), Figure 11 depicts a AUV’s obstacle avoidance in a general obstacle environment, initial position initial time t = 0 s. As the obstacles are dispersely distributed, the AUV maintains a speed of 2 m/s in (40, 40), heading angle is 0 (north), and initial speed is 2 m/s, the target position is (1200, 1600), initial the whole process ignoring the steering effect on speed. time t = 0 s. As the obstacles are dispersely distributed, the AUV maintains a speed of 2 m/s in the Figure 11a depicts the avoidance trajectory of APF, the trajectory obviously isn’t good, it has whole process ignoring the steering effect on speed. some defects in the following aspects: the path is not smooth; and the path is too long and the heading is not steady. Figure 11b depicts the obstacle avoidance trajectory of PSO, where the trajectory is smooth, and the path length is shorter than with the first algorithm, however, the heading shows a slight shock when the heading is suddenly altered. Figure 11c depicts the obstacle avoidance trajectory of RRA, where the trajectory is smooth, the heading is steady and the path length is near optimal. Figure 11d shows the heading curves of APF, PSO and RRA, respectively, and their heading stabilities are improved in sequence, and the finishing times are 1315.4 s, 1128.7 s, 1100.1 s, respectively. Therefore, RRA is the best one of the three obstacle avoidance algorithms in every aspect. Sensors 2018, 18, 438 15 of 19 Figure 11a depicts the avoidance trajectory of APF, the trajectory obviously isn’t good, it has some defects in the following aspects: the path is not smooth; and the path is too long and the heading is not steady. Figure 11b depicts the obstacle avoidance trajectory of PSO, where the trajectory is smooth, and the path length is shorter than with the first algorithm, however, the heading shows a slight shock when the heading is suddenly altered. Figure 11c depicts the obstacle avoidance trajectory of RRA, where the trajectory is smooth, the heading is steady and the path length is near optimal. Figure 11d shows the heading curves of APF, PSO and RRA, respectively, and their heading stabilities are improved in sequence, and the finishing times are 1315.4 s, 1128.7 s, 1100.1 s, respectively. Therefore, RRA is the best one of the three obstacle avoidance algorithms in every aspect. Sensors 2018, 18, x FOR PEER REVIEW 16 of 21 (a) (b) (c) (d) Figure 11. General obstacles environment obstacle avoidance algorithms. (a) APF; (b) PSO; (c) RRA; Figure 11. General obstacles environment obstacle avoidance algorithms. (a) APF; (b) PSO; (c) RRA; (d) heading curves. (d) heading curves. Figure 11 depicts the AUV’s obstacle avoidance in a cluttered obstacle environment, where the initial position is (20, 20), heading angle is 0° (north), and initial speed is 2 m/s, the target position is Figure 11 depicts the AUV’s obstacle avoidance in a cluttered obstacle environment, where the (600, 800), initial time t = 0 s. initial position is (20, 20), heading angle is 0 (north), and initial speed is 2 m/s, the target position is Figure 11a depicts obstacle avoidance trajectory of APF. The path is not smooth and it is too (600, 800), initial time t = 0 s. long and the heading is not steady and oscillates when navigating in a narrow passageway. Figure 11b depicts the obstacle avoidance trajectory of PSO, where the trajectory is not smooth like the first scene, and the path length is shorter than the APF, and the heading displays a slight shock when the heading is suddenly altered. Figure 11c depicts the obstacle avoidance trajectory of RRA, where the trajectory is still smooth, the heading is steady and the path length is near optimal. Sensors 2018, 18, 438 16 of 19 Figure 11a depicts obstacle avoidance trajectory of APF. The path is not smooth and it is too long and the heading is not steady and oscillates when navigating in a narrow passageway. Figure 11b depicts the obstacle avoidance trajectory of PSO, where the trajectory is not smooth like the first scene, and the path length is shorter than the APF, and the heading displays a slight shock when the heading is suddenly altered. Figure 11c depicts the obstacle avoidance trajectory of RRA, where the trajectory is still smooth, the heading is steady and the path length is near optimal. Sensors 2018, 18, x FOR PEER REVIEW 17 of 21 Figure 11d shows the heading curves of APF, PSO and RRA, respectively, and their heading stabilities is still improved in sequence, and the finishing times are 642.0 s, 571.1 s, 560.1 s. Therefore, Figure 11d shows the heading curves of APF, PSO and RRA, respectively, and their heading RRA is the best one among the three obstacle avoidance algorithms in every aspect. stabilities is still improved in sequence, and the finishing times are 642.0 s, 571.1 s, 560.1 s. Therefore, RRA is the best one among the three obstacle avoidance algorithms in every aspect. In a cluttered obstacle scene, maintaining the safety distances between the AUV and obstacles In a cluttered obstacle scene, maintaining the safety distances between the AUV and obstacles is is an important factor, Figure 12 displays the smallest distances between the AUV and nine detected an important factor, Figure 12 displays the smallest distances between the AUV and nine detected obstacles by three obstacle avoidance algorithms. The three columns are the smallest distances in APF, obstacles by three obstacle avoidance algorithms. The three columns are the smallest distances in PSO and RRA, respectively. APF, PSO and RRA, respectively. (a) (b) (c) (d) Figure 12. Clutter obstacles environment obstacle avoidance algorithms; (a) APF; (b) PSO; (c) RRA; Figure 12. Clutter obstacles environment obstacle avoidance algorithms; (a) APF; (b) PSO; (c) RRA; (d) heading curves. (d) heading curves. Sensors 2018, 18, 438 17 of 19 The smallest distance in APF is only 6.01 m, produced by an AUV detouring the 3rd obstacle. The smallest distance in PSO is 10.38 m produced in an AUV detouring the 2nd obstacle, and the Sensors 2018, 18, x FOR PEER REVIEW 18 of 21 smallest distance in PSO is 16.50 m produced in an AUV detouring the 7th obstacle. The distance The smallest distance in APF is only 6.01 m, produced by an AUV detouring the 3rd obstacle. between the AUV and obstacles is bigger than twice the total length of the AUV in the obstacle The smallest distance in PSO is 10.38 m produced in an AUV detouring the 2nd obstacle, and the avoidance process which is safe enough, and more than four times the total length is perfect, so RRA is smallest distance in PSO is 16.50 m produced in an AUV detouring the 7th obstacle. The distance between the AUV and obstacles is bigger than twice the total length of the AUV in the obstacle still the best one in keeping a safe distance. avoidance process which is safe enough, and more than four times the total length is perfect, so RRA Figure 13 displays the obstacle avoidance trajectories of the three algorithms in a trap obstacles is still the best one in keeping a safe distance. Figure 13. displays the obstacle avoidance trajectories of the three algorithms in a trap obstacles environment, where the initial position is (700, 1050), the heading angle is 0 (north), the initial speed environment, where the initial position is (700, 1050), the heading angle is 0° (north), the initial speed is 2 m/s, and the target position is (970, 1820), initial time t = 0 s. is 2 m/s, and the target position is (970, 1820), initial time t = 0 s. Figure 13. Smallest distance between AUV and obstacles. Figure 13. Smallest distance between AUV and obstacles. Figure 14a depicts the APF obstacle avoidance trajectory, as the inherent defect of APF, AUV is being caught in the trap and they wander around position 1. Figure 14a depicts the APF obstacle avoidance trajectory, as the inherent defect of APF, AUV is Figure 14b depicts the PSO obstacle avoidance trajectory, where although the AUV arrived at the destination, it made some detours in this method. When the AUV arrived at position 1 it detected being caught in the trap and they wander around position 1. Sensors 2018, 18, x FOR PEER REVIEW 19 of 21 a wall-shape obstacle and start walking along the wall, and when AUV arrived at position 2 the walking along wall was ended and it went to destination directly, however when the AUV arrived at position 3 and detected a wall obstacle again, it restart walking along wall again till arriving at the destination. Figure 14c depicts the PSO obstacle avoidance trajectory, where the path is smooth, and the length is shorter. When the AUV arrived at position 1, the obstacle was identified as a wall-form obstacle, the walking along wall manner was activated and obstacle points were recorded frequently by the aforementioned rules. When the AUV arrived at position 2 and position 3, the heading was pointing to the destination, but the second condition was not satisfied, so it continued maintaining this manner. When the AUV arrived at position 4, the heading was pointing to the destination again, and the second condition was not satisfied, so walking along wall manner was terminated and the AUV drived to the target directly. (a) (b) (c) Figure 14. Trap obstacles environment obstacle avoidance algorithms: (a) APF; (b) PSO; (c) RRA. Figure 14. Trap obstacles environment obstacle avoidance algorithms: (a) APF; (b) PSO; (c) RRA. 4.2. Discussion Simulations have been carried out in three different unknown obstacle scenarios. The obstacle avoidance trajectory by APF is worse, the path is not smooth and heading is not steady, and paths are the longest in all three obstacle scenes using this algorithm, moreover, the AUV can’t get to the destination in the third scenario with this algorithm. The obstacle avoidance trajectory by PSO is good in general, however, if a U-shape obstacle is more complicated, for example, in Figure 14b the left part of wall-form obstacle was as complicated as its right side, AUV could get trapped inside a wall-form obstacle by this algorithm. It is apt to become trapped in complicated U-shape obstacle environments which it cannot solve by itself. The RRA is superior in the following aspects: it can realize the goals in all three different obstacle scenes with the shortest time, respectively, planning smooth trajectories and the shortest voyages, meanwhile, it makes the AUV keep a steady heading, keeping better safe distances away from obstacles. Moreover it solves the defect of being apt to getting into traps in U-shape obstacle environments that affects APF. 5. Conclusions and Future Work In this paper, a LPAA had been proposed to change an obstacle’s irregular visual surface into a convex polygon, and a real-time reaction obstacle avoidance algorithm for AUVs is presented. The algorithm adapts to complex and cluttered unknown environments, and generates a smooth and shorter path, while guaranteeing appropriate safe distances. The simulation experiments illustrate that this algorithm is flexible in cluttered unknown environments, and in particular, it is able to solve the problem of AUVs becoming trapped by U-shape obstacles existing in other obstacle avoidance algorithms. The next stage of this work is to apply this algorithm in pool experiments, and Sensors 2018, 18, 438 18 of 19 Figure 14b depicts the PSO obstacle avoidance trajectory, where although the AUV arrived at the destination, it made some detours in this method. When the AUV arrived at position 1 it detected a wall-shape obstacle and start walking along the wall, and when AUV arrived at position 2 the walking along wall was ended and it went to destination directly, however when the AUV arrived at position 3 and detected a wall obstacle again, it restart walking along wall again till arriving at the destination. Figure 14c depicts the PSO obstacle avoidance trajectory, where the path is smooth, and the length is shorter. When the AUV arrived at position 1, the obstacle was identified as a wall-form obstacle, the walking along wall manner was activated and obstacle points were recorded frequently by the aforementioned rules. When the AUV arrived at position 2 and position 3, the heading was pointing to the destination, but the second condition was not satisfied, so it continued maintaining this manner. When the AUV arrived at position 4, the heading was pointing to the destination again, and the second condition was not satisfied, so walking along wall manner was terminated and the AUV drived to the target directly. 4.2. Discussion Simulations have been carried out in three different unknown obstacle scenarios. The obstacle avoidance trajectory by APF is worse, the path is not smooth and heading is not steady, and paths are the longest in all three obstacle scenes using this algorithm, moreover, the AUV can’t get to the destination in the third scenario with this algorithm. The obstacle avoidance trajectory by PSO is good in general, however, if a U-shape obstacle is more complicated, for example, in Figure 14b the left part of wall-form obstacle was as complicated as its right side, AUV could get trapped inside a wall-form obstacle by this algorithm. It is apt to become trapped in complicated U-shape obstacle environments which it cannot solve by itself. The RRA is superior in the following aspects: it can realize the goals in all three different obstacle scenes with the shortest time, respectively, planning smooth trajectories and the shortest voyages, meanwhile, it makes the AUV keep a steady heading, keeping better safe distances away from obstacles. Moreover it solves the defect of being apt to getting into traps in U-shape obstacle environments that affects APF. 5. Conclusions and Future Work In this paper, a LPAA had been proposed to change an obstacle’s irregular visual surface into a convex polygon, and a real-time reaction obstacle avoidance algorithm for AUVs is presented. The algorithm adapts to complex and cluttered unknown environments, and generates a smooth and shorter path, while guaranteeing appropriate safe distances. The simulation experiments illustrate that this algorithm is flexible in cluttered unknown environments, and in particular, it is able to solve the problem of AUVs becoming trapped by U-shape obstacles existing in other obstacle avoidance algorithms. The next stage of this work is to apply this algorithm in pool experiments, and we will extend this algorithm to more complicated ocean environments, where time variable ocean currents and dynamic objects exist. Acknowledgments: This work was partially funded by the National Nature Science Foundation of China under grant No. 51679057, and partly supported by province science Fund for distinguished Young Scholars No. J2016JQ0052. Author Contributions: Zheping Yan proposed the main idea, Jiyun Li design the algorithm and simulation; Jiyun Li writes the manuscript; Genshi Zhang design the sonar model; Yi Wu write part of the simulation programme; Zheping Yan supervise the research work and refined the manuscript. Conflicts of Interest: The founding sponsors had no role in the design of the study; in the collection, analyses, or interpretation of data; in the writing of the manuscript, and in the decision to publish the results. References 1. Yan, Z.P.; Yu, H.M.; Zhang, W.; Li, B.Y.; Zhou, J.J. Globally Finite-Time Stable Tracking Control of Underactuated UUVs. Ocean Eng. 2015, 107, 132–146. [CrossRef] Sensors 2018, 18, 438 19 of 19 2. Xiang, X.B.; Yu, C.Y.; Niu, Z.M.; Zhang, Q. Subsea Cable Tracking by Autonomous Underwater Vehicle with Magnetic Sensing Guidance. Sensors 2016, 16, 1335. [CrossRef] [PubMed] 3. Zeng, Z.; Lammas, A.; Sammut, K.; He, F.P.; Tang, Y.H. Shell Space Decomposition based Path Planning for AUVs Operating in a Variable Environment. Ocean Eng. 2014, 91, 181–195. [CrossRef] 4. Bi, F.Y.; Wei, Y.J.; Zhang, J.Z.; Cao, W. Position-Tracking Control of Underactuated Autonomous Underwater Vehicles in the Presence of Unknown Ocean Currents. IET Control Theory Appl. 2010, 4, 2369–2380. [CrossRef] 5. Cai, W.; Zhang, M.Y.; Zheng, Y.R. Task Assignment and Path Planning for Multiple Autonomous Underwater Vehicles Using 3D Dubins Curves. Sensors 2017, 17, 1607. [CrossRef] [PubMed] 6. Saravanakumar, S.; Asokan, T. Multipoint Potential Field Method for Path Planning of Autonomous Underwater Vehicles in 3D Space. Intell. Serv. Robot. 2013, 6, 211–224. [CrossRef] 7. Kovács, B.; Szayer, G.; Tajti, F.; Burdelis, M.; Korondi, P. A Novel Potential Field Method for Path Planning of Mobile Robots by Adapting Animal Motion Attributes. Robot. Auton. Syst. 2016, 82, 24–34. [CrossRef] 8. Lee, S.M.; Myun, H. Receding Horizon Particle Swarm Optimisation-Based Formation Control with Collision Avoidance for Non-Holonomic Mobile Robots. IET Control Theory Appl. 2015, 9, 2075–2083. [CrossRef] 9. Dadgar, M.; Jafari, S.; Hamzeh, A. A PSO-Based Multi-Robot Cooperation Method for Target Searching in Unknown Environments. Neurocomputing 2016, 177, 62–74. [CrossRef] 10. Bui, L.D.; Kim, Y.G. An Obstacle-Avoidance Technique for Autonomous Underwater Vehicles Based on BK-Products of Fuzzy relation. Fuzzy Sets Syst. 2006, 157, 560–577. [CrossRef] 11. Nakhaeinia, D.; Karasfi, B. A Behavior-Based Approach for Collision Avoidance of Mobile Robots in Unknown and Dynamic Environments. Intell. Fuzzy Syst. 2013, 24, 299–311. 12. Lee, Y.I.; Kim, Y.G.; Kohout, L.J. An Intelligent Collision Avoidance System for AUVs Using Fuzzy Relational Products. Inf. Sci. 2004, 158, 209–232. [CrossRef] 13. Liu, Y.C.; Liu, S.Y.; Wang, N. Fully-Tuned Fuzzy Neural Network based Robust Adaptive Tracking Control of Unmanned Underwater Vehicle with thruster Dynamics. Neurocomputering 2016, 196, 1–13. [CrossRef] 14. Pothal, J.K.; Parhi, D.R. Navigation of Multiple Mobile Robots in a Highly Clutter Terrains Using Adaptive Neuro-Fuzzy Inference System. Robot. Auton. Syst. 2015, 72, 48–58. [CrossRef] 15. Zhu, D.Q.; Sun, B. The Bio-Inspired Model Based Hybrid Sliding-Mode Tracking Control for Unmanned Underwater Vehicles. Eng. Appl. Artif. Intell. 2013, 26, 2260–2269. [CrossRef] 16. Zeng, Z.; Sammutb, K.; Lian, L.; He, F.P.; Lammas, A.; Tang, Y.H. A Comparison of Optimization Techniques for AUV Path Planning in Environments with Ocean Currents. Robot. Auton. Syst. 2016, 82, 61–72. [CrossRef] 17. Conti, R.; Meli, E.; Ridolfi, A.; Allotta, B. An Innovative Decentralized Strategy for I-AUVs Cooperative Manipulation Tasks. Robot. Auton. Syst. 2015, 72, 261–276. [CrossRef] 18. Zeng, Z.; Sammut, K.; Lammas, A.; He, F.B.; Tang, Y.H. Imperialist Competitive Algorithm for AUV Path Planning in a Variable Ocean. Appl. Artif. Intell. 2015, 29, 402–420. [CrossRef] 19. Das, P.K.; Behera, H.S.; Das, S.; Tripathy, H.K.; Panigrahi, B.K.; Pradhan, S.K. A Hybrid Improved PSO-DV Algorithm for Multi-Robot Path Planning in a Clutter Environment. Neurocomputing 2016, 207, 735–753. [CrossRef] 20. Aghababa, M.P. 3D Path Planning for Underwater Vehicles Using Five Evolutionary Optimization Algorithms Avoiding Static and Energetic Obstacles. Appl. Ocean Res. 2012, 38, 48–62. [CrossRef] 21. Fang, M.C.; Wang, S.M.; Wu, M.C.; Lin, Y.H. Applying the Self-Tuning Fuzzy Control with the Image Detection Technique on the Obstacle-Avoidance for Autonomous Underwater Vehicles. Ocean Eng. 2015, 93, 11–24. [CrossRef] 22. Shojaei, K.; Arefi, M.M. On the Neuro-Adaptive Feedback Linearising Control of Underactuated Autonomous Underwater Vehicles in Three-Dimensional Space. IET Control Theory Appl. 2015, 9, 1264–1273. [CrossRef] 23. Fossen, T.I.; Sagatun, S.I. Adaptive Control of Nonlinear Systems: A Case Study of Underwater Robotic Systems. J. Robot. Syst. 1991, 8, 393–412. [CrossRef] 24. Norgren, P.; Skjetne, R. Line-of-Sight iceberg Edge-Following Using an AUV Equipped With Multibeam Sonar. In Proceedings of the 2015 16th IFAC on Technology, Culture and International Stability, Sozopos, Bulgaria, 24–27 September 2015; pp. 81–88. © 2018 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/). http://www.deepdyve.com/assets/images/DeepDyve-Logo-lg.png Sensors (Basel, Switzerland) Pubmed Central

A Real-Time Reaction Obstacle Avoidance Algorithm for Autonomous Underwater Vehicles in Unknown Environments

Sensors (Basel, Switzerland) , Volume 18 (2) – Feb 2, 2018

Loading next page...
 
/lp/pubmed-central/a-real-time-reaction-obstacle-avoidance-algorithm-for-autonomous-1duRRVxzeI

References

References for this paper are not available at this time. We will be adding them shortly, thank you for your patience.

Publisher
Pubmed Central
Copyright
© 2018 by the authors.
ISSN
1424-8220
eISSN
1424-8220
DOI
10.3390/s18020438
Publisher site
See Article on Publisher Site

Abstract

sensors Article A Real-Time Reaction Obstacle Avoidance Algorithm for Autonomous Underwater Vehicles in Unknown Environments Zheping Yan, Jiyun Li *, Gengshi Zhang and Yi Wu Marine Assembly and Automatic Technology Institute, College of Automation, Harbin Engineering University, Harbin 150001, China; yanzheping@hrbeu.edu.cn (Z.Y.); zhanggengshi@hrbeu.edu.cn (G.Z.); wuyi_ivy@hrbeu.edu.cn (Y.W.) * Correspondence: jiyun5409@hrbeu.edu.cn; Tel.: +86-188-4642-2618 Received: 22 December 2017; Accepted: 29 January 2018; Published: 2 February 2018 Abstract: A novel real-time reaction obstacle avoidance algorithm (RRA) is proposed for autonomous underwater vehicles (AUVs) that must adapt to unknown complex terrains, based on forward looking sonar (FLS). To accomplish this algorithm, obstacle avoidance rules are planned, and the RRA processes are split into five steps Introduction only lists 4 so AUVs can rapidly respond to various environment obstacles. The largest polar angle algorithm (LPAA) is designed to change detected obstacle’s irregular outline into a convex polygon, which simplifies the obstacle avoidance process. A solution is designed to solve the trapping problem existing in U-shape obstacle avoidance by an outline memory algorithm. Finally, simulations in three unknown obstacle scenes are carried out to demonstrate the performance of this algorithm, where the obtained obstacle avoidance trajectories are safety, smooth and near-optimal. Keywords: autonomous underwater vehicle; forward looking sonar; obstacle avoidance; reaction obstacle avoidance algorithm 1. Introduction Recently, the question of collision avoidance for AUVs has attracted much attention from the ocean engineering and control community due to the wide commercial and military applications of AUVs, such as submarine cable inspection, submarine petroleum pipeline checking, underwater topographic surveying, ocean resource detection and oil field exploitation [1–5]. Currently, plenty of approaches were proposed for the obstacle avoidance of AUVs, such as artificial potential field algorithm (APF) [6,7], Dijkstra’s algorithm, A* algorithm, grid method, particle swarm optimization (PSO) [8,9], fuzzy algorithm [10–12], neuro-fuzzy algorithm [13,14], bio-inspired algorithm [15], etc. These methods each have their own advantages and special features, while they also have some inherent deficiencies. The artificial potential field method is characterized by a simple structure and easiness to implement, however, it is apt to be trapped in a local minimum when AUVs run into clustered obstacles environments or narrow channels [16,17], the same drawbacks can also be found in genetic algorithm (GA) [18]. Different from the aforementioned approaches, the grid method is especially suitable for terrains with known obstacles due to the capability of searching for the optimal collision avoidance path, however, the storage expense and calculation burden are disadvantages limiting its broad implementation. Motivated by the aforementioned drawbacks in the traditional algorithms, hybrid approaches have been widely presented in the literature. In [19,20], a particle swarm optimization (PSO) with a differentially perturbed velocity hybrid algorithm was proposed for collision avoidance. In [21], a method that combines adaptive fuzzy control and image Sensors 2018, 18, 438; doi:10.3390/s18020438 www.mdpi.com/journal/sensors Sensors 2018, 18, 438 2 of 19 Sensors 2018, 18, x FOR PEER REVIEW   2 of 21  detection was presented. Other classical methods such as the neural network merging fuzzy inference system can be found in [14]. combines adaptive fuzzy control and image detection was presented. Other classical methods such  Most of the above obstacle avoidance algorithms are based on hypothesis that the obstacles are as the neural network merging fuzzy inference system can be found in [14].  circle-shaped or spherical, however, real obstacles are irregular and in fact they can’t be disposed of Most of the above obstacle avoidance algorithms are based on hypothesis that the obstacles are  simply as circle-shaped or spherical, so those algorithms lack a rapid response to irregular obstacles in circle‐shaped or spherical, however, real obstacles are irregular and in fact they can’t be disposed of  unknown simply situations as circle‐shaped and ar ore spheric easilyatrapped l, so those in alU-shape gorithms obstacles. lack a rapidConsidering  response to irre thegular above  obstacles problems,   in unknown situations and are easily trapped in U‐shape obstacles. Considering the above problems,  a novel real-time reaction obstacle avoidance method is proposed based on FLS. In this strategy, a novel real‐time reaction obstacle avoidance method is proposed based on FLS. In this strategy, the  the whole obstacle avoidance process is divided into several steps: directly drive to the target, whole obstacle avoidance process is divided into several steps: directly drive to the target, keep a  keep a safe distance, obstacle avoidance, and walk along wall. The algorithm has good flexibility in safe  distance,  obstacle  avoidance,  and  walk  along  wall.  The  algorithm  has  good  flexibility  in  unknown environments. unknown environments.   The rest of the paper is organized as follows: Section 2 presents and formulates the The  rest  of  the  paper  is  organized  as  follows:  Section  2  presents  and  formulates  the   obstacle-avoidance problem description. Section 3 presents obstacle avoidance algorithms for different obstacle‐avoidance  problem  description.  Section  3  presents  obstacle  avoidance  algorithms  for  types of obstacles. In Section 4, simulation results are provided to illustrate the performance of the different  types  of  obstacles.  In  Section  4,  simulation  results  are  provided  to  illustrate  the  presented algorithm in all sorts of unknown environments. Finally, conclusions are given in Section 5. performance of the presented algorithm in all sorts of unknown environments. Finally, conclusions  are given in Section 5.  2. Problem Description and Formulation 2. Problem Description and Formulation  2.1. Kinematics Model 2.1. Kinematics Model  In this paper, the AUV is equipped with two main propellers mounted astern, which providing navigation power (surge), The AUV is also equipped with four auxiliary thrusters, two auxiliary In this paper, the AUV is equipped with two main propellers mounted astern, which providing  thrusters navigation are transverse  power (surg layout e), The pr AU oviding V is al sway so eqpower uipped and withyaw  fourmomentums,  auxiliary thrusters, the other  two two auxiauxiliary liary  thrusters are transverse layout providing sway power and yaw momentums, the other two auxiliary  thrusters are vertical layout providing power for heave and moments for pitching. In addition, thrusters are vertical layout providing power for heave and moments for pitching. In addition, a  a horizontal rudder and vertical rudder are mounted onboard the AUV to change the heading angle of horizontal rudder and vertical rudder are mounted onboard the AUV to change the heading angle of  the AUV in the horizontal and vertical directions, respectively. Accordingly, except for roll, the other the AUV in the horizontal and vertical directions, respectively. Accordingly, except for roll, the other  five degrees of freedoms such as surge, sway, heave, pitch and yaw are controllable. According to the five degrees of freedoms such as surge, sway, heave, pitch and yaw are controllable. According to  definition in [22], underactuated AUVs are those where the number of their independent actuators is the  definition  in  [22],  underactuated  AUVs  are  those  where  the  number  of  their  independent  fewer than their degrees of freedom, so the AUV in the paper belongs to this type. actuators is fewer than their degrees of freedom, so the AUV in the paper belongs to this type.  Two reference coordinates are adapted in the paper, they are North-East-Down (NED) coordinate Two  reference  coordinates  are  adapted  in  the  paper,  they  are  North‐East‐Down  (NED)  and body-fixed coordinate, as shown in Figure 1, where the linear velocity V = [u, v, w] , angular coordinate  and  body‐fixed  coordinate,  as  shown  in  Figure  1,  where  the  linear  velocity   T T velocity ! = [pT, q, r] , and attitude  = [f, Tq, y] . T V = [u, v, w] , angular velocity ω = [p, q, r] , and attitude η = [φ, θ, ψ] , .   NED coordinate BODY coordinate p (roll) u (Surge) q (pitch) r (yaw) v (sway) w(heave) Figure 1. Earth‐ inertial frames and body‐fixed reference frames.  Figure 1. Earth- inertial frames and body-fixed reference frames. Sensors 2018, 18, 438 3 of 19 According to the standard notation motion equations of an AUV, a six degrees of freedom kinematics model for an AUV is described in [23]: 2 3 2 3 x u 6 7 6 7 = J () (1) 4 y 5 4 v 5 z w 2 3 2 3 6 7 6 7 4 q 5 = J ()4 q 5 (2) 2 3 cos y cos q cos y sin q sin f sin y cos f cos y sin q cos f + sin y sin f 6 7 J () = sin y cos q sin y sin q sin f + cos y cos f sin y sin q cos f cos y sin f (3) 4 5 sin q cos q sin f cos q cos f 2 3 1 sin f tan q cos f tan q 6 7 J () = 4 0 cos f sin f 5 (4) 0 sin f/ cos q cos f/ cos q Hypothesis 1: Because the roll movement is uncontrollable for AUV, and the structure of AUV is bilateral symmetrical, define f = 0, so Equations (1) and (2) can be rewritten as: x = u cos y cos q v sin y + w cos y sin q y = u sin y cos q + v cos y + w sin y sin q (5) z = u sin q + w cos q q = q . (6) y = r/ cos q In consideration of the fact the AUV’s additional hydrodynamic resistances in the horizontal and vertical direction are greater than those in the longitudinal one, when the speed over grand (SOG) exceeds 1 knot, the propulsive efficiency of the auxiliary thrusters is very low, so the auxiliary thrusters are idle while the AUV is navigating at normal speed. In general, we take w = 0, v = 0, then Equation (1) is simplified as: x = u cos y cos q y = u sin y cos q (7) z = u sin q 2.2. Obstacle Detection A FLV is utilized for obstacle detection, and the major parameters of the FLS model are designed as follows: the detection range is 150 m; field of view is 120 ; detection frequency is 2 Hz; number of beams is 80 beams. As the pitch angle q is seldom altered during normal navigation, the FLS is installed onboard the AUV in the XOY plate. Beams are numbered 0, 1, . . . , 79 in order, range from (60 , 60 ), and beams’ detection distances are denoted as S , i = 0, 1, . . . , 79. The schematic is shown in Figure 2. In the paper, a real-time obstacle avoidance strategy is proposed based on FLS, all the obstacles are considered unknown and their shapes are irregular, the obstacles’ outlines are generated by the detection datum of FLS, only the multi-beams in the horizon plane in body coordinates are adopted in consideration of the fact the AUV’s pitch angles are seldom changed. The purple lines are the sonar beams in the horizontal plane in body coordinates, the gray body is an obstacle, and the blue curves Sensors 2018, 18, 438 4 of 19 are the detected outline of the obstacle. However, the curves, which are also determined by the shape Sensors 2018, 18, x FOR PEER REVIEW 4 of 21 and the detected position of obstacles, are not predetermined even though the environment is known. l  150 m  120 Figure 2. Obstacle detection diagram. Figure 2. Obstacle detection diagram. 2.3. Desirable Maximum Turning Radius R max In the paper, a real-time obstacle avoidance strategy is proposed based on FLS, all the obstacles are con The main sidere specifications d unknown and their of AUV model shapes ar are e as irreg follows: ular, th length e obstac is les’ o 4 m, u width tlines is are genera 1.2 m, width ted by the is 1.2 m, height detect ision d 0.8 m, atu Max m of speed FLS, on isly 3.0 the m m, rated ulti-beams speed inis th 2.0 e horizon m/s. When plane in the bod AUV y coo nar v din igates ates ar at e 2 a m/s doptin ed an in consideration of the fact the AUV’s pitch angles are seldom changed. The purple lines are the underwater environment without ocean currents, the minimum turning radius is about five times sonar beams in the horizontal plane in body coordinates, the gray body is an obstacle, and the blue the length of the AUV when the rudder angle is set at the largest steering angle of 35 , and it takes curves are the detected outline of the obstacle. However, the curves, which are also determined by about 3.5 s for the rudder angle to vary from 0 to 35 . If the time delay for steering angle transition is the shape and the detected position of obstacles, are not predetermined even though the taken account of, the trajectory deviation distance is 1–1.5 m, which is small in comparison with the environment is known. turning radius, to simplify the problem the deviation distance is ignored, in other words, the trajectory of turning is replaced by an arc with a certain radius. 2.3. Desirable Maximum Turning Radius Rmax In Figure 3, S is an arbitrary obstacle point detected by FLS, locating on the right of AUV and the The main specifications of AUV model are as follows: length is 4 m, width is 1.2 m, width is relative bearing angle is , r is the distance from obstacle point S to FLS. R is the biggest turning i i i i 1.2 m, height is 0.8 m, Max speed is 3.0 m, rated speed is 2.0 m/s. When the AUV navigates at 2 m/s in radius for AUV to detour this obstacle point, point O is the center of turning radius, and co is the an underwater environment without ocean currents, the minimum turning radius is about five times mid-perpendicular of line ab, D is safe distance, and defines: the length of the AUV when the rudder angle is set at the largest steering angle of 35°, and it takes about 3.5 s for the rudder angle to vary from 0° to 35°. If the time delay for steering angle transition is \daS = a , \dab = b , aS = r , S b = D , R = oa. i i i i i i s i taken account of, the trajectory deviation distance is 1–1.5 m, which is small in comparison with the turning radius, to simplify the problem the deviation distance is ignored, in other words, the The desirable maximum turning radius R is described as: trajectory of turning is replaced by an arc with a certain radius. aS S b In Figure 3, Si is an arbitrary obstacle point detected by FLS, locating on the right of AUV and i i = (8) the relative bearing angle is αi, ρi is the distance from obstacle point S to FLS. Ri is the biggest sin(\S ba) sin(\S ab) i i turning radius for AUV to detour this obstacle point, point O is the center of turning radius, and co \S ab = b a is the mid-perpendicular of line ab , Ds is safe distance, and defines: i i i (9) \S ab = p (p/2 b ) = p/2 + b i i i , , , , .  daS   dab  aS   Sb D Ro  a ii i ii is i Unite Equations (8) and (9), yields: The desirable maximum turning radius R is described as: a = j (i 0.5n + 0.5)/n aS S b (10) ii b = sec a + tan a (8) i i i sin( Sba) sin( S ab) ii  Sab   aS oa iii i = (11) (9) sin(\S oa) sin(\oS a)  Sab  i (/2 ) / i2 ii i Substituting Equation (10) into Equation (11), yields: Unite Equations (8) and (9), yields: (0in.50.5)/n R = r [cos a sin a / tan(2b )] (12) is i i i i i  (10)  sec tan ii i  i Sensors 2018, 18, x FOR PEER REVIEW 5 of 21 aS oa (11) sin( Soa) sin( oS a) ii Substituting Equation (10) into Equation (11), yields: Sensors 2018, 18, 438 5 of 19 R  [cos sin / tan(2 )] (12) ii i i i Figure 3. Desirable maximum turning radius for an arbitrary detection point. Figure 3. Desirable maximum turning radius for an arbitrary detection point. Remark: The AUV postures will change following steering, which makes the hydrodynamic resistance on the AUV increase, and produces roll momentum, and the larger the steering angle is, Remark: The AUV postures will change following steering, which makes the hydrodynamic the larger the effect is. If possible, we choose the steering angle as small as possible. resistance on the If the AUV AUV incr detours ease, around the obsta and produces cle frr om the ri oll momentu ght side, i m, is th and e order n theulmb arger er of be the ams, t steering he angle is, obstacle point detected by the i-th beam whose maximum turning is denoted as R , the desirable the larger the effect is. If possible, we choose the steering angle as small as possible. maximum turning radius1: If the AUV detours around the obstacle from the right side, i is the order number of beams, mRR in{ |i 40,41,,79} (13) max i the obstacle point detected by the i-th beam whose maximum turning is denoted as R , the desirable Otherwise, when the AUV detours around the obstacle from the left side, the desirable maximum turning radius1: maximum turning radius is given by: R = minfR ji = 40, 41, , 79g (13) max RR min{ |i 0,1,, 39} (14) max i Otherwise, when the AUV detours around the obstacle from the left side, the desirable maximum turning radius 2.4.is Obstacles Classification given by: R = minfR ji = 0, 1, , 39g (14) For convenient obstacle avoidance, obstacles are divided into four categories: bounded obstacle, max i left unbounded obstacle, right unbounded obstacle and unbounded obstacle. the classification criterion is when obstacles enter sonar’s segment areas and their distances to sonar is less than 80 m, 2.4. Obstacles Classification according to their visible outline (from the spot of FLS) beyond the detection range of sonar or not, which is illustrated in Figure 4. k and l is the left boundary and right boundary detected by FLS, respectively, For convenient obstacle avoidance, obstacles are divided into four categories: bounded obstacle, i, j is serial number of beams, δ, ζ is arbitrary nature number. left unbounded obstacle, right unbounded obstacle and unbounded obstacle. the classification criterion is when obstacles enter sonar ’s segment areas and their distances to sonar is less than 80 m, according to a) If all the visible outline of obstacle is within the segment areas of FLS, it was defined as a bounded obstacle (BO). their visible outline (from the spot of FLS) beyond the detection range of sonar or not, which is  kl, ,, ,kl,i, jN,i[k,l],[ j k,1 k ] [l1,l],st.0 S LS, 0 illustrated in Figure 4. k and l is the left boundary and right boundary detected ie byj FLS, respectively, i, j is serial number of beams, d, z is arbitrary nature number. b) If the left edges of obstacle exceed the FLS detection range, and the right ones are within the FLS detection range, it is defined as left unbounded obstacle (LUBO). (a) If all the visible outline of obstacle is within the segment areas of FLS, it was defined as a bounded obstacle (BO). 9 k, l, d, V 2 Z, k < l, i, j 2 N, i 2 [k, l], j 2 [k V, k 1][ [l + 1, l + d], st. 0 < S  L , S = 0 i e j (b) If the left edges of obstacle exceed the FLS detection range, and the right ones are within the FLS detection range, it is defined as left unbounded obstacle (LUBO). 9 k, d 2 Z, i 2 [0, k], j 2 [k, k + d], st. 0 < S  L , S = 0 i e j (c) If the right edges of obstacle exceed the FLS detection range, and the left one is within the FLS detection range, it is defined as a right unbounded obstacle (RUBO): 9 k, d 2 Z, i 2 [0, k], j 2 [k, k + d], st. 0 < S  L , S = 0 i j (d) If two sides of the obstacle exceed the FLS detection range, we define it as a unbounded obstacle (UBO). 9 i 2 [0, 79], st. 0 < S  L i e Sensors 2018, 18, x FOR PEER REVIEW 6 of 21 ki ,,  [0,k],j[k,k ],st.0SL,S0 ie j c) If the right edges of obstacle exceed the FLS detection range, and the left one is within the FLS detection range, it is defined as a right unbounded obstacle (RUBO): ki ,,  [0,k],j[k,k ],st.0SL,S0 ie j d) If two sides of the obstacle exceed the FLS detection range, we define it as a unbounded obstacle (UBO). is [0, 79],t. 0S L ie Sensors 2018, 18, 438 6 of 19 (a) (b) (c) (d) Figure 4. Obstacle classification. (a) bounded obstacle; (b) left unbounded obstacle; (c) right Figure 4. Obstacle classification. (a) bounded obstacle; (b) left unbounded obstacle; (c) right unbounded unbounded obstacle; (d) unbounded obstacle. obstacle; (d) unbounded obstacle. 2.5. Rules of Obstacle Avoidance 2.5. Rules of Obstacle Avoidance Hypothesis 2: The ocean current is below 0.5 knot. Hypothesis 3: The rudder angle response delay can be ignored. Hypothesis 2: The ocean current is below 0.5 knot. In general, there are two purposes for steering, one is changing of AUV’s heading angle, the other is counteracting the effect of any external disturbance on the AUV. In this paper, only the first Hypothesis 3: The rudder angle response delay can be ignored. one is taken into account, and collision avoidance is classified into two modes: Normal obstacle avoidance, which is implemented by steering to change AUV heading angle, regulating the AUV navigation speed appropriately if necessary. In general, there are two purposes for steering, one is changing of AUV’s heading angle, the other is counteracting the effect of any external disturbance on the AUV. In this paper, only the first one is taken into account, and collision avoidance is classified into two modes: Normal obstacle avoidance, which is implemented by steering to change AUV heading angle, regulating the AUV navigation speed appropriately if necessary. Emergency obstacle avoidance, which is implemented in emergency situations when obstacles are discovered, and part of the obstacle points have entered the smallest safety range of AUV, and it is impossible to avoiding a collision by normal obstacle avoidance; those obstacle points are called emergency collision avoidance points (ECAP) in the paper. The process of emergency obstacle avoidance is composed of the following steps, firstly, the main propellers slow down to zero quickly, then propeller reverse and acceleration, which make the navigation speed of AUV decline rapidly, and auxiliary thrusters will be started once the SOG is below 0.5 m/s. As the emergency obstacle avoidance process is invariable, only normal mode collision avoidance is studied in this paper. 2.5.1. Obstacle Avoidance Rules I If obstacles are BO, and there isn’t any ECAP when the AUV detours the obstacles, we select the side as detour direction from which the angle deviating from target (ADT) is smaller, the obstacle-avoidance rules described as follows: Sensors 2018, 18, 438 7 of 19 (a) If the AUV is currently in line navigation, g is the ADT when detouring around the obstacle from its left side, and is the ADT when detouring from the other side, and the detour direction is as follows: g > g , le f t g  g , right r l (b) If the AUV is currently turning around for obstacle-avoidance, keep the turn direction; (c) If AUV is currently turning around for reducing ADT, and the turning radius is R , the distance to target is L , the detour direction is as follows: L sin(g ) 2sgn(R )R > L sin(g ), le f t v r v d d l else, right where, sgn(.) is signum function, l is constant coefficient. 2.5.2. Obstacle Avoidance Rules II Obstacles are one side or both sides unbounded, and there are no ECAP, so we adopt the following rules: (a) If the obstacle is LUBO, detour around obstacles from the right side. (b) If the obstacle is RUBO, detour around obstacles from the left side. (c) If the obstacle is UBO, and the AUV is turning around, keeping the turn direction, detour around obstacles from the left side; otherwise, detour around obstacles from the right side. 2.5.3. Obstacle Avoidance Rules III There is ECAP when an AUV detours around obstacles, and we use these obstacle avoidance rules: (a) If one side of the obstacle is unbounded and exists in ECAP, there is no ECAP on the other side, detour around the other side; (b) If ECAP only exist on one side of the obstacle and this side is bounded, and the other one is unbounded, select the former as the detour direction; (c) If ECAP exist on both sides of obstacle, choose the direction to detour from which the ADT is smaller. The flowchart for obstacle avoidance rules is shown in Figure 5: Sensors 2018, 18, x FOR PEER REVIEW 8 of 21 start No no ECAP? Rule II Rule III No No LUBO Is OB? ECAP in two sides? Yes Yes Yes No RUBO Smaller ADT Obstacle avoidance Turn right ECAP and UB? Rule I side Yes Yes Yes No No Yes Turn the Current LUBO? Line navigation? Turn left other side direction No Yes Yes No Yes No No Yes Is turning Turn left Turn right Satify condition 1   rl around? Current Turn left Turn right Turn left Turn right Turn right direction Figure 5. The flowchart for obstacle avoidance rules. Figure 5. The flowchart for obstacle avoidance rules. 2.5.4. Path Update Principle If the rudder angle isn’t equal to zero and AUV is detouring obstacles, it is necessary to calculate desirable maximum turning radius Rmax and verify that the current turning radius Rc is smaller than Rmax for guaranteeing obstacle-avoidance safety, if it is insured that AUV won’t collide with obstacles, keeping the current status, otherwise, a new path needs to be redesigned. If the rudder angle is zero, in other words, the AUV is in straight line navigation, when an obstacle is detected dead ahead of the AUV at less than 80 m (Si < 80, i = 37, 38, …, 42), it is necessary to design a new path immediately, however, the distance is decreased to 60 m (S < 40, i = 37, 38, …, 42) in dense obstacles environments. 3. Obstacle Avoidance Algorithms 3.1. Reaction Obstacle Avoidance RRA is a real-time obstacle-avoidance algorithm, which is especially adapted to unknown complex environments. The process is divided into several steps: a) Directly drive to the target: when the AUV is passing a certain position, FLS doesn’t detect any obstacles, or detect obstacles which are on the flank of the AUV and the target (or next waypoint) is in the other side, the heading angle is adjusted to the target (or next waypoint) immediately. b) Keep a safe distance: obstacles are detected on the side of the AUV, so if driving to the target directly, the AUV will collide with obstacles or the safety distance margin isn’t enough, then AUV keeps safe distance from obstacles and decreases the ADT if possible, which makes the AUV drive parallel with the edges of obstacle’s outline. c) Solo obstacle avoidance: if by keeping the current posture, the AUV will collide with obstacles, then the heading angle must be adjusted to avoid colliding, the detour direction is chosen by the aforementioned rules, reducing the navigation speed if necessary. d) Cluttered obstacle avoidance: when one of obstacles has entered the 80 m range of AUV in a cluttered environment, this manner is activated, and the AUV selects an appropriate passage between obstacles considering comprehensive factors such as ADT, the width of the passage, and the unobstructed character from the spot of FLS. The AUV keeps a safe Sensors 2018, 18, 438 8 of 19 2.5.4. Path Update Principle If the rudder angle isn’t equal to zero and AUV is detouring obstacles, it is necessary to calculate desirable maximum turning radius R and verify that the current turning radius R is smaller than max c R for guaranteeing obstacle-avoidance safety, if it is insured that AUV won’t collide with obstacles, max keeping the current status, otherwise, a new path needs to be redesigned. If the rudder angle is zero, in other words, the AUV is in straight line navigation, when an obstacle is detected dead ahead of the AUV at less than 80 m (S < 80, i = 37, 38, . . . , 42), it is necessary to design a new path immediately, however, the distance is decreased to 60 m (S < 40, i = 37, 38, . . . , 42) in dense obstacles environments. 3. Obstacle Avoidance Algorithms 3.1. Reaction Obstacle Avoidance RRA is a real-time obstacle-avoidance algorithm, which is especially adapted to unknown complex environments. The process is divided into several steps: (a) Directly drive to the target: when the AUV is passing a certain position, FLS doesn’t detect any obstacles, or detect obstacles which are on the flank of the AUV and the target (or next waypoint) is in the other side, the heading angle is adjusted to the target (or next waypoint) immediately. (b) Keep a safe distance: obstacles are detected on the side of the AUV, so if driving to the target directly, the AUV will collide with obstacles or the safety distance margin isn’t enough, then AUV keeps safe distance from obstacles and decreases the ADT if possible, which makes the AUV drive parallel with the edges of obstacle’s outline. (c) Solo obstacle avoidance: if by keeping the current posture, the AUV will collide with obstacles, then the heading angle must be adjusted to avoid colliding, the detour direction is chosen by the aforementioned rules, reducing the navigation speed if necessary. (d) Cluttered obstacle avoidance: when one of obstacles has entered the 80 m range of AUV in a cluttered environment, this manner is activated, and the AUV selects an appropriate passage between obstacles considering comprehensive factors such as ADT, the width of the passage, and the unobstructed character from the spot of FLS. The AUV keeps a safe distance with the obstacles on the sides of passages, if the passage is wide enough, decreasing the ADT as far as possible. (e) Walk along the wall: if encountering UBO or gallery terrain, the AUV navigates parallel to the edges of the obstacle’s outline until there is no obstacle hampering the AUV from directly driving to the target. During the whole process of walking along wall, some obstacle points need to be saved at intervals as a judgement criterion for terminating this manner. The details will be described in Section 3.4. 3.2. Obstacles Outline Disposition It’s important to choose the right timing for obstacle avoidance. Too early, the obstacles partially enter the detection range of FLS, they aren’t detected entirely; too late, and the AUV gets so close to the obstacle that the obstacle avoidance time is hasty, which creates unnecessary difficulties for collision avoidance. The best time to take obstacle-avoidance measures is when obstacles enter into the 80 m detection range of FLV, except for dense obstacle environments, so the FLS achieves more accurate detection of the obstacles, and the time set aside for collision-avoidance is enough. By this method, not only rudder angle and the time of steering are decreased, but also a more appropriate obstacle avoidance path is produced. 3.2.1. FLS Detection Data Grouping 803 FLS detection data is saved as an array b 2 R , where each item of the array denotes the detection distance between sonar beams and obstacle points, and if some item is equal to zero it indicates this sonar beam doesn’t detect any obstacle point. Sensors 2018, 18, x FOR PEER REVIEW 9 of 21 distance with the obstacles on the sides of passages, if the passage is wide enough, decreasing the ADT as far as possible. e) Walk along the wall: if encountering UBO or gallery terrain, the AUV navigates parallel to the edges of the obstacle’s outline until there is no obstacle hampering the AUV from directly driving to the target. During the whole process of walking along wall, some obstacle points need to be saved at intervals as a judgement criterion for terminating this manner. The details will be described in Section 3.4. 3.2. Obstacles Outline Disposition It’s important to choose the right timing for obstacle avoidance. Too early, the obstacles partially enter the detection range of FLS, they aren’t detected entirely; too late, and the AUV gets so close to the obstacle that the obstacle avoidance time is hasty, which creates unnecessary difficulties for collision avoidance. The best time to take obstacle-avoidance measures is when obstacles enter into the 80 m detection range of FLV, except for dense obstacle environments, so the FLS achieves more accurate detection of the obstacles, and the time set aside for collision-avoidance is enough. By this method, not only rudder angle and the time of steering are decreased, but also a more appropriate obstacle avoidance path is produced. 3.2.1. FLS Detection Data Grouping Sensors 2018, 18, 438 9 of 19 80 3 FLS detection data is saved as an array   R , where each item of the array denotes the detection distance between sonar beams and obstacle points, and if some item is equal to zero it indicates this sonar beam doesn’t detect any obstacle point. The output datum of FLS needs to be disposed to produce obstacle outline. The first step is The output datum of FLS needs to be disposed to produce obstacle outline. The first step is taking out the second column date from array b and dividing the data into groups according to taking out the second column date from array  and dividing the data into groups according to Equations (15) and (16), and each group data is considered as belonging to an obstacle. The group Equations (15) and (16), and each group data is considered as belonging to an obstacle. The group criterion is as follows: criterion is as follows: kS SSS k < ddS , S,0 S S 6=,i0,[1,i729] [1, 79] (15) (15) i1 i i i1 ii 11 bb i i dl  d = l l (16) (16) bbe e b b where is gap width, is a select coefficient whose value range is [1, 4], is the total length of d  l b b o where d is gap width, l is a select coefficient whose value range is [1, 4], l is the total length of the b b o the AUV. All of the points which satisfy Equations (15) and (16) can be considered as a group AUV. All of the points which satisfy Equations (15) and (16) can be considered as a group datapoint of datapoint of the same obstacle. For example, in Figure 6, the detection data is divided into two the same obstacle. For example, in Figure 6, the detection data is divided into two groups. groups. Figure 6. FLS detecting data grouped. Figure 6. FLS detecting data grouped. 3.2.2. Largest Polar Angle Algorithm 3.2.2. Largest Polar Angle Algorithm The output datum of FLS are disperse points in the coordinate system, and an obstacle outline is produced by aligning those points in the same group one by one, however, the outlines aren’t always regular, which can’t be adopted in obstacle avoidance, hence LPAA is proposed to transform the irregular shape into a polygon. Figure 7 shows a detected obstacle which is LUBO. In order to improve the efficiency of collision avoidance, it is necessary to simplify the detected outline of the obstacle. The concept of LPAA involves using a convex polygon with fewer sides to surround a group of obstacle points, and satisfy that each vertex of those sides belongs to the array b, then the irregular obstacle visible outline is changed into a convex polygon. The polar angle of those sides is decreased gradually according to generated sequence, the specific steps are as follows: Step 1: Take grouped data to generate sonar beams (purple line) and generate obstacle points; Step 2: Line up obstacle points one by one to produce a detected obstacle outline (black line), choose the rightmost border point (point A) as the starting point, connect it with others points on the left of it; Step 3: Find an obstacle point as point B, which satisfies the condition that the polar angle of line AB is larger than those of other points aligned with point A. The polar angle value range is [0 , 360 ), which is taken as positive in the anti-clockwise sense without loss of generality; Step 4: Take the point found in step 3 as the new starting point, repeat step 1 and step 2 to find the next point (point C, D, . . . ) until the leftmost border point is picked out; Step 5: Align points A, B, . . . in sequence. The blue line ABCDE is the disposed result. In practice, once the detour direction is determined, we only need to deal with the points in this direction. Taking Figure 7 as an example, we only need to deal with the obstacle points from S to the rightmost border point when detouring around the right side of the obstacle, which increases the obstacle avoidance response speed by reducing unnecessary computation. Sensors 2018, 18, x FOR PEER REVIEW 10 of 21 The output datum of FLS are disperse points in the coordinate system, and an obstacle outline is produced by aligning those points in the same group one by one, however, the outlines aren’t always regular, which can’t be adopted in obstacle avoidance, hence LPAA is proposed to transform the irregular shape into a polygon. Figure 7 shows a detected obstacle which is LUBO. In order to improve the efficiency of collision avoidance, it is necessary to simplify the detected outline of the obstacle. The concept of LPAA involves using a convex polygon with fewer sides to surround a group of obstacle points, and satisfy that each vertex of those sides belongs to the array , then the irregular obstacle visible outline is changed into a convex polygon. The polar angle of those sides is decreased gradually according to generated sequence, the specific steps are as follows: Step 1: Take grouped data to generate sonar beams (purple line) and generate obstacle points; Step 2: Line up obstacle points one by one to produce a detected obstacle outline (black line), choose the rightmost border point (point A) as the starting point, connect it with others points on the left of it; Step 3: Find an obstacle point as point B, which satisfies the condition that the polar angle of line AB is larger than those of other points aligned with point A. The polar angle value range is [0°, 360°), which is taken as positive in the anti-clockwise sense without loss of generality; Step 4: Take the point found in step 3 as the new starting point, repeat step 1 and step 2 to find the next point (point C, D, …) until the leftmost border point is picked out; Step 5: Align points A, B, … in sequence. The blue line ABCDE is the disposed result. In practice, once the detour direction is determined, we only need to deal with the points in this direction. Taking Figure 7 as an example, we only need to deal with the obstacle points from S40 to Sensors 2018, 18, 438 10 of 19 the rightmost border point when detouring around the right side of the obstacle, which increases the obstacle avoidance response speed by reducing unnecessary computation. Figure 7. Obstacle detected surface disposed by LPAA. Figure 7. Obstacle detected surface disposed by LPAA. 3.3. Path Design for Single Obstacle 3.3. Path Design for Single Obstacle The aim of path design is achieving the shortest and smoothest paths free of collisions using fewer steering corrections and smaller steering rudder angles if possible. We design several The aim of path design is achieving the shortest and smoothest paths free of collisions using fewer waypoints and connect them by a straight or smooth arc (its radius is the turning radius) to construct a safe path. steering corrections and smaller steering rudder angles if possible. We design several waypoints and connect them by a straight or smooth arc (its radius is the turning radius) to construct a safe path. In Figure 8 the obstacle corresponds to LUBO, and the AUV detours around the right side of the obstacle according to the aforementioned obstacle avoidance rules. Extend the line ab, bc, cd, . . . , to intersect AUV track line at point k , k , k , . . . , respectively, and the steps are as follows: 1 2 3 Step 1: Find out the intersection k between line AB and AUV track line, calculating the length of line segment Pk . Step 2: Calculate f (t): f (t) = PK U T R  tan(0.5a) D  csc a (17) t min s 1 1 where, T is the response time of the obstacle-avoidance system, U is the speed of AUV, R min is the smallest turning radius of AUV, D is safe distance, take D  4L s s o Step 3: If f (t) is negative, turn to step 8, otherwise, calculate f (t): 1 2 f (t) = PK U T R  tan(0.5a) D  csc a (18) 2 t max s where, R is the largest turning radius for AUV. max Step 4: If f (t) is non-negative, parallel translation AK , and intersect AUV track line at point K, 2 1 KK = D csc a, design a transition point S as beginning position of steering, which satisfies: 1 s 1 PS = PK R  tan(0.5a) D  csc a (19) 1 1 max s Step 5: If f (t) is negative, the turning radius R and transition point S satisfies: 2 1 R = (PK U T D  csc a)/ tan(0.5a) 1 t s (20) PS = PK R tan(0.5a) D  csc a 1 1 Step 6: Design a transition point S as the steering end position, where the heading angle of the AUV satisfies: y = y + a where, y is the current heading angle of AUV, y is the heading angle when AUV is arriving at S . 0 t 1 Step 7: End Sensors 2018, 18, 438 11 of 19 Step 8: Replace the current line segment with the next line segment (such as: line BC instead of line AB), which is displayed in Figure 8b,c, repeat steps 1–6; Step 9: Taking the transition point S as beginning position of steering, D  2L , D  4L , 3 s1 o s2 o in Figure 8d, one can obtain: B L = D ctg(0.5a) s1 (21) LN = (D D )/ sin a s2 s1 Step 10: If D  4L and g (a) is nonnegative, take D = D , and replacing S with B : s1 o 1 s2 s1 3 g (a) = B L/ cos(0.5a) R (22) 1 min The turning radius R satisfies: R = D / sin(0.5a) (23) s1 Step 11: Else, take turning radius R = R , and point S satisfies: min 3 g (a) = R sin a cos(0.5a) + D cos a < 2 min s1 D = max(g (a), 4L ) (24) s2 2 MS = D / sin a R cos(0.5a) 3 s2 min Sensors 2018, 18, x FOR PEER REVIEW 12 of 21 Step 12: Design a transition point S as steering end position, where the heading angle of the AUV satisfies: Step 12: Design a transition point S as steering end position, where the heading angle of the y = y + a (25) AUV satisfies: where, y is the heading angle befor e the AUV arrives at S . (25) t t 0 3 Step 13: Turn to step 7. where, is the heading angle before the AUV arrives at S .  3 Step 13: Turn to step 7. (a) (b) (c) (d) Figure 8. (a–c) Path design for single obstacle; (d) Local amplification diagram. Figure 8. (a–c) Path design for single obstacle; (d) Local amplification diagram. 3.4. Wall-Form Obstacles Norgren presented the obstacle avoidance method called “iceberg Edge-Following” in [24], however, the wall form of obstacles in this paper is too simple, and this obstacle avoidance method doesn’t adapt to complex U-shape obstacles, or labyrinth obstacles. The next question is when is the optimal time for ending this manner and driving straight to the target for complex wall form obstacles, which hasn’t been proposed in the above paper. To solve this question, the paper presents a novel solution, which is obstacle outline memory algorithm. In Figure 9, the AUV detours around the wall-form obstacle from the right side, and once a wall-form obstacle is detected, the walking along wall manner is activated, and the memory function is started to recorded the rightmost obstacle points S detected by FLV at the transition position where steering is beginning or ending. Nevertheless, the wall-form obstacle outline is not described entirely by these points, so it is necessary to add other supplementary points, to ensure an obstacle point must be recorded that at least every 100 m of navigation distance. On the contrary, if the AUV detours around the wall-form obstacle from the left direction, leftmost obstacle points S are recorded at the transition position. Sensors 2018, 18, 438 12 of 19 3.4. Wall-Form Obstacles Norgren presented the obstacle avoidance method called “iceberg Edge-Following” in [24], however, the wall form of obstacles in this paper is too simple, and this obstacle avoidance method doesn’t adapt to complex U-shape obstacles, or labyrinth obstacles. The next question is when is the optimal time for ending this manner and driving straight to the target for complex wall form obstacles, which hasn’t been proposed in the above paper. To solve this question, the paper presents a novel solution, which is obstacle outline memory algorithm. In Figure 9, the AUV detours around the wall-form obstacle from the right side, and once a wall-form obstacle is detected, the walking along wall manner is activated, and the memory function is started to recorded the rightmost obstacle points S detected by FLV at the transition position where steering is beginning or ending. Nevertheless, the wall-form obstacle outline is not described entirely by these points, so it is necessary to add other supplementary points, to ensure an obstacle point must be recorded that at least every 100 m of navigation distance. On the contrary, if the AUV detours around the wall-form obstacle from the left direction, leftmost obstacle points S are recorded at the Sensors 2018, 18, x FOR PEER REVIEW 13 of 21 transition position. Figure 9. Wall-form obstacle avoidance. 1-wall form obstacle; 2-recorded obstacle point; 3-start Figure 9. Wall-form obstacle avoidance. 1-wall form obstacle; 2-recorded obstacle point; 3-start record record position; 4-supplement record position; 5-transition position; 6-obstacle trajectory; 7-target. position; 4-supplement record position; 5-transition position; 6-obstacle trajectory; 7-target. n2 These obstacle points are kept in a storage array DR  , As a demonstration in Figure 9, when n2 These obstacle points are kept in a storage array D 2 R , As a demonstration in Figure 9, when the wall-form obstacles are identified and it is decided to detour around their right side, the first the wall-form three obstacle obstacles pointsar ar ee re identified corded in and arrit ay D is decided in order,to which detour are t arh ound e leftmost their p right oint S side, , dead theahe first ad three point S and rightmost point S . Then the next obstacle point position is added into array D as 39 79 obstacle points are recorded in array D in order, which are the leftmost point S , dead ahead point S 0 39 mentioned above. This is repeated until this obstacle is passed by, and the simulation process is and rightmost point S . Then the next obstacle point position is added into array D as mentioned explained in the following section. above. This is repeated until this obstacle is passed by, and the simulation process is explained in the The manner of walking along wall is terminated if the following two conditions are satisfied: following section. firstly, ADT is smaller than  (take  = 15°); secondly, any line composed by two arbitrarily  The manner of walking along wall is terminated if the following two conditions are satisfied: adjacent points taken from array D, which doesn’t intersect line PG , where, P(x, y) is the current firstly, ADT is smaller than J (take J = 15 ); secondly, any line composed by two arbitrarily adjacent position of AUV and G(xt, yt) is the position of target. Whenever the first condition is satisfied, an points obtaken stacle p froom int w array ill beD, recwhich orded b doesn’t y the afo intersect remention line ed w PG ay,s wher at cur e, reP n(x, t po y) sitis ion the P(x, curr y) ri en ght t position now, of and the second condition will be verified. Let K1 and K2 be arbitrarily two adjacent points taken from AUV and G(x , y ) is the position of target. Whenever the first condition is satisfied, an obstacle point t t D, the second condition can be described by the following formulas: will be recorded by the aforementioned ways at current position P(x, y) right now, and the second   dPk PG    dPk PG  (26) dd 0   dkP kk 31 1 2   dkG kk  (27) 41 1 2 dd 0 where: × denotes a cross multiplication, and  denotes a scalar multiplication.  If the above Equations (26) and (27) are satisfied, which means segment doesn’t intersect kk  with segment PG , the second condition is satisfied, too. Then “wall-form walk” manner is terminated, and the AUV drives straight to the target. 3.5. Clutter Obstacles Environment When any obstacle enters into the 80 m detection range of the AUV, and the detected obstacles are more than two, the obstacles environment is considered a cluttered obstacle environment. In this Sensors 2018, 18, 438 13 of 19 condition will be verified. Let K and K be arbitrarily two adjacent points taken from D, the second 1 2 condition can be described by the following formulas: ! ! d = Pk  PG < 1 1 ! ! (26) d = Pk  PG 2 2 d  d > 0 1 2 ! ! d = k P k k < 3 1 1 2 ! ! (27) d = k G k k 4 1 1 2 d  d > 0 3 4 where:  denotes a cross multiplication, and  denotes a scalar multiplication. If the above Equations (26) and (27) are satisfied, which means segment k k doesn’t intersect 1 2 with segment PG, the second condition is satisfied, too. Then “wall-form walk” manner is terminated, and the AUV drives straight to the target. 3.5. Clutter Obstacles Environment When any obstacle enters into the 80 m detection range of the AUV, and the detected obstacles are more than two, the obstacles environment is considered a cluttered obstacle environment. In this environment, the speed is slowed down in narrow channels if necessary, and it is unnecessary to limit the steering times in this obstacles scenario for three reasons, firstly, the effect of steering is decreased steeply with the speed decline; and there is not enough space for the AUV turn round freely either; finally, sometimes obstacle avoidance can’t be accomplished without the assistance of auxiliary thrusters. There are several candidate detouring paths in dense obstacles environments, so a weight coefficient l is designed to evaluate these obstacle avoidance paths, and the path whose weight coefficient l is the largest is chosen as the optimal path, and l denoted as: c c l l L o e l = (28) r sin j d d r , first type gap r = (29) 0.5(r + r ), second type gap i i+1 0, D  4L o o 1/2 D 4L o o l = (30) ( ) , 4L < D < 8L o o o 8L 1, D  8L o o where, D is the gap width, j is ADT, r , r are the distances from FLV to the right or left border points o i j of the gap respectively, l , l are the width coefficient of the gap, and type coefficient of the gap. o b Considering the detouring path forming a gap to the target may be obstructed, which can’t be identified directly from current visual angle of FLS, so the type coefficient is introduced as the reliability factor. If there is a detection point S between a gap that satisfies the condition S = 0, i i which means the gap is unobstructed from the current visual of angle FLV, take l = 1; otherwise take l = 0.5. The weight function l is considered a comprehensive factor that includes safety, reliability and efficiency. After the detour direction is decided, a temporary waypoint needs to be designed, which is solved by the following rules: (a) If the width of gap is smaller than 8 L , choosing the midpoint of the gap as waypoint; (b) If the width of gap is larger than 8 L , choosing the point as waypoint where the ADT is the least and the distance from AUV to two border points of the gap is larger than 4 L . o Sensors 2018, 18, 438 14 of 19 When the width of path less than 6 L the AUV should navigate at low speed, In Figure 10, the AUV has five paths, and the weight function and the parameters give the following table: From Table 1, we can get the 4th path is the optimal path and the 2nd path is the worst one. Table 1. The weight factors of path in clutter environment. l D (L ) F ( ) P l l o o o d d b c Path1 1 ¥ 66 38.1 1 4.31 Path2 0.84 9.6 34 47.5 0.5 2.36 Path3 1 14.2 18 51.9 1 9.36 Path4 0.81 9.2 14 46.5 1 10.76 Path5 1 ¥ 43 47.4 1 4.64 Sensors 2018, 18, x FOR PEER REVIEW 15 of 21 Figure 10. Dense obstacles environment voidance. Figure 10. Dense obstacles environment voidance. 4. Simulation Results and Discussion 4. Simulation Results and Discussion Numerical simulations have been performed in three obstacle environments, and three Numerical simulations have been performed in three obstacle environments, and three different different obstacle avoidance algorithms are adapted in order to verify the effectiveness of the obstacle avoidance algorithms are adapted in order to verify the effectiveness of the proposed proposed obstacle-avoidance algorithm in complex environments. and software simulation experiments are carried out in the Matlab 2015 environment. We design three obstacle scenes, and obstacle-avoidance algorithm in complex environments. and software simulation experiments are the obstacles are irregular shape, the time step takes 0.1 s. The other parameters are as follows: carried out in the Matlab 2015 environment. We design three obstacle scenes, and the obstacles are irregular shape, the time step takes 0.1 s. The other parameters are as follows: l = 4 m, U = 2 m/s, Rmax = 30 m, Rmin = 30 m, λ2 = −0.663, λb = 2.0. l = 4 m, U = 2 m/s, R = 30 m, R = 30 m, l = 0.663, l = 2.0. o max min 2 4.1. Simulation 4.1. Simulation Figure 11 depicts a AUV’s obstacle avoidance in a general obstacle environment, initial position (40, 40), heading angle is 0° (north), and initial speed is 2 m/s, the target position is (1200, 1600), Figure 11 depicts a AUV’s obstacle avoidance in a general obstacle environment, initial position initial time t = 0 s. As the obstacles are dispersely distributed, the AUV maintains a speed of 2 m/s in (40, 40), heading angle is 0 (north), and initial speed is 2 m/s, the target position is (1200, 1600), initial the whole process ignoring the steering effect on speed. time t = 0 s. As the obstacles are dispersely distributed, the AUV maintains a speed of 2 m/s in the Figure 11a depicts the avoidance trajectory of APF, the trajectory obviously isn’t good, it has whole process ignoring the steering effect on speed. some defects in the following aspects: the path is not smooth; and the path is too long and the heading is not steady. Figure 11b depicts the obstacle avoidance trajectory of PSO, where the trajectory is smooth, and the path length is shorter than with the first algorithm, however, the heading shows a slight shock when the heading is suddenly altered. Figure 11c depicts the obstacle avoidance trajectory of RRA, where the trajectory is smooth, the heading is steady and the path length is near optimal. Figure 11d shows the heading curves of APF, PSO and RRA, respectively, and their heading stabilities are improved in sequence, and the finishing times are 1315.4 s, 1128.7 s, 1100.1 s, respectively. Therefore, RRA is the best one of the three obstacle avoidance algorithms in every aspect. Sensors 2018, 18, 438 15 of 19 Figure 11a depicts the avoidance trajectory of APF, the trajectory obviously isn’t good, it has some defects in the following aspects: the path is not smooth; and the path is too long and the heading is not steady. Figure 11b depicts the obstacle avoidance trajectory of PSO, where the trajectory is smooth, and the path length is shorter than with the first algorithm, however, the heading shows a slight shock when the heading is suddenly altered. Figure 11c depicts the obstacle avoidance trajectory of RRA, where the trajectory is smooth, the heading is steady and the path length is near optimal. Figure 11d shows the heading curves of APF, PSO and RRA, respectively, and their heading stabilities are improved in sequence, and the finishing times are 1315.4 s, 1128.7 s, 1100.1 s, respectively. Therefore, RRA is the best one of the three obstacle avoidance algorithms in every aspect. Sensors 2018, 18, x FOR PEER REVIEW 16 of 21 (a) (b) (c) (d) Figure 11. General obstacles environment obstacle avoidance algorithms. (a) APF; (b) PSO; (c) RRA; Figure 11. General obstacles environment obstacle avoidance algorithms. (a) APF; (b) PSO; (c) RRA; (d) heading curves. (d) heading curves. Figure 11 depicts the AUV’s obstacle avoidance in a cluttered obstacle environment, where the initial position is (20, 20), heading angle is 0° (north), and initial speed is 2 m/s, the target position is Figure 11 depicts the AUV’s obstacle avoidance in a cluttered obstacle environment, where the (600, 800), initial time t = 0 s. initial position is (20, 20), heading angle is 0 (north), and initial speed is 2 m/s, the target position is Figure 11a depicts obstacle avoidance trajectory of APF. The path is not smooth and it is too (600, 800), initial time t = 0 s. long and the heading is not steady and oscillates when navigating in a narrow passageway. Figure 11b depicts the obstacle avoidance trajectory of PSO, where the trajectory is not smooth like the first scene, and the path length is shorter than the APF, and the heading displays a slight shock when the heading is suddenly altered. Figure 11c depicts the obstacle avoidance trajectory of RRA, where the trajectory is still smooth, the heading is steady and the path length is near optimal. Sensors 2018, 18, 438 16 of 19 Figure 11a depicts obstacle avoidance trajectory of APF. The path is not smooth and it is too long and the heading is not steady and oscillates when navigating in a narrow passageway. Figure 11b depicts the obstacle avoidance trajectory of PSO, where the trajectory is not smooth like the first scene, and the path length is shorter than the APF, and the heading displays a slight shock when the heading is suddenly altered. Figure 11c depicts the obstacle avoidance trajectory of RRA, where the trajectory is still smooth, the heading is steady and the path length is near optimal. Sensors 2018, 18, x FOR PEER REVIEW 17 of 21 Figure 11d shows the heading curves of APF, PSO and RRA, respectively, and their heading stabilities is still improved in sequence, and the finishing times are 642.0 s, 571.1 s, 560.1 s. Therefore, Figure 11d shows the heading curves of APF, PSO and RRA, respectively, and their heading RRA is the best one among the three obstacle avoidance algorithms in every aspect. stabilities is still improved in sequence, and the finishing times are 642.0 s, 571.1 s, 560.1 s. Therefore, RRA is the best one among the three obstacle avoidance algorithms in every aspect. In a cluttered obstacle scene, maintaining the safety distances between the AUV and obstacles In a cluttered obstacle scene, maintaining the safety distances between the AUV and obstacles is is an important factor, Figure 12 displays the smallest distances between the AUV and nine detected an important factor, Figure 12 displays the smallest distances between the AUV and nine detected obstacles by three obstacle avoidance algorithms. The three columns are the smallest distances in APF, obstacles by three obstacle avoidance algorithms. The three columns are the smallest distances in PSO and RRA, respectively. APF, PSO and RRA, respectively. (a) (b) (c) (d) Figure 12. Clutter obstacles environment obstacle avoidance algorithms; (a) APF; (b) PSO; (c) RRA; Figure 12. Clutter obstacles environment obstacle avoidance algorithms; (a) APF; (b) PSO; (c) RRA; (d) heading curves. (d) heading curves. Sensors 2018, 18, 438 17 of 19 The smallest distance in APF is only 6.01 m, produced by an AUV detouring the 3rd obstacle. The smallest distance in PSO is 10.38 m produced in an AUV detouring the 2nd obstacle, and the Sensors 2018, 18, x FOR PEER REVIEW 18 of 21 smallest distance in PSO is 16.50 m produced in an AUV detouring the 7th obstacle. The distance The smallest distance in APF is only 6.01 m, produced by an AUV detouring the 3rd obstacle. between the AUV and obstacles is bigger than twice the total length of the AUV in the obstacle The smallest distance in PSO is 10.38 m produced in an AUV detouring the 2nd obstacle, and the avoidance process which is safe enough, and more than four times the total length is perfect, so RRA is smallest distance in PSO is 16.50 m produced in an AUV detouring the 7th obstacle. The distance between the AUV and obstacles is bigger than twice the total length of the AUV in the obstacle still the best one in keeping a safe distance. avoidance process which is safe enough, and more than four times the total length is perfect, so RRA Figure 13 displays the obstacle avoidance trajectories of the three algorithms in a trap obstacles is still the best one in keeping a safe distance. Figure 13. displays the obstacle avoidance trajectories of the three algorithms in a trap obstacles environment, where the initial position is (700, 1050), the heading angle is 0 (north), the initial speed environment, where the initial position is (700, 1050), the heading angle is 0° (north), the initial speed is 2 m/s, and the target position is (970, 1820), initial time t = 0 s. is 2 m/s, and the target position is (970, 1820), initial time t = 0 s. Figure 13. Smallest distance between AUV and obstacles. Figure 13. Smallest distance between AUV and obstacles. Figure 14a depicts the APF obstacle avoidance trajectory, as the inherent defect of APF, AUV is being caught in the trap and they wander around position 1. Figure 14a depicts the APF obstacle avoidance trajectory, as the inherent defect of APF, AUV is Figure 14b depicts the PSO obstacle avoidance trajectory, where although the AUV arrived at the destination, it made some detours in this method. When the AUV arrived at position 1 it detected being caught in the trap and they wander around position 1. Sensors 2018, 18, x FOR PEER REVIEW 19 of 21 a wall-shape obstacle and start walking along the wall, and when AUV arrived at position 2 the walking along wall was ended and it went to destination directly, however when the AUV arrived at position 3 and detected a wall obstacle again, it restart walking along wall again till arriving at the destination. Figure 14c depicts the PSO obstacle avoidance trajectory, where the path is smooth, and the length is shorter. When the AUV arrived at position 1, the obstacle was identified as a wall-form obstacle, the walking along wall manner was activated and obstacle points were recorded frequently by the aforementioned rules. When the AUV arrived at position 2 and position 3, the heading was pointing to the destination, but the second condition was not satisfied, so it continued maintaining this manner. When the AUV arrived at position 4, the heading was pointing to the destination again, and the second condition was not satisfied, so walking along wall manner was terminated and the AUV drived to the target directly. (a) (b) (c) Figure 14. Trap obstacles environment obstacle avoidance algorithms: (a) APF; (b) PSO; (c) RRA. Figure 14. Trap obstacles environment obstacle avoidance algorithms: (a) APF; (b) PSO; (c) RRA. 4.2. Discussion Simulations have been carried out in three different unknown obstacle scenarios. The obstacle avoidance trajectory by APF is worse, the path is not smooth and heading is not steady, and paths are the longest in all three obstacle scenes using this algorithm, moreover, the AUV can’t get to the destination in the third scenario with this algorithm. The obstacle avoidance trajectory by PSO is good in general, however, if a U-shape obstacle is more complicated, for example, in Figure 14b the left part of wall-form obstacle was as complicated as its right side, AUV could get trapped inside a wall-form obstacle by this algorithm. It is apt to become trapped in complicated U-shape obstacle environments which it cannot solve by itself. The RRA is superior in the following aspects: it can realize the goals in all three different obstacle scenes with the shortest time, respectively, planning smooth trajectories and the shortest voyages, meanwhile, it makes the AUV keep a steady heading, keeping better safe distances away from obstacles. Moreover it solves the defect of being apt to getting into traps in U-shape obstacle environments that affects APF. 5. Conclusions and Future Work In this paper, a LPAA had been proposed to change an obstacle’s irregular visual surface into a convex polygon, and a real-time reaction obstacle avoidance algorithm for AUVs is presented. The algorithm adapts to complex and cluttered unknown environments, and generates a smooth and shorter path, while guaranteeing appropriate safe distances. The simulation experiments illustrate that this algorithm is flexible in cluttered unknown environments, and in particular, it is able to solve the problem of AUVs becoming trapped by U-shape obstacles existing in other obstacle avoidance algorithms. The next stage of this work is to apply this algorithm in pool experiments, and Sensors 2018, 18, 438 18 of 19 Figure 14b depicts the PSO obstacle avoidance trajectory, where although the AUV arrived at the destination, it made some detours in this method. When the AUV arrived at position 1 it detected a wall-shape obstacle and start walking along the wall, and when AUV arrived at position 2 the walking along wall was ended and it went to destination directly, however when the AUV arrived at position 3 and detected a wall obstacle again, it restart walking along wall again till arriving at the destination. Figure 14c depicts the PSO obstacle avoidance trajectory, where the path is smooth, and the length is shorter. When the AUV arrived at position 1, the obstacle was identified as a wall-form obstacle, the walking along wall manner was activated and obstacle points were recorded frequently by the aforementioned rules. When the AUV arrived at position 2 and position 3, the heading was pointing to the destination, but the second condition was not satisfied, so it continued maintaining this manner. When the AUV arrived at position 4, the heading was pointing to the destination again, and the second condition was not satisfied, so walking along wall manner was terminated and the AUV drived to the target directly. 4.2. Discussion Simulations have been carried out in three different unknown obstacle scenarios. The obstacle avoidance trajectory by APF is worse, the path is not smooth and heading is not steady, and paths are the longest in all three obstacle scenes using this algorithm, moreover, the AUV can’t get to the destination in the third scenario with this algorithm. The obstacle avoidance trajectory by PSO is good in general, however, if a U-shape obstacle is more complicated, for example, in Figure 14b the left part of wall-form obstacle was as complicated as its right side, AUV could get trapped inside a wall-form obstacle by this algorithm. It is apt to become trapped in complicated U-shape obstacle environments which it cannot solve by itself. The RRA is superior in the following aspects: it can realize the goals in all three different obstacle scenes with the shortest time, respectively, planning smooth trajectories and the shortest voyages, meanwhile, it makes the AUV keep a steady heading, keeping better safe distances away from obstacles. Moreover it solves the defect of being apt to getting into traps in U-shape obstacle environments that affects APF. 5. Conclusions and Future Work In this paper, a LPAA had been proposed to change an obstacle’s irregular visual surface into a convex polygon, and a real-time reaction obstacle avoidance algorithm for AUVs is presented. The algorithm adapts to complex and cluttered unknown environments, and generates a smooth and shorter path, while guaranteeing appropriate safe distances. The simulation experiments illustrate that this algorithm is flexible in cluttered unknown environments, and in particular, it is able to solve the problem of AUVs becoming trapped by U-shape obstacles existing in other obstacle avoidance algorithms. The next stage of this work is to apply this algorithm in pool experiments, and we will extend this algorithm to more complicated ocean environments, where time variable ocean currents and dynamic objects exist. Acknowledgments: This work was partially funded by the National Nature Science Foundation of China under grant No. 51679057, and partly supported by province science Fund for distinguished Young Scholars No. J2016JQ0052. Author Contributions: Zheping Yan proposed the main idea, Jiyun Li design the algorithm and simulation; Jiyun Li writes the manuscript; Genshi Zhang design the sonar model; Yi Wu write part of the simulation programme; Zheping Yan supervise the research work and refined the manuscript. Conflicts of Interest: The founding sponsors had no role in the design of the study; in the collection, analyses, or interpretation of data; in the writing of the manuscript, and in the decision to publish the results. References 1. Yan, Z.P.; Yu, H.M.; Zhang, W.; Li, B.Y.; Zhou, J.J. Globally Finite-Time Stable Tracking Control of Underactuated UUVs. Ocean Eng. 2015, 107, 132–146. [CrossRef] Sensors 2018, 18, 438 19 of 19 2. Xiang, X.B.; Yu, C.Y.; Niu, Z.M.; Zhang, Q. Subsea Cable Tracking by Autonomous Underwater Vehicle with Magnetic Sensing Guidance. Sensors 2016, 16, 1335. [CrossRef] [PubMed] 3. Zeng, Z.; Lammas, A.; Sammut, K.; He, F.P.; Tang, Y.H. Shell Space Decomposition based Path Planning for AUVs Operating in a Variable Environment. Ocean Eng. 2014, 91, 181–195. [CrossRef] 4. Bi, F.Y.; Wei, Y.J.; Zhang, J.Z.; Cao, W. Position-Tracking Control of Underactuated Autonomous Underwater Vehicles in the Presence of Unknown Ocean Currents. IET Control Theory Appl. 2010, 4, 2369–2380. [CrossRef] 5. Cai, W.; Zhang, M.Y.; Zheng, Y.R. Task Assignment and Path Planning for Multiple Autonomous Underwater Vehicles Using 3D Dubins Curves. Sensors 2017, 17, 1607. [CrossRef] [PubMed] 6. Saravanakumar, S.; Asokan, T. Multipoint Potential Field Method for Path Planning of Autonomous Underwater Vehicles in 3D Space. Intell. Serv. Robot. 2013, 6, 211–224. [CrossRef] 7. Kovács, B.; Szayer, G.; Tajti, F.; Burdelis, M.; Korondi, P. A Novel Potential Field Method for Path Planning of Mobile Robots by Adapting Animal Motion Attributes. Robot. Auton. Syst. 2016, 82, 24–34. [CrossRef] 8. Lee, S.M.; Myun, H. Receding Horizon Particle Swarm Optimisation-Based Formation Control with Collision Avoidance for Non-Holonomic Mobile Robots. IET Control Theory Appl. 2015, 9, 2075–2083. [CrossRef] 9. Dadgar, M.; Jafari, S.; Hamzeh, A. A PSO-Based Multi-Robot Cooperation Method for Target Searching in Unknown Environments. Neurocomputing 2016, 177, 62–74. [CrossRef] 10. Bui, L.D.; Kim, Y.G. An Obstacle-Avoidance Technique for Autonomous Underwater Vehicles Based on BK-Products of Fuzzy relation. Fuzzy Sets Syst. 2006, 157, 560–577. [CrossRef] 11. Nakhaeinia, D.; Karasfi, B. A Behavior-Based Approach for Collision Avoidance of Mobile Robots in Unknown and Dynamic Environments. Intell. Fuzzy Syst. 2013, 24, 299–311. 12. Lee, Y.I.; Kim, Y.G.; Kohout, L.J. An Intelligent Collision Avoidance System for AUVs Using Fuzzy Relational Products. Inf. Sci. 2004, 158, 209–232. [CrossRef] 13. Liu, Y.C.; Liu, S.Y.; Wang, N. Fully-Tuned Fuzzy Neural Network based Robust Adaptive Tracking Control of Unmanned Underwater Vehicle with thruster Dynamics. Neurocomputering 2016, 196, 1–13. [CrossRef] 14. Pothal, J.K.; Parhi, D.R. Navigation of Multiple Mobile Robots in a Highly Clutter Terrains Using Adaptive Neuro-Fuzzy Inference System. Robot. Auton. Syst. 2015, 72, 48–58. [CrossRef] 15. Zhu, D.Q.; Sun, B. The Bio-Inspired Model Based Hybrid Sliding-Mode Tracking Control for Unmanned Underwater Vehicles. Eng. Appl. Artif. Intell. 2013, 26, 2260–2269. [CrossRef] 16. Zeng, Z.; Sammutb, K.; Lian, L.; He, F.P.; Lammas, A.; Tang, Y.H. A Comparison of Optimization Techniques for AUV Path Planning in Environments with Ocean Currents. Robot. Auton. Syst. 2016, 82, 61–72. [CrossRef] 17. Conti, R.; Meli, E.; Ridolfi, A.; Allotta, B. An Innovative Decentralized Strategy for I-AUVs Cooperative Manipulation Tasks. Robot. Auton. Syst. 2015, 72, 261–276. [CrossRef] 18. Zeng, Z.; Sammut, K.; Lammas, A.; He, F.B.; Tang, Y.H. Imperialist Competitive Algorithm for AUV Path Planning in a Variable Ocean. Appl. Artif. Intell. 2015, 29, 402–420. [CrossRef] 19. Das, P.K.; Behera, H.S.; Das, S.; Tripathy, H.K.; Panigrahi, B.K.; Pradhan, S.K. A Hybrid Improved PSO-DV Algorithm for Multi-Robot Path Planning in a Clutter Environment. Neurocomputing 2016, 207, 735–753. [CrossRef] 20. Aghababa, M.P. 3D Path Planning for Underwater Vehicles Using Five Evolutionary Optimization Algorithms Avoiding Static and Energetic Obstacles. Appl. Ocean Res. 2012, 38, 48–62. [CrossRef] 21. Fang, M.C.; Wang, S.M.; Wu, M.C.; Lin, Y.H. Applying the Self-Tuning Fuzzy Control with the Image Detection Technique on the Obstacle-Avoidance for Autonomous Underwater Vehicles. Ocean Eng. 2015, 93, 11–24. [CrossRef] 22. Shojaei, K.; Arefi, M.M. On the Neuro-Adaptive Feedback Linearising Control of Underactuated Autonomous Underwater Vehicles in Three-Dimensional Space. IET Control Theory Appl. 2015, 9, 1264–1273. [CrossRef] 23. Fossen, T.I.; Sagatun, S.I. Adaptive Control of Nonlinear Systems: A Case Study of Underwater Robotic Systems. J. Robot. Syst. 1991, 8, 393–412. [CrossRef] 24. Norgren, P.; Skjetne, R. Line-of-Sight iceberg Edge-Following Using an AUV Equipped With Multibeam Sonar. In Proceedings of the 2015 16th IFAC on Technology, Culture and International Stability, Sozopos, Bulgaria, 24–27 September 2015; pp. 81–88. © 2018 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

Journal

Sensors (Basel, Switzerland)Pubmed Central

Published: Feb 2, 2018

There are no references for this article.