# Obstacle Avoidance for Unmanned Undersea Vehicle in Unknown Unstructured Environment

Authors: Zheping Yan; Yufei Zhao; Shuping Hou; Honghan Zhang; Yalin Zheng
Journal: Mathematical Problems in Engineering
 (2013)

---

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


---

## Body

## 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 Figure1).The simulation of sonar detection.
(a)
Obstacles detecting by FLS
(b)
Obstacles information with FLSIn this paper, occupancy grids are used for mapping. In occupancy grids map, each cellC has two states: occupied and empty, denoted by OCC and EMP. The probabilistic estimates of two states are P[s(C)=OCC] and P[(s(C)=EMP], and the states are exclusive and exhaustive: OCC=¬EMP, P[s(C)=OCC]+P[s(C)=EMP]=1.


### 2.1. Coordinate Transform

There are working space and sensing space in the sailing. As shown in Figure2, polar coordinate is used in the sensing space, and the polar axis is denoted as OsXs. In the 2D plane, the sensing data of sonar have the distance and direction, denoted by (ρs,θs). Cartesian coordinate XOY 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(ρs,θs) can be converted to the Cartesian coordinate XOY. First, (ρs,θs) is converted to (xs,ys) through (1). UUV in the working space is (xr,yr,θ), and the angle between polar axis and x-axis is β; then (xs,ys) is converted to (xe,ye) in XOY space through (2). The information in two spaces is unified with the coordinate transform:
(1)[xsys]=ρs[cos(θs)sin(θs)],(2)[xeye]=[cos(β)-sin(β)sin(β)cos(β)][xsys]+[xryr].


### 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. Figure3 shows the model of the single sonar beam. In the sensing space, the obstacle detecting data is rt=(ρs,θs) in time t, and covering area can be expressed as one cone, which has range 2Δα and radius Rmax. 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 C=(ρi,θj) 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 P[s(C(ρi,θj))=OCC∣rt] can be estimated piecewise according to the region the cell belongs to. The measure distance error is defined as Δρ, and P[s(C(ρi,θj))=OCC∣rt] is given by(3)P[s(C(ρi,θj))=OCCrt]={pemp,0<ρi≤ρs-Δρ,[1-0.5(|ρi-ρs|Δρ×|θj-θs|Δα)]pocc,ρs-Δρ<ρi<ρs+Δρpunk,ρs+Δρ≤ρi<Rmax,,Figure 3
The covering area of single sonar beam.wherepemp, pocc, and punk are positive constants, which represent the initial probability of empty state, occupied state, and unknown state, respectively. These values can be pemp=0.3, pocc=0.9, and punk=0.5, and Rmax is the maximal operating range of FLS.Figure4 shows a simulation for P[s(C(ρi,θj))=OCC] in the covering area of single sonar beam. The parameters in (3) choose ρs=90m, θs=45°, Δρ=10m, and Rmax=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. Fors(Ci), historical sonar data are {r}t={r1,r2,…,rt} and current state is P[s(Ci)=OCC∣{r}t]. Given new sonar data rt+1 and according to Bayes theory, the grid state can be updated as [4]
(4)P[s(Ci)=OCC∣{r}t+1]=p[rt+1∣s(Ci)=OCC]P[s(Ci)=OCC∣{r}t]∑s(Ci)p[rt+1∣s(Ci)]P[s(Ci)∣{r}t],
where p[rt+1∣s(Ci)] is the prior probability. Current probability estimation P[s(Ci)=OCC∣{r}t] is extracted from the grids map and updated with rt+1; then new P[rt+1∣s(Ci)=OCC] is stored in the map.


## 2.1. Coordinate Transform

There are working space and sensing space in the sailing. As shown in Figure2, polar coordinate is used in the sensing space, and the polar axis is denoted as OsXs. In the 2D plane, the sensing data of sonar have the distance and direction, denoted by (ρs,θs). Cartesian coordinate XOY 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(ρs,θs) can be converted to the Cartesian coordinate XOY. First, (ρs,θs) is converted to (xs,ys) through (1). UUV in the working space is (xr,yr,θ), and the angle between polar axis and x-axis is β; then (xs,ys) is converted to (xe,ye) in XOY space through (2). The information in two spaces is unified with the coordinate transform:
(1)[xsys]=ρs[cos(θs)sin(θs)],(2)[xeye]=[cos(β)-sin(β)sin(β)cos(β)][xsys]+[xryr].


## 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. Figure3 shows the model of the single sonar beam. In the sensing space, the obstacle detecting data is rt=(ρs,θs) in time t, and covering area can be expressed as one cone, which has range 2Δα and radius Rmax. 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 C=(ρi,θj) 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 P[s(C(ρi,θj))=OCC∣rt] can be estimated piecewise according to the region the cell belongs to. The measure distance error is defined as Δρ, and P[s(C(ρi,θj))=OCC∣rt] is given by(3)P[s(C(ρi,θj))=OCCrt]={pemp,0<ρi≤ρs-Δρ,[1-0.5(|ρi-ρs|Δρ×|θj-θs|Δα)]pocc,ρs-Δρ<ρi<ρs+Δρpunk,ρs+Δρ≤ρi<Rmax,,Figure 3
The covering area of single sonar beam.wherepemp, pocc, and punk are positive constants, which represent the initial probability of empty state, occupied state, and unknown state, respectively. These values can be pemp=0.3, pocc=0.9, and punk=0.5, and Rmax is the maximal operating range of FLS.Figure4 shows a simulation for P[s(C(ρi,θj))=OCC] in the covering area of single sonar beam. The parameters in (3) choose ρs=90m, θs=45°, Δρ=10m, and Rmax=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. Fors(Ci), historical sonar data are {r}t={r1,r2,…,rt} and current state is P[s(Ci)=OCC∣{r}t]. Given new sonar data rt+1 and according to Bayes theory, the grid state can be updated as [4]
(4)P[s(Ci)=OCC∣{r}t+1]=p[rt+1∣s(Ci)=OCC]P[s(Ci)=OCC∣{r}t]∑s(Ci)p[rt+1∣s(Ci)]P[s(Ci)∣{r}t],
where p[rt+1∣s(Ci)] is the prior probability. Current probability estimation P[s(Ci)=OCC∣{r}t] is extracted from the grids map and updated with rt+1; then new P[rt+1∣s(Ci)=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 positionXR, and a circular window would be geometrically more appropriate. Active window takes the following form.Definition 1.
Active window in timet is
(5)AW(t)={X∣X∈W,d(X,XR(t))<Rw},
where Rw denotes the radius of AW(t). 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 Re and is defined as follows.Definition 2.
ROGM in timet is
(6)CW(t)={X∣X∈W,d(X,XM(t))<Re},
where XM(t) is the center of CW(t); Re=λwRw,  (λw>1), λw is the margin coefficient and guarantees that active window is contained in ROGM, so that AW(t)⊂CW(t). In the initial time t0, the initial ROGM CW(t0) has the center XM(t0)=XR(t0), where XR(t0) 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. Fromt to t+Ttra, if the travelling distance is bigger than Dcha, ROGM will move. It can be described as Dis(XR(t),XR(t+Ttra))>Dcha. Here Dcha is defined as Re-Rw>Dcha. When moving, XM(t) will move to XM(t+Ttra). XM(t+Ttra) will be given by the position of closest grid Cclose(t) to XR(t+Ttra); thus XM(t+Ttra)=X(Cclose(t)). Here, X(Cclose(t))=argminCi(t)(Dis(XR(t+Ttra),X(Ci(t)))),Ci(t)∈CW(t).After moving, The ROGMCW(t) is changed to CW(t+Ttra). Some new grids will be expanded into CW(t+Ttra), with the state P[s(C)=OCC]=0.5. If the distance from one grid in CW(t) to XR(t+Ttra) is bigger than Re, the grid would be deleted from the CW(t).In[t,t+Ttra], the position of CW(t) is static. All the grids in CW(t) are updated in every detection cycle. The update principles of grids in CW(t) 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 velocityumax, minimum linear velocity umin, maximal yaw angular velocity wmax, minimum turning radius Rmin, and minimum safety distance dsafe=λs(Rmin+Lr).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 cij with the center (xi,yj) is now treated as an obstacle vector. The direction and magnitude of the obstacle vector can be expressed as
(7)βij=arctanyj-yrtxi-xrt,mij=(cij*)2(a-bdij).
Here, βij is the direction of obstacle vector; mij is the magnitude of the obstacle vector; a, b are positive constants and a-bdmax=0; dij is the distance between active cell cij and UUV; cij* is the occupied probability. As shown in (7), mij is in inverse proportion to dij.The polar histogramH has an angular resolution α such that n=360°/α  (α=10°). Sector k corresponds to a discrete angle kα,  k=0,1,…,n-1, and sector  k  is established through
(8)k=INT(βijα).
For each sector k, the POD hk is calculated as
(9)hk=∑i,jmij.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 toH, which is defined by
(10)hk′=hk-l+hk-l+1⋯+hk⋯+hk+l-1+hk+l2l+1.
Here, l=3.


### 4.2. Candidate Heading Set

The safety sector is obtained by theH and a certain threshold hsafe. The sector with POD above the threshold (hk′≥hsafe) is treated as unsafe sector, and the sector with POD below the threshold (hk′<hsafe) is called safe sector. The consecutive safe sectors are combined as a valley. The valley combined with more than smax  (smax=8) sectors is considered as wide valley, and other valleys are called narrow valleys [10]. Each valley has two border sectors, left border kl and right border kr. If narrow valley has only one sector k, then kl=kr=k.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={θsc}. 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,θsc=((kl+kr)/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:θsc=(kl+smax/2)α and θsc=(kr-smax/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.hmax=max(hk′), k=0,1,…,n-1, which represents the closest obstacle to UUV. The average POD of all sectors is have=∑k=0n-1hk′, which represents obstacles contribution. To some extent, have represents the type of obstacles. have is large in the narrow passage, and have is low in the environment with sparse obstacles. If UUV is requested to pass through a narrow passage, the hsafe should be temporarily raised [4]. To obtain potential candidate valleys, hsafe is raised with the increase of have. The have and hmax are considered in the threshold selection, which can be defined by hsafe=ω1have+ω2hmax, where ω1,ω2∈[0,1] are weight factors and ω1+ω2=1. Because have≤hmax, we can find hsafe=ω1have+ω2hmax≤(ω1+ω2)hmax≤hmax. So at any time, the sectors with hmax are treated as unsafe sector, and UUV is aware of the close obstacle.To guarantee the safety,hsafe is chosen in the range [Hmin,Hmax], and the formula for threshold is written by
(11)hsafe={Hminif(ω1have+ω2hmax)<HminHmaxelseif(ω1have+ω2hmax)>Hmaxω1have+ω2hmaxelse.


### 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(12)s(X)=EMP,∀X∈AW(t).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(13)hk<hsafe,∀k∈[0,1,…,n-1].There is no doubt that Condition1 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(14)hk<hsafe,∀k∈{k∣|fAL(θg-kα)|<90∘},d(XR(t),XG)<Rw,
where fAL(·) is the steering angle limitation function, which limits the steering angle in (-180°,180°](15)fAL(x)={x-360∘,ifx>180∘,x+360∘,ifx≤-180∘.To sum up, SG behavior is triggered if any one of Conditions1 and 2 is satisfied. The UUV state is Xrt=(xrt,yrt,θrt), the goal point is XG=(xg,yg), and target heading in t is defined by θg=arctan(yg-yrt)/(xg-xrt). In SG behavior, the desired heading in t+1 is given by θ^rt+1=θg, and the desired linear velocity in t+1 is implemented by u^rt+1=umax. 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 Conditions1 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(16)|fAL(θg-θrt)|<90°.Condition 4.
Consider(17)|fAL(θg-θrt)|≥90°.When Condition3 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 t+1:
(18)θ^rt+1=argmin(|fAL(θg-θsc)|,θsc∈Ωcan).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 int+1 is
(19)u^rt+1=umax(1-hsafehmax).


#### 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 than90°, 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 t+1:
(20)θ^rt+1=argmin(|fAL(θc-θsc)|,θsc∈Ωcan).
The same with AG, set the desired linear velocity in t+1 as (18). To prevent the infinite loop, a watchdog TickCount is used. When TickCount exceeds the preset number of cycles Tstop, FW behavior is terminated and TickCount is cleared to zero.To sum up, the termination condition for FW behavior is defined as(21)|fAL(θg-θrt)|<90°orTickCount≥Tstop.
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(22)Xrt+1=[xrt+urtΔTcos(θrt)yrt+urtΔTsin(θrt)θrt+wrtΔT].
The linear velocity and yaw velocity satisfy the constrains: umin≤|u^rt+1|≤umax and |θ^rt+1-θrt|≤wmax. So, the linear velocity and heading in t+1 are
(23)urt+1={u^rt+1,umin<u^rt+1<umax,umin,u^rt+1≤umin,umax,u^rt+1≥umax,θrt+1={θ^rt+1,|θ^rt+1-θrt|≤wmax,θrt+sgn(θ^rt+1-θrt)wmax,|θ^rt+1-θrt|>wmax,
where sgn(·) is the symbol function and sgn(x)={-1x<00x=01x>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 (H building).
By using ROGM in Step3, H is built according to (4), (7), (8), (9), and (10).Step 5 (threshold generation).
After average POD and maximal POD are generated according toH, the adaptive threshold is calculated through (11).Step 6 (generation for candidate heading set).
Using the threshold generated in Step5, 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 Conditions1–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.


## 4.1. Polar Histogram

According to the concept of VFH [10], in the active windowAW, the active cell cij with the center (xi,yj) is now treated as an obstacle vector. The direction and magnitude of the obstacle vector can be expressed as
(7)βij=arctanyj-yrtxi-xrt,mij=(cij*)2(a-bdij).
Here, βij is the direction of obstacle vector; mij is the magnitude of the obstacle vector; a, b are positive constants and a-bdmax=0; dij is the distance between active cell cij and UUV; cij* is the occupied probability. As shown in (7), mij is in inverse proportion to dij.The polar histogramH has an angular resolution α such that n=360°/α  (α=10°). Sector k corresponds to a discrete angle kα,  k=0,1,…,n-1, and sector  k  is established through
(8)k=INT(βijα).
For each sector k, the POD hk is calculated as
(9)hk=∑i,jmij.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 toH, which is defined by
(10)hk′=hk-l+hk-l+1⋯+hk⋯+hk+l-1+hk+l2l+1.
Here, l=3.


## 4.2. Candidate Heading Set

The safety sector is obtained by theH and a certain threshold hsafe. The sector with POD above the threshold (hk′≥hsafe) is treated as unsafe sector, and the sector with POD below the threshold (hk′<hsafe) is called safe sector. The consecutive safe sectors are combined as a valley. The valley combined with more than smax  (smax=8) sectors is considered as wide valley, and other valleys are called narrow valleys [10]. Each valley has two border sectors, left border kl and right border kr. If narrow valley has only one sector k, then kl=kr=k.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={θsc}. 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,θsc=((kl+kr)/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:θsc=(kl+smax/2)α and θsc=(kr-smax/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.hmax=max(hk′), k=0,1,…,n-1, which represents the closest obstacle to UUV. The average POD of all sectors is have=∑k=0n-1hk′, which represents obstacles contribution. To some extent, have represents the type of obstacles. have is large in the narrow passage, and have is low in the environment with sparse obstacles. If UUV is requested to pass through a narrow passage, the hsafe should be temporarily raised [4]. To obtain potential candidate valleys, hsafe is raised with the increase of have. The have and hmax are considered in the threshold selection, which can be defined by hsafe=ω1have+ω2hmax, where ω1,ω2∈[0,1] are weight factors and ω1+ω2=1. Because have≤hmax, we can find hsafe=ω1have+ω2hmax≤(ω1+ω2)hmax≤hmax. So at any time, the sectors with hmax are treated as unsafe sector, and UUV is aware of the close obstacle.To guarantee the safety,hsafe is chosen in the range [Hmin,Hmax], and the formula for threshold is written by
(11)hsafe={Hminif(ω1have+ω2hmax)<HminHmaxelseif(ω1have+ω2hmax)>Hmaxω1have+ω2hmaxelse.


## 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(12)s(X)=EMP,∀X∈AW(t).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(13)hk<hsafe,∀k∈[0,1,…,n-1].There is no doubt that Condition1 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(14)hk<hsafe,∀k∈{k∣|fAL(θg-kα)|<90∘},d(XR(t),XG)<Rw,
where fAL(·) is the steering angle limitation function, which limits the steering angle in (-180°,180°](15)fAL(x)={x-360∘,ifx>180∘,x+360∘,ifx≤-180∘.To sum up, SG behavior is triggered if any one of Conditions1 and 2 is satisfied. The UUV state is Xrt=(xrt,yrt,θrt), the goal point is XG=(xg,yg), and target heading in t is defined by θg=arctan(yg-yrt)/(xg-xrt). In SG behavior, the desired heading in t+1 is given by θ^rt+1=θg, and the desired linear velocity in t+1 is implemented by u^rt+1=umax. 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 Conditions1 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(16)|fAL(θg-θrt)|<90°.Condition 4.
Consider(17)|fAL(θg-θrt)|≥90°.When Condition3 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 t+1:
(18)θ^rt+1=argmin(|fAL(θg-θsc)|,θsc∈Ωcan).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 int+1 is
(19)u^rt+1=umax(1-hsafehmax).


### 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 than90°, 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 t+1:
(20)θ^rt+1=argmin(|fAL(θc-θsc)|,θsc∈Ωcan).
The same with AG, set the desired linear velocity in t+1 as (18). To prevent the infinite loop, a watchdog TickCount is used. When TickCount exceeds the preset number of cycles Tstop, FW behavior is terminated and TickCount is cleared to zero.To sum up, the termination condition for FW behavior is defined as(21)|fAL(θg-θrt)|<90°orTickCount≥Tstop.
The switch strategy of the three behaviors is shown in Figure 5.Figure 5
The switch strategy of hybrid-behaviors.


## 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(12)s(X)=EMP,∀X∈AW(t).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(13)hk<hsafe,∀k∈[0,1,…,n-1].There is no doubt that Condition1 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(14)hk<hsafe,∀k∈{k∣|fAL(θg-kα)|<90∘},d(XR(t),XG)<Rw,
where fAL(·) is the steering angle limitation function, which limits the steering angle in (-180°,180°](15)fAL(x)={x-360∘,ifx>180∘,x+360∘,ifx≤-180∘.To sum up, SG behavior is triggered if any one of Conditions1 and 2 is satisfied. The UUV state is Xrt=(xrt,yrt,θrt), the goal point is XG=(xg,yg), and target heading in t is defined by θg=arctan(yg-yrt)/(xg-xrt). In SG behavior, the desired heading in t+1 is given by θ^rt+1=θg, and the desired linear velocity in t+1 is implemented by u^rt+1=umax. 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 Conditions1 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(16)|fAL(θg-θrt)|<90°.Condition 4.
Consider(17)|fAL(θg-θrt)|≥90°.When Condition3 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 t+1:
(18)θ^rt+1=argmin(|fAL(θg-θsc)|,θsc∈Ωcan).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 int+1 is
(19)u^rt+1=umax(1-hsafehmax).


## 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 than90°, 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 t+1:
(20)θ^rt+1=argmin(|fAL(θc-θsc)|,θsc∈Ωcan).
The same with AG, set the desired linear velocity in t+1 as (18). To prevent the infinite loop, a watchdog TickCount is used. When TickCount exceeds the preset number of cycles Tstop, FW behavior is terminated and TickCount is cleared to zero.To sum up, the termination condition for FW behavior is defined as(21)|fAL(θg-θrt)|<90°orTickCount≥Tstop.
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(22)Xrt+1=[xrt+urtΔTcos(θrt)yrt+urtΔTsin(θrt)θrt+wrtΔT].
The linear velocity and yaw velocity satisfy the constrains: umin≤|u^rt+1|≤umax and |θ^rt+1-θrt|≤wmax. So, the linear velocity and heading in t+1 are
(23)urt+1={u^rt+1,umin<u^rt+1<umax,umin,u^rt+1≤umin,umax,u^rt+1≥umax,θrt+1={θ^rt+1,|θ^rt+1-θrt|≤wmax,θrt+sgn(θ^rt+1-θrt)wmax,|θ^rt+1-θrt|>wmax,
where sgn(·) is the symbol function and sgn(x)={-1x<00x=01x>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 (H building).
By using ROGM in Step3, H is built according to (4), (7), (8), (9), and (10).Step 5 (threshold generation).
After average POD and maximal POD are generated according toH, the adaptive threshold is calculated through (11).Step 6 (generation for candidate heading set).
Using the threshold generated in Step5, 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 Conditions1–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 includeumax=3m/s, umin=0.3m/s, wmax=5°/s, Rmin=20m, Lr=10m, λs=1.5, and dsafe=λs(Rmin+Lr)=45m. The detecting range of FLS is Rmax=150m, and zero mean Gaussian noise is added into the sensing distance from FLS. The parameters of map are Rw=150m, Re=180m, and Dcha=25m. According to dsafe, the safe threshold interval chooses Hmin=30, Hmax=90, and the coefficients in (10) choose ω1=0.8, ω2=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 Case1, 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 Case1.Figure 7
The threshold of VFH in Case1.Figure8 shows the avoidance scenes and H in 60 s, 200 s, and 300 s. Figure 8(a) shows the avoidance scene in 60 s, and Figure 8(b) shows H, 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 T=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 H when T=200s, and UUV takes AG behavior. As shown in Figures 8(e) and 8(f), UUV takes SG behavior when T=300s. In this test, UUV can pass through the dense obstacles successfully and safely.Avoidance scenes in different phases andH value of Case 1.
(a)
Avoidance scene inT=60 s
(b)
TheH in T=60 s
(c)
Avoidance scene inT=200 s
(d)
TheH in T=200 s
(e)
Avoidance scene inT=300 s
(f)
TheH in T=300 sCase 2.
Avoidance test in passage environment.Curved passage is designed in the map with size of2000m×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 Case2.Figure 10
The threshold of VFH in Case2.As seen from Figures9 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.Figure11 shows the avoidance scene and H in 290 s, 600 s, and 950 s. In Figure 11(a), when T=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 H (see Figure 11(b)); UUV turns left under the guidance of target heading. In Figure 11(c), when T=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 T=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.Avoidance scenes in different phases andH value of Case 2.
(a)
Avoidance scene inT=290 s
(b)
TheH in T=290 s
(c)
Avoidance scene inT=600 s
(d)
TheH in T=600 s
(e)
Avoidance scene inT=950 s
(f)
TheH in T=950 sCase 3.
Avoidance test with U shape obstacle.A comparison of the improved VFH to traditional VFH is given in Case3. 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.Avoidance results in Case3.
(a)
The avoidance result with traditional VFH
(b)
The avoidance result with proposed improved VFH


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



---

*Source: 841376-2013-11-24.xml*