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.
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 [
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 [
Local path planning algorithms can be divided into heuristic-based path planning algorithms and trajectory generation algorithms [
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 [
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.
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
Let
Figure
The geometry of relative positions of the local-level frame map centered at the current vector and the target vector.
Definitions
Here,
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.
Construct another spatial ellipsoid based on the current vector and the target vector
If
If
Two integers
The distance of the vector to the target vector is less than the distance from the current point
This section describes the Feedback-Dubins-RRT algorithm which produces a collision-free path from the current vector
Initialize the set of vectors
Progressive growth of random trees:
Step 2.1. Random node vectors
Step 2.2. Find the node vector
Step 2.3. Connect the node vector
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
Iteration of complete growth of random trees:
Step 3.1. Check if there is a vector in
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.
Determine whether the growth of random trees ends: the algorithm ends when the vector in
For the environment with dense obstacles, it is needed to give the existence conditions of collision paths. Let
Here,
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.
A dense-obstacle environment is traversable if and only if the distance between the
It is assumed that obstacle-intensive environments are traversable. The current node vector is
As shown in Figure
Merge and get the following form:
When
The distance between obstacles meets the traversable condition.
Therefore, if the current vector is not contained by the ellipsoid
If the current vector
In addition, the obstacle environment is assumed to be traversable, indicating that
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:
All vectors from the start vector to the target vector generated by the foregoing method are placed into the set
Let the set of node vectors of the recovery path be an empty set. Then, let
Check whether there is a collision-free Dubins path between the node vector
When a node vector is found and the Dubins path between it and other node vectors is collision-free, let
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.
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
Center point coordinates and parameters of obstacles.
Obstacle number | Coordinate (m) | |||
---|---|---|---|---|
1 | (100, 200, 435) | 30 | 40 | 13 |
2 | (440, 300, 425) | 50 | 30 | 15 |
3 | (440, 740, 300) | 40 | 30 | 40 |
4 | (900, 600, 430) | 40 | 30 | 14 |
5 | (550, 550, 325) | 32 | 40 | 25 |
6 | (750, 150, 450) | 20 | 20 | 10 |
7 | (900, 900, 325) | 60 | 50 | 35 |
Distance between different obstacles.
Spacing ( | Obstacle 1 | Obstacle 2 | Obstacle 3 | Obstacle 4 | Obstacle 5 | Obstacle 6 | Obstacle 7 |
---|---|---|---|---|---|---|---|
Obstacle 1 | 308.06 | 559.25 | Not adjacent | 521.21 | Not adjacent | Not adjacent | |
Obstacle 2 | 308.06 | Not adjacent | 450.08 | 241.05 Not passable | 315.49 | Not adjacent | |
Obstacle 3 | 559.25 | Not adjacent | Not adjacent | 171.26 Not passable | Not adjacent | 427.20 | |
Obstacle 4 | Not adjacent | 450.08 | Not adjacent | 314.36 | 441.61 | 264.71 | |
Obstacle 5 | 521.21 | 241.05 Not passable | 171.26 Not passable | 314.36 | 402.52 | 430.63 | |
Obstacle 6 | Not adjacent | 315.49 | Not adjacent | 441.61 | 402.52 | Not adjacent | |
Obstacle 7 | Not adjacent | Not adjacent | 427.20 | 264.71 | 430.63 | Not adjacent |
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.
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.
In Figure
Figure
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.
The data used to support the findings of this study are available from the corresponding author upon request.
The authors declare that there is no conflict of interests regarding the publication of this paper.
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.