# Feedback-Dubins-RRT Recovery Path Planning of UUV in an Underwater Obstacle Environment

Authors: Bing Hao; Zheping Yan; Xuefeng Dai; Qi Yuan
Journal: Journal of Sensors
 (2020)

---

## Abstract

In this paper, a UUV (Unmanned Underwater Vehicle) recovery path planning method of a known starting vector and end vector is studied. The local structure diagram is designed depending on the distance and orientation information about the obstacles. According to the local structure diagram, a Rapidly exploring Random Tree (RRT) method with feedback is used to generate a 3D Dubins path that approaches the target area gradually, and the environmental characteristic of UUV reaching a specific target area is discussed. The simulation results demonstrate that this method can effectively reduce the calculation time and the amount of data storage required for planning. Meanwhile, the smooth spatial path generated can be used further to improve the feasibility of the practical application of UUV.


---

## Body

## 1. Introduction

With the development and advancement of science and technology, more and more underwater special tasks require UUV to complete. After more than ten or twenty hours of work, the recovery of UUV can be carried out on an underwater platform or a surface ship. At this time, both underwater and surface recovery will face the problem of three-dimensional autonomous motion planning for UUV [1–2]. The content of this paper is to assume that UUV and the recovery platform are not in the same plane. UUV can independently plan the path from the initial point to the target point, and it needs to make sure that the attitude angle at the target point is consistent with the recovery platform.If the environment where UUV recovery is carried out is complicated with dense obstacles, many various obstacle avoidance methods can be used. However, most obstacle avoidance methods require local replanning, which will generate several subpaths. Repeated iterations of subpaths will increase the time spent in planning recovery paths, and the movement time of UUV and the data storage capacity will also be increased.In [3], path planning is divided into two types: global path planning and local path planning. A graph search method is one of the global path planning methods. One of the graph search methods is Rapidly exploring Dense Tree (RDT) [4]. By supposing that there is a dense sequence of sample points in the C space, the new configuration points of the shortest distance from the current configuration source point are selected iteratively and connected to form a tree structure. An RDT algorithm is called Rapidly exploring Random Trees (RRT) when using a random method to select new configuration points [5]. Frazzoli et al. proposed an algorithm for constructing a two-dimensional random incremental road map [6]. It is assumed that there is a noncollision guidance loop to guide the vehicle from any state (including configuration point and speed) to any desired configuration point. In [7], Griffiths et al. proposed a RRT algorithm for constructing traversable paths by modeling prior environmental data. Examine the branches of the tree as it grows to ensure that the vehicle’s turning radius and climb rate constraints are met. The previous research results have fully confirmed that the RRT algorithm [8–10] has a very important value both in theory and in application. However, many previous studies cannot find the most solution to this algorithm theory based on the given cost function [11, 12].Local path planning algorithms can be divided into heuristic-based path planning algorithms and trajectory generation algorithms [13, 14]. Trajectory generation technology is considered to be a part of the local path planning process, which mainly realizes the construction of a traversal path between two or more path points. The Dubins curve is a type of trajectory generation technology [15]. In order to move the control object from the initial position to the target position, the optimal plane path is constructed in the form of the Dubins curve, and the heading at the initial position and the target position is fixed. The path is a combination of a curve that satisfies the maximum curvature constraint of the control object and a straight line tangent to it.


## 2. Feedback-Dubins-RRT Recovery Path Planning of UUV

In this paper, the RRT algorithm of global planning and the Dubins curve generation technology of local planning are combined, and the two technologies are extended to a three-dimensional space, taking into account the constraints of UUV, and the autonomous path planning method is given, so as to complete the precise docking of UUV and the recovery platform moves in a three-dimensional environment. The previous path planning method relies on the current sensor data, which can be called the forward path planning method here. The accuracy of forward path planning depends on the accuracy of sensor data and the UUV state model, but these are often difficult to guarantee in long-distance navigation [16].This paper will imitate the idea of a feedback control loop in control theory, plan the path and feedback the current location information, at the same time compensate the feedback location information, and carry out the next planning according to the feedback position information, which can improve the success rate and accuracy of path planning to a certain extent. The total length of the whole Dubins path can be obtained by the planning algorithm. Since the speed of UUV is seen as a constant, the time at which the UUV reaches the original target point can be calculated.


### 2.1. Spatial Obstacle Model

Based on the sensors carried by UUV, the range and shape size of the obstacles can be recognized. In this paper, the obstacle that may occur in a recovery environment is described as a spatial ellipsoid model. Let theith space obstacle model be Robi and the coordinate of the center point of the obstacle model be RobiRobin,Robie,Robid. The radius and height of the obstacle model are aobi, bobi, and cobi, respectively. In most cases, the data of the obstacle shape is detected by carrying sensors. However, after long-term underwater navigation, its navigation error has accumulated to a value that cannot be ignored. In order to avoid obstacles accurately, the obstacle model size is expanded according to navigation errors. B By assuming that the error coefficient of navigation is ξ, the obstacle ellipsoid model is defined as
(1)Robiz≜z∈Z:zn−zobin2ξaobi2+ze−zobie2ξbobi2≤1,zd−zobid≤1ξcobizobin2ξaobi2+zobie2ξbobi2.Letφmax represent the maximum rotation angle of UUV, γmax represent the maximum navigation angle of UUV, and V represent the speed of UUV. The minimum turning radius of UUV can be calculated as
(2)rmin=V2cosγmaxgtanφmax.


