Obstacle Avoidance for Unmanned Undersea Vehicle in Unknown Unstructured Environment

Abstract

To avoid obstacle in the unknown environment for unmanned undersea vehicle (UUV), an obstacle avoiding system based on improved vector field histogram (VFH) is designed. Forward looking sonar is used to detect the environment, and the divisional sonar modal is applied to deal with the measure uncertainty. To adapt to the VFH, rolling occupancy grids are used for the map building, and high accuracy details of local environment are obtained. The threshold is adaptively adjusted by the statistic of obstacles to solve the problem that VFH is sensitive to threshold. To improve the environment adaptability, the hybrid-behaviors strategy is proposed, which selects the optimal avoidance command according to the motion status and environment character. The simulation shows that UUV could avoid the obstacles fast and escape from the U shape obstacles.

1. Introduction

UUV plays an increasingly important role in underwater operations. A major challenge in the development of UUV is obstacle avoidance in unknown, complex, and unstructured environment. This demands UUV to have accurate perception of environment and make decision intelligently. Obstacle detection and avoidance strategy are two key technologies in obstacle avoidance [1, 2].

For obstacle detection, forward looking sonar (FLS) is the primary sensor to obtain the underwater information [3]. Since sonar data are incomplete and time delayed, current information measured is not enough for avoidance decision. A map with historical sonar data is essential for avoidance. Compared with geometric map and topology map, occupancy grids map is more convenient to describe the unstructured obstacle and has better capability to handle measure uncertainty [4]. Meanwhile, global map cannot be used in long voyage, because of the storage and processing constrains. To save the map storage space and improve the mapping speed, local map is widely used, which can quickly create map with accurate details.

For avoidance strategy, there are many popular real-time avoidance approaches, such as, artificial potential field (APF) [5], dynamic window approach (DWA) [6], fuzzy systems [7], expert systems [8], and neural network [9]. However, they all have drawbacks. For instance, APF and DWA are unable to grasp the environmental connectivity and are easily trapped into local minima position. Design of fuzzy system is complex, and it is difficult to determine avoidance rules. The vector field histogram (VFH) is developed based on the concept of APF and certainty grid. There is two-stage data reduction process for computing the desired commands. In the first stage the histogram grid is reduced to one-dimensional polar histogram. Each sector in the polar histogram contains a polar obstacle density (POD). In the second stage, the algorithm selects the most suitable steering direction among all polar histogram sectors with a low POD [10]. VFH and many of its variants (VFH*,VFH+) are computationally efficient and important consideration for real-time operation [10, 11]. It has been used widely in many fields and can be used for UUV avoidance. However, VFH has some shortcomings, which must be solved.(1)Threshold is sensitive and threshold selection relies heavily on experience. Some algorithms have been studied for adaptive threshold. An adaptive threshold is adjusted according to the distance from the obstacles in [12], and [13] searches for the best threshold in certain interval. However, optimization algorithms are applied in [12, 13], and the computational cost is large.(2)Adaptability of complex environments: outdoor environment brings up the challenges to the perception and control. Because the calculation of steering direction in the second stage is relatively simple, traditional VFH is unable to adapt to the environment with dense obstacles. Meanwhile, the same as other local planning methods, local minima is the inherent disadvantage of traditional VFH [14]. When sailing near the coast, UUV will be trapped in U shape ports or coasts. Following-wall strategy is widely used to get rid of trap [15, 16]. It is essential to determine when to activate and when to suspend the following-wall, and coordination mechanisms are applied for the integration of following-wall with other avoidance algorithms [16]. In [15], following-wall is combined with virtual force field method and is activated when the vehicle’s heading is more than 90° off-target. In [16], following-wall and move-to-goal are combined into the vector polar histogram method. The state memory and position prediction strategies are applied to get better coordination ability.

In this paper, the obstacle avoidance for UUV in unknown unstructured environment is considered. A novel obstacle avoidance system based on VFH is proposed. FLS is used for obstacle detecting. The measures uncertainty is considered in the sonar modal. Storage and processing constrains are considered in the mapping with long voyage. To overcome the problems of traditional VFH and to make UUV more adaptable and robust, the traditional VFH is improved. Behavior-based mechanism is introduced into avoidance process, which can respond to various environments.

The main novelties of this paper are as follows.(1)To adapt to the VFH, rolling occupancy grids are used for the map building, which can effectively reduce the measure uncertainty and provide high accuracy information for avoidance decision.(2)To solve the problem that VFH is sensitive to threshold, the threshold is adaptively adjusted by the statistic of POD.(3)To improve the environment adaptability and get rid of minima position, hybrid-behaviors, straight-to-goal, avoidance-toward-goal, and following-wall, are introduced into the second stage of VFH directly. According to the motion status and environment character, the optimal avoidance command is generated through the switching strategy. Differently from other approaches, no coordination mechanism is applied.

