1. Introduction
With the increasing frequency of human activities, the rapid consumption of global energy, and the harsh deterioration of environment, the development and application of water surface and marine areas is becoming increasingly extensive [
1,
2,
3]. Most water operations and tasks need to be accomplished by ships, due to the special working environment of water. At the same time, unmanned equipment is becoming more and more widely used in our society, with the breakthroughs brought about by multiple theories and the development of related technologies, such as artificial intelligence, bionic intelligence, and control science [
4]. With the drive of engineering application demand in some bodies of water-such as shoals, lakes, and rivers-there has been an increasing development of unmanned surface vehicles (USVs) in recent decades [
5]. The applications of USV include national and civilian uses [
6]. Successful applications can be found in diverse areas, such as the replenishment of underway ships, marine exploration, maritime search and rescue, the fishery industry, coastal patrolling, and hydrologic monitoring [
7].
The path-planning research of USVs not only determines the level of autonomy of the vehicle, but also influences the reliability of a mission and the likelihood of success [
8]. As the algorithm is the most critical part of USV path planning, this must be elucidated [
9]. Optimizing safety, energy consumption, and travelling time are the main aims of a path-planning algorithm.
To address a vehicle’s path planning, many kinds of path-planning algorithms have been proposed by researchers [
10]. Zeng et al. presented an online dynamic path re-planning system for an autonomous underwater vehicle [
11]. Aghababa applied a numerical solution of the nonlinear optimal control problem (NOCP) to determine optimal paths in environments with obstacles [
12]. Warren presented an algorithm using artificial potential fields to aid in path planning [
13]. Petres et al. presented a novel fast marching (FM)-based approach to address path planning [
14]. In the sense of energy saving, the effect of the sea current in path planning was presented and an A* search algorithm with a time-optimal cost was proposed by Garau et al. [
15]. Several computational approaches comprising evolutionary methods have been applied in path planning for marine vehicles. Song et al. proposed an algorithm combining nonholonomic constraints of vehicles with a double extended rapidly-exploring random tree (RRT), which not only improves the efficiency of searching but also guarantees the feasibility of the path at the same time [
16]. Singh et al. proposed a grid-based path-planning approach for USVs considering static and moving obstacles together with sea surface currents [
17]. Du et al. proposed an algorithm combining the environmental constraints and the constraints of intelligent vehicles with RRTs [
18].
The advantage of the RRT algorithm is that it can be used to plan a path in a complex environment without building a spatial modeling [
19,
20]. Meanwhile, the RRT-based path-planning method also has some deficiencies, such as high randomness, inflexible stepsize, slow rate of calculation, and so on. The route generating from path planning is seriously affected by these deficiencies [
21]. In the sense of optimizing route generation, improving the speed of calculation, maximizing ease of control, and saving energy, these deficiencies of RRT should be overcome by forming a new algorithm that is a hybrid of superior mechanisms.
Some improved methods have also been used by many studies. Lin et al. proposed an adaptive RRT algorithm based on dynamic stepsize for path planning, in order to solve the problem that the traditional RRT algorithm easily falls into a local minimum area when applied to unmanned aerial vehicles [
22]. Melchior et al. defined a new fractional attractive force for robust path planning of mobile robot, and this method obtained robust path planning despite robot mass variations [
23]. Liu et al. introduced an algorithm based on RRT by adding a gravity component of the target to solve the disadvantages of the high randomness and long path length [
24]. Cao et al. added a dynamic stepsize and target gravity to the basic RRT algorithm. They applied this to the path planning of an agricultural mechanical arm and obtained a good result for litchi picking [
25].
However, it should be noted that some shortcomings still exist in these traditional RRT algorithms, such as the high randomness of the search tree growth directions, especially when these path-planning algorithms are applied in certain open areas [
26]. In addition, there is often a complex environment with many narrow areas and open areas in the USV’s workspace. To deal with these deficiencies of the existing RRT-based algorithms, a novel algorithm is proposed in this paper that is improved through a hybrid of the basic RRT and two mechanisms: dynamic stepsize and target attractive forces. The proposed algorithm is used to improve the performance of USV tasks in a complex workspace. To verify the effect of the enhanced algorithm, this algorithm must be applied to a simulation experiment before being put into a real ship. A real map of a gulf which has a complex terrain was selected as the simulation area, and some necessary processing was done on this map for simulation. In addition, there were some points set for testing according to actual work requirements. Simulation results show that the proposed algorithm definitely optimizes the path-planning algorithm for USVs, especially in a few particular areas.
This paper is organized as follows. In the current section, the necessity of path-planning research has been described with the relevant development and background of USVs. In
Section 2, the problem definition is presented and some symbols are defined. In
Section 3, the basic RRT path-planning algorithm is given, its merit and demerits are analyzed, and the proposed adaptive hybrid dynamic stepsize and target attractive force-RRT(AHDSTAF-RRT) is also described in detail. In
Section 4, after describing the experimental preparation, the results of the simulation are presented and analyzed. Finally, in
Section 5, conclusions and further work are given.
2. Problem Definition
The setup of the path-planning algorithms addressed in this paper is described in this section. The primary task of the path-planning problem is to find a feasible path from the initial state to the goal region in the shortest distance and least amount of computation time possible [
19,
27].
The optimal path-planning problem is formally composed of a cost function, boundary conditions, and parameters to optimize the cost function. The state space, or configuration space, is represented by the set , and , where is a particular configuration of the USV and is the dimension of state space. is the set of states representing obstacle configurations and are forbidden regions for the USV, and in general. The set of states denotes the traversable states for the USV. Let represent the RRT’s tree growing from the initial and goal state. Let and be the vertices andedges of such that . Let and represent the initial and goal state, respectively.
Definition 1. Feasible path planning is performed to find a feasible path , such that , let be a collision-free path be denoted by , for a given path planning problem .
Definition 2. For a given path planning problem , let be the cost to reach along a path . The cost function can hence be formally determined by the following: 3. Path-Planning Algorithms and Improvements
Path planning is a critical step for the USV, as the level of endurance, efficiency, and autonomy of a USV are affected by the path-planning strategy [
28]. In this section, the basic RRT algorithm and two improved RRT algorithms are first presented based on the current path-planning research. Building off the presented algorithms, the adaptive hybrid dynamic stepsize and target attractive force-RRT(AHDSTAF-RRT) is proposed, and the procedures of the AHDSTAF-RRT are also introduced.
3.1. Basic RRT Algorithm
LaValle proposed a concept of a rapidly-exploring random tree (RRT) as a randomized data structure that is designed for a broad class of path-planning problems [
29]. Basic RRT is a data structure and algorithm that is designed for efficiently searching non-convex high-dimensional spaces, and it is constructed incrementally in a way that quickly reduces the expected distance of a randomly-chosen point to the tree.
The basic RRT algorithm, presented in Algorithm 1, is a qualified path-planning algorithm. The basic RRT begins with a tree rooted at the only start node with no edge and incrementally extends the tree of collision-free paths. When RRT connects a new state , it needs to generate other nodes to create its final path. First, a node would generated through goal node or from free space randomly, called ; second, the nearest neighbor node of node , would be searched out of existing tree; third, another node would be searched in a circle with radius equaling one unit stepsize and centered at , to find node that located in line between node and node , and makes a path with the line segment from the node to the node ; then the node would be abandoned if the line segment from node to node is not collision-free; or the node will be added to tree if the line segment is in the collision-free space; final, it would restart node generating and searching, until the distance between goal and node in the tree is smaller than given value, which is end of this calculation.
Algorithm 1 Basic Rapidly-Exploring Random Tree (RRT) |
RRTmain() |
1. |
2. = |
3. while Distance(, ) < ErrTolerancedo |
4. = SampleTarget() |
5. = NearestVertex(Tree, ) |
6. = ExtendTowards(, ) |
7. Tree.add() |
8. end while |
9. return Trajectory(Tree, ) |
The pseudocode of SampleTarget(), which is one of functions in the basic RRT algorithm, is presented in Algorithm 2.
Algorithm 2 SampleTarget() |
1: if Rand()<GoalSamplingProb then |
2: return |
3: else |
4: return RandomConfiguration() |
5: end if |
The algorithms discussed above contain the following functions:
Distance: Given two states and , it returns the distance between these two states by a distance function. Euclidean distance is usually used for the distance function.
SampleTarget: Given a graph configuration space, it returns a state , which is either the state or a state randomly selected from the configuration space.
NearestVertex: Given a graph tree and a state , it returns the state that is the nearest state to on the graph tree.
ExtendTowards: Given two states and , it returns a state that is located on the line segment from to , and such that the distance between and is equal to unit stepsize.
The objective of the basic RRT algorithm is to start from an initial state located in the collision-free space and find a path to the goal state . This is done by continuously adding nodes to the , which is grown from the starting state in the collision-free space until the distance between the goal and a node in the is within the visible range.
Time complexity and space complexity describe how much time and space, respectively, are required by the given algorithm. Large
notation is often used to describe the complexity of an algorithm [
20,
30].
The time complexity analyses of the basic RRT algorithm, as shown in Algorithm 1, are as follows. Time complexity is usually defined as the number of calls for the most time-consuming procedure, which is the procedure of adding the vertex
in basic RRT. For a problem with a data size of
samples, the time it takes to add vertex
to the tree can be calculated as the sum of each step:
where
,
,
, and
correspond to the functions in lines 4–7 of Algorithm 1, respectively. With data size of
samples,
,
, and
are all simple operations, and can be completed in linear time; thus, the time complexity of them is
. The extension time,
, of the tree can take considerably longer if collision checking or other intelligent extension strategies are used. However, these calculations do not depend on the number of vertices already in the tree, and the time complexity for inserting a vertex is
. Each time the nearest vertex has to be found, the distance from the vertex
to all previously added vertices must be calculated. Thus:
By adding the derived complexity for the sample, near, extend, and add operations, the combined time complexity for the basic RRT algorithm is
Therefore, the time complexity of the basic RRT is for a data size of samples.
Space complexity is defined as the amount of memory space used by the given algorithm. Basic RRT maintains a tree , and the size of the tree determines the amount of memory space. The space complexity of the basic RRT is , because the size of can be calculated as the sum of the absolute values of and . Clearly, , and . Therefore, , and the space complexity of the basic RRT is .
3.2. Improved Methods of RRT
While a basic RRT algorithm alone is insufficient to solve a path-planning problem, it is customarily considered as a component that can be incorporated into the development of a variety of different planning algorithms. Therefore, relative to RRT’s shortcomings, which are randomness and a weak ability to pass through a narrow area, two mechanisms-dynamic stepsize and forces due to target attraction-are chosen and added to the basic RRT algorithm to solve its deficiencies.
3.2.1. Target Attractive Force-RRT
Due to the high randomness, there are many cases in which the RRT algorithm tree branches grow in an irrelevant direction. To solve this deficiency, the mechanism of a target attractive force was introduced into the basic RRT algorithm, forming the target attractive force-RRT(TAF-RRT) algorithm.
The following is the method of the basic RRT with a target attractive force added:
where:
is the angle between the line pointing from to and the coordinate axis;
is the angle between the line pointing from to and the coordinate axis;
is the final angle between the direction of the branch node growth and the coordinate axis;
and represent the coefficients of and , respectively. In general, .
Every time a new node generated, it would be slightly biased toward the target node from the original direction toward the sample node , with the bias angle generated by target attractive force. Due to this mechanism added, the final collision-free path and cost of path would change each time the biases were added together after the process of path planning.
Compared to the basic RRT algorithm, branches of the TAF-RRT are more likely to grow in the direction of the target in an open area after adding the target attractive force. Therefore, the randomness and the length of the path created by this method are reduced. The number of branches represents the successful attempts of connected samples to the tree before a feasible path generated by this algorithm.
3.2.2. Dynamic Step Size-RRT
Another problem with the RRT-based algorithm is that it is difficult to pass through narrow areas of a map because the RRT-based algorithm when the stepsize is too large. Another consequence of this shortcoming is that the branches grow too slowly in an open area with a small stepsize, which results in the lower speed of the algorithm. An approach is proposed that aims to solve this problem by adding the mechanism of a dynamic stepsize.
The principle of this improved algorithm, dynamic stepsize-RRT(DS-RRT), is that the stepsize of this algorithm is variable depending on different situations of the
, threshold
, and node
. When branches growing in narrow area, the stepsize
will become smaller, and likewise will become larger when they face an open field:
After the dynamic stepsize mechanism is added, the modified algorithm has a better passage ability in narrow area with smaller stepsize, and also has an higher speed in open area with larger stepsize. Usually, with the fixed initial stepsize , the value of is a half of , and the value of is 1.0 to 1.2 times , which are generated by trial and error.
3.2.3. Adaptive Hybrid Dynamic Step Size and Target Attractive Force-RRT
To modify both deficiencies of the RRT algorithm mentioned above, the two described mechanisms are simultaneously added to the basic RRT algorithm, forming the dynamic stepsize and target attractive force-RRT (DSTAF-RRT) algorithm.
However, some test experiments show that the modified algorithm DSTAF-RRT sometimes performs unsatisfactorily in segmental processes, especially in narrow areas, and sometimes even worse than the singly-added mechanisms. Through analysis, it was found that the target attractive force usually hinders the correct development of the tree in the narrow area, because the branch nodes easily hit obstacles due to the attractive force impact and thus grow unhealthily. In view of the above, the following proposal was proposed: when RRT branch nodes grow in a narrow area, the dynamic stepsize mechanism should be added into the basic RRT algorithm and the weight of the target attractive force should be reduced or even completely removed. Likewise, when RRT branch nodes grow in a very open area, the target attractive force and dynamic stepsize should be added simultaneously, and the weight of the target attractive force should be increased. Ultimately, the adaptive hybrid dynamic stepsize and target attractive force-RRT (AHDSTAF-RRT), is born.
Algorithm 3: AHDSTAF-RRT(Improved RRT) |
Improved RRTmain() |
1. |
2. |
3. while Distance() < ErrTolerancedo |
4. = Sample Target() |
5. = NearestVertex() |
6. if CheckObstacle(, d) |
7. ExtendTowards←AttractiveForce(); |
8. p = DynamicSize(T, ); |
9. else |
10. ExtendTowards←Reduce AttractiveForce(); |
11. p = DynamicSize(T, ); |
12. endif |
13. = ExtendTowards() |
14. Tree.add() |
15. end while |
16. return Trajectory(Tree,) |
The AHDSTAF-RRT algorithm, presented in Algorithm 3, is similar to the basic RRT algorithm in general, except in the critical steps where the adaptive hybrid adjustment is added through a judgement program statement.
Algorithm 3 employs the following functions:
CheckObstacle: Given the node , obstacle , and threshold , it returns whether the distance between the node and obstacle is bigger than the threshold . This is used toadjustthe values of the stepsize and the attractive force. Usually the threshold is 1.5 to 2 times larger than the stepsize , which is generated by trial and error.
AttractiveForce: Given two nodes and , this puts a minor force on when connecting to with the function ExtendTowards. The addition of a minor bias angle in the ExtendTowards function represents the attractive force from the target.
DynamicSize: Given a graph and , it returns a dynamical value of the stepsize . It dynamically generates the stepsize depending on its input status, providing a numerical stepsize value which is relevant to the current situation.
A flow chart summarizing the improved AHDSTAF-RRT algorithm is shown in
Figure 1.
The time complexity analysis of the improved RRT algorithm is as follows. The time it takes to add
vertices to the tree can be calculated as the sum of the time for
samples of each of the lines 4–14 in Algorithm 3:
where
,
,
, and
are the same as the basic RRT, respectively, so the time complexity of
and
are
, and the time complexity of
also is
. The judgement program statement of the improved RRT algorithm, CheckObstacle, is an additional step compared with the basic RRT algorithm.
is the time complexity of this judgement program statement. Two branch statements in the judgment program statement are the same time complexity,
. By adding the derived complexity for the sample, near, extend, add, and check operations, the combined time complexity for the improved RRT algorithm is calculated as the following:
Therefore, the time complexity of the improved RRT is .
The space complexity analysis of the improved RRT algorithm is as follows. Similar to the basic RRT, the improved RRT algorithm also maintains a tree and the size of determines the amount of memory space. Given , obviously the space complexity of the improved RRT is also .
It can be easily seen from the calculation that the improved algorithm keeps the same time complexity and space complexity as the basic RRT, while improving the search speed and keeping the terse advantage of the program statement.
This AHDSTAF-RRT algorithm, improved by adding the adaptive adjustment mechanisms, must be applied in simulations and experiments to test its performance.
4. Simulation Results
It is essential to do an experiment by simulating an actual USV workspace to verify whether the improved AHDSTAF-RRT algorithm is valid. The map area, process, and results of the simulations are included in this section.
4.1. Map Processing and Coordinate System Establishment
The Xinglin Gulf, which is located north of Xiamen, is chosen as the task simulation area due to its complex terrain environment. The map must be processed before it can be used in the simulation experiments. The process of map sharpening is shown in
Figure 2. An image of the Xinglin Gulf with a size of 1500 × 1500 pixels is obtained after the map processing.
First, a map of Xinglin Gulf is extracted from the map network and then made into an image with a suitable size, as shown in
Figure 2a. After this processing step, information of collision-free space, obstacle space and comparing rule are saved. Second, this image needs to be sharpened to make its outlines clearer and prepare it for the next step, as shown in
Figure 2b. Next, special areas of the image need to be painted with a suitable color, usually black and white, as shown in
Figure 2c. Usually white represents collision-free space and black represents obstacle space. Finally, some noisy points need to be removed from this image and the image is converted into binary data so that it can be correctly read by a computer, as shown in
Figure 2d. It is necessary to explain that this method of map processing only applies to the simulation experiment to verify the effectiveness of the algorithm proposed in this study, while there are other methods to process maps for real tasks.
It is important to establish a coordinate system for map after the map processing and test point selection. The top left vertex of the map is set to the origin of the coordinates, with the , axis point from origin toward bottom and right respectively. After processing, this image is transformed into a two-dimensional array space with obstacle space and collision-free space , which has a coordinate system and size of 1500 × 1500 pixels.
4.2. Simulation Experiments of the Algorithm Applied to the Xinglin Gulf Map
The test points are selected according to the technical specifications of actual water sampling. It is well known that sources of water pollution are often in the shore areas of water, therefore all selected test points are located in shallow water areas close to the shore. With the vector space
, the selection of test points refers to an actual water sampling work, and there are six test points and six path segments which appear from these points being connected in turn. The coordinates of Xinglin Gulf test points are listed in
Table 1, and the locations of the test points in vector space
of map image are shown in
Figure 3.
From
Figure 3, it can be easily seen that the test points are all located in the shore areas of the Xinglin Gulf, with several narrow regions between them. The purpose of this selection is to test whether the performance of the proposed algorithm is improved.
After the test point selection and the determination of the simulation task, five different path-planning algorithms were applied to the simulation of the Xinglin Gulf map in order to test their performance: the basic RRT, DS-RRT, TAF-RRT, DSTAF-RRT, and AHDSTAF-RRT. To avoid the single test randomness, each algorithm will simulate 20 times, and result of each simulation will be recorded to calculate the average quantity.
When these RRT-based algorithms perform path planning, the tree branches extend from the root node
to search the space of map until the tree stops near the target node
. The number of branches represents the successful attempts of connected all random samples to the tree before a feasible path generated by RRT-based algorithms, therefore it could also stand for the computational time cost level. The average lengths of the paths determined from the different algorithms simulated on the Xinglin Gulf map are shown in
Table 2, and the average number of branches for each algorithm’s growth in the simulation experiments is shown in
Table 3.
Histograms comparing the results are shown in
Figure 4, the average length of each path through the different algorithms histogram is shown in
Figure 4a, and the average number of branches for each algorithm’s growth is shown in
Figure 4b.
It can be obviously seen that the basic RRT performed worst in terms of the average length of each path compared with the other improved algorithms. On the other hand, the AHDSTAF-RRT has the best performance among these algorithms, having the lowest average length cost for each test. In terms of the average number of branches, the TAF-RRT algorithm and DSTAF-RRT perform poorer than the basic RRT in narrow areas, which confirms our previous statements. Furthermore, the AHDSTAF-RRT performs among best of these algorithms in terms of the average number of branches.
The final simulation experiment of five algorithms is shown in
Figure 5. Each path is composed of six segmented paths, which start point and goal point are respectively set in
Table 1 and
Figure 3. It is easily seen from
Figure 5 that the basic RRT has a high randomness. Additionally, a feasible path in the map of Xinglin Gulf generated by the AHDSTAF-RRT algorithm, which performed better than the other four algorithms both in terms of length and computational rate.
5. Conclusions
An RRT-based optimal path-planning algorithm is proposed in this paper, namely the adaptive hybrid dynamic stepsize and target attractive force-RRT(AHDSTAF-RRT). The main idea of this improved algorithm is that it adds two mechanisms—a dynamic stepsize and a target attractive force—into the basic RRT algorithm in the procedure of new node generation and tree growth. This improves on the basic RRT while retaining its beneficial characteristics. Both in terms of length and number of branches, it is obvious that the proposed AHDSTAF-RRT algorithm has created an enhanced path-planning method while keeping the same time and space complexity as the basic RRT. Furthermore, the improved algorithm is not only able to find a better solution to pass narrow areas but is also able to pass open areas with a higher computational speed. In addition, AHDSTAF-RRT is still a tree-extending algorithm, and it can also be combined with any sampling strategy or graph-pruning algorithm to take advantage of any other excellent properties.
In future research, more advantageous mechanisms will be adapted into this algorithm to improve its path-planning performance and computational speed, more complex environments will be studied, and more experiments will be done to observe its passing ability and performance. In addition, with a view to the motion control of USVs, there is a great deal of work to do in the processing of curved sliding after path generation.