Unmanned ship path planning method based on dynamic situation field combined with RRT algorithm

Abstract

The invention provides an unmanned ship path planning method based on a dynamic situation field in combination with an RRT algorithm, and belongs to the technical field of path planning. The method comprises the following steps: determining a starting point and a target point of a to-be-planned path; taking the starting point as an initial node in the map space; selecting a corresponding sampling point for the current step node through a cost function; searching a node closest to the sampling point from existing nodes; generating a next node by taking the nearest node as a starting point and combining a threat index based on an artificial potential field method; selecting a corresponding sampling point for each step of node until the distance between the next step of node and the target node is within a preset threshold value, and generating an initial path from the target point to the starting point; and smoothing the initial path by using a cubic spline difference method, and outputting a planned path. By reducing the sampling cost and evaluating the potential risk of the obstacle to the unmanned surface vessel, the next node generation efficiency is improved, effective obstacle avoidance is realized, and the efficiency and quality of path planning are further improved.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to an unmanned ship path planning method based on a dynamic potential field combined with an RRT algorithm. Background Art

[0002] With the rapid development of artificial intelligence technology, unmanned surface vessels are increasingly used in fields such as ocean monitoring, exploration, and rescue. Effective path planning is crucial for unmanned surface vessels to complete their missions.

[0003] Although existing path planning algorithms such as RRT (Rapidly-exploring Random Tree) have good environmental exploration and search capabilities, they have shortcomings in terms of path length, number of iterations, and redundancy of sampling points. Because of its randomness in the sampling process, the path generated by the RRT algorithm is usually rough, may have many redundant nodes and twists, and is not smooth enough. This not only increases the total length of the path, but may also cause the unmanned ship to frequently change direction in actual operation, increasing energy consumption and running time. In addition, the traditional RRT algorithm is mainly applicable to static environments and has poor response capabilities to dynamic obstacles. In a dynamic environment, the path needs to be constantly replanned, which increases the computational burden; the lack of effective target guidance when looking for the target point causes the algorithm to blindly search the entire map and explore a large number of unnecessary areas. This not only reduces the search efficiency, but also increases the complexity of path planning.

[0004] Therefore, an unmanned ship path planning method with stable planning path quality, flexible obstacle avoidance and high computational efficiency is needed. Summary of the invention

[0005] In view of this, the present invention provides an unmanned ship path planning method based on dynamic potential field combined with RRT algorithm, which improves the efficiency of the next step node generation by reducing the sampling cost; and effectively avoids dynamic obstacles by evaluating the potential risks of dynamic obstacles to unmanned surface vessels, thereby improving the efficiency and quality of path planning and having good adaptability in dynamic environments.

[0006] To this end, the present invention provides the following technical solutions:

[0007] A path planning method for an unmanned ship based on a dynamic potential field combined with an RRT algorithm, comprising:

[0008] Determine the starting point and target point of the path to be planned;

[0009] In the map space, the starting point is used as the initial node;

[0010] Select the corresponding sampling point for the current step node through the cost function;

[0011] Search for the node closest to the sampling point among the existing nodes;

[0012] Taking the closest node as the starting point, generate the next node based on the artificial potential field method combined with the threat index;

[0013] Select the corresponding sampling point for each step node until the distance between its next node and the target node is within the preset threshold, and generate the initial path by backtracking from the target point to the starting point;

[0014] Smooth the initial path using the cubic spline interpolation method and output the planned path.

[0015] Further, it is characterized in that the generating of the next node based on the artificial potential field method combined with the threat index with the closest node as the starting point includes:

[0016] Taking the closest node as the starting point, determine the expansion direction of the next node based on the artificial potential field method combined with the threat index;

[0017] Taking the closest node as the starting point, determine the expansion step length of the next node according to the total potential field value of the current step node.

[0018] Further, the determining of the expansion direction of the next node based on the artificial potential field method combined with the threat index includes:

[0019] Calculate the sum of the repulsive forces generated by all obstacles within a certain range on the current step node based on the artificial potential field method combined with the threat index;

[0020] Determine the expansion direction of the next node based on the attraction of the target point to the closest node, the attraction of the sampling point to the closest node, and the repulsive force of the obstacle to the closest node.

[0021] Further, the determining of the expansion step length of the next node according to the total potential field value of the current step node includes:

[0022] If the total potential field value of the current step node is greater than the preset threshold, the expansion step length is the preset minimum step length;

[0023] If the total potential field value of the current step node is less than or equal to the preset threshold, the expansion step length linearly increases according to the total potential field value until the preset maximum step length is reached.

[0024] Further, the threat index includes:

[0025]

[0026] Among them, η represents the threat index, S is the size of the dynamic obstacle, V is the moving speed of the dynamic obstacle, d is the Euclidean distance between the dynamic obstacle and the unmanned surface vehicle, and ΔΘ is the angle difference between the moving direction of the dynamic obstacle and the line connecting the dynamic obstacle to the unmanned surface vehicle; w1, w2, w3, and w4 are weight coefficients; ζ is a constant used to avoid division by zero operations.

[0027] Furthermore, the cost function includes:

[0028]

[0029] Among them, q goal represents the distance from the sampling point to the target point, d(q, obstacles) represents the distance from the sampling point to the nearest obstacle, and λ1 and λ2 are parameters for controlling the relative importance of each factor.

[0030] Advantages and positive effects of the present invention:

[0031] By introducing a cost function and a dynamic obstacle avoidance strategy, the present invention generates a shorter path, reduces the sailing distance of the unmanned surface vehicle, and improves the task execution efficiency.

[0032] The present invention improves the search efficiency of the next node through an improved sampling strategy and a dynamic step size strategy, reduces the number of iterations and calculation time, and can generate a path faster.

[0033] By introducing a threat index into the repulsive field function of the artificial potential field, the present invention evaluates the potential risk of dynamic obstacles to the unmanned surface vehicle, enhances the dynamic environment adaptability, effectively avoids dynamic obstacles, and ensures the safe navigation of the unmanned surface vehicle.

[0034] The present invention optimizes the path through cubic spline interpolation, reduces the turning angle of the unmanned surface vehicle during navigation, and improves the sailing stability. Description of the Drawings

[0035] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the drawings in the following description are some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0036] Figure 1 It is a flowchart of the unmanned ship path planning method based on the dynamic potential field combined with the RRT algorithm in the embodiments of the present invention;

[0037] Figure 2 It is a schematic diagram of the expansion direction of the next node in the embodiments of the present invention. Detailed implementation manners

[0038] In order to enable those skilled in the art of the present technology to better understand the solution of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0039] It should be noted that the terms "first", "second", etc. in the description and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and do not necessarily need to be used to describe a specific order or sequence. It should be understood that such data can be interchanged under appropriate circumstances so that the embodiments of the present invention described here can be implemented in an order other than those illustrated or described here. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product or device including a series of steps or units does not necessarily have to be limited to those steps or units clearly listed, but may include other steps or units not clearly listed or inherent to these processes, methods, products or devices.

[0040] The present invention provides an unmanned ship path planning method based on a dynamic potential field combined with the RRT algorithm. The path is planned through the RRT algorithm, and a cost function is introduced to guide the expansion of the tree to be close to the target point and far from obstacles; a threat index is introduced into the repulsive field function of the artificial potential field to achieve effective obstacle avoidance; combined with an improved APF method, efficient search is achieved; cubic spline interpolation is used to optimize the generated trajectory nodes to obtain a smooth trajectory suitable for the navigation of an unmanned surface vehicle.

[0041] S1. Introduce a cost function to select sampling points;

[0042] In the sampling process of the RRT algorithm, a cost function is introduced. The cost function comprehensively considers the distance from the sampling point to the target point and the degree of proximity to obstacles. By adjusting the sampling probability, low-cost sampling points are more likely to be selected, thereby guiding the expansion direction of the tree towards an area that is both close to the target and far from obstacles. Specifically, the cost function is defined as:

[0043]

[0044] where q goal represents the distance from the sampling point to the target point, d(q, obstacles) represents the distance from the sampling point to the nearest obstacle, λ1 and λ2 are parameters for controlling the relative importance of each factor, and ζ is a small constant used to avoid division by zero operations.

[0045] S2. Select the nearest node corresponding to the sampling point;

[0046] S3. Determine the position of the next node by introducing the threat index into the artificial potential field method;

[0047] S31. Determine the expansion direction of the next node by combining the method of introducing the threat index APF (Artificial Potential Field Method) with the RRT algorithm;

