Dynamic path planning method for cruise missile based on fast optimal random tree

Abstract

The present invention discloses a method for dynamic path planning of cruise missiles based on fast optimal random trees, which can effectively solve the path planning problems with dynamic obstacles and variable start and end points; adopts a regional storage strategy, and accelerates the convergence of the algorithm by regionalizing the storage of obstacles and nodes; adds a local connection method under dynamic obstacles, and based on the local connection method, uses effective path segments to quickly repair blocked paths; designs a balance strategy of exploration and utilization, realizes the balance of sampling new nodes and using nodes on feasible path segments of old trees, and improves the utilization rate of old tree nodes. The method of the present invention still has superior performance in a static environment, has fast capabilities in a dynamic environment with both dynamic obstacles and variable start and end points, and can plan a smoother path, which can better meet the flight requirements of cruise missiles.

Description

Technical Field

[0001] The present invention relates to the technical field of cruise missile path planning in a dynamic environment, and in particular to a cruise missile dynamic path planning method based on a fast optimal random tree. Background Art

[0002] Loitering missiles are unmanned aircraft controlled by remote or automated control systems and capable of performing various missions without human control. Their high efficiency, maneuverability, and low cost have led to their widespread application in various fields, such as agriculture and urban management. The use of loitering missiles not only reduces risks and safety hazards for personnel but also improves work efficiency and promotes social productivity. Given that loitering missiles typically operate in dynamic environments, research on dynamic path planning for loitering missiles is a crucial topic. Grid-based and sampling-based algorithms are popular for dynamic path planning. However, grid-based algorithms, due to their limited resolution, struggle to maintain both accuracy and planning speed. Sampling-based algorithms, such as the RRT algorithm, are widely used due to their strong search capabilities and ability to quickly obtain planned paths in high-dimensional environments. However, current RRT algorithms and their variants primarily focus on environments with dynamic obstacles or variable targets. Current research lacks robust support for environments with both variable start and end points and dynamic obstacles. Summary of the Invention

[0003] The object of the present invention is to provide a method for dynamic path planning of a loitering munition based on a fast optimal random tree.

[0004] The technical solution to achieve the purpose of the present invention is: a method for dynamic path planning of a loitering munition based on a fast optimal random tree, comprising the following steps:

[0005] Step 1: Build a map of the environment where the cruise missile is located, including known obstacle information, the cruise missile's starting point and target point;

[0006] Step 2: Initialize the random tree ;

[0007] Step 3: Use regional storage strategy to pre-store obstacles and regionalize the storage tree Node;

[0008] Step 4: Update the obstacle position, update the current position of the cruise missile, and update the tree The root node and target node;

[0009] Step 5: Align the Tree The K-step path or feasible path in the process is judged to determine whether the path is broken. If it is broken, go to step 6, otherwise, go to step 7;

[0010] Step 6: Repair the path based on the local connection strategy;

[0011] Step 7: Align the Tree Search to determine whether a feasible path is found. If so, go to step 8; otherwise, go to step 9.

[0012] Step 8: Based on the balancing strategy, reconnect the nodes on the feasible path;

[0013] Step 9: Sample new nodes in the space and perform collision detection on the new nodes. If there is no collision, reconnect the new nodes. Otherwise, go to step 10.

[0014] Step 10: Reconnect the root node and move the cruise missile along the K-step path or feasible path;

[0015] Step 11: Determine whether the cruise missile has reached the target point. If it has, end; otherwise, go to step 4.

[0016] Preferably, in step 1, the environment is scanned by a laser radar and point cloud data is obtained and processed. A two-dimensional map of the environment is generated; the generated map is used express, It is an obstacle area. It is a free area without obstacles. The initial point of the cruise missile is express, , the target point of the cruise missile is used express, .

[0017] Preferably, in step 2, initialize the random tree , a tree consists of nodes and edges, where , , ,as well as .