### 2.2. Feedback-Dubins-RRT Path Planning

#### 2.2.1. Spatial Model of a Current Vector and Target Vector

Figure1 shows the local structure of the current vector model and the target vector model of UUV in a recovery path.Figure 1
The geometry of relative positions of the local-level frame map centered at the current vector and the target vector.DefinitionsRzct and Rzf, respectively, represent the ellipsoidal regions with the current vector point zct=zcn,zce,zcd,zcϕ and the target vector point zf=zfn,zfe,zfd,zfφ as the center point of the model. The four elements of the vector represent the north-east earth coordinate and the heading angle of UUV at this point, respectively.
(3)Rzct≜z∈Z:zn−zcn2a2+ze−zce2b2≤1,zd−zcd≤1czcn2a2+zce2b2,Rzf≜z∈Zfree:zn−zfn2a2+ze−zfe2b2≤1,zd−zfd≤1czfn2a2+zfe2b2.Here,Z represents the set of all space vectors. Zobs represents the set of position vectors where UUV is located which inevitably intersect with obstacles. Let Zfree=Z\Zobs.Then, the goal of the planning is tried to find a path so that all vectors on the path are in the set and satisfy the kinematic constraints of UUV.a, b, and c are the parameters of the ellipsoid model, and its value can be designed according to the navigation environment.Construct another spatial ellipsoid based on the current vector and the target vectorGzft, the geometric positional relationship is shown in Figure 1, and the expressed area is calculated as the following equation.
(4)Gzft≜z∈Z:zn−zfn2A2+ze−zfe2B2=1,zd−zfd≤1Czfn2A2+zfe2B2,where A, B, and C are the parameters of the ellipsoid, maxA,B=zct−zf2D.∂Rzct represents the boundary of the current vector region model Rzct.
(5)∂Rzct≜z∈Z:zn−zcn2a2+ze−zce2b2=1,zd−zcd≤1czcn2a2+zce2b2.zi1zi2 and zi3zi4 are at the intersection of ellipsoid Gzf and Rzct.Cz represents the vector on the boundary ∂Rzct, which is also located in Gzf. Next, the problem of recovery path planning can be converted to find a collision-free path on ∂Rzct, so that the distance between UUV and ellipsoid Gzf gradually reduces until UUV can reach the target vector zf.Definition 1.
Ifzcn−zfn2/a2+zce−zfe2/b2>1 and zcd−zfd>1/czfn2/a2+zfe2/b2, then z∉Rzf.Definition 2.
Ifzn′−zfn2/a2+ze′−zfe2/b2≤zcn−zfn2/a2+zce−zfe2/b2 and zd′−zfd≤zd−zfd, then z′∈Cz.Definition 3.
Two integersρr and ρd satisfy
(6)ρr<mina,b,ρd<1czfn2a2+zfe2b2.
Oz is a subset of Cz.
(7)Oz≜zcn−zfn2+zce−zfe2−zn′−zfn2+ze′−zfe2≥ρr,z′∈Cz:zcd−zfd−zd′−zfd≥ρd.
The distance of the vector to the target vector is less than the distance from the current pointzct to the target vector.


#### 2.2.2. The Basic Idea to Generate a Recovery Path

This section describes the Feedback-Dubins-RRT algorithm which produces a collision-free path from the current vectorzct to the target model region in Oz, such that the distance between the current vector zct and the target vector zf is progressively shortened.Step 1.
Initialize the set of vectorsz. Let the current vector zct be the only vector in the tree structure z.Step 2.
Progressive growth of random trees:
Step 2.1. Random node vectorszrand are generated in set RzOz and set Oz with a fixed probability P and 1−P, respectively.
Step 2.2. Find the node vectorznear closest to the randomly generated vector zrand in the structure of the existing tree.
Step 2.3. Connect the node vectorznear and the random vector zrand with the Dubins curve.
Step 2.4. Check whether the generated Dubins curve is collision-free and meets the kinematic constraints of UUV.
Step 2.5. If the result of Step 2.4 is collision-free, the random vectorzrand is added to the tree structure to become the node vector of the tree and the Dubins curve path is used as the branch of the tree.Step 3.
Iteration of complete growth of random trees:
Step 3.1. Check if there is a vector inOz that has been connected to the structure of the tree.
Step 3.2. If the condition in Step 3.1 is met, the path from the current node vector to this vector is extracted. If the condition in Step 3.1 is not satisfied, a node vector is randomly generated and connected to the current node vector and the Dubins path between them is extracted.Step 4.
Determine whether the growth of random trees ends: the algorithm ends when the vector inOz becomes the structure of the tree or the preset number of cycles is reached.


#### 2.2.3. Existence Condition of a Collision-Free Path

