A method and device for surface unmanned boat path planning based on RRT algorithm

Abstract

The present invention belongs to the field of unmanned boat path planning, and specifically relates to a surface unmanned boat path planning method and device based on an RRT algorithm, including: using the RRT algorithm to select each path node from a starting point to a target point to realize the path planning of the unmanned boat, wherein when selecting each path node, randomly taking a point in a sampling area for randomly sampling the next path node based on the current path node, and selecting the best from a number of candidate nodes that meet the turning angle value constraint based on a cost function as the next path node; the cost function is a linear weighting of the navigation energy consumption from the current path node to the candidate node, the path length between the two nodes, and the turning angle value between the unmanned boat bow direction at the two nodes; the navigation energy consumption and the bow direction are both calculated by the kinematic model of the unmanned boat under the action of actual environmental factors. The present invention takes actual environmental factors into consideration, so that the unmanned boat can realize path planning and obstacle avoidance between designated target points based on the principle of low energy consumption.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned boat path planning, and more specifically, relates to a surface unmanned boat path planning method and device based on an RRT algorithm. Background Art

[0002] Unmanned surface vessels (USVs) are important execution tools for various modern water applications, and have important application value in port navigation, rescue, environmental control, military, etc. Path planning for USVs is a necessary prerequisite for them to complete their assigned tasks. Optimizing the path planning algorithm can make the USVs run as quickly, safely, and for as long as possible.

[0003] The Rapidly-exploring Random Trees (RRT) algorithm is a sampling-based motion planning algorithm that has been widely developed and applied in the past decade. It has many advantages such as fast generation speed and strong adaptability. According to the sampling idea of ​​RRT, a series of points randomly sampled from an obstacle-free space are connected, which is suitable for solving the path planning problem of multi-degree-of-freedom unmanned surface vehicles in complex and dynamic environments, thereby establishing a path from the initial state to the target state. For USV working scenarios, there are mainly the following three problems:

[0004] (1) To ensure the completeness of probability, the path search of the RRT algorithm in a large space has a strong randomness. Spatial overfitting and repeated search will cause the convergence speed to be too slow.

[0005] (2) The spatial path search process can obtain the relatively shortest path by repeatedly searching the path. However, this path does not take into account the energy model of the operation. It is difficult to improve the operation time of the unmanned surface vehicle under the influence of actual wind and water flow.

[0006] (3) Due to the randomness of node growth, the path planned by the RRT algorithm is difficult to meet the requirements of the motion model of the unmanned surface vessel.

[0007] Therefore, there is an urgent need for a RRT unmanned boat path planning and navigation method that considers energy consumption and motion constraint models. Summary of the invention

[0008] In view of the defects of the prior art and the need for improvement, the present invention provides a surface unmanned boat path planning method and device based on the RRT algorithm, which aims to take into account the actual environmental factors of the water surface and enable the unmanned boat to achieve path planning and navigation between designated target points based on the principle of low energy consumption.

[0009] To achieve the above object, according to one aspect of the present invention, a method for unmanned surface vehicle path planning based on an RRT algorithm is provided, comprising:

[0010] The RRT algorithm is used to select each path node from the starting point to the target point to realize the path planning of the unmanned boat. When selecting each path node, a random point is taken in the sampling area for random sampling of the next path node based on the current path node, and a plurality of candidate nodes that meet the corner value constraint are selected based on the cost function as the next path node.

[0011] The cost function is a linear weighted combination of the navigation energy consumption from the current path node to the candidate node, the path length between the two nodes, and the turning angle between the bow directions of the unmanned boat at the two nodes; the navigation energy consumption and the bow direction are both calculated through the kinematic model of the unmanned boat under the influence of actual environmental factors.

[0012] Further, the current path node ξ init The state information in the global coordinate system {e} is described as: The candidate node ξ new The state information in the global coordinate system {e} is described as: x, y and β represent the horizontal and vertical coordinate values ​​and the angle between the bow direction and the horizontal axis respectively; the kinematic model is:

[0013]

