A multi-target path planning method for tracked vehicle based on rolling horizon graph search

Abstract

The application provides a tracked vehicle multi-target path planning method based on a rolling horizon graph search, and relates to the technical field of tracked vehicle path planning and navigation. The application first receives a global reference path and real-time perception sensor data. When the global reference path is blocked by an obstacle, a candidate state node set is generated by rollout and rollin sampling in the path neighborhood, and a directed graph structure is constructed. Collision risk cost, heading cost and distance cost are set on the directed graph structure, a dictionary optimization ranking is constructed, and graph search is performed to obtain a local path. When the path is not feasible, the directed graph structure is regenerated based on a sliding window and re-planned. The application can reduce the dependence on weight parameter adjustment, and improve the safety, smoothness and real-time performance of dynamic obstacle avoidance of the tracked vehicle.

Description

Technical Field

[0001] This invention relates to the field of tracked vehicle path planning and navigation technology, and in particular to a multi-objective path planning method for tracked vehicles based on rolling time-domain graph search. Background Technology

[0002] Tracked mobile robots, due to their advantages such as low ground pressure, strong mobility, and high load-bearing capacity, have been widely used in fields such as material transportation, disaster relief, agricultural operations, and military reconnaissance in complex terrain environments. Compared with wheeled robots, tracked vehicles can maintain better driving stability and traction performance in unstructured terrains such as mud, soft, and rugged terrain. However, in actual deployment, the environment faced by tracked vehicles is often dynamic, uncertain, and changeable, such as pedestrians, other working vehicles, and temporarily piled obstacles that suddenly appear in the work area, which poses a severe challenge to their path planning system.

[0003] Currently, path planning methods for tracked vehicles or ground mobile robots can be mainly divided into two categories: one is offline planning methods based on global static maps, such as A One type of algorithm, including Dijkstra's algorithm and its variants, assumes that environmental information is known in advance and does not change, making it ineffective in dealing with dynamic obstacles. Another type is online planning methods based on local perception, typically represented by dynamic window methods, time-elastic band planners, and OpenPlanner. These methods generate local paths using real-time sensor data and possess a certain degree of dynamic obstacle avoidance capability.

[0004] However, existing technologies still have the following shortcomings. Most online planning methods use a weighted summation approach to integrate multiple objectives such as safety, smoothness, and economy into a single cost function. This method requires users to manually adjust the weight coefficients of each objective, and the optimal weights vary significantly with environmental changes, lacking adaptive capabilities. In complex dynamic scenarios, inappropriate weight configuration may lead to overly conservative or risky paths, or even situations where no feasible solution can be generated. Some methods manage the priority of multiple objectives by imposing constraints, but when the environment is narrow or obstacles are dense, these constraints may conflict simultaneously, causing the planner to be unable to return any feasible paths, forcing the system to stop or oscillate. Finally, existing planning methods have limited dynamic object handling capabilities. Existing methods typically treat moving objects as static obstacles and perform inflated avoidance, lacking explicit modeling and prediction of the object's motion state. This leads to frequent replanning in areas with dense pedestrians or vehicles, resulting in significant trajectory oscillations and reduced driving smoothness and control stability. Summary of the Invention

[0005] The purpose of this invention is to provide a multi-objective path planning method for tracked vehicles based on rolling temporal graph search. This method can solve the problems of existing tracked vehicle path planning methods, such as reliance on weight parameter adjustment, unclear target priority handling, easy deviation from the global reference path in local obstacle avoidance, and insufficient stability of replanning in dynamic environments. It realizes path search according to the hierarchical priority of collision risk cost, heading cost, and distance cost, and completes online replanning when obstacles block or the path is infeasible, thereby improving the safety, smoothness, real-time performance, and global reference path following ability of tracked vehicle path planning.

[0006] To achieve the above objectives, the present invention provides the following solution: A multi-objective path planning method for tracked vehicles based on rolling temporal graph search includes: Receive global reference path and real-time perception sensor data, and determine whether the global reference path is blocked by obstacles within the current scrolling window based on the current pose of the tracked vehicle. If the global reference path is blocked by an obstacle, a candidate state node is generated in the neighborhood of the global reference path. The candidate state node is generated using a sampling strategy that combines rollout and rollin. In the rollout stage, starting from the current pose, the candidate state node is gradually deviated from the global reference path to bypass the obstacle according to the tracked vehicle dynamics model. In the rollin stage, the candidate state node is gradually converged back to the global reference path. Based on the dynamic constraints of the tracked vehicle, the generated candidate state nodes are constrained and filtered to form a set of candidate state nodes; Connect adjacent candidate state nodes in the candidate state node set with directed edges to form a directed graph structure for multi-objective search. On a directed graph structure, collision risk cost, heading cost, and distance cost are set in order of priority from high to low to form a multi-level cost function; Optimize sorting by constructing a dictionary based on a multi-level cost function; Based on dictionary-optimized sorting, a dictionary-optimized graph search algorithm is executed. During the node expansion process, higher-level costs are optimized first, and lower-level costs are compared only when higher-level costs are equal, so as to obtain local paths that satisfy the level optimality. Define the local path as the current path to be executed; If no obstacle is detected within the current scrolling window, the global reference path is used, and the used global reference path is determined as the current path to be executed. During the execution of the tracked vehicle along the current path to be executed, the system determines whether the current path to be executed is infeasible based on the latest perception data. When the current path to be executed is infeasible, the system regenerates the directed graph structure based on the sliding window and executes the dictionary-optimized graph search algorithm based on the regenerated directed graph structure to obtain a new feasible local path. Update the new feasible local path to the current path to be executed; The tracked vehicle's motion control is executed based on the current path to be executed, so that the tracked vehicle follows the global reference path and avoids obstacles, forming a smooth trajectory that satisfies the priority of multiple objectives.

[0007] Preferably, receiving the global reference path and real-time sensing sensor data, and determining whether the global reference path is blocked by obstacles within the current scrolling window based on the current pose of the tracked vehicle, includes: Determine obstacle space based on real-time sensor data; Determine the global reference path point within the current scrolling window based on the current pose of the tracked vehicle; Determine whether the global reference path point within the current scrolling window falls into the obstacle space; If there is a global reference path point within the current scrolling window that has fallen into the obstacle space, then the global reference path is determined to be blocked by the obstacle. If there is no global reference path point falling into the obstacle space within the current scrolling window, it is determined that no obstacle has been detected within the current scrolling window.