For the environment with dense obstacles, it is needed to give the existence conditions of collision paths. LetRob1,Rob2,⋯,Robn,n∈N represent the set of all obstacles. Robi,Robj,i≠j,i,j∈N are two arbitrarily adjacent obstacles. The shortest distance from the ith obstacle to the jth obstacle is defined as
(8)dij=minpi∈∂Robi,pj∈∂Robjpin−pjn2+pie−pje2.Here,pi=pin,pie,pid and pj=pjn,pje,pjd represent any point on the boundary of the ith obstacle and the jth obstacle.According to the aforementioned method, a threatening circle is set on the outside of each obstacle. If there is a collision-free path through a dense obstacle, the distance between two obstacles should be greater than the difference between the radius of the threatening circle and the minimum turning radius. In other words, if there is a collision-free path through obstacles in a dense-obstacle environment, no point on the threaten circle can be covered by other obstacle areas.Definition 4.
A dense-obstacle environment is traversable if and only if the distance between theith obstacle and the jth obstacle satisfies
(9)dij>maxminaobic,bobic−maxaobi,bobi,minaobjc,bobjc−maxaobj,bobj.
aobic,bobic,aobjc,bobjc are the parameters of threatening zones Robic and Robjc, respectively. minaobic,bobic,minaobjc,bobjc are the radius of the threatening circle of the ith obstacle and the jth obstacle, respectively. aobi,bobi,aobj,bobj are the parameters of the ith obstacle and the jth obstacle, respectively.Theorem 5.
It is assumed that obstacle-intensive environments are traversable. The current node vector iszct=zcnt,zcet,zcdt,zcϕt. The minimum turning radius of UUV is rmin. Robi∈Rob1,Rob2,⋯,Robn represents any obstacle model, and its radius and height are aobi, bobi, and cobi, respectively. Ellipsoid Robic is a model expanded according to the obstacle model Robi, and its parameters are aobic, bobic, and cobic and satisfied minaobic,bobic=maxaobi,bobi+rmin2−rmin2 and cobic=cobi. If the current node vector zct is not included in ellipsoid Robic, then a collision-free path can be found by the Feedback-Dubins-RRT algorithm.Proof.
As shown in Figure2, the obstacle circle is generated with the radius of D=maxaobi,bobi and a threatening circle is set to the radius of D+ΔD, where D+ΔD=minaobic,bobic. This radius is the minimum distance between UUV and obstacles when UUV navigates with the maximum rotation angle and maximum navigation angle. Z′ is the tangent point between the circle generated with the minimum turning radius and the obstacle circle. By projecting their geometric positions in the x-y plane, the geometric relationship is expressed as follows:
(10)D+ΔD+rminsinαi2+rmincosαi2=D+rmin2,D+rminsinαi2+ΔD2+2ΔDD+rminsinαi+rmincosαi2=D2+rmin2+2Drmin.
Merge and get the following form:(11)D2+2ΔDD+rminsinαi+2Drminsinαi−2Drmin=0,Δ=4D+rminsinαi2−4×2Drminsinαi−1=4D2+rminsinαi2+2Drminsinαi−2Drminsinαi−1=4D2+rminsinαi2+2Drmin,ΔD=−2D+rminsinαi±4D2+rminsinαi2+2Drmin2=−D+rminsinαi±D2+rminsinαi2+2Drmin.
ΔD is positive, so there is
(12)ΔD=−D+rminsinαi+D2+rminsinαi2+2Drmin.
Whenαi=0,
(13)ΔDmax=−D+D+rmin2−rmin2,D+ΔDmax=D+rmin2−rmin2.Figure 2
The distance between obstacles meets the traversable condition.Therefore, if the current vector is not contained by the ellipsoidRobic, UUV is navigating with the direction angle π/2 or −π/2 and the collision-free path can be obtained by the planning algorithm.If the current vectorzct is not included in the ellipsoid Robic, then there is an arbitrary node vector ZA that approaches the boundary of the obstacle Robi with the direction angle π/2 or −π/2 and makes the path of the current node vector zct to the vector ZA collision-free.In addition, the obstacle environment is assumed to be traversable, indicating thatZA must be outside the ellipsoid Robjc. In other words, the next node vector ZB can also be found to approach the boundary of the obstacle Robj with direction angle π/2 or −π/2 and make ZA to ZB a collision-free path. This process is repeated multiple times until a collision-free path from the current vector to the target vector is obtained.


### 2.3. Feedback-Dubins-RRT Path Pruning

Although it is efficient and feasible to use the proposed algorithm for path planning, the results of the algorithm are still not optimal due to the random growth of spatial trees. In order to reduce unnecessary navigation and rotation of UUV, some unnecessary path node vectors need to be deleted. In this paper, a simple and effective method is used to remove the unnecessary path node vectors so that the recovery path can be quickly generated. The generated Feedback-Dubins-RRT vector set is trimmed as follows:Step 1.
All vectors from the start vector to the target vector generated by the foregoing method are placed into the setz1,⋯,zN, where z1 represents the starting vector and zN represents the target vector.Step 2.
Let the set of node vectors of the recovery path be an empty set. Then, letj=N and add vector zj to the trimmed set of recovery path vectors.Step 3.
Check whether there is a collision-free Dubins path between the node vectorzi and other node vectors in the order of the foot labels, i∈1,⋯,j−1.Step 4.
When a node vector is found and the Dubins path between it and other node vectors is collision-free, letj=i and add vector zi to the set of trimmed path points. Repeat this process until a complete collision-free path is generated from the starting vector to the target vector.By trimming the path node vector generated by the Feedback-Dubins-RRT algorithm, some unnecessary path node vectors are deleted, so that the original path can be optimized.