[0014] In the formula, and α represent the water velocity vector and its direction in the global coordinate system {e} in the current actual environment respectively; is the speed of the unmanned surface vehicle in still water; v b Represents the desired combined velocity, which is along the positive x-axis of the coordinate system {b}, and the positive x-axis is the current path node ξ init With candidate node ξ new The connection direction; λ is the current path node ξ init To candidate node ξ new The geometric angle.

[0015] Further, the angle value is determined as follows:

[0016]

[0017] In the formula, is the turning angle of the bow at the candidate node relative to the current path node;

[0018] The rotation angle value constraint is: Where ε β is the maximum turning angle of the unmanned surface vehicle, which is a known quantity.

[0019] Furthermore, the navigation energy consumption from the current path node to the candidate node is

[0020]

[0021] In the formula, f w,s represents the water flow resistance f on the unmanned boat w exist The projection value in the direction, v r With x v The angle of direction, is the speed of the water flow relative to the unmanned boat, They are v r At the speed of the unmanned boat in still water The direction is the projection speed on the horizontal and vertical axes in the coordinate system {v} constructed in the positive direction, and d is ξ init To new The Euclidean distance, C wind is the drag coefficient, ρ a is the air density, v wind is the wind speed, S w is the windward area of ​​the unmanned boat, ψ is the angle between the wind speed direction and the horizontal direction, which is [0,2π], ρ 0 is the density of water, C aq With C f are the attached drag coefficient and the fluid friction drag coefficient, S s For unmanned boats in v r The projected area in the direction of v r direction.

[0022] Furthermore, the sampling area is composed of the gravitational potential field and the repulsive potential field at the node ξ init The resultant force at the position generates a random sampling area Random_sampling;

[0023] Wherein, the gravitational potential field U att (ξ init ) is expressed as:

[0024]

[0025] The repulsive potential field U rep (ξ init ) is expressed as:

[0026]

[0027] The sampling area Random_sampling is expressed as:

[0028]

[0029] In the formula, k att With k rep are the gravitational gain constant and the repulsive gain constant, d 0 is a threshold of gravitational distance, d(ξ init ,ξ goal ) represents the current node ξ init Distance to target point goal The Euclidean distance, d obs is the influence radius of the obstacle obs, d(ξ init ,obs) represents the current node ξ init The shortest distance to the obstacle centroid; and are the current path nodes ξ in the generated sampling area. init The x and y coordinates of the global coordinate system {e}; K F is the side length coefficient of the sampling area, which determines the size of the sampling area; θ x is the current path node ξ init The angle between the potential force at and the x-axis direction of the global coordinate system {e}, θ max It is half of the maximum desired area angle.

[0030] Further, the cost function is expressed as:

[0031]

[0032] Where n is the number of sampling points randomly selected in the sampling area, k is length and k angle is the weight of the path length and the turning angle value, P k-1,i From the previous node ξ k-1 To the current sampling point ξ k,i The energy consumption, and P k-1,i It can take a negative value, k is interpreted as the kth growth of the RRT tree, and then the random point with the smallest cost function value is selected from the n randomly selected sampling points as the new node ξ for the kth growth of the RRT tree k .

[0033] The present invention also provides a computer-readable storage medium, which includes a stored computer program, wherein when the computer program is executed by a processor, the device where the storage medium is located is controlled to execute the surface unmanned boat path planning method based on the RRT algorithm as described above.

[0034] The present invention also provides a surface unmanned boat path planning device based on the RRT algorithm, comprising: a processor, a transceiver, and the computer-readable storage medium as described above, wherein:

[0035] The transceiver is used to send and receive data under the control of the processor;

[0036] When the processor executes the computer program on the computer-readable storage medium, the steps of the surface unmanned boat path planning method based on the RRT algorithm as described above are implemented.

[0037] In general, the above technical solutions conceived by the present invention can achieve the following beneficial effects:

[0038] (1) Based on the kinematic model of the unmanned boat, the present invention obtains factors such as the distance, turning angle, and speed between two nodes. Based on these factors, a cost function (weighted function) between nodes is obtained from the perspective of energy consumption. In the process of sampling points through RRT, the cost function is used as the optimal point selection rule, thereby making the energy consumption of the entire planned path the lowest or relatively lowest.