[0008] Preferably, if the global reference path is blocked by an obstacle, candidate state nodes are generated in the neighborhood of the global reference path. These candidate state nodes are generated using a sampling strategy combining rollout and rollin, including: The neighborhood of the global reference path is determined based on the neighborhood width parameter; Use the current pose as the starting node for the candidate state nodes; During the rollout phase, starting from the initial node, candidate state nodes that gradually deviate from the global reference path are generated according to the tracked vehicle dynamics model; During the rolling phase, candidate state nodes are generated based on the reference poses on the global reference path, gradually converging back to the global reference path. This ensures that both the candidate state nodes generated in the rollout phase and the candidate state nodes generated in the rollin phase are located within the neighborhood of the global reference path.

[0009] Preferably, the generated candidate state nodes are constrained and screened based on the dynamic constraints of the tracked vehicle to form a set of candidate state nodes, including: The state transition relationship between adjacent candidate state nodes is determined according to the discrete dynamics function of the tracked vehicle. The control inputs used to generate candidate state nodes are limited to those that satisfy the constraints of maximum steering angle, maximum linear velocity, and angular velocity. The control inputs include linear velocity and steering angle. During the rollout phase, the control input is superimposed with an offset term on the ideal control value along the global reference path; During the rolling phase, the control input is corrected by feedback based on the deviation between the reference pose and the candidate state node on the global reference path. Retain candidate state nodes that satisfy the state transition relationship and control input constraints to form a candidate state node set.

[0010] Preferably, adjacent candidate state nodes in the candidate state node set are connected by directed edges to form a directed graph structure for multi-objective search, including: The candidate state nodes in the candidate state node set are used as nodes in the directed graph structure; The adjacent candidate state nodes in the candidate state node set are determined based on the connection radius. Perform dynamic feasibility and collision-free checks on the paths between adjacent candidate state nodes; If the path between adjacent candidate state nodes satisfies the requirements of dynamic feasibility and collision-free operation, then the adjacent candidate state nodes are connected by a directed edge. A directed graph structure consisting of nodes and directed edges is used for multi-objective search.

[0011] Preferably, collision risk cost, heading cost, and distance cost are arranged in descending order of priority on the directed graph structure to form a multi-level cost function, including: The collision risk cost is set as a cost item characterizing the proximity between the tracked vehicle and the obstacle, and the collision risk cost is activated when the tracked vehicle enters the preset safe area; Set the heading cost as a cost term that represents the difference between the tracked vehicle's orientation and the orientation of the nearest point on the global reference path, and activate the heading cost when the difference exceeds the heading threshold. Set the distance cost as a strictly positive cost term that characterizes the physical length of the candidate path; Based on the priority order of collision risk cost, heading cost, and distance cost, a multi-level cost function is formed.

[0012] Preferably, the dictionary optimization ranking is constructed based on a multi-level cost function, including: A cost vector for candidate paths is constructed based on collision risk cost, heading cost, and distance cost. The lexicographical order of the cost vector is used as the criterion for judging the merits of candidate paths; When comparing two candidate paths, first compare the collision risk cost; When the collision risk costs of two candidate paths are equal, compare the heading costs; When the collision risk cost and heading cost of two candidate paths are equal, compare the distance cost; The comparison results of distance costs do not change the priority of collision risk costs and heading costs, and the comparison results of heading costs do not change the priority of collision risk costs.

[0013] Preferably, a dictionary-optimized graph search algorithm is executed based on dictionary-optimized sorting. During node expansion, higher-level costs are optimized first, and lower-level costs are compared only when higher-level costs are equal, resulting in local paths that satisfy hierarchical optimality. This includes: Use the candidate state node corresponding to the current pose as the starting node; Starting from the initial node, select candidate state nodes to be expanded, and determine the adjacent candidate state nodes connected to the candidate state nodes to be expanded through directed edges. Calculate the incremental cost vector corresponding to the directed edge; The incremental cost vector is added to the cost vector for reaching the candidate state node to be expanded to obtain the candidate cost vector for reaching the adjacent candidate state node. Compare the candidate cost vector with the recorded cost vectors corresponding to the adjacent candidate state nodes in lexicographical order; If a candidate cost vector is lexicographically superior to a recorded cost vector, then update the cost vector and parent node of the adjacent candidate state nodes. Based on the updated parent node relationships, a local path that satisfies hierarchical optimality is formed by backtracking.

[0014] Preferably, during the execution of the tracked vehicle along the current path to be executed, the system determines whether the current path to be executed is infeasible based on the latest perception data. When the current path to be executed is infeasible, a directed graph structure is regenerated based on a sliding window, and a dictionary-optimized graph search algorithm is executed based on the regenerated directed graph structure to obtain a new feasible local path, including: The sliding window is determined based on the current pose of the tracked vehicle and the length of the sliding window. Define the global reference path point in front of the sliding window as the local target; Determine the obstacle space based on the latest sensing data; Determine if the current path to be executed contains any path points that fall into the obstacle space; If the current path to be executed contains path points that fall into the obstacle space, then the directed graph structure is regenerated within the sliding window, starting from the current pose and targeting the local target. A dictionary-optimized graph search algorithm is performed on the regenerated directed graph structure to obtain new feasible local paths.

[0015] Preferably, the tracked vehicle motion control is performed based on the current path to be executed, so that the tracked vehicle follows the global reference path and avoids obstacles, forming a smooth trajectory that satisfies the priority of multiple objectives, including: Generate tracked vehicle motion control commands based on the current path to be executed; Based on the motion control commands of the tracked vehicle, the tracked vehicle is controlled to travel along the current path to be executed, so that the tracked vehicle follows the global reference path and avoids obstacles; The motion control process of the tracked vehicle is updated based on its current position and orientation. The local path segments obtained from each replanning are spliced ​​together in chronological order, and the continuity of adjacent local path segments is satisfied at the splicing position. A smooth trajectory that satisfies the priority of multiple objectives is formed based on the spliced ​​local path segments.

[0016] The present invention discloses the following beneficial effects: First, this invention sets collision risk cost, heading cost, and distance cost in descending order of priority on a directed graph structure, and constructs a dictionary optimization ranking based on a multi-level cost function. This ensures that the path search process makes decisions according to the hierarchical order of safety, path attitude consistency, and path length. Compared to existing methods that fuse multiple planning objectives into a single cost function through weighted summation, this invention eliminates the need to repeatedly set weight coefficients for collision risk, heading deviation, and distance length. This reduces the impact of weight parameter dependence on the stability of planning results and avoids problems such as overly conservative, overly risky, or deviating paths from expected planning objectives due to improper weight configuration, thereby improving the reliability of path planning for tracked vehicles in complex environments.