[0048] In the embodiment of the present invention, by introducing a threat index into the repulsive field function of the artificial potential field, the potential risk of dynamic obstacles to the unmanned surface vehicle is evaluated; and the threat index is introduced to calculate the sum of the repulsive forces of all obstacles on the nearest node. Specifically:

[0049] 1) The threat index combines factors such as the size, speed, distance from the unmanned surface vehicle, and the alignment degree between the moving direction and the position of the unmanned surface vehicle of the obstacle. By calculating the threat index, the unmanned surface vehicle can more effectively avoid dynamic obstacles. The threat index is defined as:

[0050]

[0051] where η represents the threat index, S is the area of the dynamic obstacle, V is the moving speed of the dynamic obstacle, d is the Euclidean distance between the dynamic obstacle and the unmanned surface vehicle, ΔΘ is the angle difference between the moving direction of the dynamic obstacle and the line connecting the dynamic obstacle to the unmanned surface vehicle; w1, w2, w3, w4 are weight coefficients; ζ is a constant used to avoid division by zero operations.

[0052] 2) The formula for the repulsive force field introducing the threat index is as follows:

[0053]

[0054] where k rep,dyn is the repulsive force field coefficient of the dynamic obstacle, η j is the threat index, ρ(q, q j ) is the Euclidean distance from the nearest node to the position of the dynamic obstacle, ρ 0,dynamic is the influence range of the repulsive force field of the dynamic obstacle.

[0055] 3) The formula for the repulsive force field of the static obstacle remains unchanged and is as follows:

[0056]

[0057] where k rep,static is the repulsive force field coefficient of the static obstacle, η j is the threat index, ρ(q, q i) is the Euclidean distance from the nearest node to the position of the static obstacle, ρ 0,static is the influence range of the repulsive force field of the static obstacle.

[0058] 4) Therefore, the total repulsive force formula of the static obstacle and the dynamic obstacle on the nearest node is as follows:

[0059]

[0060] Among them, U rep (q) is the sum of the repulsive forces generated by all obstacles within a certain range on the nearest node.

[0061] 5) Calculate the attractive force of the target point on the nearest node, the attractive force of the sampling point on the nearest node, and the repulsive force of the obstacle on the nearest node. The specific calculation formulas are as follows:

[0062] F att1 (q) = k att ρ(q, q rand )

[0063] F att2 (q) = k att ρ(q, q qoal )

[0064] F att (q) = F att1 (q) + F att2 (q)

[0065]

[0066] Among them, F att1 (q) is the attractive force of the sampling point on the nearest node, and F att2 (q) is the attractive force of the target point on the nearest node. F att (q) is the resultant force of F att1 (q) and F att2 (q), and F rep (q) is the sum of the repulsive forces of all obstacles on the nearest node.

[0067] 6) As Figure 2 shown, the resultant force Ftotal is obtained by synthesizing the attractive force of the target point on the nearest node, the attractive force of the sampling point on the nearest node, and the repulsive force of the obstacle on the nearest node; the direction of the resultant force Ftotal is the expansion direction of the next node.

[0068] S32. Adopt a dynamic step size strategy to dynamically adjust the expansion step size of the next node according to the total potential field value of the current step node, so as to improve the accuracy in a complex environment and the generation efficiency of the next node in a relatively loose area. Specifically, the dynamic step size adjustment strategy is as follows:

[0069]

[0070] Among them, U total (q) is the total potential field value of the current step node, U threshold is the potential field threshold, Δs min and Δs max are the preset minimum step length and preset maximum step length respectively, and U max is the sum of the maximum potential field values of the attraction field and the repulsion field.

[0071] When the total potential field value U total (q) is greater than the threshold U threshold , the current position is significantly affected by the obstacle, and at this time the step length is the preset minimum step length Δs min to ensure accuracy and safety.

[0072] When the total potential field value U total (q) is less than or equal to the threshold U threshold , the current position is relatively safe, and the step length will be linearly adjusted according to the potential field value, gradually approaching the preset maximum step length Δs min in the area with less constraints. In the theoretical calculation method, U max is determined according to the maximum potential field values of the attraction field and the repulsion field.

