UUV path planning method based on improved fast search random tree

Abstract

The invention discloses a UUV path planning method based on an improved fast search random tree. The method is suitable for the fields of autonomous navigation, task execution, obstacle avoidance, safety guarantee and the like of UUVs. The method comprises the following steps: S1, determining an initial step length; dynamically adjusting the initial step length according to the scale of the task space and the complexity of the environment; and S2, carrying out target bias sampling. In the sampling process, according to a set bias threshold value, an algorithm biases a sampling point to a target area according to a certain probability, so that the target guidance quality of search is enhanced; and S3, removing redundant points. A Dijkstra algorithm is adopted to simplify a path, unnecessary turning points are removed, and more nodes are inserted into the path to improve the intensity of the path, so that the optimization space is increased. Then, redundant point removal is executed again, so that the path is further simplified; and S4, smoothing the path. Through the maximum curvature constraint, it is ensured that the turning angle of the path does not exceed the preset limit, meanwhile, in combination with a polynomial fitting method, abrupt turning in the path is reduced, and the smoothness of the path is improved.

Description

Technical Field

[0001] The present invention belongs to the field of automatic control technology, and relates to the fields of autonomous navigation, mission execution, obstacle avoidance and safety assurance of UUV, and in particular to a UUV path planning method based on an improved fast search random tree. Background Art

[0002] With the widespread application of unmanned underwater vehicles (UUVs), especially in the fields of marine exploration, environmental monitoring, military reconnaissance and underwater search and rescue, the path planning technology of UUVs has become one of the key technologies to ensure their efficient and safe execution of tasks. Due to the complexity and dynamics of the underwater environment, the path planning of UUVs faces a series of challenges, such as ocean currents, obstacles, communication delays, motion constraints and other factors. These factors make it difficult for traditional path planning methods to meet the needs of UUVs to quickly and accurately avoid obstacles and complete tasks efficiently in practical applications. In past studies, many path planning methods have been applied to the autonomous navigation and obstacle avoidance of UUVs, mainly including graph search-based methods (such as A* algorithm), optimization-based methods (such as shortest path algorithm) and sampling-based methods (such as rapid search random tree RRT). Among them, the rapid search random tree (RRT) is particularly suitable for path planning in dynamic environments because it can perform effective random sampling in high-dimensional space, and is widely used in robot path planning. However, the RRT algorithm also has some shortcomings, mainly reflected in the poor smoothness of the path planning results, low planning efficiency and weak response to dynamic obstacles.

[0003] In this context, the present invention proposes a UUV path planning method based on an improved fast search random tree, which aims to optimize the smoothness and planning efficiency of the path through an improved RRT algorithm, and effectively respond to dynamic obstacle avoidance requirements, so as to provide an efficient and stable path planning solution for the autonomous navigation of UUV in complex environments. Summary of the invention

[0004] In order to solve the above problems in the prior art, the present invention provides a UUV path planning method based on an improved fast search random tree.

[0005] To achieve the above object, the present invention provides the following technical solution: a UUV path planning method based on an improved fast search random tree, the method comprising the following steps:

[0006] S1. Determine the initial step size. Dynamically adjust the initial step size according to the scale of the task space and the complexity of the environment;

[0007] S2, target bias sampling. During the sampling process, according to the set bias threshold, the algorithm biases the sampling points towards the target area with a certain probability, thereby enhancing the target orientation of the search;

[0008] S3, redundant point removal. Use Dijkstra algorithm to simplify the path, remove unnecessary turning points, and increase the density of the path by inserting more nodes in the path, thereby increasing the optimization space. Subsequently, redundant point removal is performed again to further simplify the path;

[0009] S4. Smooth path. By using the maximum curvature constraint, we ensure that the path turning angle does not exceed the preset limit. At the same time, we combine the polynomial fitting method to reduce the abrupt turns in the path and improve the smoothness of the path.

[0010] Preferably, the specific content of S1 is:

[0011] The initial step size is dynamically adjusted according to the scale of the task space and the complexity of the environment.

[0012] The formula for setting the initial step length is as follows:

[0013]

[0014] Where: M is the size of the task space, usually the diagonal length of the task space; ω1 is the environmental complexity coefficient, which reflects the distribution of obstacles in the environment; ω1 can be calculated by the size of the obstacles, the average distance between obstacles, and the number of obstacles; is the average distance between obstacles in the environment; is the average radius of the obstacle.