[0017] Second, this invention employs a dictionary-optimized graph search algorithm, prioritizing the optimization of higher-level costs during node expansion. Lower-level costs are only compared when higher-level costs are equal, ensuring that the resulting local paths satisfy hierarchical optimality. Through this process, the path planning process no longer relies on manual weighted trade-offs between objectives such as risk, heading, and distance, nor does it require treating multiple planning objectives with equally stringent constraints. When feasible candidate paths exist, the optimization of collision risk costs is prioritized, while simultaneously considering heading consistency and distance economy while ensuring safety meets hierarchical optimality requirements. This reduces planning stagnation, path oscillations, or local path unavailability caused by objective conflicts in multi-objective planning, improving the online replanning capability of tracked vehicles in narrow passages, obstacle-rich areas, and dynamically changing environments.

[0018] Third, when the global reference path is blocked by obstacles, this invention generates candidate state nodes in the neighborhood of the global reference path and constructs a directed graph structure using a sampling strategy combining rollout and rollin. The rollout phase causes the candidate state nodes to gradually deviate from the global reference path to bypass obstacles, while the rollin phase causes them to gradually converge back to the global reference path. This maintains the connection between the local path and the global reference path while achieving obstacle avoidance. Compared to existing local planning methods that easily deviate significantly from the reference route during local obstacle avoidance, this invention reduces unnecessary "shortcut" path deviations, allowing the tracked vehicle to return to the predetermined travel route after avoiding dynamic or static obstacles. This is particularly suitable for applications requiring adherence to the global reference path, such as mining transportation, inspection operations, and fixed-route traffic.

[0019] Fourth, during the execution of the tracked vehicle along the current path to be executed, this invention utilizes the latest perception data to determine whether the current path is infeasible. If the current path is infeasible, a directed graph structure is regenerated based on a sliding window, and then a dictionary-optimized graph search algorithm is executed to obtain a new feasible local path. The sliding window limits the replanning scope to a local area in front of the tracked vehicle, which reduces the computational burden of global replanning and allows for timely responses to newly appearing obstacles based on the latest perception data. Therefore, this invention can achieve online path adjustment in dynamic environments while following the global reference path, forming a smooth trajectory that satisfies the priorities of multiple objectives, thus improving the real-time performance, smoothness, and safety of tracked vehicle path planning. Attached Figure Description

[0020] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0021] Figure 1 A flowchart of the method provided in an embodiment of the present invention; Figure 2 This is a schematic diagram of the overall processing flow for rolling time-domain graph search provided in an embodiment of the present invention; Figure 3 This is a schematic diagram of Gazebo simulation and sensor data update for a tracked vehicle provided in an embodiment of the present invention; Figure 4 This invention provides a node diagram of a tracked vehicle and a schematic diagram of optimal path generation for embodiments of the invention. Figure 5 This is a schematic diagram of the algorithm test in a narrow passage scenario provided in an embodiment of the present invention. Detailed Implementation

[0022] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0023] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0024] Figure 1 The method flowchart provided in the embodiments of the present invention is as follows: Figure 1 As shown, this embodiment provides a multi-objective path planning method for tracked vehicles based on rolling time-domain graph search, including: Step 100: Receive global reference path and real-time perception sensor data, and determine whether the global reference path is blocked by obstacles within the current scrolling window based on the current pose of the tracked vehicle; Step 101: If the global reference path is blocked by an obstacle, a candidate state node is generated in the neighborhood of the global reference path. The candidate state node is generated using a sampling strategy that combines rollout and rollin. In the rollout stage, starting from the current pose, the candidate state node is gradually deviated from the global reference path to bypass the obstacle according to the tracked vehicle dynamics model. In the rollin stage, the candidate state node is gradually converged back to the global reference path. Step 102: Based on the dynamic constraints of the tracked vehicle, the generated candidate state nodes are constrained and filtered to form a set of candidate state nodes; Step 103: Connect adjacent candidate state nodes in the candidate state node set with directed edges to form a directed graph structure for multi-objective search; Step 104: Set up collision risk cost, heading cost, and distance cost in descending order of priority on the directed graph structure to form a multi-level cost function; Step 105: Construct a dictionary based on a multi-level cost function to optimize the sorting; Step 106: Execute the dictionary-optimized graph search algorithm based on dictionary-optimized sorting. In the process of expanding nodes, prioritize optimizing the cost of higher levels. Only when the costs of higher levels are equal, compare the costs of lower levels to obtain local paths that satisfy the level optimality. Step 107: Determine the local path as the current path to be executed; Step 108: If no obstacle is detected in the current scrolling window, the global reference path is used, and the used global reference path is determined as the current path to be executed; Step 109: During the execution of the tracked vehicle along the current path to be executed, determine whether the current path to be executed is infeasible based on the latest perception data. When the current path to be executed is infeasible, regenerate the directed graph structure based on the sliding window, and execute the dictionary-optimized graph search algorithm based on the regenerated directed graph structure to obtain a new feasible local path. Step 110: Update the new feasible local path to the current path to be executed; Step 111: Execute tracked vehicle motion control based on the current path to be executed, so that the tracked vehicle follows the global reference path and avoids obstacles, forming a smooth trajectory that satisfies the priority of multiple objectives.

[0025] like Figure 2 As shown, Figure 2 The overall processing flow of the rolling time-domain graph search in this embodiment is shown. Figure 2 The diagram sequentially displays processing nodes such as "Start," "Global Path + Real-time Perception Point Cloud," "Obstacle Detection + Directed Graph Construction," "Dictionary Optimization Search," "Path Execution and Dynamic Replanning," and "Optimal Trajectory." After receiving the global path and real-time perception point cloud, the tracked vehicle performs obstacle detection within the current scrolling window. When the global path is blocked by obstacles, it enters the directed graph structure construction process. After completing the directed graph structure construction, it performs dictionary optimization search. After obtaining a local path that satisfies hierarchical optimality, it performs path execution and dynamic replanning, ultimately forming the optimal trajectory that satisfies the priority of multiple objectives. Figure 2 This is used to illustrate the processing order and data transfer relationships between rolling time-domain graph search, dictionary-optimized search, and dynamic reprogramming.

