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.
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 the th space obstacle model be and the coordinate of the center point of the obstacle model be . The radius and height of the obstacle model are , , and , 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
Let represent the maximum rotation angle of UUV, represent the maximum navigation angle of UUV, and represent the speed of UUV. The minimum turning radius of UUV can be calculated as
2.2. Feedback-Dubins-RRT Path Planning
2.2.1. Spatial Model of a Current Vector and Target Vector
Figure 1 shows the local structure of the current vector model and the target vector model of UUV in a recovery path.
Definitions and , respectively, represent the ellipsoidal regions with the current vector point and the target vector point 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.
Here, represents the set of all space vectors. represents the set of position vectors where UUV is located which inevitably intersect with obstacles. Let .
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. , , and 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 , the geometric positional relationship is shown in Figure 1, and the expressed area is calculated as the following equation. where , , and are the parameters of the ellipsoid, .
represents the boundary of the current vector region model .
and are at the intersection of ellipsoid and .
represents the vector on the boundary , which is also located in . Next, the problem of recovery path planning can be converted to find a collision-free path on , so that the distance between UUV and ellipsoid gradually reduces until UUV can reach the target vector .
Definition 1. If and , then .
Definition 2. If and , then .
Definition 3. Two integers and satisfy is a subset of . The distance of the vector to the target vector is less than the distance from the current point 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 vector to the target model region in , such that the distance between the current vector and the target vector is progressively shortened.
Step 1. Initialize the set of vectors . Let the current vector be the only vector in the tree structure .
Step 2. Progressive growth of random trees:
Step 2.1. Random node vectors are generated in set and set with a fixed probability and , respectively.
Step 2.2. Find the node vector closest to the randomly generated vector in the structure of the existing tree.
Step 2.3. Connect the node vector and the random vector 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 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 in 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 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. Let represent the set of all obstacles. are two arbitrarily adjacent obstacles. The shortest distance from the obstacle to the obstacle is defined as
Here, and represent any point on the boundary of the obstacle and the 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 the obstacle and the obstacle satisfies are the parameters of threatening zones and , respectively. are the radius of the threatening circle of the obstacle and the obstacle, respectively. are the parameters of the obstacle and the obstacle, respectively.
Theorem 5. It is assumed that obstacle-intensive environments are traversable. The current node vector is . The minimum turning radius of UUV is . represents any obstacle model, and its radius and height are , , and , respectively. Ellipsoid is a model expanded according to the obstacle model , and its parameters are , , and and satisfied and . If the current node vector is not included in ellipsoid , then a collision-free path can be found by the Feedback-Dubins-RRT algorithm.
Proof. As shown in Figure 2, the obstacle circle is generated with the radius of and a threatening circle is set to the radius of , where . This radius is the minimum distance between UUV and obstacles when UUV navigates with the maximum rotation angle and maximum navigation angle. is the tangent point between the circle generated with the minimum turning radius and the obstacle circle. By projecting their geometric positions in the - plane, the geometric relationship is expressed as follows: Merge and get the following form: is positive, so there is When ,
Therefore, if the current vector is not contained by the ellipsoid , UUV is navigating with the direction angle or and the collision-free path can be obtained by the planning algorithm.
If the current vector is not included in the ellipsoid , then there is an arbitrary node vector that approaches the boundary of the obstacle with the direction angle or and makes the path of the current node vector to the vector collision-free.
In addition, the obstacle environment is assumed to be traversable, indicating that must be outside the ellipsoid . In other words, the next node vector can also be found to approach the boundary of the obstacle with direction angle or and make to 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 set , where represents the starting vector and represents the target vector.
Step 2. Let the set of node vectors of the recovery path be an empty set. Then, let and add vector to the trimmed set of recovery path vectors.
Step 3. Check whether there is a collision-free Dubins path between the node vector and other node vectors in the order of the foot labels, .
Step 4. When a node vector is found and the Dubins path between it and other node vectors is collision-free, let and add vector 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 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. , where is the maximum value of the length, width, and height of the area.
(a)
(b)
(c)
(d)
(e)
(f)
(g)
(h)
(i)
(j)
(a)
(b)
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 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.
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.
Acknowledgments
This work is partially supported by the Basic Scientific Research Business Cost Scientific Research Project of Heilongjiang Provincial University (135209236 and 135309453) and the Joint Guiding Project of Natural Science Foundation of Heilongjiang Province under Grant LH2019F038.