## 2.1. Spatial Obstacle Model

Based on the sensors carried by UUV, the range and shape size of the obstacles can be recognized. In this paper, the obstacle that may occur in a recovery environment is described as a spatial ellipsoid model. Let theith space obstacle model be Robi and the coordinate of the center point of the obstacle model be RobiRobin,Robie,Robid. The radius and height of the obstacle model are aobi, bobi, and cobi, respectively. In most cases, the data of the obstacle shape is detected by carrying sensors. However, after long-term underwater navigation, its navigation error has accumulated to a value that cannot be ignored. In order to avoid obstacles accurately, the obstacle model size is expanded according to navigation errors. B By assuming that the error coefficient of navigation is ξ, the obstacle ellipsoid model is defined as
(1)Robiz≜z∈Z:zn−zobin2ξaobi2+ze−zobie2ξbobi2≤1,zd−zobid≤1ξcobizobin2ξaobi2+zobie2ξbobi2.Letφmax represent the maximum rotation angle of UUV, γmax represent the maximum navigation angle of UUV, and V represent the speed of UUV. The minimum turning radius of UUV can be calculated as
(2)rmin=V2cosγmaxgtanφmax.


## 2.2. Feedback-Dubins-RRT Path Planning

### 2.2.1. Spatial Model of a Current Vector and Target Vector

Figure1 shows the local structure of the current vector model and the target vector model of UUV in a recovery path.Figure 1
The geometry of relative positions of the local-level frame map centered at the current vector and the target vector.DefinitionsRzct and Rzf, respectively, represent the ellipsoidal regions with the current vector point zct=zcn,zce,zcd,zcϕ and the target vector point zf=zfn,zfe,zfd,zfφ as the center point of the model. The four elements of the vector represent the north-east earth coordinate and the heading angle of UUV at this point, respectively.
(3)Rzct≜z∈Z:zn−zcn2a2+ze−zce2b2≤1,zd−zcd≤1czcn2a2+zce2b2,Rzf≜z∈Zfree:zn−zfn2a2+ze−zfe2b2≤1,zd−zfd≤1czfn2a2+zfe2b2.Here,Z represents the set of all space vectors. Zobs represents the set of position vectors where UUV is located which inevitably intersect with obstacles. Let Zfree=Z\Zobs.Then, the goal of the planning is tried to find a path so that all vectors on the path are in the set and satisfy the kinematic constraints of UUV.a, b, and c are the parameters of the ellipsoid model, and its value can be designed according to the navigation environment.Construct another spatial ellipsoid based on the current vector and the target vectorGzft, the geometric positional relationship is shown in Figure 1, and the expressed area is calculated as the following equation.
(4)Gzft≜z∈Z:zn−zfn2A2+ze−zfe2B2=1,zd−zfd≤1Czfn2A2+zfe2B2,where A, B, and C are the parameters of the ellipsoid, maxA,B=zct−zf2D.∂Rzct represents the boundary of the current vector region model Rzct.
(5)∂Rzct≜z∈Z:zn−zcn2a2+ze−zce2b2=1,zd−zcd≤1czcn2a2+zce2b2.zi1zi2 and zi3zi4 are at the intersection of ellipsoid Gzf and Rzct.Cz represents the vector on the boundary ∂Rzct, which is also located in Gzf. Next, the problem of recovery path planning can be converted to find a collision-free path on ∂Rzct, so that the distance between UUV and ellipsoid Gzf gradually reduces until UUV can reach the target vector zf.Definition 1.
Ifzcn−zfn2/a2+zce−zfe2/b2>1 and zcd−zfd>1/czfn2/a2+zfe2/b2, then z∉Rzf.Definition 2.
Ifzn′−zfn2/a2+ze′−zfe2/b2≤zcn−zfn2/a2+zce−zfe2/b2 and zd′−zfd≤zd−zfd, then z′∈Cz.Definition 3.
Two integersρr and ρd satisfy
(6)ρr<mina,b,ρd<1czfn2a2+zfe2b2.
Oz is a subset of Cz.
(7)Oz≜zcn−zfn2+zce−zfe2−zn′−zfn2+ze′−zfe2≥ρr,z′∈Cz:zcd−zfd−zd′−zfd≥ρd.
The distance of the vector to the target vector is less than the distance from the current pointzct to the target vector.


### 2.2.2. The Basic Idea to Generate a Recovery Path