[0026] like Figure 3 As shown, Figure 3 The Gazebo simulation of the tracked vehicle and the sensor data update process are shown. Figure 3 On the left is the Gazebo 3D simulation environment. The gray grid represents the simulated ground, the yellow tracked vehicle is in front of the obstacle, the black cube obstacle and spherical obstacle are in the tracked vehicle's driving area, and the red rectangle represents the current perception area or obstacle detection area. Figure 3 The right side shows a two-dimensional environment representation based on LiDAR point cloud data updates. The green rectangle represents the current local perception window or local planning window; the red area represents obstacles detected by the sensors; the black or gray area represents the safety boundary layer after expanding the obstacle area according to the tracked vehicle's dimensions; the white area represents the free space deemed safe to pass through; and the surrounding black area represents map areas outside the current planning window or areas not used as free space. Figure 3Real-time sensor data can be transformed into obstacle space and free space, and used as input for global reference path blocking judgment, directed graph structure construction, and dictionary-optimized graph search.

[0027] In step 100, this embodiment receives the global reference path. With real-time sensing sensor data And based on the current position of the tracked vehicle Determine the global reference path within the current scrolling window. Is it blocked by an obstacle? Real-time sensor data. This can include point cloud data collected in real time by a lidar system mounted on a tracked vehicle. By updating the point cloud data, the space of obstacles within the current perception range can be determined. Free space that allows passage. Obstacle space. This includes the spatial area corresponding to actual obstacles such as walls, cones, and other vehicles, and may also include the safety boundary layer after the obstacle has been expanded according to the external dimensions of the tracked vehicle.

[0028] Assume the configuration space of the tracked vehicle is ,in The pose dimension is usually 1. , , Global reference path neighborhood For formula (1): (1) in, This is the neighborhood width parameter. Global reference path. The condition for being judged as blocked under the current perception is formula (2): (2) Formula (2) represents the global reference path. There is at least one point that falls into the obstacle space. If formula (2) is satisfied, then local graph generation and replanning are triggered; if there is no obstacle space within the current scrolling window... If the global reference path point is not detected, then no obstacle is detected within the current scrolling window, and the global reference path can be directly used subsequently. .

[0029] In steps 101 and 102, when the global reference path When blocked by obstacles, this embodiment uses the global reference path. neighborhood Candidate state nodes are generated internally, and the generated candidate state nodes are constrained and filtered based on the dynamic constraints of the tracked vehicle to form a set of candidate state nodes. Candidate state nodes are generated using a sampling strategy combining rollout and rollin. The rollout phase starts from the current pose. Starting from there, simulate several steps forward according to the tracked vehicle dynamics model, gradually causing the candidate state nodes to deviate from the global reference path. To bypass obstacles; the rolling phase allows candidate state nodes to gradually converge back to the global reference path. This ensures that the final path is connectable. All generated candidate state nodes must satisfy the kinematic and dynamic constraints of the tracked vehicle, including the maximum steering angle, maximum linear velocity, and angular velocity.

[0030] make The total number of steps to generate nodes. For rollout steps, The number of rollin steps satisfies formula (3): (3) In the Rollout phase, i.e. The generation of candidate state nodes follows the discrete dynamics function of the tracked vehicle shown in formula (4): (4) in, For the discrete dynamics function of the tracked vehicle, For control inputs, the control inputs include linear velocity. With steering angle , This is the feasible control set. The maximum steering angle does not exceed and the maximum linear velocity does not exceed To deviate from the global reference path The reference input uses the offset term shown in formula (5). : (5) in, To follow the global reference path The ideal control quantity. To be used to deviate candidate state nodes from the global reference path offset item, For offset items The upper limit of amplitude.

[0031] In the Rollin stage, i.e. Candidate state nodes need to gradually converge back to the global reference path. The feedback control law shown in formula (6) is adopted: (6) in, It is a proportional gain matrix. Global reference path The corresponding parameters above The reference pose. This feedback control law is based on the candidate state nodes. With reference pose Deviation correction control input This enables candidate state nodes to move to the global reference path. convergence.

[0032] The Rollin phase ensures that the end nodes Located in the global reference path Above, or with the global reference path The distance between them is less than the threshold As shown in formula (7): (7) The generated candidate state nodes also need to satisfy the neighborhood range constraint shown in formula (8): (8) In addition, the total span of rollout and rollin From arrive Cumulative distance along the path, total span It needs to be larger than the sensor range. This is to prevent candidate state nodes from converging in unknown regions. Through the above processing, the rollout phase provides the deviation capability needed to bypass obstacles, and the rollin phase provides the regression to the global reference path. The required convergence capability enables candidate state nodes to balance obstacle avoidance and path splicing.

[0033] In step 103, this embodiment sets the candidate state node set. Adjacent candidate state nodes in the directed edge The connections form a directed graph structure for multi-objective search. Candidate state node set Defined as formula (9): (9) in, That is, the starting position of the node is the current position of the tracked vehicle. Edge set Defined as formula (10): (10) in, For the connection radius, Used to verify the node To the node Does the path satisfy dynamic feasibility and avoid collisions? If the distance between two candidate state nodes does not exceed the connection radius... And from the node To the node If the path satisfies both dynamic feasibility and collision-free behavior, then it is based on the directed edge. Connecting nodes and nodes The final directed graph structure shown in formula (11) is obtained: (11) Directed graph structure This serves as input for subsequent dictionary optimization searches. Through a sampling strategy combining rollout and rollin, this embodiment can optimize the global reference path. When blocked, in the global reference path A set of candidate state nodes that satisfy the dynamic constraints of the tracked vehicle are generated in the neighborhood of the vehicle, and a discretized, low-dimensional and dynamically feasible search space is constructed.

[0034] like Figure 4 As shown, Figure 4 The node diagram of the tracked vehicle and the optimal path generation process are shown. Figure 4 The left side shows the Gazebo 3D simulation environment, illustrating the state of a tracked vehicle performing local path planning near obstacles; Figure 4 The right side shows the node graph generation and path search results. The black rectangular border represents the local planning area or sliding window range, the red area represents the detected obstacles, the black or gray inflated area represents the obstacle safety boundary layer, the yellow dotted or mesh area represents the set of candidate state nodes generated in the neighborhood of the global reference path, and the green curve represents the optimal trajectory or the current path to be executed selected by the dictionary-optimized graph search algorithm. Figure 4 After completing environmental modeling and setting up the expansion layer, this embodiment generates a set of candidate state nodes guided by the global reference path, and constructs directed edges between the candidate state nodes to form a complete graph search space. Within this graph search space, the dictionary-optimized graph search algorithm selects the optimal trajectory marked in green based on the hierarchical priority of collision risk cost, heading cost, and distance cost.