[0015] During the path expansion process, if the current step size causes a node to collide with an obstacle, the algorithm will adjust the step size and re-perform collision detection.

[0016] The formula for variable step size adjustment is as follows:

[0017] L new =L curr ω2

[0018] Where: L curr is the step size of the current node, ω2 is the step size variation coefficient, and each time it is adjusted, ω2 takes a random value between [0.1, 0.6].

[0019] If the adjusted step size avoids collision, the node will be successfully added to the search tree, further expanding the path search.

[0020] Preferably, the specific content of S2 is:

[0021] During the sampling process, according to the set bias threshold, the algorithm will bias the sampling points towards the target area with a certain probability, thereby enhancing the target orientation of the search. The rules of the target bias sampling strategy determine the sampling strategy based on the generation of random numbers. The specific strategies can be summarized into the following three cases:

[0022] The first case: when the random number is less than the set threshold, the target position is directly used as the sampling point, and the extension path from the nearest node in the current search tree to the target is calculated and expanded to the target position.

[0023] The second case: when the random number is greater than the set threshold but less than the second threshold, five nodes are randomly sampled in the workspace, and then the node closest to the target is selected as the new sampling point for expansion.

[0024] The third case: If the new node cannot be successfully generated during the current expansion process, node sampling is performed in a completely random manner using the Rand function.

[0025] Preferably, the specific content of S3 is:

[0026] Starting from the starting point, gradually expand outward to find the shortest path to the target point. By continuously selecting the node with the smallest path cost among the currently unprocessed nodes, the path cost of the adjacent nodes is updated until the optimal path from the starting point to the end point is found. It includes the following three stages:

[0027] Step 1: During the first redundant point removal process, assuming that the line between two nodes on the path will not collide, the two nodes can be directly connected by a straight line. Even if there are several path points in the middle, if this line does not collide, the redundant path points in the middle will be removed.

[0028] Step 2: For the path after the first optimization, interpolate between any adjacent nodes and at fixed intervals to generate more nodes.

[0029] Step 3: By repeating the process in step 1, check whether the connection between each pair of nodes on the path will collide; if there is no collision, the two nodes can be directly connected, removing the redundant path points in the middle.

[0030] Preferably, the specific content of S4 is:

[0031] Calculate the curvature of each curve in the path and compare it with the preset maximum curvature threshold. If the curvature exceeds the preset threshold, adjust the path to smooth the turning angles in the path and ensure that the path meets the curvature constraint. The specific steps of the maximum curvature constraint are as follows:

[0032] Step 1: Starting from the second node in the path, calculate the cosine value of the angle between each adjacent node in the path. If the cosine value of the angle is greater than the preset threshold, it means that there is a large turning angle in the path segment and the path needs to be further optimized.

[0033] Step 2: When a node with a large turning angle is detected, determine whether the parent node and child nodes of the node need to be reconnected to the path. If the node is an interpolation node (i.e., an intermediate node generated by an interpolation algorithm), delete the node from the path and restore the original connection relationship of the path.

[0034] Step 3: Continue to perform the above operation, and gradually traverse each node in the path until all turning angles are smoothed, thereby ensuring that there are no large turning angles in the path and the curvature constraint of the path is satisfied.

[0035] Combined with the fifth-order polynomial fitting, the path is interpolated and optimized by the least squares method to further reduce the abrupt turns in the path. The expression of the fifth-order polynomial fitting is:

[0036] P(x)=a0+a1x+a2x 2 +a3x 3 +a4x 4 +a5x 5

[0037] Wherein, P(x) represents a point on the path, x is a parameter of the path, and a0, a1, a2, a3, a4, and a5 are polynomial coefficients obtained by least squares fitting.

[0038] Implementing the embodiments of the present invention will have the following beneficial effects:

[0039] The UUV path planning method based on the improved fast search random tree proposed in the present invention significantly improves the efficiency of path planning, avoids collisions, and improves the planning speed by dynamically adjusting the step size, target bias sampling and other strategies. At the same time, through the maximum curvature constraint and polynomial fitting, the path simplification is ensured, the motion constraints are met, and it is suitable for smooth movement in underwater environments. The improved fast search random tree algorithm can effectively cope with dynamic obstacles and complex environmental changes, enhance the obstacle avoidance capability, and ensure that the UUV can navigate freely in complex underwater environments. The method can dynamically adjust the step size and enhance the target orientation according to the complexity of the environment, thereby adapting to the dynamic changes and uncertain factors in the underwater environment. In summary, the path planning method not only improves efficiency and quality, but also enhances adaptability in complex dynamic environments, and has broad application prospects. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] In order to more clearly illustrate the embodiments of the present invention or the technical solutions of the prior art, the relevant drawings are briefly introduced below. These drawings only show some embodiments of the present invention and are intended to provide sufficient information for technicians so that they can extract other relevant content from them without engaging in creative thinking. The drawings intuitively present the structure, components or processes of the present invention, which helps to better understand the specific details of the embodiments. In general, the purpose of these drawings is to help professionals in the technical field understand the implementation methods of the present invention and provide them with the necessary information to design and implement similar or equivalent solutions. The specific contents are as follows:

[0041] Figure 1 It is a flow chart of a UUV path planning method based on improved fast search random tree;

[0042] Figure 2 The result after adjusting the step size in one embodiment;

[0043] Figure 3 The target bias sampling result in one embodiment;

[0044] Figure 4 This is the result after path simplification described in one embodiment.

[0045] Figure 5 This is the result after path smoothing in one embodiment. DETAILED DESCRIPTION

[0046] Next, the technical solution will be described in detail and clearly in conjunction with the drawings in the embodiments of the present invention. It should be noted that the embodiments are only part of the present invention, not all of the embodiments. Based on the embodiments of the present invention, any other embodiments that can be derived by any technician in this field without creative work belong to the protection scope of the present invention. The implementation of the embodiments of the present invention will bring the following beneficial effects: In order to solve the deficiencies in the prior art, this embodiment proposes a UUV path planning method based on an improved fast search random tree, such as Figures 1 to 5 As shown, the specific methods include:

[0047] S1. Determine the initial step size. The initial step size is adjusted dynamically according to the scale of the task space and the complexity of the environment; S2. Target bias sampling. During the sampling process, according to the set bias threshold, the algorithm biases the sampling points toward the target area with a certain probability, thereby enhancing the target orientation of the search; S3. Redundant point removal. The Dijkstra algorithm is used to simplify the path, remove unnecessary turning points, and increase the density of the path by inserting more nodes in the path, thereby increasing the optimization space. Subsequently, redundant point removal is performed again to further simplify the path; S4. Smooth the path. Through the maximum curvature constraint, ensure that the turning angle of the path does not exceed the predetermined limit. At the same time, the polynomial fitting method is combined to reduce abrupt turns in the path and improve the smoothness of the path.

[0048] In some specific embodiments, the specific content of S1 is:

[0049] The initial step size is dynamically adjusted according to the scale of the task space and the complexity of the environment.

[0050] The formula for setting the initial step length is as follows:

[0051]

[0052] Where: M is the size of the task space, usually the diagonal length of the task space; ω1 is the environmental complexity coefficient, which reflects the distribution of obstacles in the environment; ω1 can be calculated by the size of the obstacles, the average distance between obstacles, and the number of obstacles; is the average distance between obstacles in the environment; is the average radius of the obstacle.

[0053] During the path expansion process, if the current step size causes a node to collide with an obstacle, the algorithm will adjust the step size and re-perform collision detection.

[0054] The formula for variable step size adjustment is as follows:

[0055] L new =L curr ω2

[0056] Where: L curr is the step size of the current node, ω2 is the step size variation coefficient, and each time it is adjusted, ω2 takes a random value between [0.1, 0.6].

[0057] If the adjusted step size avoids collision, the node will be successfully added to the search tree to further expand the path search. Using a map with 20 obstacles and a size of 100 meters by 100 meters, the starting point is [5, 5] and the target point is [95, 95]. Figure 2 This is the result diagram after adjusting the step size.

[0058] In some specific embodiments, the specific content of S2 is:

[0059] During the sampling process, according to the set bias threshold, the algorithm biases the sampling points towards the target area with a certain probability, thereby enhancing the target orientation of the search; the rules of the target bias sampling strategy determine the sampling strategy based on the generation of random numbers. The specific strategies can be summarized into the following three cases:

[0060] The first case: when the random number is less than the set threshold, the target position is directly used as the sampling point, and the extension path from the nearest node in the current search tree to the target is calculated, and the path is extended to the target position;

[0061] The second case: when the random number is greater than the set threshold but less than the second threshold, five nodes are randomly sampled in the workspace, and then the node closest to the target is selected as the new sampling point for expansion;

[0062] The third case: If the current expansion process fails to successfully generate new nodes, the node sampling is performed in a completely random manner through the Rand function. Figure 3 It is the result diagram after the target bias sampling.