This section describes the Feedback-Dubins-RRT algorithm which produces a collision-free path from the current vectorzct to the target model region in Oz, such that the distance between the current vector zct and the target vector zf is progressively shortened.Step 1.
Initialize the set of vectorsz. Let the current vector zct be the only vector in the tree structure z.Step 2.
Progressive growth of random trees:
Step 2.1. Random node vectorszrand are generated in set RzOz and set Oz with a fixed probability P and 1−P, respectively.
Step 2.2. Find the node vectorznear closest to the randomly generated vector zrand in the structure of the existing tree.
Step 2.3. Connect the node vectorznear and the random vector zrand with the Dubins curve.
Step 2.4. Check whether the generated Dubins curve is collision-free and meets the kinematic constraints of UUV.
Step 2.5. If the result of Step 2.4 is collision-free, the random vectorzrand is added to the tree structure to become the node vector of the tree and the Dubins curve path is used as the branch of the tree.Step 3.
Iteration of complete growth of random trees:
Step 3.1. Check if there is a vector inOz that has been connected to the structure of the tree.
Step 3.2. If the condition in Step 3.1 is met, the path from the current node vector to this vector is extracted. If the condition in Step 3.1 is not satisfied, a node vector is randomly generated and connected to the current node vector and the Dubins path between them is extracted.Step 4.
Determine whether the growth of random trees ends: the algorithm ends when the vector inOz becomes the structure of the tree or the preset number of cycles is reached.


### 2.2.3. Existence Condition of a Collision-Free Path

For the environment with dense obstacles, it is needed to give the existence conditions of collision paths. LetRob1,Rob2,⋯,Robn,n∈N represent the set of all obstacles. Robi,Robj,i≠j,i,j∈N are two arbitrarily adjacent obstacles. The shortest distance from the ith obstacle to the jth obstacle is defined as
(8)dij=minpi∈∂Robi,pj∈∂Robjpin−pjn2+pie−pje2.Here,pi=pin,pie,pid and pj=pjn,pje,pjd represent any point on the boundary of the ith obstacle and the jth obstacle.According to the aforementioned method, a threatening circle is set on the outside of each obstacle. If there is a collision-free path through a dense obstacle, the distance between two obstacles should be greater than the difference between the radius of the threatening circle and the minimum turning radius. In other words, if there is a collision-free path through obstacles in a dense-obstacle environment, no point on the threaten circle can be covered by other obstacle areas.Definition 4.
A dense-obstacle environment is traversable if and only if the distance between theith obstacle and the jth obstacle satisfies
(9)dij>maxminaobic,bobic−maxaobi,bobi,minaobjc,bobjc−maxaobj,bobj.
aobic,bobic,aobjc,bobjc are the parameters of threatening zones Robic and Robjc, respectively. minaobic,bobic,minaobjc,bobjc are the radius of the threatening circle of the ith obstacle and the jth obstacle, respectively. aobi,bobi,aobj,bobj are the parameters of the ith obstacle and the jth obstacle, respectively.Theorem 5.
It is assumed that obstacle-intensive environments are traversable. The current node vector iszct=zcnt,zcet,zcdt,zcϕt. The minimum turning radius of UUV is rmin. Robi∈Rob1,Rob2,⋯,Robn represents any obstacle model, and its radius and height are aobi, bobi, and cobi, respectively. Ellipsoid Robic is a model expanded according to the obstacle model Robi, and its parameters are aobic, bobic, and cobic and satisfied minaobic,bobic=maxaobi,bobi+rmin2−rmin2 and cobic=cobi. If the current node vector zct is not included in ellipsoid Robic, then a collision-free path can be found by the Feedback-Dubins-RRT algorithm.Proof.
As shown in Figure2, the obstacle circle is generated with the radius of D=maxaobi,bobi and a threatening circle is set to the radius of D+ΔD, where D+ΔD=minaobic,bobic. This radius is the minimum distance between UUV and obstacles when UUV navigates with the maximum rotation angle and maximum navigation angle. Z′ is the tangent point between the circle generated with the minimum turning radius and the obstacle circle. By projecting their geometric positions in the x-y plane, the geometric relationship is expressed as follows:
(10)D+ΔD+rminsinαi2+rmincosαi2=D+rmin2,D+rminsinαi2+ΔD2+2ΔDD+rminsinαi+rmincosαi2=D2+rmin2+2Drmin.
Merge and get the following form:(11)D2+2ΔDD+rminsinαi+2Drminsinαi−2Drmin=0,Δ=4D+rminsinαi2−4×2Drminsinαi−1=4D2+rminsinαi2+2Drminsinαi−2Drminsinαi−1=4D2+rminsinαi2+2Drmin,ΔD=−2D+rminsinαi±4D2+rminsinαi2+2Drmin2=−D+rminsinαi±D2+rminsinαi2+2Drmin.
ΔD is positive, so there is
(12)ΔD=−D+rminsinαi+D2+rminsinαi2+2Drmin.
Whenαi=0,
(13)ΔDmax=−D+D+rmin2−rmin2,D+ΔDmax=D+rmin2−rmin2.Figure 2
The distance between obstacles meets the traversable condition.Therefore, if the current vector is not contained by the ellipsoidRobic, UUV is navigating with the direction angle π/2 or −π/2 and the collision-free path can be obtained by the planning algorithm.If the current vectorzct is not included in the ellipsoid Robic, then there is an arbitrary node vector ZA that approaches the boundary of the obstacle Robi with the direction angle π/2 or −π/2 and makes the path of the current node vector zct to the vector ZA collision-free.In addition, the obstacle environment is assumed to be traversable, indicating thatZA must be outside the ellipsoid Robjc. In other words, the next node vector ZB can also be found to approach the boundary of the obstacle Robj with direction angle π/2 or −π/2 and make ZA to ZB a collision-free path. This process is repeated multiple times until a collision-free path from the current vector to the target vector is obtained.