[0035] In steps 104 and 105, this embodiment uses a directed graph structure. The system sets collision risk cost, heading cost, and distance cost in descending order of priority, forming a multi-level cost function. A dictionary-based optimization ranking is then constructed based on this multi-level cost function. Collision risk cost quantifies the proximity between the tracked vehicle and obstacles and is activated only when the tracked vehicle enters a preset safe zone to ensure safety. Heading cost penalizes deviations from the global reference path. The directional error is used to improve motion smoothness; distance cost is a strictly positive cost term used for comparison when costs are equal at higher levels, prioritizing the path with shorter physical length. Collision risk cost, heading cost, and distance cost together constitute the dictionary optimization ranking.

[0036] set up Directed graph structure From the starting node To the target node A path, path Composed of several continuous edges Composition. Three cost functions are defined as follows.

[0037] First, the collision risk cost is shown in formula (12): (12) in, As shown in formula (13): (13) Defined as The reciprocal of the distance to the nearest obstacle, i.e.: .

[0038] threshold Used to define a safe zone. Collision risk cost is non-zero only when the tracked vehicle enters the danger zone, and is used to prioritize paths that are as far away from obstacles as possible.

[0039] Second, the heading cost is shown in formula (14): (14) in, As shown in formula (15): (15) Indicates the tracked vehicle is in status Orientation and Global Reference Path The absolute value of the difference between the orientations of the nearest points, ranging from 1 to 2. Threshold Used to avoid excessive steering angles. Heading cost is used to encourage tracked vehicles to maintain alignment with the global reference path. Consistent driving direction.

[0040] Third, the distance cost is shown in formula (16): (16) Distance cost is the physical length of the path. Strictly speaking, it is positive, that is Define the cost vector: .

[0041] The goal of dictionary optimization is to find paths. , make the path The cost vector is minimized in the lexicographical sense, as shown in formula (17): (17) The lexicographical order is defined as shown in formula (18): (18) That is, first compare ,like If they are equal, then compare. ,like If they are still equal, then compare. By comparing lexicographical order, this embodiment ensures that collision risk cost has a higher priority than heading cost, heading cost has a higher priority than distance cost, and optimizing low-priority costs will not change the priority of high-priority costs.

[0042] In step 106, this embodiment executes a dictionary-optimized graph search algorithm based on dictionary-optimized sorting. During node expansion, it prioritizes optimizing higher-level costs, comparing lower-level costs only when higher-level costs are equal, thus obtaining a local path that satisfies hierarchical optimality. Specifically, let's assume... To the node The current optimal cost vector is For a path passing through a node Reaching neighbor nodes The candidate paths and their cumulative cost vectors are shown in formula (19): (19) in, For the edge The incremental cost is obtained by integration. The node relaxation criterion uses the lexicographical comparison shown in formula (20): (20) like Then update The cost of all levels is and set the parent node as ;like If the candidate path is rejected, the decision process ends; if Then continue comparing the next level. The aforementioned relaxation rules ensure that an update is triggered as soon as a high-priority cost becomes strictly better; if high-priority costs are equal, low-priority costs are allowed to improve; and high-priority costs are prevented from degrading. Thus, the resulting local path achieves global optimality in collision risk cost, local optimality in heading cost within regions with equal collision risk cost, and local optimality in distance cost within regions with equal collision risk cost and heading cost.

[0043] In steps 107 and 108, this embodiment determines the local path obtained by the dictionary-optimized graph search algorithm as the current path to be executed; if no obstacle is detected within the current scrolling window, the global reference path is used. and will continue to use the global reference path The current path to be executed is determined. In an accessibility scenario, formula (21) is satisfied: (twenty one) Then the global reference path will be used directly. As a path Without performing graph generation and search, the cost vector is shown in formula (22): (twenty two) At this point, both collision risk cost and heading cost are zero, the entire path lies within an unobstructed area and meets the heading threshold condition, thus conforming to dictionary optimality. Through the above processing, this embodiment can avoid unnecessary graph generation and search when there are no obstacles, reducing computational overhead; when obstacles exist, it can return a path that satisfies hierarchical optimality according to the decision logic of safety first, smoothness second, and distance last. The time complexity of the dictionary optimization search algorithm increases linearly with the number of cost levels, exhibiting good real-time performance.

[0044] In steps 109 and 110, this embodiment constructs a local path replanning mechanism based on a sliding window and implements online path adjustment during the execution of the tracked vehicle along the current path to be executed. While the tracked vehicle is executing along the current path to be executed, this embodiment acquires the latest sensing data at a fixed frequency. It also detects in real time whether new obstacles appear on the current execution path. If the latest sensing data... If the current path to be executed is infeasible, a replanning process is triggered; the replanning process uses the current pose of the tracked vehicle. Starting from a global reference path point within a certain distance in front of the sliding window. For local objectives, regenerate the directed graph structure within the sliding window. And based on the regenerated directed graph structure Perform a dictionary-optimized graph search algorithm to return a new feasible local path. New feasible local path The current path to be executed is updated. The sliding window moves forward with the tracked vehicle to achieve continuous online path adjustment in a dynamic environment.

[0045] Let the current position of the tracked vehicle be... The global reference path is Sliding window Defined as formula (23): (twenty three) in, The length of the sliding window. Indicates from arrive Along the global reference path The arc length distance. Local target. Defined as formula (24): (twenty four) in, .

[0046] Current pose In global reference path Projection parameters on, This represents the total length of the global path. The current path to be executed is based on the latest sensing data. When the following is determined to be infeasible, a replanning is triggered, and the trigger time is... The reprogramming conditions are shown in formula (25): (25) Formula (25) represents the current path to be executed. There is a possibility of falling into the latest perception data. Defined obstacle space When replanning waypoints, replan the markers. The value is 1, and a replanning is triggered.

[0047] After triggering replanning, in the current pose Starting from local objectives For the target, in the sliding window The directed graph structure is regenerated according to formula (26): (26) in, Includes rollout / rollin sampling and edge connection operations. Performing a dictionary-optimized search yields the new path shown in formula (27): (27) New feasible local paths Satisfying formula (28): (28) Formula (28) represents the new feasible local path. In the sliding window The corresponding candidate path set The internal condition satisfies lexicographical optimality.

