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

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.


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][9][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.

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 the ith space obstacle model be R obi and the coordinate of the center point of the obstacle model be R obi ðR obin , R obie , R obid Þ. The radius and height of the obstacle model are a obi , b obi , and c obi , respectively. In most cases, the data of the obstacle shape is detected by carrying sensors. However, after longterm 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 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    R z c t ð Þ ð Þ≜ z ∈ Z : , Here, Z represents the set of all space vectors. Z obs represents the set of position vectors where UUV is located which inevitably intersect with obstacles. Let Z free = Z \ Z obs .
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 vector Gðz f ðtÞÞ, the geometric positional relationship is shown in Figure 1, and the expressed area is calculated as the following equation.
where A, B, and C are the parameters of the ellipsoid, max ðA, BÞ = kz c ðtÞ − z f k 2D . ∂Rðz c ðtÞÞ represents the boundary of the current vector region model Rðz c ðtÞÞ.
z i1 z i2 and z i3 z i4 are at the intersection of ellipsoid Gðz f Þ and Rðz c ðtÞÞ.
CðzÞ represents the vector on the boundary ∂Rðz c ðtÞÞ, which is also located in Gðz f Þ. Next, the problem of recovery path planning can be converted to find a collision-free path on ∂Rðz c ðtÞÞ, so that the distance between UUV and ellipsoid Gðz f Þ gradually reduces until UUV can reach the target vector z f .
Definition 3. Two integers ρ r and ρ d satisfy OðzÞ is a subset of CðzÞ.
The distance of the vector to the target vector is less than the distance from the current point z c ðtÞ to the target vector.

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 vector z c ðtÞ to the target model region in OðzÞ, such that the distance between the current vector z c ðtÞ and the target vector z f is progressively shortened.
Step 1. Initialize the set of vectors fzg. Let the current vector z c ðtÞ be the only vector in the tree structure fzg.
Step 2. Progressive growth of random trees: Step 2.1. Random node vectors z rand are generated in set RðzÞOðzÞ and set OðzÞ with a fixed probability P and 1 − P, respectively.
Step 2.2. Find the node vector z near closest to the randomly generated vector z rand in the structure of the existing tree.
Step 2.3. Connect the node vector z near and the random vector z rand 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 vector z rand 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:

Journal of Sensors
Step 3.1. Check if there is a vector in OðzÞ 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 in OðzÞ becomes the structure of the tree or the preset number of cycles is reached.

Existence Condition of a Collision-Free Path.
For the environment with dense obstacles, it is needed to give the existence conditions of collision paths. Let ðR ob1 , R ob2 ,⋯, R obn Þ, n ∈ N represent the set of all obstacles. R obi , R obj , i ≠ j, i, j ∈ N are two arbitrarily adjacent obstacles. The shortest distance from the ith obstacle to the jth obstacle is defined as Here, p i = ðp in , p ie , p id Þ and p j = ðp jn , p je , p jd Þ 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 collisionfree 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 the ith obstacle and the jth obstacle satisfies    Proof. As shown in Figure 2, the obstacle circle is generated with the radius of D = max ða obi , b obi Þ and a threatening circle is set to the radius of D + ΔD, where D + ΔD = min ða c obi , b c obi Þ. 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: D + r min sin α i ð Þ 2 + ΔD 2 + 2ΔD D + r min sin α i ð Þ + r min cos α i ð Þ 2 = D 2 + r 2 min + 2Dr min : Merge and get the following form: ΔD is positive, so there is When α i = 0, Therefore, if the current vector is not contained by the ellipsoid R c obi , 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 vector z c ðtÞ is not included in the ellipsoid R c obi , then there is an arbitrary node vector Z A that approaches the boundary of the obstacle R obi with the direction angle π/2 or −π/2 and makes the path of the current node vector z c ðtÞ to the vector Z A collision-free.
In addition, the obstacle environment is assumed to be traversable, indicating that Z A must be outside the ellipsoid R c obj . In other words, the next node vector Z B can also be found to approach the boundary of the obstacle R obj with direction angle π/2 or −π/2 and make Z A to Z B a collisionfree path. This process is repeated multiple times until a collision-free path from the current vector to the target vector is obtained.

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 set ðz 1 ,⋯,z N Þ, where z 1 represents the starting vector and z N represents the target vector.
Step 2. Let the set of node vectors of the recovery path be an empty set. Then, let j = N and add vector z j to the trimmed set of recovery path vectors.    Journal of Sensors Step 3. Check whether there is a collision-free Dubins path between the node vector z i 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, let j = i and add vector z i 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.

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 Table 1. 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. r min = 0:05 * dx, where dx is the maximum value of the length, width, and height of the area.
In Figure 3, 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. Figure 4 shows the result of the rerun of the same initial conditions as Figure 3. It can be seen from Figure 4 that the 7 Journal of Sensors 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.

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

Data Availability
The data used to support the findings of this study are available from the corresponding author upon request.

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