[0039] (2) The kinematic model of the unmanned boat established in the present invention takes into account the under-constrained control characteristics of the unmanned boat during its movement. The relative water flow velocity and the corresponding water flow resistance are solved based on the still water speed of the unmanned boat, while taking into account the influence of wind force. Finally, the orientation of the unmanned boat under the influence of the corresponding actual environment (water flow field, wind field) is solved. The motion constraints (angle, etc.) of the unmanned boat are reconsidered through this orientation between the parent and child nodes, thereby avoiding the difference in routes under the influence of theoretical angle constraints and actual environmental factors.

[0040] (3) The present invention adopts an improved APFS (artificial potential field function) to perform local planning for obstacle avoidance during movement. The artificial potential field function is achieved by artificially constructing a repulsive field and a gravitational field centered on the obstacle and the target point, and then determining the movement direction of the local planning of the unmanned boat through the combined force of repulsion and gravity. When the unmanned boat is close to the obstacle, the repulsive force is significantly greater than the gravitational force, guiding the unmanned boat to avoid the obstacle. When the unmanned boat is far away from the obstacle, the gravitational force of the target point plays a leading role, guiding the unmanned boat to continue to move to the next node to achieve local planning. Certain optimization and improvements have been made to the conventional artificial potential field function (APF), achieving accelerated convergence under local conditions and ensuring that the power consumption is relatively minimal under the condition that the path is feasible. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 A schematic diagram of an energy consumption model scenario provided by an embodiment of the present invention;

[0042] Figure 2A flow chart of path planning provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0043] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0044] Embodiment 1

[0045] A path planning method for an unmanned surface vehicle based on an RRT algorithm, comprising:

[0046] The RRT algorithm is used to select each path node from the starting point to the target point to realize the path planning of the unmanned boat. When selecting each path node, a random point is taken in the sampling area for random sampling of the next path node based on the current path node, and a plurality of candidate nodes that meet the corner value constraint are selected based on the cost function as the next path node.

[0047] The cost function is a linear weighted combination of the navigation energy consumption from the current path node to the candidate node, the path length between the two nodes, and the turning angle between the bow directions of the unmanned boat at the two nodes; the navigation energy consumption and the bow direction are both calculated through the kinematic model of the unmanned boat under the influence of actual environmental factors.

[0048] That is, the method of this embodiment needs to first establish an unmanned boat motion model that takes into account the influence of water flow according to the actual working conditions of the unmanned surface boat, and obtain the navigation direction of the unmanned boat under the influence of the specified flow field based on this motion model. And according to the predicted motion mode of the unmanned boat, the influence of wind force on the unmanned boat in this mode is considered, and then the energy consumption model considering water flow and wind force is obtained. Secondly, according to the kinematic characteristics of the under-constrained control of the unmanned surface boat in actual conditions, the kinematic model of the unmanned boat is established, and the turning angle constraint conditions are given. Finally, the improved artificial potential field function method is used to establish a feasible detection area, and a certain number of points are randomly selected in the area. Combined with the cost function considering the above energy consumption model and constraints, the better points are taken as child nodes. It is judged whether the path planning is successful, that is, whether the distance between the new node and the end point is less than the specified threshold. If successful, the planning is terminated and the path is output. Otherwise, the above point selection process is repeated.

[0049] Preferably, the kinematic model is constructed as follows:

[0050] like Figure 1 As shown in the figure, in the global coordinate system {e}, the state information of the unmanned surface vehicle is described in the form of three-degree-of-freedom coordinates, where ξinit With ξ new For the current node and the next possible node:

[0051] ξ init :[x init ,y init ,β init ] T

[0052] ξ new :[x new ,y new ,β new ] T

[0053] where β init With β new All are quantities to be demanded. and α represent the water velocity vector and its direction in the current environment respectively. is the speed of the unmanned surface vehicle in still water. init Node and ξ new The angle between the node connection line and the x-axis in the global coordinate system, then the coordinate system {b} is established in this direction, then:

[0054]