[0048] Sliding window The update rule is shown in formula (29): (29) When the distance between the current position of the tracked vehicle and the local target is less than the threshold At that time, the local target is extended forward according to formula (30): (30) Using formulas (29) and (30), the sliding window It can move forward as the tracked vehicle's current position is updated, targeting local areas. The tracked vehicle can reference the global path after approaching the current local target. The system extends forward to ensure that the local planning continuously covers the area in front of the tracked vehicle.

[0049] In step 111, this embodiment performs tracked vehicle motion control based on the current path to be executed, so that the tracked vehicle follows the global reference path. It avoids obstacles and forms a smooth trajectory that satisfies the priority of multiple objectives. Specifically, in this embodiment, the path returned by the dictionary-optimized graph search algorithm... Extract the control commands at the current moment, including linear velocity. and steering angle Tracked vehicles can use tracking algorithms such as pure tracking control or model predictive control to travel along the current path to be executed, enabling the tracked vehicle to follow the global reference path. Simultaneously, it avoids dynamic or static obstacles. During the control process, this embodiment provides real-time feedback on the tracked vehicle's position and attitude, and updates the subsequent path tracking process based on the feedback, thereby forming a closed-loop control.

[0050] Let the local path segment returned by each replanning be . ,in, .

[0051] Adjacent path segments satisfy the continuity shown in formula (31): (31) The final global trajectory The concatenation of each path segment is shown in formula (32): (32) in, This is the path concatenation operator. Since each local path segment is generated based on dictionary-optimized sorting, the concatenated global trajectory... It can meet the priority requirements of multiple objectives within each local segment and achieve safe, smooth and efficient path planning as a whole.

[0052] like Figure 5 As shown, Figure 5 This is a schematic diagram of algorithm testing in a narrow passage scenario. Figure 5 The test scenario includes two narrow passages, one above the other. The gray area represents the scene background or drivable area, the purple vertical obstacles represent fixed obstacles that form the narrow passage, the yellow dotted area represents the set of candidate state nodes generated within the sliding window, the green curve represents the final planned path, the left arrow indicates the starting direction for entering the narrow passage, and the right arrow indicates the target direction or driving direction for leaving the narrow passage. Figure 5 This describes how, when a tracked vehicle travels in a narrow passage with a width only slightly greater than its own width, a sliding window-based replanning mechanism can generate a candidate state node graph that fits the central axis of the passage under the constraints of the passage. It also uses dictionary optimization search to ensure that the planned path maintains a relatively balanced distance from obstacles on both sides. At the same time, the heading cost ensures that the direction of the tracked vehicle's front end is consistent with the direction of the passage, thereby reducing over-steering.

[0053] In one specific implementation, this embodiment can build a tracked vehicle test scenario in the Gazebo simulation environment to verify the basic obstacle avoidance capability of the tracked vehicle multi-objective path planning method based on rolling temporal graph search in a dynamic obstacle environment. The tracked vehicle is equipped with a LiDAR, which collects point cloud data of the surrounding environment in real time. This embodiment uses the point cloud data as input to continuously update the surrounding environment and completes obstacle space modeling based on detected obstacles, a safety boundary layer expanded according to the tracked vehicle's external dimensions, and passable free space. Figure 3 The red obstacle area, the black or gray safety boundary layer, and the white free space shown correspond to the above modeling results.

[0054] After completing the environment modeling and expansion layer settings, this embodiment further introduces a global reference path. As a guide, in the global reference path Generate a set of candidate state nodes within the neighborhood. Candidate state node set A sampling strategy combining rollout and rollin is used to generate the candidate state nodes, ensuring that all candidate state nodes satisfy the kinematic and dynamic constraints of the tracked vehicle. Adjacent candidate state nodes are connected by directed edges, forming a complete graph search space. Figure 4 The set of candidate state nodes corresponding to the yellow nodes in the graph. The green trajectory represents the optimal trajectory selected by the dictionary-optimized graph search algorithm, which establishes connections between adjacent nodes and corresponding to the green nodes. This embodiment executes the dictionary-optimized graph search algorithm, comprehensively considering the hierarchical priority of collision risk cost, heading cost, and distance cost, to select a local path that satisfies hierarchical optimality from multiple candidate paths. Experimental results show that the average time for node graph generation and dictionary search is less than 100 milliseconds, which can meet the real-time planning requirements of tracked vehicles at typical driving speeds.

[0055] In another specific implementation, to verify the robustness and path quality of this embodiment in extreme environments, tests can be conducted in narrow-path scenarios. For example... Figure 5 As shown, the tracked vehicle needs to pass through a narrow passage with a width only slightly greater than its own width, and obstacles exist on both sides of the passage. When the current path to be executed becomes infeasible due to obstacles, this embodiment triggers a sliding window-based replanning mechanism, revising the global reference path. Under constraints, a candidate state node diagram conforming to the central axis of the channel is generated. Dictionary optimization prioritizes minimizing collision risk cost, ensuring the planned path maintains a balanced distance from obstacles on both sides. Based on the collision risk cost meeting the hierarchical optimality requirement, heading cost ensures the tracked vehicle's heading aligns with the channel's direction, thus avoiding over-steering. Distance cost further selects the path with the shorter physical length when both collision risk cost and heading cost are equal. Figure 5 As shown in the narrow passage scenario, this embodiment can maintain good safety, smoothness, and global reference path following capability in environments with dense obstacles or limited passage space.

[0056] Compared to path planning methods that use a weighted summation approach to integrate multiple objectives such as safety, smoothness, and economy into a single cost function, this embodiment eliminates the need to repeatedly set weight coefficients between collision risk, heading deviation, and distance, thus reducing the impact of weight parameters on the stability of the planning results. Compared to path planning methods that set multiple planning objectives as equally strong hard constraints, this embodiment achieves hierarchical optimization between collision risk, heading, and distance through dictionary-based optimization ranking. When feasible candidate paths exist, it reduces planning stagnation, path oscillation, or local path unavailability caused by multi-objective conflicts. This embodiment employs a sampling strategy combining rollout and rollin, ensuring that local paths converge back to the global reference path after bypassing obstacles. This can reduce path deviations from the reference route during local obstacle avoidance, making tracked vehicles suitable for application scenarios that require following a global reference path, such as mining area transportation, inspection operations, and fixed route passage.