2. Sonar Detection

FLS is used for obstacles detecting and mapping, which is installed in front of UUV. The maximal operating range is 200 m, number of beam is 60, horizontal beam width is 120°, and vertical beam width is 15°. The obstacles are detected in horizontal plane with simulation (see Figure 1).

(a) Obstacles detecting by FLS

(b) Obstacles information with FLS

Figure 1 

The simulation of sonar detection.

In this paper, occupancy grids are used for mapping. In occupancy grids map, each cell 𝐶 has two states: occupied and empty, denoted by OCC and EMP. The probabilistic estimates of two states are 𝑃[𝑠(𝐶)=OCC] and 𝑃[(𝑠(𝐶)=EMP], and the states are exclusive and exhaustive: OCC=¬EMP, 𝑃[𝑠(𝐶)=OCC]+𝑃[𝑠(𝐶)=EMP]=1.

2.1. Coordinate Transform

There are working space and sensing space in the sailing. As shown in Figure 2, polar coordinate is used in the sensing space, and the polar axis is denoted as 𝐎_𝑠𝐗_𝑠. In the 2D plane, the sensing data of sonar have the distance and direction, denoted by (𝜌_𝑠,𝜃_𝑠). Cartesian coordinate 𝑋𝑂𝑌 is used in the working space.

Figure 2 

The working space and sensing space.

To further analyze the relationship between the two spaces, coordinate transform is solved. The polar coordinates (𝜌_𝑠,𝜃_𝑠) can be converted to the Cartesian coordinate 𝑋𝑂𝑌. First, (𝜌_𝑠,𝜃_𝑠) is converted to (𝑥_𝑠,𝑦_𝑠) through (1). UUV in the working space is (𝑥_𝑟,𝑦_𝑟,𝜃), and the angle between polar axis and 𝑥-axis is 𝛽; then (𝑥_𝑠,𝑦_𝑠) is converted to (𝑥_𝑒,𝑦_𝑒) in 𝑋𝑂𝑌 space through (2). The information in two spaces is unified with the coordinate transform: 𝑥_𝑠𝑦_𝑠=𝜌_𝑠cos𝜃_𝑠sin𝜃_𝑠𝑥_𝑒,𝑦_𝑒=𝛽𝛽𝛽𝛽cos−sin𝑥sincos𝑦^𝑠_𝑠+𝑥_𝑟𝑦_𝑟. (1)(2)

2.2. Measure Uncertainty Handling

Due to the poor directionality of FLS and the error in range measurement [3], it is necessary to handle measure uncertainty in sonar model. The previous ideal model does not adapt to complex underwater environment. So, one order gauss probability distribution is applied for the Dolphin sonar in [4]. Divisional sonar modal is proposed in [17, 18], and the effectiveness of proposed modal has been proved.

Here, the divisional sonar modal is applied to handle the measure uncertainty. Figure 3 shows the model of the single sonar beam. In the sensing space, the obstacle detecting data is 𝑟_𝑡=(𝜌_𝑠,𝜃_𝑠) in time 𝑡, and covering area can be expressed as one cone, which has range 2Δ𝛼 and radius 𝑅_max. The cone is divided into 3 regions: Region I denotes the empty region, where no obstacle exists; Region II denotes the occupancy region, where the obstacles are found; Region III denotes the unknown region, and it cannot be determined whether there is obstacle [17]. The measure uncertainty of any cell 𝐶=(𝜌_𝑖,𝜃_𝑗) in the covering area is associated with the cell’s position. The closer the cell to the axis of beam is, the less noise the raw data has and vice versa. So the occupied probability of cell 𝑃[𝑠(𝐶(𝜌_𝑖,𝜃_𝑗))=OCC∣𝑟_𝑡] can be estimated piecewise according to the region the cell belongs to. The measure distance error is defined as Δ𝜌, and 𝑃[𝑠(𝐶(𝜌_𝑖,𝜃_𝑗))=OCC∣𝑟_𝑡] is given by𝑃𝑠𝐶𝜌_𝑖,𝜃_𝑗=OCC𝑟_𝑡=𝑝_emp,0<𝜌_𝑖≤𝜌_𝑠𝜌−Δ𝜌,1−0.5^𝑖−𝜌_𝑠×𝜃Δ𝜌^𝑗−𝜃_𝑠𝑝Δ𝛼^occ,𝜌_𝑠−Δ𝜌<𝜌_𝑖<𝜌_𝑠𝑝+Δ𝜌_unk,𝜌_𝑠+Δ𝜌≤𝜌_𝑖<𝑅_max,, (3)

Figure 3 

The covering area of single sonar beam.

where 𝑝_emp, 𝑝_occ, and 𝑝_unk are positive constants, which represent the initial probability of empty state, occupied state, and unknown state, respectively. These values can be 𝑝_emp=0.3, 𝑝_occ=0.9, and 𝑝_unk=0.5, and 𝑅_max is the maximal operating range of FLS.

Figure 4 shows a simulation for 𝑃[𝑠(𝐶(𝜌_𝑖,𝜃_𝑗))=OCC] in the covering area of single sonar beam. The parameters in (3) choose 𝜌_𝑠=90m, 𝜃_𝑠=45^∘, Δ𝜌=10m, and 𝑅_max=150m. In Figure 4, the occupied probability is represented with the color. The light blue region is the empty region, the green region is the unknown region, and the red region is the occupied region. The probabilities in different region have significant difference. The states are distinguished easily and are in accordance with the physical character of sonar.

Figure 4 

Simulation for occupied probability in the covering area of single sonar beam.

Always, one grid may be covered by multiple beams and has different states. Information conflicts among multiple beams cause inaccurate map. To obtain high-precision map, grid state needs to be fused and updated. For 𝑠(𝐶_𝑖), historical sonar data are {𝑟}_𝑡={𝑟₁,𝑟₂,…,𝑟_𝑡} and current state is 𝑃[𝑠(𝐶_𝑖)=OCC∣{𝑟}_𝑡]. Given new sonar data 𝑟_𝑡+1 and according to Bayes theory, the grid state can be updated as [4] 𝑃𝑠𝐶_𝑖=OCC∣{𝑟}_𝑡+1=𝑝𝑟_𝑡+1∣𝑠𝐶_𝑖=OCC𝑃𝑠𝐶_𝑖=OCC∣{𝑟}_𝑡∑_{𝑠(𝐶𝑖)}𝑝𝑟_𝑡+1∣𝑠𝐶_𝑖𝑃𝑠𝐶_𝑖∣{𝑟}_𝑡, (4) where 𝑝[𝑟_𝑡+1∣𝑠(𝐶_𝑖)] is the prior probability. Current probability estimation 𝑃[𝑠(𝐶_𝑖)=OCC∣{𝑟}_𝑡] is extracted from the grids map and updated with 𝑟_𝑡+1; then new 𝑃[𝑟_𝑡+1∣𝑠(𝐶_𝑖)=OCC] is stored in the map.

3. Rolling Occupancy Grids Map

Real-time information storage and processing capacity for UUV are limited. In the long voyage, the environment information increases continuously. If there is no limitation on the mapping, the data will overflow storage capacity and processing time will become longer.

Quad tree model is used in the mapping [19], and there is a map optimization strategy in [20] by cutting the low utilization grids. Here, the rolling occupancy grids map (ROGM) is proposed to adapt the VFH. According to the vision features of UUV, the grids far from UUV have low utilization and will not have impact on the decision-making. On the premise that the information in the active window is complete and accurate, the grids with low utilization can be deleted.

Active window is always centered around the UUV position 𝑋_𝑅, and a circular window would be geometrically more appropriate. Active window takes the following form.

Definition 1. Active window in time 𝑡 is 𝐴𝑊(𝑡)=𝑋∣𝑋∈𝑊,𝑑𝑋,𝑋_𝑅(𝑡)<𝑅_𝑤, (5) where 𝑅_𝑤 denotes the radius of 𝐴𝑊(𝑡). The size of ROGM must be bigger than active window. It moves with the movement of UUV. A circular window is also used in ROGM, which has radius 𝑅_𝑒 and is defined as follows.

Definition 2. ROGM in time 𝑡 is 𝐶𝑊(𝑡)=𝑋∣𝑋∈𝑊,𝑑𝑋,𝑋_𝑀(𝑡)<𝑅_𝑒, (6) where 𝑋_𝑀(𝑡) is the center of 𝐶𝑊(𝑡); 𝑅_𝑒=𝜆_𝑤𝑅_𝑤,  (𝜆_𝑤>1), 𝜆_𝑤 is the margin coefficient and guarantees that active window is contained in ROGM, so that 𝐴𝑊(𝑡)⊂𝐶𝑊(𝑡). In the initial time 𝑡₀, the initial ROGM 𝐶𝑊(𝑡₀) has the center 𝑋_𝑀(𝑡₀)=𝑋_𝑅(𝑡₀), where 𝑋_𝑅(𝑡₀) is the initial position of UUV.

Differently from active window,  ROGM does not move with UUV on time and only moves when the triggering condition is satisfied. From 𝑡 to 𝑡+𝑇_tra, if the travelling distance is bigger than 𝐷_cha, ROGM will move. It can be described as Dis(𝑋_𝑅(𝑡),𝑋_𝑅(𝑡+𝑇_tra))>𝐷_cha. Here 𝐷_cha is defined as 𝑅_𝑒−𝑅_𝑤>𝐷_cha. When moving, 𝑋_𝑀(𝑡) will move to 𝑋_𝑀(𝑡+𝑇_tra). 𝑋_𝑀(𝑡+𝑇_tra) will be given by the position of closest grid 𝐶_close(𝑡) to 𝑋_𝑅(𝑡+𝑇_tra); thus 𝑋_𝑀(𝑡+𝑇_tra)=𝑋(𝐶_close(𝑡)). Here, 𝑋(𝐶_close(𝑡))=argmin_{𝐶𝑖(𝑡)}(Dis(𝑋_𝑅(𝑡+𝑇_tra),𝑋(𝐶_𝑖(𝑡)))),𝐶_𝑖(𝑡)∈𝐶𝑊(𝑡).

After moving, The ROGM 𝐶𝑊(𝑡) is changed to 𝐶𝑊(𝑡+𝑇_tra). Some new grids will be expanded into 𝐶𝑊(𝑡+𝑇_tra), with the state 𝑃[𝑠(𝐶)=OCC]=0.5. If the distance from one grid in 𝐶𝑊(𝑡) to 𝑋_𝑅(𝑡+𝑇_tra) is bigger than 𝑅_𝑒, the grid would be deleted from the 𝐶𝑊(𝑡).

In [𝑡,𝑡+𝑇_𝑡𝑟𝑎], the position of 𝐶𝑊(𝑡) is static. All the grids in 𝐶𝑊(𝑡) are updated in every detection cycle. The update principles of grids in 𝐶𝑊(𝑡) are shown as follows.(A)New sonar data are added into grid map. The sonar data are converted to the working space with (1) and (2). By using (3), the occupied probability of grids is obtained.(B)The states of overlapping grids are updated following (4).

Because the size of ROGM is constant, unlimited expansion problem of the information can be solved. The speed of mapping is improved, and requirements for VFH are satisfied.

4. Obstacle Avoidance System Design

Based on ROGM, the obstacle avoidance system is designed based on improved VFH. The UUV motion constrains include maximal linear velocity 𝑢_max, minimum linear velocity 𝑢_min, maximal yaw angular velocity 𝑤_max, minimum turning radius 𝑅_min, and minimum safety distance 𝑑_safe=𝜆_𝑠(𝑅_min+𝐿_𝑟).

VFH establishes polar coordinate system with the center of mobile robot and converts the impact of obstacle to POD histogram. The recommended direction is derived from an analysis of POD histogram.

4.1. Polar Histogram

According to the concept of VFH [10], in the active windowAW, the active cell 𝑐_𝑖𝑗 with the center (𝑥_𝑖,𝑦_𝑗) is now treated as an obstacle vector. The direction and magnitude of the obstacle vector can be expressed as 𝛽_𝑖𝑗𝑦=arctan^𝑗−𝑦^𝑡_𝑟𝑥_𝑖−𝑥^𝑡_𝑟,𝑚_𝑖𝑗=𝑐^∗_𝑖𝑗²𝑎−𝑏𝑑_𝑖𝑗. (7) Here, 𝛽_𝑖𝑗 is the direction of obstacle vector; 𝑚_𝑖𝑗 is the magnitude of the obstacle vector; 𝑎, 𝑏 are positive constants and 𝑎−𝑏𝑑_max=0; 𝑑_𝑖𝑗 is the distance between active cell 𝑐_𝑖𝑗 and UUV; 𝑐^∗_𝑖𝑗 is the occupied probability. As shown in (7), 𝑚_𝑖𝑗 is in inverse proportion to 𝑑_𝑖𝑗.

The polar histogram 𝐇 has an angular resolution 𝛼 such that 𝑛=360^∘/𝛼  (𝛼=10^∘). Sector 𝑘 corresponds to a discrete angle 𝑘𝛼,  𝑘=0,1,…,𝑛−1, and sector  𝑘  is established through 𝛽𝑘=INT^𝑖𝑗𝛼. (8) For each sector 𝑘, the POD ℎ_𝑘 is calculated as ℎ_𝑘=_𝑖,𝑗𝑚_𝑖𝑗. (9)

Because of the discrete nature of the histogram grid, the result of mapping may appear ragged and cause errors in the selection of steering direction. Therefore, a smoothing function is applied to 𝐇, which is defined by ℎ^′_𝑘=ℎ_𝑘−𝑙+ℎ_{𝑘−𝑙+1}⋯+ℎ_𝑘⋯+ℎ_{𝑘+𝑙−1}+ℎ_𝑘+𝑙.2𝑙+1 (10) Here, 𝑙=3.

4.2. Candidate Heading Set

The safety sector is obtained by the 𝐇 and a certain threshold ℎ_safe. The sector with POD above the threshold (ℎ^′_𝑘≥ℎ_safe) is treated as unsafe sector, and the sector with POD below the threshold (ℎ^′_𝑘<ℎ_safe) is called safe sector. The consecutive safe sectors are combined as a valley. The valley combined with more than 𝑠_max  (𝑠_max=8) sectors is considered as wide valley, and other valleys are called narrow valleys [10]. Each valley has two border sectors, left border 𝑘_𝑙 and right border 𝑘_𝑟. If narrow valley has only one sector 𝑘, then 𝑘_𝑙=𝑘_𝑟=𝑘.

The desired heading of the next planning cycle is selected in the candidate valleys. The desired headings generated in each valley compose the candidate heading set Ω_can={𝜃^𝑐_𝑠}. The candidate heading should guarantee the safety and smoothness of the movement, which is generated according to valley type in the following discussions.(1)For narrow valley, candidate heading is selected as the middle direction of the valley, 𝜃^𝑐_𝑠=((𝑘_𝑙+𝑘_𝑟)/2)𝛼. UUV will maintain a course centered between obstacles.(2)For wide valley, if the candidate heading is still the middle direction of the valley, sometimes the candidate heading is far from current heading, and UUV would adjust the heading acutely. The best situation is that UUV travel at a proper distance from the obstacle, so there are two candidate headings within the wide valley: 𝜃^𝑐_𝑠=(𝑘_𝑙+𝑠_max/2)𝛼 and 𝜃^𝑐_𝑠=(𝑘_𝑟−𝑠_max/2)𝛼.

4.3. Adaptive Threshold

To reduce the steering frequency, it is not necessary to avoid forane obstacles; meanwhile, the close obstacles should not be neglected. It is important for UUV to find fine-tuned threshold. With the same polar histogram, the bigger the threshold is, the more safe the sectors are selected and vice versa. If the threshold is much too large, UUV is not aware of obstacle and may approach obstacle closely. If threshold is much too low, some potential candidate valleys will be precluded and UUV will not pass through narrow passage [4].

Here, a novel method to get threshold is proposed. The statistics of POD are used for the threshold selection. ℎ_max=max(ℎ^′_𝑘), 𝑘=0,1,…,𝑛−1, which represents the closest obstacle to UUV. The average POD of all sectors is ℎ_ave=∑^𝑛−1_𝑘=0ℎ^′_𝑘, which represents obstacles contribution. To some extent, ℎ_ave represents the type of obstacles. ℎ_ave is large in the narrow passage, and ℎ_ave is low in the environment with sparse obstacles. If UUV is requested to pass through a narrow passage, the ℎ_safe should be temporarily raised [4]. To obtain potential candidate valleys, ℎ_safe is raised with the increase of ℎ_ave. The ℎ_ave and ℎ_max are considered in the threshold selection, which can be defined by ℎ_safe=𝜔₁ℎ_ave+𝜔₂ℎ_max, where 𝜔₁,𝜔₂∈[0,1] are weight factors and 𝜔₁+𝜔₂=1. Because ℎ_ave≤ℎ_max, we can find ℎ_safe=𝜔₁ℎ_ave+𝜔₂ℎ_max≤(𝜔₁+𝜔₂)ℎ_max≤ℎ_max. So at any time, the sectors with ℎ_max are treated as unsafe sector, and UUV is aware of the close obstacle.

To guarantee the safety, ℎ_safe is chosen in the range [𝐻_min,𝐻_max], and the formula for threshold is written by ℎ_safe=𝐻_minif𝜔₁ℎ_ave+𝜔₂ℎ_max<𝐻_min𝐻_maxelseif𝜔₁ℎ_ave+𝜔₂ℎ_max>𝐻_max𝜔₁ℎ_ave+𝜔₂ℎ_maxelse. (11)

4.4. Hybrid-Behaviors for Avoidance

Single steering strategy of VFH cannot meet the demand for avoidance in complex environment. So hybrid avoidance strategies are applied. The behaviors of UUV are divided into 3 kinds: straight-to-goal (SG), avoidance-toward-goal (AG), following-wall (FW). SG behavior is traveling to the goal directly without avoidance, AG behavior is the avoidance under the guidance of goal, and FW behavior is the avoidance ignoring the goal. Only one behavior is taken at the same time, and behaviors are triggered according to their triggering conditions.

4.4.1. SG Behavior

When there is no obstacle in the active window or the surrounding obstacles do not affect UUV to sail directly to goal point, SG behavior is triggered. The situation that there is no obstacle in the active window is written by 𝑠(𝑋)=EMP,∀𝑋∈𝐴𝑊(𝑡). (12)

However, in the real application, it is difficult to satisfy (12). Always, there are some obstacles in the active window. We can accept that UUV can take SG behavior if all the sectors are safe. This criterion is defined as follows.

Condition 1. Consider ℎ_𝑘<ℎ_safe,∀𝑘∈[0,1,…,𝑛−1]. (13)

There is no doubt that Condition 1 must also satisfy (12). Besides, SG behavior should be triggered, if goal is found in active window and there is no unsafe sector within the range of 90^∘ from the target heading. It implies that obstacles have no impact on the target heading. UUV can take SG behavior. It is written as follows.

Condition 2. Consider ℎ_𝑘<ℎ_safe,∀𝑘∈𝑘∣𝑓_AL𝜃_𝑔−𝑘𝛼<90^∘,𝑑𝑋_𝑅(𝑡),𝑋_𝐺<𝑅_𝑤, (14) where 𝑓_AL(⋅) is the steering angle limitation function, which limits the steering angle in (−180^∘,180^∘]𝑓_AL(𝑥)=𝑥−360^∘,if𝑥>180^∘,𝑥+360^∘,if𝑥≤−180^∘. (15)

To sum up, SG behavior is triggered if any one of Conditions 1 and 2 is satisfied. The UUV state is 𝑋^𝑡_𝑟=(𝑥^𝑡_𝑟,𝑦^𝑡_𝑟,𝜃^𝑡_𝑟), the goal point is 𝑋_𝐺=(𝑥_𝑔,𝑦_𝑔), and target heading in 𝑡 is defined by 𝜃_𝑔=arctan(𝑦_𝑔−𝑦^𝑡_𝑟)/(𝑥_𝑔−𝑥^𝑡_𝑟). In SG behavior, the desired heading in 𝑡+1 is given by 𝜃_𝑟^𝑡+1=𝜃_𝑔, and the desired linear velocity in 𝑡+1 is implemented by 𝑢_𝑟^𝑡+1=𝑢_max. SG behavior can make UUV approach the goal quickly under safe conditions.

4.4.2. AG Behavior

AG behavior is the main behavior and UUV avoids obstacles under the guidance of goal point. If none of Conditions 1 and 2 is satisfied, obstacles may collide with UUV, and UUV should avoid the obstacles. The relationship between current heading and target heading can be divided into 2 conditions.

Condition 3. Consider 𝑓_AL𝜃_𝑔−𝜃^𝑡_𝑟<90^∘. (16)

Condition 4. Consider 𝑓_AL𝜃_𝑔−𝜃^𝑡_𝑟≥90^∘. (17)

When Condition 3 is satisfied, AG behavior is triggered. Guidance of goal point means that the candidate heading closest to the target heading in Ω_can is selected as the desired heading in 𝑡+1: 𝜃_𝑟^𝑡+1=argmin𝑓_AL𝜃_𝑔−𝜃^𝑐_𝑠,𝜃^𝑐_𝑠∈Ω_can. (18)

The magnitude of the linear velocity is affected by POD. Bigger POD means that UUV approaches closer to obstacle, so it requires UUV to turn a big direction and slow down to obtain enough time for avoidance. The magnitude of the linear velocity is inversely proportional to the POD, and the desired linear velocity in 𝑡+1 is 𝑢_𝑟^𝑡+1=𝑢_maxℎ1−^safeℎ_max. (19)

4.4.3. FW Behavior

Usually, there is local minima in the working space filled with U shape obstacles or complex obstacles. Local minima is generated under both influence of goal point and local obstacle. UUV may be trapped in the local minima or makes reciprocating traveling along closed route. Following-wall is an effective mechanism to get rid of local minima. Take U shape obstacle as an example; following-wall will give up the guidance of goal temporarily and follow the edges of the U shape obstacle, until UUV escapes from the trap.

When the difference between current heading and target heading is more than 90^∘, UUV may be trapped in the local minima. It implies that Condition 4 is satisfied; then FW behavior is triggered. In FW behavior, the candidate heading closest to the current heading in Ω_can is selected as the desired heading in 𝑡+1: 𝜃_𝑟^𝑡+1=argmin𝑓_AL𝜃_𝑐−𝜃^𝑐_𝑠,𝜃^𝑐_𝑠∈Ω_can. (20) The same with AG, set the desired linear velocity in 𝑡+1 as (18). To prevent the infinite loop, a watchdog TickCount is used. When TickCount exceeds the preset number of cycles 𝑇_stop, FW behavior is terminated and TickCount is cleared to zero.

To sum up, the termination condition for FW behavior is defined as 𝑓_AL𝜃_𝑔−𝜃^𝑡_𝑟<90^∘or𝑇𝑖𝑐𝑘𝐶𝑜𝑢𝑛𝑡≥𝑇_stop. (21) The switch strategy of the three behaviors is shown in Figure 5.

Figure 5 

The switch strategy of hybrid-behaviors.

4.5. UUV Motion

The first-order motion model of UUV moving in two dimensions is 𝑋_𝑟^𝑡+1=𝑥^𝑡_𝑟+𝑢^𝑡_𝑟Δ𝑇cos𝜃^𝑡_𝑟𝑦^𝑡_𝑟+𝑢^𝑡_𝑟Δ𝑇sin𝜃^𝑡_𝑟𝜃^𝑡_𝑟+𝑤^𝑡_𝑟Δ𝑇. (22) The linear velocity and yaw velocity satisfy the constrains: 𝑢_min≤|𝑢_𝑟^𝑡+1|≤𝑢_max and |𝜃_𝑟^𝑡+1−𝜃^𝑡_𝑟|≤𝑤_max. So, the linear velocity and heading in 𝑡+1 are 𝑢_𝑟^𝑡+1=𝑢_𝑟^𝑡+1,𝑢_min<𝑢_𝑟^𝑡+1<𝑢_max,𝑢_min,𝑢_𝑟^𝑡+1≤𝑢_min,𝑢_max,𝑢_𝑟^𝑡+1≥𝑢_max,𝜃_𝑟^𝑡+1=𝜃_𝑟^𝑡+1,𝜃_𝑟^𝑡+1−𝜃^𝑡_𝑟≤𝑤_max,𝜃^𝑡_𝑟+sgn𝜃_𝑟^𝑡+1−𝜃^𝑡_𝑟𝑤_max,𝜃_𝑟^𝑡+1−𝜃^𝑡_𝑟>𝑤_max, (23) where sgn(⋅) is the symbol function and sgn(𝑥)=^{−1𝑥<00𝑥=0}_1𝑥>0.

4.6. Algorithm Steps

Through the FLS detection and ROGM building, obstacle avoidance algorithm steps are given as follows.

Step 1 (initialization). The initialization includes the motion constrains and the parameters of sonar and map.

Step 2 (sonar detecting). The obstacles are detected by FLS, and the sonar data are converted to the position information in working space by using (1) and (2).

Step 3 (ROGM building). The occupied probability of new data is calculated using (3). The ROGM is updated according to the principle.

Step 4 (𝐇 building). By using ROGM in Step 3, 𝐇 is built according to (4), (7), (8), (9), and (10).

Step 5 (threshold generation). After average POD and maximal POD are generated according to 𝐇, the adaptive threshold is calculated through (11).

Step 6 (generation for candidate heading set). Using the threshold generated in Step 5, the valleys are determined. The candidate heading is calculated according to the valley type.

Step 7 (hybrid-behaviors switch). SG, AG, and FW behaviors are chosen according to Conditions 1–4. The desired heading is selected in Ω_can generated in Step 6.

Step 8 (UUV state update). Under the new command, the speed and position of UUV are updated by (22). If the goal is arrived, the algorithm quits and else returns to Step 8.

5. Simulation

A simulation framework is developed in Matlab to test proposed method. In the simulation, the size and position of the obstacles are completely unknown, and UUV makes decision relying on the obstacle data detected by FLS. The motion constrains include 𝑢_max=3m/s, 𝑢_min=0.3m/s, 𝑤_max=5^∘/s, 𝑅_min=20m, 𝐿_𝑟=10m, 𝜆_𝑠=1.5, and 𝑑_safe=𝜆_𝑠(𝑅_min+𝐿_𝑟)=45m. The detecting range of FLS is 𝑅_max=150m, and zero mean Gaussian noise is added into the sensing distance from FLS. The parameters of map are 𝑅_𝑤=150m, 𝑅_𝑒=180m, and 𝐷_cha=25m. According to 𝑑_safe, the safe threshold interval chooses 𝐻_min=30, 𝐻_max=90, and the coefficients in (10) choose 𝜔₁=0.8, 𝜔₂=0.2. Simulations are developed in the map with dense obstacles, narrow passage, and U shape obstacle, respectively.

Case 1. Avoidance test with dense obstacles.

In Case 1, 6 obstacles with different size are designed in the map with size of 800m×800m. UUV sails from the start point (0,0) to goal (600,600). The total traveling time is 327 s, and Figure 6 shows the map and complete avoidance results for Case 1. Edges of obstacles (dashed line) and the sonar data (blue point) are plotted in the map. The average POD, maximum POD, and adaptive threshold are shown in Figure 7.

Figure 6 

Avoidance result in Case 1.

Figure 7 

The threshold of VFH in Case 1.

Figure 8 shows the avoidance scenes and 𝐇 in 60 s, 200 s, and 300 s. Figure 8(a) shows the avoidance scene in 60 s, and Figure 8(b) shows 𝐇, current heading (black solid line), target heading (green dashed line), and desired heading (magenta solid line) calculated by the proposed algorithm. As shown in Figure 8(a), when 𝑇=60s, UUV takes AG behavior and chooses the closest candidate heading to target heading. As shown in Figures 8(c) and 8(d), there are 3 narrow valleys in 𝐇 when 𝑇=200s, and UUV takes AG behavior. As shown in Figures 8(e) and 8(f), UUV takes SG behavior when 𝑇=300s. In this test, UUV can pass through the dense obstacles successfully and safely.

(a) Avoidance scene in 𝑇=60 s

(b) The 𝐇 in 𝑇=60 s

(c) Avoidance scene in 𝑇=200 s

(d) The 𝐇 in 𝑇=200 s

(e) Avoidance scene in 𝑇=300 s

(f) The 𝐇 in 𝑇=300 s

Figure 8 

Avoidance scenes in different phases and 𝐇 value of Case 1.

Case 2. Avoidance test in passage environment.

Curved passage is designed in the map with size of 2000m×1800m in Case 2. Width of passage is about 200 m, and it is difficult for UUV to traverse. UUV sails from the start point (1650,350) to goal (400,1000). Traveling time is 1086 s, and Figure 9 shows the map and complete avoidance results for Case 2. The average POD, maximum POD and adaptive threshold are shown in Figure 10.

Figure 9 

Avoidance result in Case 2.

Figure 10 

The threshold of VFH in Case 2.

As seen from Figures 9 and 10, from 350 s to 500 s, average POD is low while passing through the wide passage and the threshold is low. However, average POD is large when UUV passes through the narrow passage from 600 s to 750 s, and threshold is raised. The adaptive threshold helps UUV to pass through the narrow passage.

Figure 11 shows the avoidance scene and 𝐇 in 290 s, 600 s, and 950 s. In Figure 11(a), when 𝑇=290s, UUV detects the frontal and right obstacles in the corner of the passage, and there are a wide valley and a narrow valley in 𝐇 (see Figure 11(b)); UUV turns left under the guidance of target heading. In Figure 11(c), when 𝑇=600s, the obstacles on the two sides of the passage cause 2 peaks and 2 narrow valleys in the polar histogram (see Figure 11(d)). So, UUV travels in the center path of the passage. In Figure 11(e), when 𝑇=950s, UUV takes SG behavior. In this test, UUV can pass through the narrow passage and has the ability to choose new heading agilely in the complex passage.

(a) Avoidance scene in 𝑇=290 s

(b) The 𝐇 in 𝑇=290 s

(c) Avoidance scene in 𝑇=600 s

(d) The 𝐇 in 𝑇=600 s

(e) Avoidance scene in 𝑇=950 s

(f) The 𝐇 in 𝑇=950 s

Figure 11 

Avoidance scenes in different phases and 𝐇 value of Case 2.

Case 3. Avoidance test with U shape obstacle.

A comparison of the improved VFH to traditional VFH is given in Case 3. U shape obstacle is designed in the map with size of 1300m×1000m, and UUV sails from start point (800,200) to goal (800,800). Figure 12 shows the map and avoidance results for Case 3. The avoidance result with traditional VFH described in [10] is shown in Figure 12(a), and UUV makes reciprocating traveling along closed route and fails to reach the goal. Figure 12(b) shows the avoidance result by using improved VFH with hybrid-behaviours. The traveling time is 608 s. As seen from Figure 12(b), UUV turns left when the front obstacles are found in 80 s. After a period of sailing, the right obstacles are found, and UUV turns left. UUV takes FW behavior in 125 s and terminates FW behavior in 160 s. Then, UUV takes AG behavior until leaving the obstacles. Compared with Figure 12(a), this test clearly illustrates that UUV chooses right decisions in U shape obstacle and gets rid of the minima trap through following the edges of obstacle.

(a) The avoidance result with traditional VFH

(b) The avoidance result with proposed improved VFH

Figure 12 

Avoidance results in Case 3.

6. Conclusions

To improve the avoidance ability of UUV in the unknown unstructured environment, an obstacle avoidance system based on improved VFH is designed. The FLS is used for obstacle detection, and the divisional sonar modal is applied to handle the measures uncertainty. To adapt to the VFH, rolling occupancy grids are used for the map building, which can effectively reduce the measure uncertainty and provide high accuracy information for avoidance decision. There are two main improvements over traditional VFH. First, the threshold is adaptively adjusted by the statistic of POD, which represents the obstacle type and solves the problem that VFH is sensitive to threshold. In narrow passage, threshold is raised adaptively to make UUV pass through. Second, to improve the environment adaptability and get rid of minima position, hybrid-behaviours are proposed into the second stage of VFH, which are straight-to-goal, avoidance-toward-goal, and following-wall. According to the motion status and environment character, the optimal avoidance command is generated through the switching strategy.

Simulations are developed in three different scenarios. Simulation results illustrate that UUV can avoid the dense obstacles fast, pass through the curved passage, and escape from the U shape obstacles successfully. The proposed approach can help UUV to adapt to various environments. The feasibility of the proposed approach is confirmed. A future extension of this work is a realization of proposed avoidance system in a real UUV.

Conflict of Interests

The authors declare that there is no conflict of interests regarding the publication of this paper.

Acknowledgments

This work is partially supported by the Natural Science Foundation of China (51179038, 51109043), and the Program of New Century Excellent Talents in University (NCET-10-0053).