[0063] In some specific embodiments, the specific content of S3 is:

[0064] Starting from the starting point, gradually expand outward to find the shortest path to the target point. By continuously selecting the node with the smallest path cost among the currently unprocessed nodes, the path cost of the adjacent nodes is updated until the optimal path from the starting point to the end point is found. It includes the following three stages:

[0065] Step 1: During the first redundant point removal process, assuming that the line between two nodes on the path will not collide, the two nodes can be directly connected by a straight line. Even if there are several path points in the middle, if this line does not collide, the redundant path points in the middle will be removed.

[0066] Step 2: For the path after the first optimization, interpolate between any adjacent nodes and at fixed intervals to generate more nodes.

[0067] Step 3: By repeating the process in step 1, check whether the connection between each pair of nodes on the path will collide; if there is no collision, the two nodes can be directly connected, removing the redundant path points in the middle. Figure 4 It is the resulting graph after the path is simplified.

[0068] In some specific embodiments, the specific content of S4 is:

[0069] Calculate the curvature of each curve in the path and compare it with the preset maximum curvature threshold. If the curvature exceeds the preset threshold, adjust the path to smooth the turning angles in the path and ensure that the path meets the curvature constraint. The specific steps of the maximum curvature constraint are as follows:

[0070] Step 1: Starting from the second node in the path, calculate the cosine value of the angle between each adjacent node in the path. If the cosine value of the angle is greater than the preset threshold, it means that there is a large turning angle in the path segment and the path needs to be further optimized.

[0071] Step 2: When a node with a large turning angle is detected, determine whether the parent node and child nodes of the node need to be reconnected to the path. If the node is an interpolation node (i.e., an intermediate node generated by an interpolation algorithm), delete the node from the path and restore the original connection relationship of the path.

[0072] Step 3: Continue to perform the above operation, and gradually traverse each node in the path until all turning angles are smoothed, thereby ensuring that there are no large turning angles in the path and the curvature constraint of the path is satisfied.

[0073] Combined with the fifth-order polynomial fitting, the path is interpolated and optimized by the least squares method to further reduce the abrupt turns in the path. The expression of the fifth-order polynomial fitting is:

[0074] P(x)=a0+a1x+a2x 2 +a3x 3 +a4x 4 +a5x 5

[0075] Where P(x) represents a point on the path, x is the path parameter, and a0, a1, a2, a3, a4, and a5 are polynomial coefficients obtained by least squares fitting. Figure 5 It is the result image after the path is smoothed.

[0076] Aiming at the problems of low efficiency, poor path feasibility and insufficient optimization of existing algorithms in complex underwater environments, resulting in waste of computing resources and inappropriate path planning, this paper proposes a UUV path planning method based on improved fast search random tree. This method combines adaptive step size adjustment strategy, target bias sampling strategy and redundant point removal with path optimization to improve the path planning efficiency, convergence speed and path quality of the algorithm in complex underwater environments, ensuring smooth, low-cost path generation that meets the UUV motion requirements.

[0077] The above description only represents a preferred implementation in the field of UUV path planning and does not limit the scope of application of the present invention. The protection scope of the present invention covers more possible implementation methods and technical solutions. The technical solution mentioned is only a specific embodiment based on the principle of the present invention, and not the only form of implementation. It should be pointed out in particular that the protection scope of the present invention far exceeds the above-mentioned specific examples. Within the framework of the basic principles of the present invention, any improvement, change, application or other implementation scheme is within its protection scope. For those skilled in the art, as long as they do not deviate from the core idea of ​​the present invention, any modification or change should be deemed to be within the scope of protection of the present invention. Therefore, the protection scope of the present invention includes a variety of implementation methods, allowing technicians to make appropriate adjustments and innovations according to actual needs without departing from the basic principles of the present invention.

Claims

1. A UUV path planning method based on improved fast search random tree, characterized in that It includes the following steps: S1. Determine the initial step size. Dynamically adjust the initial step size according to the scale of the task space and the complexity of the environment; S2, target bias sampling. During the sampling process, according to the set bias threshold, the algorithm biases the sampling points towards the target area with a certain probability, thereby enhancing the target orientation of the search; S3, redundant point removal. Use Dijkstra algorithm to simplify the path, remove unnecessary turning points, and increase the density of the path by inserting more nodes in the path, thereby increasing the optimization space. Subsequently, redundant point removal is performed again to further simplify the path; S4. Smooth path. By using the maximum curvature constraint, we ensure that the path turning angle does not exceed the preset limit. At the same time, we combine the polynomial fitting method to reduce the abrupt turns in the path and improve the smoothness of the path.