[0057] This embodiment also provides a multi-objective path planning system for tracked vehicles based on rolling temporal graph search. The system includes a data receiving unit, a blocking judgment unit, a candidate state node generation unit, a constraint filtering unit, a directed graph construction unit, a multi-level cost function setting unit, a dictionary optimization sorting construction unit, a dictionary optimization graph search unit, a sliding window replanning unit, and a motion control unit. The data receiving unit is used to receive the global reference path. With real-time sensing sensor data The blocking determination unit is used to determine the current pose of the tracked vehicle. Determine the global reference path within the current scrolling window. Is it blocked by an obstacle? The candidate state node generation unit is used in the global reference path. When blocked by an obstacle, in the global reference path neighborhood Candidate state nodes are generated internally. These candidate state nodes are generated using a sampling strategy combining rollout and rollin. A constraint filtering unit is used to filter the generated candidate state nodes based on the tracked vehicle's dynamic constraints, forming a set of candidate state nodes. The directed graph building unit is used to construct a set of candidate state nodes. Adjacent candidate state nodes in the directed edge The connections form a directed graph structure for multi-objective search. Multi-level cost function setting units are used in directed graph structures. The system sets collision risk cost, heading cost, and distance cost in descending order of priority. The dictionary optimization ranking unit constructs a dictionary optimization ranking based on a multi-level cost function. The dictionary optimization graph search unit executes a dictionary optimization graph search algorithm based on the dictionary optimization ranking to obtain local paths that satisfy hierarchical optimality. The sliding window replanning unit regenerates the directed graph structure based on a sliding window when the current path to be executed is infeasible. And based on the regenerated directed graph structure Perform a dictionary-optimized graph search algorithm to obtain new feasible local paths. The motion control unit is used to perform motion control of the tracked vehicle based on the current path to be executed, forming a smooth trajectory that satisfies the priority of multiple objectives.

[0058] The functional units in this embodiment can be implemented in hardware or software. Each functional unit can be integrated into a single processing unit, exist as a separate physical unit, or two or more functional units can be integrated into one unit. Coupling, direct coupling, or communication connections between functional units can be achieved through interfaces, electrical connections, or other communication methods. Functional units described as separate components can be physically separate or not; components shown as units can be located in one location or distributed across multiple units. Those skilled in the art can select some or all of the functional units to implement the technical solution of this embodiment according to actual needs.

[0059] This embodiment also provides an electronic device. The electronic device includes a processor, a memory, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the multi-objective path planning method for tracked vehicles based on rolling time-domain graph search in any of the above embodiments.

[0060] This embodiment also provides a computer-readable storage medium. The computer-readable storage medium stores a computer program. When executed by a processor, the computer program implements the multi-objective path planning method for tracked vehicles based on rolling time-domain graph search in any of the above embodiments. The computer-readable storage medium includes media capable of storing program code, such as a USB flash drive, read-only memory, random access memory, portable hard disk, magnetic disk, or optical disk.

[0061] This embodiment also provides a computer program product. The computer program product includes a computer program or computer instructions. When the computer program or computer instructions are executed by a processor, they implement the multi-objective path planning method for tracked vehicles based on rolling time-domain graph search in any of the above embodiments.

[0062] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the systems disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the descriptions are relatively simple; relevant parts can be referred to the method section.

[0063] Specific examples have been used to illustrate the principles and implementation methods of this invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of this invention. Furthermore, those skilled in the art will recognize that, based on the ideas of this invention, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of this invention.

Claims

1. A multi-objective path planning method for tracked vehicles based on rolling time-domain graph search, characterized in that, include: Receive global reference path and real-time sensing sensor data, and determine whether the global reference path is blocked by an obstacle within the current scrolling window based on the current pose of the tracked vehicle. If the global reference path is blocked by an obstacle, a candidate state node is generated in the neighborhood of the global reference path. The candidate state node is generated using a sampling strategy that combines rollout and rollin. In the rollout phase, starting from the current pose, the candidate state node is gradually deviated from the global reference path to bypass the obstacle according to the tracked vehicle dynamics model. In the rollin phase, the candidate state node is gradually converged back to the global reference path. Based on the dynamic constraints of the tracked vehicle, the generated candidate state nodes are constrained and filtered to form a set of candidate state nodes; Connect adjacent candidate state nodes in the candidate state node set with directed edges to form a directed graph structure for multi-objective search. On the directed graph structure, collision risk cost, heading cost, and distance cost are arranged in descending order of priority to form a multi-level cost function; Based on the aforementioned multi-level cost function, a dictionary optimization sorting is constructed; Based on the dictionary-optimized sorting, the dictionary-optimized graph search algorithm is executed. During the node expansion process, the higher-level cost is optimized first, and the lower-level cost is compared only when the higher-level costs are equal, so as to obtain a local path that satisfies the level optimality. The local path is determined as the current path to be executed. If no obstacle is detected in the current scrolling window, the global reference path is used, and the used global reference path is determined as the current path to be executed. During the execution of the tracked vehicle along the current path to be executed, the system determines whether the current path to be executed is infeasible based on the latest perception data. When the current path to be executed is infeasible, the system regenerates the directed graph structure based on the sliding window and executes the dictionary-optimized graph search algorithm based on the regenerated directed graph structure to obtain a new feasible local path. Update the new feasible local path to the current path to be executed; Based on the current path to be executed, the tracked vehicle motion control is performed so that the tracked vehicle follows the global reference path and avoids obstacles, forming a smooth trajectory that satisfies the priority of multiple objectives.

2. The multi-objective path planning method for tracked vehicles based on rolling time-domain graph search according to claim 1, characterized in that, Receiving global reference path and real-time perception sensor data, and determining whether the global reference path is blocked by obstacles within the current scrolling window based on the current pose of the tracked vehicle, including: The obstacle space is determined based on the real-time sensing sensor data; Determine the global reference path point within the current scrolling window based on the current pose of the tracked vehicle; Determine whether the global reference path point within the current scrolling window falls into the obstacle space; If there is a global reference path point within the current scrolling window that falls into the obstacle space, then it is determined that the global reference path is blocked by the obstacle. If there is no global reference path point falling into the obstacle space within the current scrolling window, it is determined that no obstacle has been detected within the current scrolling window.