[0055] In the formula, v b represents the desired resultant velocity, and its direction is along the x-axis of {b}. Then:

[0056]

[0057] in:

[0058] That is, under the action of this water flow, the unmanned surface boat should move towards the β direction.

[0059] Preferably, the energy consumption is determined by:

[0060] In In the {v} system with the direction being the positive x direction, the resistance of the water flow to the unmanned surface vehicle is calculated according to the following relationship:

[0061]

[0062]

[0063] in is the velocity of the water flow relative to the unmanned surface vehicle, ρ 0 is the density of water, C aq With C f are the attached resistance coefficient and the fluid friction resistance coefficient, which can be taken as 10% and 20% respectively.s For unmanned surface vehicles in v r The projected area in the direction of v r Direction, f w Indicates the magnitude of the water flow resistance on the unmanned boat. For the relative speed v r ,have:

[0064]

[0065]

[0066]

[0067]

[0068] in v r With x v The angle of direction, They are v r Projected velocity in the {v} frame.

[0069] There is water flow resistance at The projection value in the direction f w,s :

[0070]

[0071] Then there is from ξ init To new The power consumption cost function is:

[0072]

[0073] Where d is the node ξ init To new The Euclidean distance, C wind is the drag coefficient, taken as 30%, ρ a is the air density, v wind is the wind speed, S w is the windward area of ​​the unmanned boat, ψ is the angle between the wind speed direction and the horizontal direction, and its size is [0,2π].

[0074] Can remember from node ξ init Departure to node ξ new In the path of init Similarly, β init-1 Represents the slave node ξ init-1 To node ξ init The angle of the bow in the path. Then there is the turning angle value:

[0075]

[0076] in For node ξ init To node ξ new The geometric angles;

[0077] The available angle constraints are:

[0078]

[0079] where ε β It is the maximum turning angle of the unmanned surface vehicle.

[0080] Establish an artificial potential field function and set a random node ξ init The potential field function U(ξ init ) is the gravitational potential field U of the target point att (ξ init ) and the repulsive potential field U of the obstacle rep (ξ init ). The improved potential field function expressions are:

[0081]

[0082]

[0083] where k att With k rep are the gravitational gain constant and the repulsive gain constant, d 0 is a threshold of gravitational distance, d(ξ init ,ξ goal ) represents the current node ξ init Distance to target point goal The Euclidean distance, d obs is the influence radius of the obstacle obs, d(ξ init ,obs) represents the current node ξ init The closest distance to the obstacle centroid. rep (ξ init ) is a conventional quadratic inverse proportional repulsion function multiplied by a new factor d(ξ init ,obs), so that when the distance to the target point is close, the repulsion is close to 0, which can effectively solve the problem of unreachable target point.

[0084] It can be obtained that at node ξ init The resultant force at is expressed as the vector sum of the gradients of the two potential fields:

[0085]

[0086] Then, a random sampling area Random_sampling is generated by the resultant force F, which is the area under the current random node with a bias toward the target point ξ goal The sampling area of ​​the given margin satisfies:

[0087]

[0088] In the formula, and are the current path nodes ξ in the generated sampling area. init The x and y coordinates of the global coordinate system {e}; K F is the side length coefficient of the sampling area, which determines the size of the sampling area; θ x is the current path node ξ init The angle between the potential force at and the x-axis direction of the global coordinate system {e}, θ max It is half of the maximum desired area angle.

[0089] The shape of this sampling area is a fan-shaped area, which can effectively solve the problem that the local repulsion in the conventional artificial potential field function is much greater than the attraction, and accelerate the convergence speed.

[0090] Furthermore, from the starting point ξ 0 Towards the target point goal In the planning of k-1 is used to determine the next node ξ k The area where random sampling is performed, n sampling points ξ are randomly selected in the area k,1 ~ξ k,n , then a cost function considering energy consumption, path length and corner value is established. The function combines the power consumption cost function obtained in step 1 and uses a linear weighted method as the cost value for the three:

[0091]