[0018] Preferably, in step 3, the regional storage strategy is used to pre-store obstacles and regionalize the storage tree. The specific method of the node is:

[0019] First, the environment is regionalized, and then obstacles are pre-stored in each region. At the same time, during the tree growth process, the nodes in the tree are regionalized and stored, thus reducing the time spent on searching for the nearest neighbor nodes and nodes in the neighborhood.

[0020] Preferably, in step 4, the position of the detected new obstacle or the moving obstacle is updated, thereby updating the entire map; at the same time, the position of the cruise missile in the random tree needs to be updated. Position in ; Finally, update the tree The root node , and the target node .

[0021] Preferably, in step 5, the implementation process is:

[0022] Pair of Trees The K-step path or feasible path in the tree is judged, where the K-step path is to find the path composed of the K points closest to the target point in the tree; determine whether the path is blocked by an obstacle, that is, intersecting with the obstacle. If it is broken, go to step 6, otherwise, go to step 7;

[0023] Preferably, in step 6, the path destroyed in step 5 is divided into two segments: a front segment of the path and a back segment of the path; the nodes on the two segments are directly connected in sequence and collision detection is performed, thereby reconnecting them.

[0024] Preferably, in step 7, in the tree Find a feasible path from the current position of the cruise missile to the target point. If there is an obstacle-free path, go to step 8; otherwise, go to step 9.

[0025] Preferably, in step 8, nodes on the feasible path are reconnected based on the balancing strategy. The balancing formula is as follows:

[0026] Formula (1)

[0027] in, It indicates the ratio of the number of nodes on the feasible path that have reconnected with neighboring nodes to the total number of nodes on the feasible path. When , the nodes on the feasible path are reconnected to optimize the path.

[0028] Preferably, in step 9, let the target point Become a sampling point with a preset probability Otherwise, uniform sampling is performed , when the sampling point is obtained Then, find the nearest neighbor node in the tree ,from Start with a fixed step size Growth, thereby further generating new nodes .right Perform collision detection. If there is no collision, it means that the sampling is successful, and the parent node is reselected and the node is reconnected. Then, the new node is added to the tree. In the middle, Join Otherwise, go to step 10.

[0029] Preferably, in step 10, the root node is reconnected, and the nodes in the neighborhood are searched with the root node as the center and a fixed length r as the radius. The parent node is reselected for the nodes in the neighborhood. Find a feasible path from the cruise missile's current position to the target point. If there is no feasible path, find the node closest to the target node and backtrack to the parent node until K nodes starting from the root node are found, forming a K-step path. Move the cruise missile forward along the K-step path or feasible path.

[0030] Preferably, in step 11, it is determined whether the cruise missile has reached the target point. If it has, the path search ends. Otherwise, it is necessary to continue growing and repeat steps 4 to 11.

[0031] Compared with the existing technology, the significant advantages of the present invention are: proposing a regional storage strategy for dynamic environments, which reduces the time-consuming number of collision detections by regionalizing the storage of obstacles, and at the same time regionalizing the storage tree to reduce the spatial search time of nodes; adding a local connection method under dynamic obstacles, thereby taking into account the real-time path planning under target variables and dynamic obstacle scenarios; designing a balanced strategy of exploration and utilization, achieving a balance between sampling new nodes and utilizing nodes on feasible path segments of the old tree, thereby improving the utilization rate of old tree nodes.

[0032] The present invention is described in further detail below with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 This is a flow chart of the present invention's method for dynamic path planning of a loitering munition based on a fast optimal random tree.

[0034] Figure 2 This is a process diagram of the present invention based on the regional storage strategy.

[0035] Figure 3 This is a diagram of the path optimization process based on the balance strategy of the present invention, where the left figure is the figure before optimization, and the right figure is the figure after node reconnection optimization.

[0036] Figure 4 The cost curves of the method of the present invention, the RRT* algorithm, the RT-RRT* algorithm, and the RRT*FND algorithm under different numbers of obstacles are shown.

