Ground unit real-time planning method for improving incremental heuristic search
By introducing a value node reproduction mechanism and an improved LPA* algorithm into the road network topology map, the efficiency and timeliness of path planning in complex road network environments are solved, and real-time, feasible and optimal ground unit path planning is achieved. Improved the survival and coordination capabilities of ground units.
Patent Information
- Application Number
- CN202411177033.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-26
- Publication Date
- 2024-12-06
AI Technical Summary
In complex road network environment, traditional graph search algorithms are difficult to meet the needs of real-time path planning, resulting in low planning efficiency and poor timeliness. Especially due to the complexity and irregularity of nodes and paths in the road network topology map, closed-loop invalid search results, which leads to low-loop , affects the feasibility and optimization of searches.
Introduce a value node reproduction mechanism, filter and refine the previous round of prior information, combine traditional LPA* algorithm, and build an improved incremental heuristic search algorithm AR-LPA* to avoid closed-loop invalid search, and provide prior information to support in a dynamic environment Real-time path planning ensures speed and optimality.
It significantly improves the real-time, feasibility and optimization of ground unit path planning in complex road network environments, ensures the safe transfer and threat avoidance maneuverability of ground units in dynamic environments, and improves survivability and coordination capabilities.
Smart Images

Figure CN119085674A_ABST
Abstract
Description
Technical Field
[0001] The invention discloses a ground unit real-time planning method for improving incremental heuristic search, belonging to the technical field of path planning. Background Art
[0002] With the continuous development of high-tech, ground units, as typical vehicles and ground robots in the civilian field, are facing increasingly severe survival problems. The ground unit transfer and deployment path planning technology is a path planning technology that ensures that the ground unit can respond to incoming threats, thereby improving the survivability of the ground unit. The ground unit transfer and deployment path planning technology under the actual road network can be extended to the path planning technology under the actual road network. This technology is a key technology for the transfer and deployment path planning of the ground unit under the actual road network. By introducing relevant technologies in the field of autonomous driving, the ground unit can be safely transferred and threat avoidance maneuvers can be achieved, thereby improving the survivability and coordination capabilities of the ground unit. As for the path planning problem under the actual road network, the path planning technology under the road network faces technical difficulties such as low path planning efficiency and poor real-time performance caused by the dynamic environment with high time variation and strong response and the diverse and changing needs. Therefore, how to achieve fast, optimal and feasible ground unit path planning in a dynamic environment has become a key problem that needs to be solved urgently.
[0003] At present, the path planning algorithms used in this scenario mainly adopt graph search path planning and intelligent search algorithms. In the case of path planning problems in actual road network environments, intelligent search algorithms are prone to high learning costs and poor robustness, which makes them unfavorable for solving real-time planning problems in complex situations; graph search path planning algorithms have simple solution principles and good real-time expansion capabilities, and can solve the problem of fast global optimal / suboptimal paths in low-dimensional simple maps. Therefore, compared with other algorithms, graph search algorithms can take into account both optimality and algorithm efficiency in the search process for path planning problems in actual road networks, and have good real-time performance, feasibility and engineering applicability for path planning problems in road networks. However, traditional graph search algorithms are difficult to meet the application requirements of real-time path planning in complex road network environments. Considering that the LPA* algorithm is a heuristic incremental search algorithm proposed for grid maps, it provides prior information for re-planning searches through an incremental mechanism based on the A* algorithm, thereby ensuring good timeliness. However, unlike raster maps, the complexity and irregularity of nodes and paths in the road network topology map information cause closed-loop invalid searches, which reduces the feasibility and optimality of the search and makes it impossible to guarantee the re-planning capability, resulting in low planning efficiency and poor timeliness under the road network topology map.
[0004] In summary, considering the adaptability of the LPA* algorithm to the road network, and introducing the value node reproduction mechanism to screen and refine the previous round of prior information, a feasible path endpoint is given to avoid invalid closed-loop search, and the optimality is greatly improved while ensuring speed, thereby achieving real-time, feasible, and optimal ground unit path planning in a complex road network environment. Summary of the invention
[0005] In order to solve the problem of real-time planning of ground units under road network topology maps, the purpose of the present invention is to provide a real-time planning method for ground units with improved incremental heuristic search. Considering the difficulty that the closed-loop invalid search of traditional algorithms in dynamic environments leads to a decrease in search feasibility and optimality and cannot guarantee the re-planning ability, a value node reproduction mechanism is introduced to screen and refine the prior information of the previous round, provide prior information for the re-planning search to promote the dynamic environment search, and provide a feasible solution to avoid closed-loop invalid search, so as to realize real-time ground unit path planning in complex road network environments, ensure the safe transfer and threat avoidance maneuver of ground units, and enhance the survivability and coordination capabilities of ground units.
[0006] The objective of the present invention is achieved through the following technical solutions:
[0007] The present invention discloses a real-time planning method for ground units with improved incremental heuristic search, which aims at the real-time path planning problem of complex road network topology map, and carries out modeling of ground unit planning problem under road network environment and improved ground unit real-time planning method with improved incremental heuristic search. Considering the closed-loop invalid search caused by the complexity and irregularity of nodes and paths in the topology map information under complex road network environment, the ground unit planning problem under the road network topology map is constructed: for the road network road model, the spatial topological structure relationship of the traffic road network is analyzed, the road network map is abstracted into a road network road model based on paths and road intersection nodes, and an adjacency matrix is used to store the road data of the road network map, and planning costs and task settings are performed to provide model support for the follow-up; in order to realize the real-time planning of ground units in a high dynamic environment, the real-time planning method for ground units with improved incremental heuristic search is modeled, the traditional LPA* algorithm is applied to the road network environment, and the value node reproduction mechanism is combined with the traditional LPA* algorithm to obtain a customized AR-LPA* algorithm. Based on the above-mentioned ground unit planning problem model under the road network topology map and the ground unit real-time planning method model with improved incremental heuristic search, the planning optimality is significantly improved while ensuring the rapidity of planning, and real-time, feasible and optimal ground unit path planning is achieved in a complex road network environment.
[0008] The present invention discloses a ground unit real-time planning method with improved incremental heuristic search, comprising the following steps:
[0009] Step 1: Considering the road network topology map environment information and combining the constraints of the ground unit and the road network environment information, establish a ground unit planning problem model under the road network environment;
[0010] The specific implementation method of step 1 is as follows:
[0011] Step 1.1: Road network topology map modeling;
[0012] The road network model is described by using the data storage structure of the road network node adjacency matrix; the adjacency matrix constructs the adjacency relationship and weight between each vertex in the graph in matrix form; let the model G = (N, R, f), where R represents the connection relationship, N is the number of vertices, and f is the path cost function. At this time, the adjacency matrix of the model G is represented by an N×N matrix, as shown in Table 1;
[0013] Table 1 Adjacency matrix diagram
[0014]
[0015] Step 1.2: Set up node cost functions and tasks;
[0016] The node cost function includes the forward-looking current cost rhs(s), the historical current cost g(s), and the heuristic cost h(s), which can be specifically expressed as:
[0017]
[0018] Where s represents any node, s' represents the parent node of node s, Pre(s) is the parent node set of node s, and s start As the starting point, s goal As the end point, * (s',s) represents the path cost from parent node s' to node s, with the Euclidean distance between the two nodes as the path cost;
[0019] The planning task setting mainly considers the planning capabilities between fixed starting points and end points;
[0020] Step 2: Initialize the road network topology map model and initialize the ground unit planning problem of the road network topology map;
[0021] Step 2.1: The road network topology map model initialization information includes: the location of the road intersection nodes and the Euclidean distance between the nodes;
[0022] Step 2.2: Set the starting point coordinates and the end point coordinates. Set the starting point as the exploration node, and the parent node of the starting point is the starting point itself.
[0023] Step 3: Solve the optimal path under the ground unit planning problem of the current road network topology map;
[0024] The specific implementation method of step 3 is as follows:
[0025] Step 3.1: Ensure that the priority of the top node in the current priority queue U is less than the priority of the end point under the current exploration, or ensure that g(u)≠rhs(u) at this time. If the above conditions are met, go to step 3.2; if not, go to step 3.5;
[0026] Step 3.2: Put the explored node into the priority queue U;
[0027] Step 3.3: If g(u)>rhs(u) of the explored node, set g(u)=rhs(u);
[0028] Step 3.4: Find all child nodes of the exploration node, set the parent node of all child nodes as the exploration node, and update all child nodes. After all updates are completed, take out the top node of the current priority queue U as the exploration node and jump back to step 3.1;
[0029] Step 3.5: Store the destination of the current exploration into the path set, find the parent node of the destination of the current exploration, store it into the path set, and continuously recurse the parent node and store the parent node into the path set; when the last parent node is the starting point, it is judged that the planning is successful, if not the starting point, the planning fails; jump to step 2 to reset the starting point and end point settings;
[0030] Step 4: When the road network topology map information is updated, rapid replanning iteration is performed to achieve real-time planning of ground units in a complex road network environment, and obtain real-time planning results of ground units in a complex road network environment.
[0031] If the current optimal path changes after the current round of environmental changes, with the help of the value node reproduction mechanism, the cost of the path end point in the previous round of planning is reset and re-added to the priority list U, and then step 3 is restarted to solve the optimal path; if the current optimal path is not affected by the current round of environmental changes, the optimal path in the previous round is still used as the optimal path in this round; fast replanning realizes real-time planning of ground units, and iteratively obtains real-time planning results of ground units in complex road network environments.
[0032] The specific steps are as follows:
[0033] Step 4.1: Check whether the change in the road network topology map information caused by the environmental change is the current optimal path; if the current optimal path is changed, proceed to step 4.2; if the current optimal path is not affected, the optimal path of the previous round is still used as the optimal path of this round;
[0034] Step 4.2: Update all intersection nodes where data has changed;
[0035] Step 4.3: Find all value nodes s that meet the conditions* , and add all value nodes to the priority queue U;
[0036] Step 4.3.1: Find s * ; said * Must meet:
[0037]
[0038] Among them, Path(s * ) represents the set of path points ending with node s*, and Succ(s*) is the set of child nodes of node s*;
[0039] Step 4.3.2: Find the s * Perform information processing of formula (3):
[0040] g(s * )=rhs(s * )=∞ (3)
[0041] Step 4.3.3: The processed s * Join the priority queue U;
[0042] Step 4.4: Go to step 3 and perform rapid re-planning iteration to realize real-time planning of ground units in a complex road network environment, and obtain the real-time planning results of ground units in a complex road network environment.
[0043] Based on the real-time planning results of the ground unit in the complex road network environment obtained in step 4, the safe transfer and threat avoidance maneuvers of the ground unit are ensured, thereby improving the survivability and coordination capabilities of the ground unit.
[0044] The method for updating the nodes in step 3 and step 4 is as follows:
[0045] Step ①: Determine whether the current node is the starting point; if so, skip this step; if not, calculate the current forward-looking current cost rhs value;
[0046] Step ②: Determine whether the current node is in the priority queue U; if so, remove the node from the priority queue U; if not, skip this step;
[0047] Step 3: Determine whether the forward-looking current cost rhs value of the current node is equal to the historical current cost g value; if not, calculate the priority of the node and add it back to the priority queue U; if equal, skip this step;
[0048] Step ④: Iterate steps ① to ③ to implement node update.
[0049] Beneficial effects:
[0050] 1. The present invention discloses a method for real-time planning of ground units with improved incremental heuristic search, which aims at the real-time path planning problem under the topological map of a complex road network, and carries out modeling of the ground unit planning problem under the road network topological map. Considering the closed-loop invalid search caused by the complexity and irregularity of nodes and paths in the topological map information in the complex road network environment, a road network topological map model is constructed, and planning cost definition and task setting are performed, so as to provide a precise ground unit planning problem model under the road network environment for subsequent real-time planning of ground units, and facilitate path planning.
[0051] 2. The present invention discloses an improved incremental heuristic search method for real-time planning of ground units. Aiming at the real-time path planning problem under complex road network topology maps, the improved incremental heuristic search method for real-time planning of ground units is modeled. In order to achieve real-time planning of ground units in a highly dynamic environment, the traditional LPA* algorithm is applied to the road network environment, and the value node reproduction mechanism is combined with the traditional LPA* algorithm to obtain a customized AR-LPA* algorithm. The optimality of planning is greatly improved under the premise of ensuring the rapidity of planning, and the real-time, feasible and optimal path planning is achieved under the complex road network environment. BRIEF DESCRIPTION OF THE DRAWINGS
[0052] Figure 1 It is a schematic diagram of node-path relationship;
[0053] Figure 2 A real-time path planning flow chart under a complex situation road network topology map disclosed by the present invention;
[0054] Figure 3 Initialized map information;
[0055] Figure 4 Simulation results for the initial planning of the algorithm;
[0056] Figure 5 Re-plan the simulation results for the algorithm. DETAILED DESCRIPTION
[0057] In order to better illustrate the purpose and advantages of the present invention, the present invention is further described below by taking the ground unit path planning example under the road network topology map in combination with the accompanying drawings and tables. Figure 2 .
[0058] Embodiment 1:
[0059] The simulation hardware is Intel Core i5-6200 CPU 2.30GHz, 8G memory, and the simulation environment is MATLABR2023b.
[0060] like Figure 1As shown, the present embodiment discloses a method for real-time planning of ground units with improved incremental heuristic search, and the specific implementation steps are as follows:
[0061] In order to simulate the real road network environment and verify the effectiveness of the deployed algorithm, the road network map range is set to 250km×250km.
[0062] Step 1: Modeling the ground unit planning problem under the road network topology map;
[0063] Considering the road network topology map environment information, and combining the constraints of ground units and road network environment information, a ground unit planning problem model under the road network environment is established;
[0064] The specific implementation method of step 1 is as follows:
[0065] Step 1.1: Road network topology map modeling;
[0066] The road network model is described using the data storage structure of the road network node adjacency matrix; the adjacency matrix constructs the adjacency relationship and weight between each vertex in the graph in matrix form. The specific schematic diagram is as follows Figure 1 As shown; let model G = (N, R, f), where R represents the connection relationship, N is the number of vertices, and f is the path cost function. At this time, the adjacency matrix of model G is represented by an N×N matrix, as shown in Table 1;
[0067] Step 1.2: Set up node cost functions and tasks;
[0068] The node cost function includes the forward-looking current cost rhs(s), the historical current cost g(s), and the heuristic cost h(s), which is specifically expressed as shown in formula (1).
[0069] The algorithm planning tasks are: mainly considering the planning capabilities between fixed starting and end points;
[0070] Step 2: Initialize the road network topology map model and initialize the ground unit planning problem of the road network topology map;
[0071] The specific implementation method of step 2 is as follows:
[0072] Step 2.1: The road network topology map model initialization information includes: the location of the road intersection nodes and the Euclidean distance between the nodes. Specific information such as Figure 3 As shown;
[0073] Step 2.2: Set the starting point and end point. Set the starting point as the exploration node, and the parent node of the starting point is the starting point itself. Figure 3 As shown;
[0074] Step 3: Solve the optimal path under the ground unit planning problem of the current road network topology map;
[0075] The AR-LPA* algorithm determines the node status generally in the following situations:
[0076] (1) When g(s) = rhs(s), the node status is locally consistent, which means that after the current node s is updated, the current cost before and after is consistent, and both can correspond to the shortest path from the starting point to this point.
[0077] (2) When g(s) > rhs(s), the node status is locally over-consistent, which means that after the current node s is updated, the prospective current cost is less than the historical current cost, indicating that there is a shorter path from the starting point to this point.
[0078] (3) When g(s) < rhs(s), the node status is locally under-consistent, which means that after the current node s is updated, the prospective current cost is greater than the historical current cost, resulting in the invalidation of the shortest path from the starting point to this point.
[0079] The AR-LPA* algorithm uses the priority queue U as the node expansion criterion. This queue is sorted by the two-dimensional [k1(s), k2(s)], where the two-dimensional queue is specifically represented as:
[0080]
[0081] Among them, k1(s) is the most complete node cost, and its priority is higher than k2(s). Specifically, when a node is added to the priority list U, the k1(s) value is compared first. Only when the k1(s) values are the same, the k2(s) value is further compared.
[0082] The specific implementation method of step 3 is as follows:
[0083] Step 3.1: Ensure that the priority of the node at the top of the current priority queue U is less than the priority of the end point under the current exploration, or ensure that g(u) ≠ rhs(u) at this time. When the above conditions are met, go to step 3.2; if not, go to step 3.5;
[0084] Step 3.2: Put the exploration node into the priority queue U;
[0085] Step 3.3: If g(u) > rhs(u) for the exploration node, then set g(u) = rhs(u);
[0086] Step 3.4: Find all the child nodes of the exploration node, set the parent node of all child nodes to be the exploration node, and perform node updates on all child nodes. After all updates are completed, take out the node at the top of the current priority queue U as the exploration node, and jump back to step 3.1;
[0087] Step 3.5: Store the destination of the current exploration into the path set, find the parent node of the destination of the current exploration, store it into the path set, and continuously recurse the parent node and store the parent node into the path set; when the last parent node is the starting point, it is judged that the planning is successful, if not the starting point, the planning fails; jump to step 2 to reset the starting point and end point settings;
[0088] Step 4: When the road network topology map information is updated, perform rapid replanning;
[0089] If the current optimal path changes after the current environmental changes, the algorithm uses the path endpoint recurrence mechanism to reset the cost of the path endpoint in the previous round of planning and re-add it to the priority list U, and then restart step 3 to solve the optimal path; if the current optimal path is not affected by the current environmental changes, the optimal path in the previous round is still used as the optimal path in this round;
[0090] The specific steps are as follows:
[0091] Step 4.1: Check whether the change in the road network topology map information caused by the environmental change is the current optimal path; if the current optimal path is changed, proceed to step 4.2; if the current optimal path is not affected, the optimal path of the previous round is still used as the optimal path of this round;
[0092] Step 4.2: Update all intersection nodes where data has changed;
[0093] Step 4.3: Find all value nodes s that meet the conditions * , and add all value nodes to the priority queue U;
[0094] Step 4.3.1: Find s * ; said * The condition of formula (2) must be met;
[0095] Step 4.3.2: Find the s * Perform information processing of formula (3):
[0096] Step 4.3.3: The processed s * Join the priority queue U;
[0097] Step 4.4: Go to step 3;
[0098] Situation changes such as Figure 5 As shown, the algorithm ends and the optimal path is obtained. Figure 4 , Figure 5 shown.
[0099] The simulation results of ground unit real-time planning based on improved incremental heuristic search are shown below. This table shows that the LPA* algorithm can adapt to ground unit path planning in a complex road network environment, and through the traceability mechanism, AR-LPA* can achieve fast, efficient, and optimal ground unit real-time path planning. In summary, the results show that this method can generate fast and accurate ground unit real-time planning.
[0100] Table 2 Algorithm performance evaluation under tasks
[0101]
[0102] According to the simulation results and analysis of the aforementioned ground unit path planning example, it can be seen that the path planning method described in this embodiment can provide the ground unit with a feasible path taking into account a complex road network environment, and the path generation speed is fast and efficient. Therefore, the present invention has strong engineering practicality and can achieve the intended purpose of the invention.
[0103] The above specific description is a further detailed explanation of the purpose, technical solutions and beneficial effects of the invention. It should be understood that the above is only a specific implementation example of the present invention, which is only used to explain the present invention and is not used to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.
Claims
1. A real-time planning method for ground units with improved incremental heuristic search, characterized in that: The following steps are included: Step 1: Considering the road network topology map environment information and combining the constraints of the ground unit and the road network environment information, establish a ground unit planning problem model under the road network environment; Step 1.1: Road network topology map modeling; The road network model is described by using the data storage structure of the road network node adjacency matrix; the adjacency matrix constructs the adjacency relationship and weight between each vertex in the graph in matrix form; let the model G = (N, R, f), where R represents the connection relationship, N is the number of vertices, and f is the path cost function. At this time, the adjacency matrix of the model G is represented by an N×N matrix, as shown in Table 1; Table 1 Adjacency matrix diagram Step 1.2: Set up node cost functions and tasks; The node cost function includes the forward-looking current cost rhs(s), the historical current cost g(s), and the heuristic cost h(s), which can be specifically expressed as: Where s represents any node, s' represents the parent node of node s, Pre(s) is the parent node set of node s, and s start As the starting point, s goal As the end point, * (s',s) represents the path cost from parent node s' to node s, with the Euclidean distance between the two nodes as the path cost; The planning task setting mainly considers the planning capabilities between fixed starting points and end points; Step 2: Initialize the road network topology map model and initialize the ground unit planning problem of the road network topology map; Step 2.1: The road network topology map model initialization information includes: the location of the road intersection nodes and the Euclidean distance between the nodes; Step 2.2: Set the starting point coordinates and the end point coordinates. Set the starting point as the exploration node, and the parent node of the starting point is the starting point itself. Step 3: Solve the optimal path under the ground unit planning problem of the current road network topology map; Step 3.1: Ensure that the priority of the top node in the current priority queue U is less than the priority of the end point under the current exploration, or ensure that g(u)≠rhs(u) at this time; if the above conditions are met, go to step 3.2; if not, go to step 3.5; Step 3.2: Put the explored node into the priority queue U; Step 3.3: If g(u)>rhs(u) of the explored node, set g(u)=rhs(u); Step 3.4: Find all child nodes of the exploration node, set the parent node of all child nodes as the exploration node, and update all child nodes. After all updates are completed, take out the top node of the current priority queue U as the exploration node and jump back to step 3.1; Step 3.5: Store the destination of the current exploration into the path set, find the parent node of the destination of the current exploration, store it into the path set, and continuously recurse the parent node and store the parent node into the path set; when the last parent node is the starting point, it is judged that the planning is successful, if not the starting point, the planning fails; jump to step 2 to reset the starting point and end point settings; Step 4: When the road network topology map information is updated, fast re-planning is performed to achieve real-time planning of ground units in a complex road network environment, and the real-time planning results of ground units in a complex road network environment are obtained by iteration; If the current optimal path changes after the current round of environmental changes, with the help of the value node reproduction mechanism, the cost of the path end point in the previous round of planning is reset and re-added to the priority list U, and then step 3 is restarted to solve the optimal path; if the current optimal path is not affected by the current round of environmental changes, the optimal path in the previous round is still used as the optimal path in this round; fast replanning realizes real-time planning of ground units, and iteratively obtains real-time planning results of ground units in complex road network environments.
2. The method for real-time planning of ground units with improved incremental heuristic search according to claim 1, characterized in that: Based on the real-time planning results of the ground unit in the complex road network environment obtained in step 4, the safe transfer and threat avoidance maneuvers of the ground unit are ensured, thereby improving the survivability and coordination capabilities of the ground unit.
3. A method for real-time planning of ground units with improved incremental heuristic search as claimed in claim 1 or 2, characterized in that: The specific implementation method of step 4 is: Step 4.1: Check whether the change in the road network topology map information caused by the environmental change is the current optimal path; if the current optimal path is changed, proceed to step 4.2; if the current optimal path is not affected, the optimal path of the previous round is still used as the optimal path of this round; Step 4.2: Update all intersection nodes where data has changed; Step 4.3: Find all value nodes s that meet the conditions * , and add all value nodes to the priority queue U; Step 4.3.1: Find s * ; said * Must meet: Among them, Path(s * ) represents the node s * is the set of path points at the end, Succ(s * ) is node s * The set of child nodes of Step 4.3.2: Find the s * Perform information processing of formula (3): g(s * )=rhs(s * )=∞ (3) Step 4.3.3: The processed s * Join the priority queue U; Step 4.4: Go to step 3 and perform rapid re-planning iteration to realize real-time planning of ground units in a complex road network environment, and obtain the real-time planning results of ground units in a complex road network environment.
4. The method for real-time planning of ground units with improved incremental heuristic search according to claim 3, characterized in that: The method for updating the nodes described in step 3 and step 4 is shown in the following steps. Step ①: Determine whether the current node is the starting point; if so, skip this step; if not, calculate the forward-looking current cost rhs value; Step ②: Determine whether the current node is in the priority queue U; if so, remove the node from the priority queue U; if not, skip this step; Step 3: Determine whether the forward-looking current cost rhs value of the current node is equal to the historical current cost g value; if not, calculate the priority of the node and add it back to the priority queue U; If they are equal, skip this step; Step ④: Iterate steps ① to ③ to implement node update.