## 2.2.1. Spatial Model of a Current Vector and Target Vector

Figure1 shows the local structure of the current vector model and the target vector model of UUV in a recovery path.Figure 1
The geometry of relative positions of the local-level frame map centered at the current vector and the target vector.DefinitionsRzct and Rzf, respectively, represent the ellipsoidal regions with the current vector point zct=zcn,zce,zcd,zcϕ and the target vector point zf=zfn,zfe,zfd,zfφ as the center point of the model. The four elements of the vector represent the north-east earth coordinate and the heading angle of UUV at this point, respectively.
(3)Rzct≜z∈Z:zn−zcn2a2+ze−zce2b2≤1,zd−zcd≤1czcn2a2+zce2b2,Rzf≜z∈Zfree:zn−zfn2a2+ze−zfe2b2≤1,zd−zfd≤1czfn2a2+zfe2b2.Here,Z represents the set of all space vectors. Zobs represents the set of position vectors where UUV is located which inevitably intersect with obstacles. Let Zfree=Z\Zobs.Then, the goal of the planning is tried to find a path so that all vectors on the path are in the set and satisfy the kinematic constraints of UUV.a, b, and c are the parameters of the ellipsoid model, and its value can be designed according to the navigation environment.Construct another spatial ellipsoid based on the current vector and the target vectorGzft, the geometric positional relationship is shown in Figure 1, and the expressed area is calculated as the following equation.
(4)Gzft≜z∈Z:zn−zfn2A2+ze−zfe2B2=1,zd−zfd≤1Czfn2A2+zfe2B2,where A, B, and C are the parameters of the ellipsoid, maxA,B=zct−zf2D.∂Rzct represents the boundary of the current vector region model Rzct.
(5)∂Rzct≜z∈Z:zn−zcn2a2+ze−zce2b2=1,zd−zcd≤1czcn2a2+zce2b2.zi1zi2 and zi3zi4 are at the intersection of ellipsoid Gzf and Rzct.Cz represents the vector on the boundary ∂Rzct, which is also located in Gzf. Next, the problem of recovery path planning can be converted to find a collision-free path on ∂Rzct, so that the distance between UUV and ellipsoid Gzf gradually reduces until UUV can reach the target vector zf.Definition 1.
Ifzcn−zfn2/a2+zce−zfe2/b2>1 and zcd−zfd>1/czfn2/a2+zfe2/b2, then z∉Rzf.Definition 2.
Ifzn′−zfn2/a2+ze′−zfe2/b2≤zcn−zfn2/a2+zce−zfe2/b2 and zd′−zfd≤zd−zfd, then z′∈Cz.Definition 3.
Two integersρr and ρd satisfy
(6)ρr<mina,b,ρd<1czfn2a2+zfe2b2.
Oz is a subset of Cz.
(7)Oz≜zcn−zfn2+zce−zfe2−zn′−zfn2+ze′−zfe2≥ρr,z′∈Cz:zcd−zfd−zd′−zfd≥ρd.
The distance of the vector to the target vector is less than the distance from the current pointzct to the target vector.


## 2.2.2. The Basic Idea to Generate a Recovery Path

This section describes the Feedback-Dubins-RRT algorithm which produces a collision-free path from the current vectorzct to the target model region in Oz, such that the distance between the current vector zct and the target vector zf is progressively shortened.Step 1.
Initialize the set of vectorsz. Let the current vector zct be the only vector in the tree structure z.Step 2.
Progressive growth of random trees:
Step 2.1. Random node vectorszrand are generated in set RzOz and set Oz with a fixed probability P and 1−P, respectively.
Step 2.2. Find the node vectorznear closest to the randomly generated vector zrand in the structure of the existing tree.
Step 2.3. Connect the node vectorznear and the random vector zrand with the Dubins curve.
Step 2.4. Check whether the generated Dubins curve is collision-free and meets the kinematic constraints of UUV.
Step 2.5. If the result of Step 2.4 is collision-free, the random vectorzrand is added to the tree structure to become the node vector of the tree and the Dubins curve path is used as the branch of the tree.Step 3.
Iteration of complete growth of random trees:
Step 3.1. Check if there is a vector inOz that has been connected to the structure of the tree.
Step 3.2. If the condition in Step 3.1 is met, the path from the current node vector to this vector is extracted. If the condition in Step 3.1 is not satisfied, a node vector is randomly generated and connected to the current node vector and the Dubins path between them is extracted.Step 4.
Determine whether the growth of random trees ends: the algorithm ends when the vector inOz becomes the structure of the tree or the preset number of cycles is reached.


## 2.2.3. Existence Condition of a Collision-Free Path