[0037] Figure 5The graphs are time curves of the method of the present invention, the RRT* algorithm, the RT-RRT* algorithm, and the RRT*FND algorithm under different numbers of obstacles.

[0038] Figure 6 Iterative curve diagram of the method of the present invention, RRT* algorithm, RT-RRT* algorithm, and RRT*FND algorithm in a dynamic obstacle environment.

[0039] Figure 7 This is a time box plot of the method of the present invention, the RRT* algorithm, the RT-RRT* algorithm, and the RRT*FND algorithm in a dynamic obstacle environment.

[0040] Figure 8 It is an iterative curve diagram of the method of the present invention and the RRT* algorithm, RT-RRT* algorithm, and RRT*FND algorithm under a variable start and end point environment.

[0041] Figure 9 It is a time box plot of the method of the present invention and the RRT* algorithm, RT-RRT* algorithm, and RRT*FND algorithm under a variable start and end point environment.

[0042] Figure 10 It is an iterative graph of the method of the present invention and the RRT* algorithm, RT-RRT* algorithm, and RRT*FND algorithm in an environment with dynamic obstacles and variable start and end points.

[0043] Figure 11 This is a time box plot of the method of the present invention and the RRT* algorithm, RT-RRT* algorithm, and RRT*FND algorithm in an environment with dynamic obstacles and variable start and end points. DETAILED DESCRIPTION

[0044] The present invention will be further described below with reference to the embodiments, but the scope of protection of the present invention is not limited thereto:

[0045] like Figure 1 As shown in FIG, a dynamic path planning method for a cruise missile based on a fast optimal random tree includes the following steps:

[0046] In step 1, the environment is scanned by LiDAR and point cloud data is obtained and processed to generate a 2D map of the environment. express, It is an obstacle area. It is a free area without obstacles. The initial point of the cruise missile is express, , considering that there are multiple target points in this paper, the target point of the cruise missile is and express, .

[0047] In step 2, initialize the random tree , a tree consists of nodes and edges, where , , ,as well as .in The nodes on the feasible path are stored in the network, and the nodes in the neighborhood of the node have been reconnected.

[0048] In step 3, use the regional storage strategy to pre-store obstacles and regionalize the storage tree The specific method of the node is:

[0049] First, the environment is regionalized, and then obstacles are pre-stored. At the same time, during the tree growth process, the nodes in the tree are regionalized and stored, thereby reducing the time spent on searching for the nearest neighbor nodes and nodes in the neighborhood. The schematic diagram of the regional storage strategy is as follows Figure 2 As shown in the figure, the environment map is divided into 16 areas. The areas where obstacles are located are recorded in advance.

[0050] When the tree is growing, a random point is sampled . At this time, you need to search the distance in the tree The nearest point. First find The area where it is located is Area A. Then search for the distance between the points in Area A The nearest point.

[0051] Getting a new node After that, collision detection is needed to determine whether the node can be added to the tree. First find The area where it is located is Area B. Then look for obstacles in Area B. Next, you only need to judge Is it within an obstacle, and and Whether the line connecting the two objects intersects with the obstacle.

[0052] In step 4, the position of the detected new obstacle or the moving obstacle is updated, thereby updating the entire map. At the same time, the position of the cruise missile in the random tree needs to be updated. Position in Finally, update the tree The root node , and the target node .

[0053] In step 5, the implementation process is:

[0054] Pair of Trees The K-step path or feasible path in the tree is determined. The K-step path is a path consisting of the K points closest to the target point in the tree. Determine whether the path is blocked by an obstacle, that is, if it intersects with the obstacle, go to step 6, otherwise go to step 7;

[0055] In step 6, the path destroyed in step 5 is divided into two segments: the front segment, which is the valid path segment near the cruise missile, and the back segment, which is the valid path segment near the target point. The nodes on these two segments are directly connected and then collided with each other to reconnect them. If a valid path segment cannot be reconnected, a regrowth method is used, using the nodes in the back segment of the valid path as offset points to grow the tree.