[0092] where k length and k angle P is the weight of the path length and the turning angle. In practice, the reference values ​​are 60% and 80%. k-1,i is the node ξ established in step 1 from the previous node k-1 To the current sampling point ξ k,i The energy consumption, and P k-1,i It can take a negative value, k is interpreted as the kth growth of the RRT tree. Then, the random point with the smallest cost function value is selected from the n randomly selected sampling points as the new node ξ for the kth growth of the RRT tree k .

[0093] That is, on the one hand, this embodiment uses the kinematic model of the unmanned boat to establish the angle constraint model of path planning, and at the same time adopts the RRT idea based on the improved artificial potential field function to make certain optimization and improvements to the conventional artificial potential field function (APF), thereby achieving accelerated convergence under local conditions and ensuring that the power consumption is relatively minimum under the condition that the path is feasible; on the other hand, a power consumption model of the unmanned surface vessel is established and used as a weighted weight for path planning, so that the ideal power consumption of the unmanned boat is minimized or sub-minimized, which meets the needs of actual application scenarios.

[0094] Further preferably, when finding the node ξ 1 In the planning process, we get ξ in the global coordinate system {e} 1 Relative goal The direction of β 0 The value of ξ 1 In the subsequent node growth process, it is necessary to first make angle constraint judgments on the n random points taken:

[0095] |Δβ k-1，i |=|β k-1,i -β k-2 |≤ε β ,k≥2;

[0096] Eliminate the points that do not meet the corner constraints, and then optimize the energy consumption cost.

[0097] like Figure 2 As shown, by judging the distance from the new sampling point to the target point ξ goal Is the distance less than the threshold d? target , if so, directly connect the current sampling point and the target point ξ goal , output the complete path node sequence. Among them, the threshold d target By default, it is 0.5m.

[0098] The method of this embodiment establishes the velocity equation of the unmanned boat under the impact of water flow, and realizes the prediction of the motion trajectory of the unmanned boat under the influence of a certain water flow; further, the cost function considering energy consumption between the designated nodes of the unmanned boat under the combined influence of water flow and wind force is obtained, and the optimal sampling of the path points based on the lowest energy consumption is realized; further, a sampling area based on the artificial potential field method is established, which reduces the randomness of RRT sampling and greatly improves the convergence speed of the algorithm; finally, according to the motion characteristics of the unmanned surface boat, a motion constraint model is constructed, so that the planning path generated by the algorithm is more in line with the motion characteristics of the unmanned surface boat and more meaningful. The overall algorithm achieves the effect of enabling the unmanned boat to realize path planning and obstacle avoidance between designated target points based on the principle of low energy consumption.

[0099] Embodiment 2

[0100] A computer-readable storage medium includes a stored computer program, wherein when the computer program is executed by a processor, the device where the storage medium is located is controlled to execute a surface unmanned boat path planning method based on an RRT algorithm as described above.

[0101] The relevant technical solution is the same as that in Embodiment 1 and will not be described in detail here.

[0102] Embodiment 3

[0103] A surface unmanned boat path planning device based on an RRT algorithm comprises: a processor, a transceiver, and a computer-readable storage medium as described in Embodiment 2, wherein:

[0104] The transceiver is used to send and receive data under the control of the processor;

[0105] When the processor executes the computer program on the computer-readable storage medium, the steps of a method for path planning of an unmanned surface vehicle based on an RRT algorithm as described in Example 1 are implemented.

[0106] The relevant technical solution is the same as that in Embodiment 1 and will not be described in detail here.