3. The multi-objective path planning method for tracked vehicles based on rolling time-domain graph search according to claim 1, characterized in that, If the global reference path is blocked by an obstacle, candidate state nodes are generated in the neighborhood of the global reference path. These candidate state nodes are generated using a sampling strategy combining rollout and rollin, including: The neighborhood of the global reference path is determined based on the neighborhood width parameter; The current pose is used as the starting node of the candidate state node; During the rollout phase, candidate state nodes that gradually deviate from the global reference path are generated starting from the starting node and according to the tracked vehicle dynamics model. During the rolling phase, candidate state nodes are generated based on the reference poses on the global reference path, gradually converging back to the global reference path. The candidate state nodes generated in the rollout phase and the candidate state nodes generated in the rollin phase are both located in the neighborhood of the global reference path.

4. The multi-objective path planning method for tracked vehicles based on rolling time-domain graph search according to claim 1, characterized in that, Based on the dynamic constraints of the tracked vehicle, the generated candidate state nodes are constrained and filtered to form a set of candidate state nodes, including: The state transition relationship between adjacent candidate state nodes is determined according to the discrete dynamics function of the tracked vehicle. The control input used to generate candidate state nodes is limited to control inputs that satisfy the constraints of maximum steering angle, maximum linear velocity, and angular velocity, wherein the control inputs include linear velocity and steering angle. During the rollout phase, the control input is superimposed with an offset term on the ideal control value along the global reference path; During the rolling phase, the control input is corrected by feedback based on the deviation between the reference pose and the candidate state node on the global reference path. Candidate state nodes that satisfy the state transition relationship and the control input constraints are retained to form the candidate state node set.

5. The multi-objective path planning method for tracked vehicles based on rolling time-domain graph search according to claim 1, characterized in that, Connecting adjacent candidate state nodes in the candidate state node set with directed edges forms a directed graph structure for multi-objective search, including: The candidate state nodes in the candidate state node set are used as nodes of the directed graph structure; The adjacent candidate state nodes in the candidate state node set are determined based on the connection radius; The path between the adjacent candidate state nodes is tested for dynamic feasibility and collision-free behavior. If the path between adjacent candidate state nodes satisfies both dynamic feasibility and collision-free nature, then the adjacent candidate state nodes are connected by a directed edge. The directed graph structure for multi-objective search is formed by the nodes and the directed edges.

6. The multi-objective path planning method for tracked vehicles based on rolling time-domain graph search according to claim 1, characterized in that, On the directed graph structure, collision risk cost, heading cost, and distance cost are arranged in descending order of priority, forming a multi-level cost function, including: The collision risk cost is set as a cost item characterizing the proximity between the tracked vehicle and the obstacle, and the collision risk cost is activated when the tracked vehicle enters a preset safe area; The heading cost is set as a cost term representing the difference between the tracked vehicle's orientation and the orientation of the nearest point on the global reference path, and the heading cost is activated when the difference exceeds a heading threshold. The distance cost is set as a strictly positive cost term characterizing the physical length of the candidate path; The multi-level cost function is formed according to the priority order of the collision risk cost, the heading cost, and the distance cost.

7. The multi-objective path planning method for tracked vehicles based on rolling time-domain graph search according to claim 1, characterized in that, Constructing a dictionary-optimized ranking based on the aforementioned multi-level cost function includes: A cost vector for candidate paths is constructed based on the collision risk cost, the heading cost, and the distance cost. The lexicographical order of the cost vector is used as the criterion for judging the merits of candidate paths; When comparing two candidate paths, the collision risk cost is compared first. When the collision risk costs of two candidate paths are equal, the heading costs are compared. When the collision risk cost and heading cost of two candidate paths are equal, the distance cost is compared. The comparison result of the distance cost does not change the priority of the collision risk cost and the heading cost, and the comparison result of the heading cost does not change the priority of the collision risk cost.

8. The multi-objective path planning method for tracked vehicles based on rolling time-domain graph search according to claim 1, characterized in that, Based on the dictionary-optimized sorting, a dictionary-optimized graph search algorithm is executed. During the node expansion process, higher-level costs are optimized first, and lower-level costs are compared only when higher-level costs are equal, to obtain local paths that satisfy level optimality, including: The candidate state node corresponding to the current pose is taken as the starting node; Starting from the starting node, select candidate state nodes to be expanded, and determine the adjacent candidate state nodes connected to the candidate state nodes to be expanded through directed edges; Calculate the incremental cost vector corresponding to the directed edge; The incremental cost vector is added to the cost vector for reaching the candidate state node to be expanded to obtain the candidate cost vector for reaching the adjacent candidate state node. The candidate cost vector is compared lexicographically with the recorded cost vectors corresponding to the adjacent candidate state nodes. If the candidate cost vector is lexicographically superior to the recorded cost vector, then update the cost vector and parent node of the adjacent candidate state node; The local path that satisfies the hierarchical optimality is formed by backtracking based on the updated parent node relationship.

9. The multi-objective path planning method for tracked vehicles based on rolling time-domain graph search according to claim 1, characterized in that, During the execution of the tracked vehicle along the current path to be executed, the system determines whether the current path to be executed is infeasible based on the latest perception data. If the current path to be executed is infeasible, a directed graph structure is regenerated based on a sliding window, and the dictionary-optimized graph search algorithm is executed based on the regenerated directed graph structure to obtain new feasible local paths, including: The sliding window is determined based on the current pose of the tracked vehicle and the length of the sliding window. The global reference path point in front of the sliding window is determined as the local target; Determine the obstacle space based on the latest sensing data; Determine whether the current path to be executed contains any path points that fall into the obstacle space; If the current path to be executed contains path points that fall into the obstacle space, then the directed graph structure is regenerated within the sliding window, starting from the current pose and targeting the local target. The dictionary-optimized graph search algorithm is applied to the regenerated directed graph structure to obtain the new feasible local path.

10. The multi-objective path planning method for tracked vehicles based on rolling time-domain graph search according to claim 1, characterized in that, Based on the current path to be executed, tracked vehicle motion control is performed to make the tracked vehicle follow the global reference path and avoid obstacles, forming a smooth trajectory that satisfies multiple objective priorities, including: Generate tracked vehicle motion control commands based on the current path to be executed; Based on the tracked vehicle motion control command, the tracked vehicle is controlled to travel along the current path to be executed, so that the tracked vehicle follows the global reference path and avoids obstacles; The motion control process of the tracked vehicle is updated based on its current position and orientation. The local path segments obtained from each replanning are spliced ​​together in chronological order, and the continuity of adjacent local path segments is satisfied at the splicing position. The smooth trajectory that satisfies the priority of multiple objectives is formed based on the spliced ​​local path segments.

Images