[0056] In step 7, in the tree Find a feasible path from the current position of the cruise missile to the target point. If there is an obstacle-free path, go to step 8; otherwise, go to step 9.

[0057] In step 8, based on the balancing strategy, nodes on the feasible path are reconnected. Nodes in the neighborhood of the feasible path are more likely to reduce path costs and should be prioritized. However, the potential for new nodes to reduce path costs cannot be ignored. Therefore, a balance is established between these two strategies, using the following formula:

[0058] Formula (1)

[0059] in, It indicates the ratio of the number of nodes on the feasible path that have reconnected with neighboring nodes to the total number of nodes on the feasible path. When , perform node operations on the nodes on the feasible path, such as Figure 3 As shown. First, randomly select a node on the feasible path. Then, with the node as the center and R as the radius, select the nodes in the circle as the neighborhood nodes. Among them, R is greater than the expansion step. Then, the neighborhood nodes are reselected as parent nodes. The optimization results are as follows Figure 3 shown.

[0060] In step 9, let the target point Become a sampling point with a preset probability Otherwise, uniform sampling is performed When the sampling point is obtained Then, find the nearest neighbor node in the tree ,from Start with a fixed step size Growth, thus generating new nodes .right Perform collision detection. If there is no collision, it means that the sampling is successful, and the parent node is reselected and the node is reconnected. As the center of the circle, with a fixed length r as the radius, find a better parent node in the neighborhood. And reselect the parent node for all nodes in the neighborhood. Then, add the new node to the tree In the middle, Join Otherwise, go to step 10.

[0061] In step 10, the root node is reconnected, and the nodes in the neighborhood are searched with the root node as the center and a fixed length r as the radius. The parent node of the nodes in the neighborhood is reselected, and the node is modified. , to generate better trees In the tree Find a feasible path from the cruise missile's current position to the target point. If there is no feasible path, find the node closest to the target node and backtrack to the parent node until K nodes starting from the root node are found, forming a K-step path. Move the cruise missile forward along the K-step path or feasible path.

[0062] In step 11, it is determined whether the cruise missile has reached the target point. If it has, the path search ends. Otherwise, it is necessary to continue growing and repeat steps 4 to 11.

[0063] Example

[0064] In order to verify the performance of the method of the present invention, the algorithm performance was tested in static and dynamic environments. In the static environment, all obstacles are stationary and there are no variable starting and ending points, but the number of obstacles increases in sequence. The RRT* algorithm, RT-RRT* algorithm, RRT*FND algorithm and the present method were used to plan the path of the cruise missile respectively. The results are shown in the figure below. Figure 4 and Figure 5 As shown in the figure, the dynamic environment is set up with three cases: dynamic obstacles only, variable start and end points only, and both dynamic obstacles and variable start and end points. In the dynamic environment, the RRT* algorithm, RT-RRT* algorithm, RRT*FND algorithm and this method are used to plan the path of the cruise missile respectively. The results are shown in the figure. Figures 6-11 As shown. Figure 4 and 5 It can be seen that the regional storage obstacle strategy can greatly reduce the impact of the number of obstacles on the algorithm, maintaining the algorithm's rapid convergence and path optimization capabilities. Figure 6 and Figure 7 It can be seen that this method can quickly optimize the path within 3 seconds, and the optimized path solution is lower than other curves. Figure 8 and Figure 9It can be seen that the curve of this method is below the other three curves, which shows that the algorithm can maintain high quality of the path while maintaining speed. Figure 10 and Figure 11 As can be seen, this method maintains its ability to quickly find an initial path, and the path quality is superior to other algorithms. The method of the present invention maintains superior performance in static environments, and is fast in dynamic environments with both dynamic obstacles and variable starting and ending points. It can also plan a smoother path, better meeting the flight requirements of loitering munitions.