2. The UUV path planning method based on improved fast search random tree according to claim 1 is characterized by: The specific content of S1 is: Dynamically adjust the initial step size according to the scale of the task space and the complexity of the environment; The formula for setting the initial step length is as follows: Where: M is the size of the task space, usually the diagonal length of the task space; ω1 is the environmental complexity coefficient, which reflects the distribution of obstacles in the environment; ω1 can be calculated by the size of the obstacles, the average distance between obstacles, and the number of obstacles; is the average distance between obstacles in the environment; is the average radius of the obstacle; During the path expansion process, if the current step length causes a node to collide with an obstacle, the algorithm will adjust the step length and re-perform collision detection; The formula for variable step size adjustment is as follows: L new =L curr ·ω2 Where: L curr is the step size of the current node, ω2 is the step size change coefficient, and each time it is adjusted, ω2 takes a random value between [0.1, 0.6]; If the adjusted step size avoids collision, the node will be successfully added to the search tree, further expanding the path search.

3. The UUV path planning method based on improved fast search random tree according to claim 1 is characterized by: The specific content of S2 is: During the sampling process, according to the set bias threshold, the algorithm biases the sampling points towards the target area with a certain probability, thereby enhancing the target orientation of the search; the rules of the target bias sampling strategy determine the sampling strategy based on the generation of random numbers. The specific strategies can be summarized into the following three cases: The first case: when the random number is less than the set threshold, the target position is directly used as the sampling point, and the extension path from the nearest node in the current search tree to the target is calculated, and the path is extended to the target position; The second case: when the random number is greater than the set threshold but less than the second threshold, five nodes are randomly sampled in the workspace, and then the node closest to the target is selected as the new sampling point for expansion; The third case: If the new node cannot be successfully generated during the current expansion process, node sampling is performed in a completely random manner using the Rand function.

4. The UUV path planning method based on improved fast search random tree according to claim 1 is characterized by: The specific content of S3 is: Starting from the starting point, gradually expand outward to find the shortest path to the target point. By continuously selecting the node with the smallest path cost among the currently unprocessed nodes, the path cost of the adjacent nodes is updated until the optimal path from the starting point to the end point is found. It includes the following three stages: Step 1: In the first redundant point removal process, assuming that the line between two nodes on the path will not collide, the two nodes can be directly connected by a straight line. Even if there are several path points in the middle, if this line does not collide, the redundant path points in the middle will be removed; Step 2: For the path after the first optimization, interpolate between any adjacent nodes and at fixed intervals to generate more nodes; Step 3: By repeating the process in step 1, check whether the connection between each pair of nodes on the path will collide; if there is no collision, the two nodes can be directly connected, removing the redundant path points in the middle.

5. The UUV path planning method based on improved fast search random tree according to claim 1 is characterized by: The specific content of S4 is: Calculate the curvature of each curve in the path and compare it with the preset maximum curvature threshold. If the curvature exceeds the preset threshold, adjust the path to smooth the turning angles in the path and ensure that the path meets the curvature constraint. The specific steps of the maximum curvature constraint are as follows: Step 1: Starting from the second node in the path, calculate the cosine value of the angle between each adjacent node in the path. If the cosine value of the angle is greater than the preset threshold, it means that the path segment has a large turning angle and the path needs to be further optimized. Step 2: When a node with a large turning angle is detected, determine whether the parent node and child nodes of the node need to be reconnected to the path. If the node is an interpolation node (i.e., an intermediate node generated by an interpolation algorithm), delete the node from the path and restore the original connection relationship of the path; Step 3: Continue to perform the above operation, and gradually traverse each node in the path until all turning angles are smoothed, thereby ensuring that there are no large turning angles in the path and the curvature constraint of the path is satisfied. Combined with the fifth-order polynomial fitting, the path is interpolated and optimized by the least squares method to further reduce the abrupt turns in the path. The expression of the fifth-order polynomial fitting is: P(x)=a0+a1x+a2x 2 +a3x 3 +a4x 4 +a5x 5 Wherein, P(x) represents a point on the path, x is a parameter of the path, and a0, a1, a2, a3, a4, and a5 are polynomial coefficients obtained by least squares fitting.

Images