[0107] It will be easily understood by those skilled in the art that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A surface unmanned boat path planning method based on RRT algorithm, characterized in that: include: The RRT algorithm is used to select each path node from the starting point to the target point to realize the path planning of the unmanned boat. When selecting each path node, a random point is taken in the sampling area for random sampling of the next path node based on the current path node, and a plurality of candidate nodes that meet the corner value constraint are selected based on the cost function as the next path node. The cost function is a linear weighted sum of the navigation energy consumption from the current path node to the candidate node, the path length between the two nodes, and the turning angle between the bow directions of the unmanned boat at the two nodes; the navigation energy consumption and the bow direction are both calculated by the kinematic model of the unmanned boat under the influence of actual environmental factors; Among them, the current path node ξ init The state information in the global coordinate system {e} is described as: The candidate node ξ new The state information in the global coordinate system {e} is described as: x, y and β represent the horizontal and vertical coordinate values ​​and the angle between the bow direction and the horizontal axis respectively; the kinematic model is: In the formula, and α represent the water velocity vector and its direction in the global coordinate system {e} in the current actual environment respectively; is the speed of the unmanned surface vehicle in still water; v b Represents the desired combined velocity, which is along the positive x-axis of the coordinate system {b}, and the positive x-axis is the current path node ξ init With candidate node ξ new The connection direction; λ is the current path node ξ init To candidate node ξ new The geometric angles; The angle value is determined as follows: In the formula, is the turning angle of the bow at the candidate node relative to the current path node; The rotation angle value constraint is: Where ε β is the maximum turning angle of the unmanned surface vehicle, which is a known quantity.

2. The path planning method for an unmanned surface vehicle according to claim 1, characterized in that: The navigation energy consumption from the current path node to the candidate node In the formula, f w,s represents the water flow resistance f on the unmanned boat w exist The projection value in the direction, v r With x v The angle of direction, is the speed of the water flow relative to the unmanned boat, They are v r At the speed of the unmanned boat in still water The direction is the projection speed on the horizontal and vertical axes of the coordinate system {v} constructed in the positive direction, and d is ξ init To new The Euclidean distance, C wind is the drag coefficient, ρ a is the air density, v wind is the wind speed, S w is the windward area of ​​the unmanned boat, ψ is the angle between the wind speed direction and the horizontal direction, which is [0,2π], ρ0 is the density of water, C aq With C f are the attached drag coefficient and the fluid friction drag coefficient, S s For unmanned boats in v r The projected area in the direction of v r direction.

3. The method for path planning of an unmanned surface vehicle according to claim 1, characterized in that: The sampling area consists of the gravitational potential field and the repulsive potential field at the node ξ init The resultant force at the position generates a random sampling area Random_sampling; Wherein, the gravitational potential field U att (ξ init ) is expressed as: The repulsive potential field U rep (ξ init ) is expressed as: The sampling area Random_sampling is expressed as: In the formula, k att With k rep are the gravitational gain constant and the repulsive gain constant respectively, d0 is a threshold of gravitational distance, d(ξ init ,ξ goal ) represents the current node ξ init Distance to target point goal The Euclidean distance, d obs is the influence radius of the obstacle obs, d(ξ init ,obs) represents the current node ξ init The shortest distance to the obstacle centroid; and are the current path nodes ξ in the generated sampling area. init The x and y coordinates of the global coordinate system {e}; K F is the side length coefficient of the sampling area, which determines the size of the sampling area; θ x is the current path node ξ init The angle between the potential force at and the x-axis direction of the global coordinate system {e}, θ max It is half of the maximum desired area angle.

4. The path planning method for an unmanned surface vehicle according to claim 3, characterized in that: The cost function is expressed as: Where n is the number of sampling points randomly selected in the sampling area, k is length and k angle is the weight of the path length and the turning angle, P k-1,i From the previous node ξ k-1 To the current sampling point ξ k,i The energy consumption, and P k-1,i It can take a negative value, k is interpreted as the kth growth of the RRT tree, and then the random point with the smallest cost function value is selected from the n randomly selected sampling points as the new node ξ for the kth growth of the RRT tree k .

5. A computer-readable storage medium, characterized in that: The computer-readable storage medium includes a stored computer program, wherein, when the computer program is executed by the processor, the device where the storage medium is located is controlled to execute the surface unmanned boat path planning method based on the RRT algorithm as described in any one of claims 1 to 4.

6. A surface unmanned boat path planning device based on RRT algorithm, characterized in that: include: A processor, a transceiver, and a computer-readable storage medium as claimed in claim 5, wherein: The transceiver is used to send and receive data under the control of the processor; When the processor executes the computer program on the computer-readable storage medium, it implements the steps of a surface unmanned boat path planning method based on an RRT algorithm as described in any one of claims 1-4.

Images