[0065] The specific embodiments described herein are merely illustrative of the spirit of the present invention. Persons skilled in the art may make various modifications, additions, or substitutions to the described specific embodiments without departing from the spirit of the present invention or exceeding the scope of the appended claims.

Claims

1. A method for dynamic path planning of a cruise missile based on a fast optimal random tree, characterized in that: The following steps are involved: Step 1: Build an environment map where the cruise missile is located, including known obstacle information, the starting point and target point of the cruise missile; Step 2: Initialize the random tree ; Step 3: Use the regional storage strategy to pre-store obstacles and regionalize the storage tree The specific method is: First, the environment is regionalized, and then obstacles are pre-stored in each region; during the growth of the tree, the nodes in the tree are regionalized and stored; Step 4: Update the obstacle position, update the current position of the cruise missile, and update the tree The root node and the target node; Step 5: Align the Tree The K-step path or feasible path in the process is judged to determine whether the path is destroyed. If so, go to step 6; otherwise, go to step 7. Step 6: Repair the path based on the local connection strategy; Step 7: Align the Tree Search to determine whether a feasible path is found. If found, go to step 8; otherwise, go to step 9; Step 8: Based on the balancing strategy, reconnect the nodes on the feasible path. The balancing formula is as follows: Formula (1) in, It indicates the ratio of the number of nodes on the feasible path that have reconnected with neighboring nodes to the total number of nodes on the feasible path; when the random number is less than When , reconnect the nodes on the feasible path to optimize the path; Step 9: Sample new nodes in the space and perform collision detection on the new nodes. If there is no collision, reconnect the new nodes. Otherwise, go to step 10. Step 10: Reconnect the root node and move the cruise missile along the K-step path or feasible path; Step 11: Determine whether the cruise missile has reached the target point. If it has, end; otherwise, go to step 4.

2. The method according to claim 1, characterized in that In step 1, the environment is scanned by laser radar to obtain point cloud data, and the point cloud data is processed; a two-dimensional map of the environment is generated; and the generated map is used express, It is an obstacle area. It is a free area without obstacles; the initial point of the cruise missile is express, , the target point of the cruise missile is express, .

3. The method according to claim 2, characterized in that In step 2, initialize the random tree , a tree consists of nodes and edges, where , , ,as well as ,in The nodes on the feasible path are stored in the memory.

4. The method according to claim 1, characterized in that: In step 4, the position of the detected new obstacle or moving obstacle is updated, thereby updating the entire map; at the same time, the position of the cruise missile in the random tree needs to be updated. Position in ; Finally, update the tree The root node , and the target node .

5. The method according to claim 1, characterized in that In step 5, The K-step path or feasible path in the tree is used to judge, where the K-step path is to find the path composed of the K points closest to the target point in the tree.

6. The method according to claim 1, characterized in that In step 6, the path destroyed in step 5 is divided into two sections: the front section of the path and the back section of the path; the nodes on the two sections of the path are directly connected in sequence and collision detection is performed to reconnect them.

7. The method according to claim 1, characterized in that In step 9, let the target point Become a sampling point with a preset probability , otherwise uniform sampling is performed , when the sampling point is obtained Then, find the nearest neighbor node in the tree ,from Start with a fixed step size Growth, thus generating new nodes ;right Perform collision detection. If there is no collision, the sampling is successful, and the parent node is reselected and the node is reconnected. Then, the new node is added to the tree. In the middle, the side Join middle.

8. The method according to claim 1, characterized in that In step 10, the root node is reconnected, and the nodes in the neighborhood are searched with the root node as the center and a fixed length r as the radius; the parent node of the nodes in the neighborhood is reselected; Find a feasible path from the current position of the cruise missile to the target point. If there is no feasible path, find the node closest to the target node and trace back to the parent node until K nodes starting from the root node are found to form a K-step path; move the cruise missile forward along the K-step path or the feasible path.

Images