For the environment with dense obstacles, it is needed to give the existence conditions of collision paths. LetRob1,Rob2,⋯,Robn,n∈N represent the set of all obstacles. Robi,Robj,i≠j,i,j∈N are two arbitrarily adjacent obstacles. The shortest distance from the ith obstacle to the jth obstacle is defined as
(8)dij=minpi∈∂Robi,pj∈∂Robjpin−pjn2+pie−pje2.Here,pi=pin,pie,pid and pj=pjn,pje,pjd represent any point on the boundary of the ith obstacle and the jth obstacle.According to the aforementioned method, a threatening circle is set on the outside of each obstacle. If there is a collision-free path through a dense obstacle, the distance between two obstacles should be greater than the difference between the radius of the threatening circle and the minimum turning radius. In other words, if there is a collision-free path through obstacles in a dense-obstacle environment, no point on the threaten circle can be covered by other obstacle areas.Definition 4.
A dense-obstacle environment is traversable if and only if the distance between theith obstacle and the jth obstacle satisfies
(9)dij>maxminaobic,bobic−maxaobi,bobi,minaobjc,bobjc−maxaobj,bobj.
aobic,bobic,aobjc,bobjc are the parameters of threatening zones Robic and Robjc, respectively. minaobic,bobic,minaobjc,bobjc are the radius of the threatening circle of the ith obstacle and the jth obstacle, respectively. aobi,bobi,aobj,bobj are the parameters of the ith obstacle and the jth obstacle, respectively.Theorem 5.
It is assumed that obstacle-intensive environments are traversable. The current node vector iszct=zcnt,zcet,zcdt,zcϕt. The minimum turning radius of UUV is rmin. Robi∈Rob1,Rob2,⋯,Robn represents any obstacle model, and its radius and height are aobi, bobi, and cobi, respectively. Ellipsoid Robic is a model expanded according to the obstacle model Robi, and its parameters are aobic, bobic, and cobic and satisfied minaobic,bobic=maxaobi,bobi+rmin2−rmin2 and cobic=cobi. If the current node vector zct is not included in ellipsoid Robic, then a collision-free path can be found by the Feedback-Dubins-RRT algorithm.Proof.
As shown in Figure2, the obstacle circle is generated with the radius of D=maxaobi,bobi and a threatening circle is set to the radius of D+ΔD, where D+ΔD=minaobic,bobic. This radius is the minimum distance between UUV and obstacles when UUV navigates with the maximum rotation angle and maximum navigation angle. Z′ is the tangent point between the circle generated with the minimum turning radius and the obstacle circle. By projecting their geometric positions in the x-y plane, the geometric relationship is expressed as follows:
(10)D+ΔD+rminsinαi2+rmincosαi2=D+rmin2,D+rminsinαi2+ΔD2+2ΔDD+rminsinαi+rmincosαi2=D2+rmin2+2Drmin.
Merge and get the following form:(11)D2+2ΔDD+rminsinαi+2Drminsinαi−2Drmin=0,Δ=4D+rminsinαi2−4×2Drminsinαi−1=4D2+rminsinαi2+2Drminsinαi−2Drminsinαi−1=4D2+rminsinαi2+2Drmin,ΔD=−2D+rminsinαi±4D2+rminsinαi2+2Drmin2=−D+rminsinαi±D2+rminsinαi2+2Drmin.
ΔD is positive, so there is
(12)ΔD=−D+rminsinαi+D2+rminsinαi2+2Drmin.
Whenαi=0,
(13)ΔDmax=−D+D+rmin2−rmin2,D+ΔDmax=D+rmin2−rmin2.Figure 2
The distance between obstacles meets the traversable condition.Therefore, if the current vector is not contained by the ellipsoidRobic, UUV is navigating with the direction angle π/2 or −π/2 and the collision-free path can be obtained by the planning algorithm.If the current vectorzct is not included in the ellipsoid Robic, then there is an arbitrary node vector ZA that approaches the boundary of the obstacle Robi with the direction angle π/2 or −π/2 and makes the path of the current node vector zct to the vector ZA collision-free.In addition, the obstacle environment is assumed to be traversable, indicating thatZA must be outside the ellipsoid Robjc. In other words, the next node vector ZB can also be found to approach the boundary of the obstacle Robj with direction angle π/2 or −π/2 and make ZA to ZB a collision-free path. This process is repeated multiple times until a collision-free path from the current vector to the target vector is obtained.


## 2.3. Feedback-Dubins-RRT Path Pruning

Although it is efficient and feasible to use the proposed algorithm for path planning, the results of the algorithm are still not optimal due to the random growth of spatial trees. In order to reduce unnecessary navigation and rotation of UUV, some unnecessary path node vectors need to be deleted. In this paper, a simple and effective method is used to remove the unnecessary path node vectors so that the recovery path can be quickly generated. The generated Feedback-Dubins-RRT vector set is trimmed as follows:Step 1.
All vectors from the start vector to the target vector generated by the foregoing method are placed into the setz1,⋯,zN, where z1 represents the starting vector and zN represents the target vector.Step 2.
Let the set of node vectors of the recovery path be an empty set. Then, letj=N and add vector zj to the trimmed set of recovery path vectors.Step 3.
Check whether there is a collision-free Dubins path between the node vectorzi and other node vectors in the order of the foot labels, i∈1,⋯,j−1.Step 4.
When a node vector is found and the Dubins path between it and other node vectors is collision-free, letj=i and add vector zi to the set of trimmed path points. Repeat this process until a complete collision-free path is generated from the starting vector to the target vector.By trimming the path node vector generated by the Feedback-Dubins-RRT algorithm, some unnecessary path node vectors are deleted, so that the original path can be optimized.


