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

In this paper, occupancy grids are used for mapping. In occupancy grids map, each cell has two states: occupied and empty, denoted by and . The probabilistic estimates of two states are and , and the states are exclusive and exhaustive: , .

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.

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:

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 and radius . 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 can be estimated piecewise according to the region the cell belongs to. The measure distance error is defined as , and is given by

where , , and are positive constants, which represent the initial probability of empty state, occupied state, and unknown state, respectively. These values can be , , and , and is the maximal operating range of FLS.

Figure 4 shows a simulation for in the covering area of single sonar beam. The parameters in (3) choose , , , and . 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.

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 . Given new sonar data and according to Bayes theory, the grid state can be updated as [4] where is the prior probability. Current probability estimation is extracted from the grids map and updated with ; then new 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 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 where is the center of ; ,  , 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 , if the travelling distance is bigger than , ROGM will move. It can be described as . Here is defined as . When moving, will move to . will be given by the position of closest grid to ; thus . Here, .

After moving, The ROGM is changed to . Some new grids will be expanded into , with the state . If the distance from one grid in to 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 , minimum linear velocity , maximal yaw angular velocity , minimum turning radius , and minimum safety distance .

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 Here, is the direction of obstacle vector; is the magnitude of the obstacle vector; , are positive constants and ; is the distance between active cell and ; is the occupied probability. As shown in (7), is in inverse proportion to .

The polar histogram has an angular resolution such that   (). Sector corresponds to a discrete angle ,  , and sector    is established through For each sector , the POD is calculated as

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 Here, .

4.2. Candidate Heading Set

The safety sector is obtained by the and a certain threshold . The sector with POD above the threshold () is treated as unsafe sector, and the sector with POD below the threshold () is called safe sector. The consecutive safe sectors are combined as a valley. The valley combined with more than   () 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 . 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, . 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: and .

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. , , which represents the closest obstacle to UUV. The average POD of all sectors is , which represents obstacles contribution. To some extent, represents the type of obstacles. is large in the narrow passage, and is low in the environment with sparse obstacles. If UUV is requested to pass through a narrow passage, the should be temporarily raised [4]. To obtain potential candidate valleys, is raised with the increase of . The and are considered in the threshold selection, which can be defined by , where are weight factors and . Because , we can find . So at any time, the sectors with are treated as unsafe sector, and UUV is aware of the close obstacle.

To guarantee the safety, is chosen in the range , and the formula for threshold is written by

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

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

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 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 where is the steering angle limitation function, which limits the steering angle in

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 . In SG behavior, the desired heading in is given by , and the desired linear velocity in is implemented by . 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

Condition 4. Consider

When Condition 3 is satisfied, AG behavior is triggered. Guidance of goal point means that the candidate heading closest to the target heading in is selected as the desired heading in :

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 is

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 , 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 is selected as the desired heading in : The same with AG, set the desired linear velocity in as (18). To prevent the infinite loop, a watchdog TickCount is used. When TickCount exceeds the preset number of cycles , FW behavior is terminated and TickCount is cleared to zero.

To sum up, the termination condition for FW behavior is defined as The switch strategy of the three behaviors is shown in Figure 5.

4.5. UUV Motion

The first-order motion model of UUV moving in two dimensions is The linear velocity and yaw velocity satisfy the constrains: and . So, the linear velocity and heading in are where is the symbol function and .

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 14. The desired heading is selected in 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 , , , , , , and . The detecting range of FLS is , and zero mean Gaussian noise is added into the sensing distance from FLS. The parameters of map are , , and . According to , the safe threshold interval chooses , , and the coefficients in (10) choose , . 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 . UUV sails from the start point to goal . 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 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 , 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 , and UUV takes AG behavior. As shown in Figures 8(e) and 8(f), UUV takes SG behavior when . In this test, UUV can pass through the dense obstacles successfully and safely.

Case 2. Avoidance test in passage environment.

Curved passage is designed in the map with size of in Case 2. Width of passage is about 200 m, and it is difficult for UUV to traverse. UUV sails from the start point to goal . 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.

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

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 , and UUV sails from start point to goal . 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.

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.


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