[0073] S4. Repeat steps S1 - S3 to select corresponding sampling points for each step node until the distance between its next step node and the target node is within the preset threshold; generate an initial path by backtracking from the target point to the starting point;

[0074] S5. Path optimization;

[0075] Use cubic spline interpolation to optimize the generated initial path to obtain a smooth trajectory suitable for the navigation of the unmanned surface vehicle. Cubic spline interpolation constructs a series of cubic polynomials to ensure the smoothness and continuity of the curve in each interval, thereby improving the navigability of the path.

[0076] Use a comparative experiment to further verify the beneficial effects of the method of the present invention:

[0077] In this experiment, the unmanned surface vehicle needs to navigate from the starting point A to the target point B in a sea area, and there are multiple static obstacles (such as reefs, buoys, etc.) and dynamic obstacles (such as other sailing vessels) in this sea area.

[0078] The unmanned ship path planning method based on dynamic potential field combined with the RRT algorithm of the present invention is as follows:

[0079] Input: starting point A, target point B, step length Δs max , minimum step length Δs ,in and the obstacle potential field threshold;

[0080] 1. Initialize the RRT tree and use the starting point A as the root node of the tree.

[0081] 2. Randomly generate a number of random sampling points in the free configuration space, calculate the cost value of each random sampling point, and select the one with the lowest cost as the sampling point.

[0082] 3. Find the node in the RRT tree that is closest to the sampling point as the nearest node.

[0083] 4. Calculate the attraction of the target point to the nearest node, the attraction of the sampling point to the nearest node, and the repulsion of the obstacle to the nearest node according to the APF algorithm;

[0084] Determine the expansion direction of the next node by synthesizing the directions of the attraction of the target point to the nearest node, the attraction of the sampling point to the nearest node, and the repulsion of the obstacle to the nearest node.

[0085] 5. Dynamically adjust the step size according to the total potential field value of the current step node, generate the next step node and add it to the RRT tree.

[0086] 6. Repeat steps 2 - 5 until the RRT tree grows from the starting point to the target point or reaches the maximum number of iterations.

[0087] 7. If the path is successfully generated, use cubic spline interpolation to optimize the path to obtain the final smooth path; if the path is not generated, return the information that the path was not found.

[0088] Traditional RRT algorithm:

[0089] 1. In the initial stage, add the starting point Xstart to the root node of the random tree.

[0090] 2. Randomly sample a coordinate point in the map space as the random point Xrand, and find the tree node Xnear that is closest to the random point Xrand. Next, expand a given step size along the direction from Xnear to Xrand to obtain the next step node Xnew.

[0091] 3. The generation of the next step node Xnew is divided into two cases:

[0092] (1) If the distance from Xnear to Xrand is greater than the preset step size, expand to the position of one step size in the direction from Xnear to Xrand as Xnew;

[0093] (2) If the distance from Xnear to Xrand is less than or equal to the preset step size, directly use Xrand as the position of Xnew.

[0094] 4. Detect whether the next node Xnew has passed the collision detection: If it has passed the collision detection, add it to the root node of the random tree; if it has not passed the collision detection, re-iterate to find a new node.

[0095] 5. Determine whether the next node Xnew is within the threshold range of the target point Xgoal. If so, return the root node and the path planning is completed. If not, jump to 2 and continue to execute.

[0096] The comparative experiments of the method of the present invention and other path planning methods based on the RRT algorithm are carried out in different environments, and the results are shown in Table 1:

[0097] Table 1

[0098]

[0099] As can be seen from Table 1:

[0100] In the static environment, the path length generated by the method of the present invention is shortened by about 13.2% compared with the traditional RRT algorithm (from 1237 meters to 1074 meters); in the mixed environment, the path length generated by the method of the present invention is shortened by about 17.8% compared with the BiRRT algorithm (from 1413 meters to 1162 meters).

[0101] In the static environment, the average running time of the method of the present invention is shortened by about 76.6% compared with the RRT algorithm (from 1.24 seconds to 0.29 seconds); in the mixed environment, the average running time of the method of the present invention is shortened by about 47.6% compared with the BiRRT algorithm (from 2.12 seconds to 1.11 seconds).

[0102] In the static environment, the average number of iterations of the method of the present invention is reduced by about 85.9% compared with the RRT algorithm (from 702 times to 98 times); in the mixed environment, the average number of iterations of the method of the present invention is reduced by about 53.0% compared with the BiRRT algorithm (from 564 times to 265 times).