## 3. Simulation

In this section, a set of cases will be given to verify the effectiveness of the proposed recovery path planning algorithm.In the three-dimensional geodetic coordinate system, the range of length, width, and height of a space is 1000 m, 1000 m, and 100 m. The initial point position is set at (0, 0, 98), and the target point position is set at (1000, 1000, 40). In order to prove the generality of the algorithm, the heading angle and pitch angle of UUV at the initial position and the target position are randomly generated. Suppose that there are seven intensive obstacles in the environment. The coordinates of the center point and its parameters of the obstacles are shown in Table1. The distance between adjacent obstacles is shown in Table 2, in which the second and fifth obstacles, as well as the third and fifth obstacles, do not satisfy the conditions adopted in Theorem 5. The program has been running many times, and the results of the two versions are given in Figures 3 and 4. The radius of the Dubins path depends on the scope of the area. rmin=0.05∗dx, where dx is the maximum value of the length, width, and height of the area.Table 1
Center point coordinates and parameters of obstacles.
Obstacle numberCoordinate (m)aobi (m)bobi (m)cobi (m)1(100, 200, 435)3040132(440, 300, 425)5030153(440, 740, 300)4030404(900, 600, 430)4030145(550, 550, 325)3240256(750, 150, 450)2020107(900, 900, 325)605035Table 2
Distance between different obstacles.
Spacing (m)Obstacle 1Obstacle 2Obstacle 3Obstacle 4Obstacle 5Obstacle 6Obstacle 7Obstacle 1∗308.06559.25Not adjacent521.21Not adjacentNot adjacentObstacle 2308.06∗Not adjacent450.08241.05 Not passable315.49Not adjacentObstacle 3559.25Not adjacent∗Not adjacent171.26 Not passableNot adjacent427.20Obstacle 4Not adjacent450.08Not adjacent∗314.36441.61264.71Obstacle 5521.21241.05 Not passable171.26 Not passable314.36∗402.52430.63Obstacle 6Not adjacent315.49Not adjacent441.61402.52∗Not adjacentObstacle 7Not adjacentNot adjacent427.20264.71430.63Not adjacent∗Figure 3
The recovery path generated by the Feedback-Dubins-RRT algorithm: (a) three-dimensional graph of the second node generated, (b) top view of the second node generated, (c) three-dimensional graph of the fourth node generated, (d) top view of the fourth node generated, (e) three-dimensional graph of the sixth node generated, (f) top view of the sixth node generated, (g) three-dimensional graph of the eighth node generated, (h) top view of the eighth node generated, (i) three-dimensional graph of all the nodes generated, and (j) top view of all the nodes generated.
(a)(b)(c)(d)(e)(f)(g)(h)(i)(j)Figure 4
The second recovery path generated by the Feedback-Dubins-RRT algorithm: (a) three-dimensional graph of all the nodes generated and (b) top view of all the nodes generated.
(a)(b)In Figure3, the growth of the random tree and the process of obtaining the final path can be seen in detail at dynamically intercepting the output of the second, fourth, sixth, and eighth path nodes generated.Figure4 shows the result of the rerun of the same initial conditions as Figure 3. It can be seen from Figure 4 that the path passes through two obstacles ob1 and ob2 which are determined to be inaccessible in Theorem 5, and the path does not intersect the obstacle. The path satisfies the constraints and reaches the target point. The length of the path is 1933 m. Thus, it can be seen that Theorem 5 gives a sufficient condition to ensure traversability rather than a necessary condition.


## 4. Conclusion

In this paper, the planning method of a spatial path is given for the known recovery starting point vector and target point vector in a 3D environment. For the obstacle-intensive environment, a method which constructs a local structure map between the obstacle and the target vector based on the distance and orientation information is proposed. The three-dimensional Dubins planning path is generated by Feedback-Dubins-RRT according to the local structure map, and the obstacle can be avoided infinitely and reach the target area. The simulation results show that the proposed algorithm can solve the problem of path planning and obstacle avoidance in a three-dimensional environment.The Feedback-Dubins-RRT recovery path planning algorithm still needs to be improved in the following two aspects. (1) In a three-dimensional environment, when the environment is unknown or there are dynamic obstacles, the path planning algorithm in this paper needs to be improved to add a dynamic obstacle avoidance strategy to generate a real-time planned path. (2) The path generated by Feedback-Dubins-RRT can only be asymptotically optimal, and the algorithm idea needs to be improved to generate the optimal path under the current initial conditions.



---

*Source: 8824202-2020-08-01.xml*