[0103] It can be seen from this that although the RRT algorithm has the advantages of fast exploration and simple implementation in path planning, its disadvantages such as unstable path quality, strong randomness, not guaranteeing the optimal solution, and poor adaptability to complex environments limit its application in some scenarios. In the mixed environment, the method of the present invention can effectively avoid dynamic obstacles and generate safe paths, while the traditional RRT algorithm performs poorly in the dynamic environment and is prone to collisions. Therefore, the method of the present invention has beneficial effects such as good path optimization effect, short running time, high computational efficiency, and high accuracy of dynamic obstacle avoidance. It has a wide range of application scenarios, such as:

[0104] Ocean Monitoring and Exploration: Unmanned surface vessels perform monitoring tasks in the marine environment, such as water quality detection, marine biological surveys, seabed topography mapping, etc. The method of the present invention can help USVs plan paths in complex marine environments and avoid static obstacles such as reefs and buoys, as well as dynamic obstacles such as passing ships.

[0105] Marine Rescue: In marine search and rescue missions, unmanned surface vessels need to quickly reach the designated area and avoid obstacles. The method of the present invention can plan the optimal path in real time and improve the rescue efficiency.

[0106] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements for some or all of the technical features; and these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A path planning method for an unmanned ship based on a dynamic potential field combined with the RRT algorithm, characterized in that, Including: Determine the starting point and the target point of the path to be planned; In the map space, use the starting point as the initial node; Select the corresponding sampling point for the current step node through the cost function; Search for the node closest to the sampling point among the existing nodes; Using the closest node as the starting point, generate the next step node based on the artificial potential field method combined with the threat index; Select the corresponding sampling point for each step node until the distance between its next step node and the target node is within the preset threshold, and generate the initial path by backtracking from the target point to the starting point; Use the cubic spline interpolation method to smooth the initial path and output the completed planned path.

2. A method for path planning of an unmanned ship based on a dynamic potential field combined with the RRT algorithm according to claim 1, wherein The generating the next step node based on the artificial potential field method combined with the threat index using the closest node includes: Using the closest node as the starting point, determine the expansion direction of the next step node based on the artificial potential field method combined with the threat index; Using the closest node as the starting point, determine the expansion step length of the next step node according to the total potential field value of the current step node.

3. The method for path planning of an unmanned ship based on the combination of a dynamic potential field and the RRT algorithm according to claim 2, wherein, The determining the expansion direction of the next step node based on the artificial potential field method combined with the threat index includes: Calculate the sum of the repulsive forces generated by all obstacles within a certain range on the current step node based on the artificial potential field method combined with the threat index; Determine the expansion direction of the next step node based on the attraction of the target point to the closest node, the attraction of the sampling point to the closest node, and the repulsive force of the obstacle to the closest node.

4. The method for path planning of an unmanned ship based on the combination of dynamic potential field and RRT algorithm according to claim 2, wherein The determining the expansion step length of the next step node according to the total potential field value of the current step node includes: If the total potential field value of the current step node is greater than the preset threshold, the expansion step length is the preset minimum step length; If the total potential field value of the current step node is less than or equal to the preset threshold, the expansion step length linearly increases according to the total potential field value until it reaches the preset maximum step length.

5. The path planning method for an unmanned ship based on the combination of dynamic potential field and RRT algorithm according to claim 3, wherein, The threat index includes: Where η represents the threat index, S is the area of the dynamic obstacle, V is the moving speed of the dynamic obstacle, d is the Euclidean distance between the dynamic obstacle and the unmanned surface vehicle, ΔΘ is the angle difference between the moving direction of the dynamic obstacle and the line connecting the dynamic obstacle to the unmanned surface vehicle; w1, w2, w3, w4 are weight coefficients; ζ is a constant used to avoid division by zero operation.

6. The path planning method for an unmanned ship based on the combination of dynamic potential field and RRT algorithm according to claim 1, characterized in that, The cost function includes: Among them, q goal represents the distance from the sampling point to the target point, d(q, obstacles) represents the distance from the sampling point to the nearest obstacle, and λ1 and λ2 are parameters for controlling the relative importance of each factor.

Images