A Global Path Planning Method for Constrained Spaces Based on Multi-Step Ant Colony Orientation Guidance

Abstract

This invention discloses a Hybrid A* global path planning method for constrained spaces based on multi-step ant colony orientation guidance. The method first performs 2D raster modeling of the environmental map data and constructs a 2D raster map containing safety boundary constraints by combining obstacle inflation processing. Then, a Hybrid A* search space is established based on the starting state, target state, and kinematic constraints of the mobile platform, and a node evaluation function composed of actual cost and heuristic cost is constructed. During node expansion, a multi-step ant colony orientation guidance mechanism is introduced to constrain the orientation of candidate motion primitives, perform probabilistic screening, and prioritize expansion. Finally, a global reference path satisfying the kinematic constraints is generated through search update, backtracking, and path reconstruction. This invention can effectively reduce search redundancy and improve path continuity, smoothness, and executability.

Description

Technical Field

[0001] This invention belongs to the field of autonomous navigation and path planning technology for mobile robots, and in particular relates to a confined space Hybrid A* global path planning method based on multi-step ant colony direction guidance. Background Technology

[0002] With the continuous development of mobile robots, autonomous navigation, and unmanned systems technologies, path planning methods have been widely applied in tasks such as industrial inspection, warehousing and logistics, underground passage passage, unmanned transportation, and autonomous movement in complex environments. Global path planning, as a key component of autonomous navigation systems, primarily aims to generate a feasible path from the starting state to the target state for the mobile platform under known environmental map conditions, and to provide reference input for subsequent path tracking, local planning, and navigation control.

[0003] In confined spaces, traversable areas are typically constrained by both environmental boundaries and obstacles, characterized by narrow passages, densely distributed obstacles, and limited local traversable ranges. For such scenarios, path planning results must not only meet collision-free passage requirements but also consider path continuity, smoothness, and executability under the kinematic constraints of the mobile platform. Therefore, global path planning methods for confined spaces have always been an important research topic in the field of autonomous navigation for mobile robots.

[0004] Among existing global path planning methods, grid-based algorithms such as A* and Dijkstra can search for connected paths from the starting point to the end point in discrete maps. They have advantages such as simple implementation and stable search. However, these methods usually only perform graph search at the location level and do not explicitly consider the heading changes and kinematic constraints of the mobile platform. Therefore, the resulting paths often have obvious polylines, frequent turns, and are difficult to use directly for actual trajectory tracking.

[0005] To improve the shortcomings of traditional grid search methods in terms of kinematic executability, the Hybrid A* method introduces position and heading angle information into the search state simultaneously, and combines discrete control input and kinematic model for state advancement, which can generate feasible paths that meet certain kinematic constraints. Therefore, it has been widely used in the field of mobile robot and vehicle path planning.

[0006] However, existing Hybrid A* methods still have certain limitations in confined spatial environments. On the one hand, if all candidate motion primitives are fully expanded at each current node, a large number of collision branches or low-quality branches are easily generated, leading to increased search redundancy and decreased computational efficiency. On the other hand, in narrow passages, areas with dense obstacles, and near complex boundaries, if there is a lack of an effective directional guidance mechanism for candidate motion primitives, the search process is prone to frequently expanding candidate directions with poor proximity to the target, weak directional consistency, or significant interference from obstacles, resulting in problems such as more path bends, insufficient smoothness, and poor trackability.

[0007] Furthermore, existing methods in heuristic search typically focus on estimating distance costs, while failing to adequately consider factors such as target approximation, pose alignment, curvature constraints, and the impact of obstacle distribution. In confined spaces, relying solely on traditional heuristic costs for expansion selection often fails to balance search efficiency and path quality, easily leading to ineffective searches and inefficient expansion in locally complex regions.

[0008] Therefore, how to construct a Hybrid A* global path planning method for confined space environments that can satisfy the kinematic constraints of mobile platforms and effectively guide, filter, and prioritize candidate motion primitives during the node expansion stage, so as to reduce invalid expansion, improve search efficiency, and enhance path continuity and smoothness, has become an urgent technical problem to be solved in this field. Summary of the Invention

[0009] The purpose of this invention is to provide a confined space Hybrid A* global path planning method based on multi-step ant colony direction guidance, in order to solve the problems of existing global path planning methods in confined space environments, such as excessive invalid expansion, decreased search efficiency, obvious path polymorphism, and insufficient smoothness and executability, thereby improving the efficiency and quality of global path planning.

[0010] To achieve the objectives of this invention, a confined space Hybrid A* global path planning method based on multi-step ant colony orientation guidance is provided, comprising the following steps:

[0011] S1. Acquire environmental map data, perform two-dimensional raster modeling on the environmental map data, and expand the obstacle boundaries in combination with the external dimensions of the mobile platform and the preset safety distance to construct a two-dimensional raster map containing safety boundary constraints.

[0012] S2. Establish a Hybrid A* search space based on the starting state, target state, and kinematic constraints of the mobile platform, wherein the starting state and target state are both represented by position and heading angle; establish a node evaluation function for the nodes in the search process, wherein the node evaluation function includes true cost and heuristic cost, wherein the true cost is used to represent the cumulative path cost from the starting state to the current node, and the heuristic cost is used to estimate the remaining cost from the current node to the target state.

[0013] S3. In the node expansion stage, multiple candidate motion primitives are generated based on the current node's state information and preset discrete control inputs. A multi-step ant colony direction guidance mechanism is used to calculate the direction guidance weights of the candidate motion primitives. Based on the direction guidance weights, the candidate motion primitives are subjected to direction bias constraints, probabilistic screening, and priority expansion sorting to obtain an expansion direction set.

[0014] S4. Expand the current node according to the set of expansion directions to generate a new node; perform collision constraint detection and safe distance constraint detection on the new node based on the two-dimensional grid map containing safety boundary constraints, and calculate the comprehensive evaluation value of the new node based on the node evaluation function; add the new node that meets the constraint conditions and search update conditions to the open list, and add the current node that has been expanded to the closed list to complete the iterative update of the search process;

[0015] S5. When the search process in S4 finds the target state or meets the preset target neighborhood arrival conditions, backtracking and path reconstruction are performed based on the parent-child relationship between nodes to generate a global reference path that satisfies the kinematic constraints of the mobile platform; otherwise, steps S3 and S4 are executed.

[0016] S6. Output the global reference path for path tracking, local planning, or navigation control of the mobile platform in a confined space environment.

[0017] The significant advancement of this invention compared to existing technologies lies in:

[0018] (1) In view of the problem that the traditional Hybrid A* method in confined space environment is prone to generating a large number of invalid branches in the node expansion stage, the present invention introduces a multi-step ant colony direction guidance mechanism in the candidate motion primitive expansion process. By constructing direction guidance weights, the candidate motion primitives are subjected to direction bias constraints, probabilistic screening and priority expansion sorting, thereby reducing the entry of low-quality expansion directions with poor consistency with the target state direction and strong interference from obstacles into the search process, which can effectively reduce search redundancy and improve path search efficiency;

[0019] (2) The present invention introduces both real cost and heuristic cost in the node evaluation function. The real cost comprehensively considers length cost, turning cost, obstacle cost and reversing cost, while the heuristic cost comprehensively considers distance term, heading term and curvature term. This allows the path planning process to not only take into account target approach, but also attitude alignment, obstacle avoidance and curvature constraints. The generated path has better continuity, smoothness and traceability.

[0020] (3) This invention performs two-dimensional raster modeling of the environmental map and expands the obstacle boundaries by combining the dimensions of the mobile platform and the preset safety distance, so that obstacle constraints and safety boundary constraints can be directly mapped to the path planning environment. Compared with planning methods that do not consider the influence of safety boundaries, the path generated by this invention is more in line with the actual passage requirements in a confined space environment, and can improve the safety and engineering feasibility of the path planning results;

[0021] (4) In the search process, this invention manages nodes uniformly through open and closed lists, and prioritizes nodes based on comprehensive evaluation values, enabling the search process to proceed in an orderly manner while satisfying kinematic and obstacle avoidance constraints. Compared with existing methods that rely solely on traditional distance heuristics or full expansion strategies, this invention is more conducive to achieving effective convergence of the search direction in complex and constrained spaces;

[0022] (5) After the search is completed, the present invention performs reverse backtracking, order reversal, redundant node removal and optimization on the discrete path node sequence to obtain a global reference path that satisfies the kinematic constraints of the mobile platform. Compared with the method of directly outputting discrete polyline paths, the present invention can further improve the executability of the path and the adaptability of subsequent control, and is more convenient for the path tracking module, local planning module or navigation control module to call directly.

[0023] (6) This invention is applicable to various confined space environments with narrow passages, dense distribution of obstacles, and limited local passable areas. It has good versatility and scalability and can be widely used in global path planning tasks for mobile robots, autonomous platforms and unmanned systems. Attached Figure Description

[0024] The accompanying drawings, which are included to provide a further understanding of the invention and form part of this application, illustrate exemplary embodiments of the invention and, together with their description, serve to explain the invention and do not constitute an undue limitation thereof. In the drawings:

[0025] Figure 1 This is a flowchart of the steps of the present invention;

[0026] Figure 2 This is a schematic diagram of node expansion guided by the multi-step ant colony direction in this invention. Detailed Implementation

[0027] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. 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.

[0028] This invention provides a confined space Hybrid A* global path planning method based on multi-step ant colony orientation guidance, combined with... Figure 1 This includes the following steps:

[0029] S1. Acquire environmental map data, perform two-dimensional raster modeling on the environmental map data, and expand the obstacle boundaries in combination with the external dimensions of the mobile platform and the preset safety distance to construct a two-dimensional raster map containing safety boundary constraints.

[0030] Step S1 includes the following steps:

[0031] S11. Obtain environmental map data and extract obstacle information and passable area information from the environmental map data;

[0032] Specifically, environmental map data can be derived from lidar point cloud maps, raster maps, occupancy maps, or other map data capable of representing the spatial distribution relationships of the environment. The acquired environmental map data should at least reflect obstacle boundaries, free passage areas, and spatial occupancy relationships within the environment;

[0033] S12. Perform two-dimensional raster modeling on the environmental map data, discretize the environment into multiple raster units, and generate an initial two-dimensional raster map according to the occupancy status of each raster unit. Raster units occupied by the obstacle information are marked as obstacle rasters, and raster units not occupied by the obstacle information are marked as passable rasters.

[0034] Specifically, after extracting obstacle and passable area information, the environmental map is discretized in two dimensions, dividing the continuous environmental space into multiple regularly arranged grid cells. Each grid cell corresponds to a local area in the environment, and its occupancy status is determined based on whether there are obstacles within that local area.

[0035] S13. Combining the external dimensions of the mobile platform and the preset safety distance, perform dilation processing on the obstacle grid boundaries in the initial two-dimensional grid map to generate a two-dimensional grid map containing safety boundary constraints.

[0036] Specifically, because the mobile platform has a certain external dimension during actual operation, and usually needs to maintain a preset safe distance from obstacles when moving in confined spaces, path planning cannot be performed directly based on the initial two-dimensional grid map. Therefore, this embodiment expands the boundary of the obstacle grid according to the external dimensions of the mobile platform and the preset safe distance.

[0037] S2. Establish a Hybrid A* search space based on the starting state, target state, and kinematic constraints of the mobile platform, wherein the starting state and target state are both represented by position and heading angle; establish a node evaluation function for the nodes in the search process, wherein the node evaluation function includes true cost and heuristic cost, wherein the true cost is used to represent the cumulative path cost from the starting state to the current node, and the heuristic cost is used to estimate the remaining cost from the current node to the target state.

[0038] Step S2 includes the following steps:

[0039] S21. Obtain the starting state and target state of the mobile platform, and represent the starting state and target state as pose states containing position and heading angle information respectively;

[0040] In this embodiment, the mobile platform state vector is defined as follows:

[0041] ;

[0042] in, and This represents the position coordinates of the mobile platform in a two-dimensional map coordinate system. This indicates the current heading angle of the mobile platform; correspondingly, the starting state can be represented as:

[0043] ;

[0044] The target state can be represented as:

[0045] ;

[0046] S22. A Hybrid A* search space is established based on the kinematic constraints of the mobile platform. The state transitions in the search space are obtained by discretizing the kinematic equations corresponding to the preset discrete control inputs.

[0047] In this embodiment, discrete time is assumed The control input is:

[0048] ;

[0049] in, This represents the longitudinal velocity at the current moment. This represents the steering control input at the current moment. Based on the discretization of the kinematic model, the state transition relationship can be expressed as:

[0050] ;

[0051] in, Indicates the current state. Indicates the state at the next moment. This is the state transition function obtained by discretizing the kinematic model of the mobile platform. By traversing the preset discrete control inputs, multiple possible successor states can be generated from the current state, thereby establishing a Hybrid A* search space that satisfies kinematic constraints;

[0052] S23. Based on the preset discrete control input and state transition method, the current node is state-advanced;

[0053] S24. Establish a node evaluation function for the nodes in the search process. The node evaluation function includes the true cost and the heuristic cost. The true cost includes at least the length cost, turning cost, obstacle cost and reversing cost. The heuristic cost includes at least the distance term, heading term and curvature term.

[0054] In this embodiment, the overall evaluation function is defined as:

[0055] ;

[0056] in, For the real cost, The cost of inspiration;

[0057] The actual cost is used to characterize the cumulative path cost from the starting state to the current node, and can be expressed as:

[0058] ;

[0059] in, Indicates from state Extend to state The cost per step;

[0060] Furthermore, the single-step cost can be expressed as:

[0061] ;

[0062] in, The arc length corresponding to the current expansion step size. This represents the angle normalization function. This indicates the safe distance from the current obstacle. An indicator item indicating whether to use the reversing motion element. , , , These are the corresponding weighting coefficients; therefore, the actual cost includes length cost, turning cost, obstacle cost, and reversing cost, respectively.

[0063] The heuristic cost is used to estimate the remaining cost from the current state to the target state, and can be expressed as:

[0064] ;

[0065] in, This represents the Euclidean distance between the current state and the target state. Indicates the target state heading angle. This represents the curvature corresponding to the current motion. , , These are the corresponding weighting coefficients. Therefore, the heuristic cost includes distance, heading, and curvature terms, respectively.

[0066] S25. Calculate the comprehensive evaluation value of the node based on the actual cost and the heuristic cost, so as to use it for node priority evaluation in the subsequent search process.

[0067] S3, Combination Figure 2 During the node expansion phase, multiple candidate motion primitives are generated based on the current node's state information and preset discrete control inputs. A multi-step ant colony direction guidance mechanism is used to calculate the direction guidance weights of the candidate motion primitives, and direction bias constraints, probabilistic screening, and priority expansion sorting are performed on the candidate motion primitives according to the direction guidance weights to obtain an expansion direction set.

[0068] Step S3 includes the following steps:

[0069] S31. At the current node, generate multiple candidate motion elements according to the preset discrete control input, and extract the direction information, pose advancement results and relative relationship information between each candidate motion element and the target state.

[0070] In this embodiment, the current state is assumed to be Its scalable set of motion primitives is denoted as Defines a path from the current state to the target state. The target direction angle is

[0071] ;

[0072] in and Representing the position coordinates of the mobile platform in the two-dimensional map coordinate system, respectively, for candidate motion primitives The generated successor state Let its heading angle be . The direction error corresponding to the candidate motion element is defined as:

[0073] ;

[0074] The direction error is used to characterize the degree of deviation between the propulsion direction of the candidate motion element and the direction of the target state. The smaller the direction error, the more conducive the candidate motion element is to guiding the search towards the direction of the target state.

[0075] Combination Figure 2 The following further explains the candidate motion primitive expansion method in this invention. In the figure, S represents the current expansion node, G represents the target state node, gray arrows radiating outwards from the current node represent multiple candidate motion primitives generated based on preset discrete control inputs, red arrows represent the expansion directions and search progress trends preferentially retained after screening by a multi-step ant colony direction guidance mechanism, blue areas represent traversable areas in a confined space environment, and the central star-shaped marker represents the intermediate expansion node selected during the search process. Figure 2 It can be seen that the present invention calculates the directional guidance weights, performs probabilistic screening and prioritizes the expansion of candidate motion primitives, so that the expansion directions that are more consistent with the target state direction and better meet the passage constraints participate in the search first, thereby reducing invalid expansion and improving path search efficiency.

[0076] S32. Based on the pose state of the current node, the target state, the orientation information of the candidate motion primitives, the pose advancement result, the relative relationship information between the current node and the target state, and the obstacle distribution information in the two-dimensional grid map containing safety boundary constraints, construct the orientation guidance weights corresponding to each candidate motion primitive.

[0077] In this embodiment, a directional consistency pheromone is first constructed based on the directional error, and is represented as follows:

[0078] ;

[0079] in, As candidate motion primitives, The direction penalty coefficient, The larger the value, the closer the extended heading of the candidate motion element is to the target state direction;

[0080] To ensure that the direction guidance weights are consistent with the heuristic costs in the node evaluation function, the heuristic attractiveness of the candidate motion primitives is further defined as follows:

[0081] ;

[0082] in, Candidate motion primitives The corresponding successor state, This represents the heuristic cost corresponding to the successor state. Therefore, when the successor state generated by a candidate motion primitive is closer to the target state, has a higher degree of attitude alignment, and better curvature constraints, its heuristic attractiveness is... Larger;

[0083] S33. A multi-step ant colony directional guidance mechanism is used to iteratively update the directional guidance weights of each candidate motion primitive, so that the candidate motion primitives with higher consistency with the target state direction, better travel trend and less interference from obstacles are given higher expansion priority.

[0084] In this embodiment, the candidate motion primitive set is... Middle Action The selection probability is defined as:

[0085] ;

[0086] in, and These are the pheromone weight coefficient and the heuristic attraction weight coefficient, used to adjust the relative influence of directional consistency and heuristic cost in the candidate motion primitive selection process. Through this method, the directional guidance weights of candidate motion primitives can be updated, allowing candidate motion primitives with better directional and mobility characteristics to receive higher expansion priority.

[0087] S34. Based on the updated directional guidance weights, perform directional bias constraints and probabilistic screening on the candidate motion primitives, and prioritize and sort the candidate motion primitives according to the screening results.

[0088] Specifically, after obtaining the selection probability of each candidate motion element, in order to balance the convergence and exploratory nature of the search process, this embodiment uses a pseudo-random proportional rule to probabilistically screen the candidate motion elements. For the candidate motion elements at the current node, if the greedy selection condition is met, the candidate motion element with the largest directional guidance weight is selected first; if the greedy selection condition is not met, random sampling is performed according to the selection probability distribution of the candidate motion elements, thereby preserving the target guidance while avoiding the search process from prematurely falling into local inefficient expansion.

[0089] In this embodiment, the candidate motion primitive selection rule actually used for expansion is expressed as follows:

[0090] ;

[0091] in, , This is a preset pseudo-random threshold. When... When, prioritize the candidate motion primitive with the largest directional guidance weight; when At that time, random sampling is performed according to the selection probability distribution;

[0092] Meanwhile, in order to suppress expansion directions that deviate significantly from the target state direction or are easily interfered with by obstacles, this embodiment applies a direction offset constraint to the corresponding candidate motion primitives, causing them to move to the back of the priority expansion sorting, thereby reducing the possibility of low-quality expansion directions entering the preferred set.

[0093] S35. Based on the sorting results, retain the candidate motion primitives with higher priority to form the set of expansion directions corresponding to the current node, which will be used for subsequent node expansion and search.

[0094] Specifically, after completing probabilistic screening and priority expansion sorting, in order to further control the branching factor and improve search efficiency, this embodiment only retains a preset number of candidate motion primitives at the top of the sort as the set of expansion directions for the current node. By limiting the number of candidate motion primitives participating in the actual expansion in each round of search, invalid expansions and collision branches can be effectively reduced, thereby improving search efficiency in confined space environments.

[0095] In this embodiment, the comprehensive directional guidance weight of the candidate motion primitive is defined as...

[0096] ;

[0097] Furthermore, retain the top-weighted... The candidate motion primitives are represented as the set of extended directions, as follows:

[0098] ;

[0099] in, The preset number of elements to retain is determined by the Top-K pruning method described above. Candidate motion primitives that are more consistent with the target state direction, have lower heuristic costs, and are less affected by obstacles are prioritized for retention, forming the expansion direction set corresponding to the current node for subsequent node expansion and search.

[0100] S4. Expand the current node according to the set of expansion directions to generate a new node; perform collision constraint detection and safe distance constraint detection on the new node based on the two-dimensional grid map containing safety boundary constraints, and calculate the comprehensive evaluation value of the new node based on the node evaluation function; add the new node that meets the constraint conditions and search update conditions to the open list, and add the current node that has been expanded to the closed list to complete the iterative update of the search process;

[0101] Step S4 includes the following steps:

[0102] S41. Perform expansion on the current node according to the set of expansion directions to generate a corresponding new node;

[0103] Specifically, after obtaining the set of expansion directions at the current node, the candidate motion primitives in the set of expansion directions are used as the actual expansion objects to advance the state of the current node; each selected candidate motion primitive corresponds to a set of discrete control inputs, and the state transition function is used to advance the state of the current node. Generate a corresponding successor state. The successor state is the new node obtained by expanding the current node.

[0104] S42. Based on the two-dimensional grid map containing safety boundary constraints, perform collision constraint detection and safety distance constraint detection on the new nodes, and remove invalid nodes that collide with obstacle grids or do not meet safety boundary constraints.

[0105] Specifically, after generating a new node, a feasibility test is performed on each new node to determine whether it meets the safe passage requirements in a confined space environment. The feasibility test includes collision constraint detection and safe distance constraint detection.

[0106] The collision constraint detection is used to determine whether the pose state of the new node falls into the obstacle grid area, or whether the motion trajectory of the new node overlaps with the obstacle area; if a collision occurs, the new node is determined to be an invalid node. The safety distance constraint detection is used to determine whether the new node enters the obstacle expansion area or violates the preset safety boundary constraint; if the safety boundary requirement is not met, the new node is also determined to be an invalid node.

[0107] S43. Based on the node evaluation function, calculate the true cost, heuristic cost, and comprehensive evaluation value for new nodes that meet the constraints, and determine the sorting priority of the new node in the open list based on the comprehensive evaluation value.

[0108] Specifically, for valid new nodes detected through collision constraint detection and safe distance constraint detection, their costs are evaluated using the aforementioned node evaluation function. First, the true cost corresponding to the new node is calculated. This is used to characterize the cumulative path cost from the starting state through the current node to the new node; then, the heuristic cost corresponding to the new node is calculated. This is used to estimate the remaining cost from the new node to the target state; finally, the actual cost and the heuristic cost are combined to obtain the comprehensive evaluation value of the new node.

[0109] ;

[0110] The smaller the comprehensive evaluation value, the more favorable it is for the corresponding new node to advance towards the target state while satisfying kinematic constraints and obstacle avoidance requirements;

[0111] Therefore, this embodiment determines the sorting priority of each new node in the open list based on its comprehensive evaluation value, so that new nodes with better comprehensive evaluation values ​​are selected for expansion in subsequent search processes.

[0112] S44. Add new nodes that meet the search update conditions to the open list, and add current nodes that have completed expansion to the closed list;

[0113] Specifically, after completing the comprehensive evaluation of new nodes, new nodes that meet the search update conditions are added to the open list for priority ranking and node selection in subsequent search processes. The search update conditions may include, but are not limited to: the new node has not been visited, or although the new node already exists in the open list, reaching the new node via the current path can yield a better actual cost or comprehensive evaluation value;

[0114] For a current node that has already completed its expansion process, it is added to the closed list, indicating that the candidate expansion directions for that node have been filtered, expanded, and updated, and it will no longer participate in subsequent searches. Through the combined management of open and closed lists, this embodiment can avoid repeatedly expanding processed nodes and ensure that the search process proceeds in an orderly manner driven by comprehensive evaluation values.

[0115] S45. Select the node with the best comprehensive evaluation value from the open list as the next current node, and repeat the node expansion, constraint detection, cost calculation and list update process until the target state is found, the preset target neighborhood reach condition is met or other preset termination conditions are met.

[0116] Specifically, after each round of search iteration, the node with the best comprehensive evaluation value is selected from the open list as the next current node, and the operations such as generating the expansion direction set, expanding the new node, collision constraint detection, safety distance constraint detection, calculating the comprehensive evaluation value, and updating the open list and closed list are repeatedly performed on this node;

[0117] The search process can terminate when any of the following conditions are met: First, a node with the same state as the target is found; second, the distance and heading deviation between the current node and the target state meet the preset target neighborhood arrival conditions; third, other preset termination conditions are met, such as the open list being empty or the number of search iterations reaching the set limit.

[0118] S5. When the search process in S4 finds the target state or meets the preset target neighborhood arrival conditions, reverse backtracking and path reconstruction are performed based on the parent-child relationship between nodes to generate a global reference path that meets the kinematic constraints of the mobile platform.

[0119] Step S5 includes the following steps:

[0120] S51. When the search process in S4 finds a target state or the distance and heading deviation between the current node and the target state meet the preset target neighborhood arrival conditions, the search process is terminated.

[0121] Specifically, after updating the open and closed lists, it is determined whether the current search process meets the path search termination conditions. The termination conditions include: first, a node consistent with the target state has been generated during the search process; second, the Euclidean distance and heading angle deviation between the current node and the target state are both less than a preset threshold, that is, the preset target neighborhood reach condition is met.

[0122] When any of the above conditions are met, it can be considered that the current search process has obtained a feasible path result that can represent the path from the starting state to the target state, thereby terminating the search process and entering the path backtracking and reconstruction stage.

[0123] S52. Take the node that meets the termination condition as the target associated node, and backtrack from the target associated node to the starting node according to the parent-child relationship between the nodes to obtain the discrete path node sequence.

[0124] Specifically, after the search process terminates, the nodes that meet the termination conditions are recorded as target associated nodes. Since each new node records its parent node information when it is generated during the search process, the parent-child relationship between nodes can be followed to backtrack from the target associated node to the starting node, thus obtaining a discrete path node sequence arranged from the end point to the starting point.

[0125] S53. Reverse the order of the discrete path node sequence and connect the paths to obtain an initial global path from the starting state to the target state;

[0126] Specifically, since the discrete path node sequence obtained by backtracking through the parent-child relationship is arranged in the order from the target associated node to the starting node, it is necessary to perform an order reversal process on the discrete path node sequence to convert it into a forward path sequence from the starting state to the target state.

[0127] S54. Perform redundancy removal and optimization processing on the initial global path to generate a global reference path that satisfies the kinematic constraints of the mobile platform.

[0128] Specifically, since the initial global path obtained through discrete search may still have problems such as a large number of redundant nodes, frequent local bends, or insufficient path continuity, this embodiment further optimizes the initial global path. The optimization process includes redundant node removal and path smoothing optimization.

[0129] Among them, the redundancy removal process is used to delete redundant nodes that contribute little to the overall shape of the path and do not affect the connectivity and feasibility of the path after deletion, so as to reduce the number of path nodes and improve the simplicity of path expression; the path smoothing optimization process is used to improve the polyline characteristics between adjacent path segments in the initial global path, so that the path has better continuity and smoothness under the premise of satisfying obstacle constraints, safety boundary constraints and mobile platform kinematic constraints.

[0130] S6. Output the global reference path for path tracking, local planning, or navigation control of the mobile platform in a confined space environment;

[0131] Specifically, after obtaining a global reference path that satisfies the kinematic constraints of the mobile platform, the global reference path is output to the path tracking module, local planning module, or navigation control module of the mobile platform as a reference input for subsequent trajectory tracking, local obstacle avoidance, and navigation control.

[0132] The path tracking module can extract path point sequences, heading information, or curvature change information based on the global reference path to generate tracking control commands for the mobile platform; the local planning module can combine current sensor perception results and local environmental changes to perform local path adjustment and dynamic obstacle avoidance under the constraints of the global reference path; and the navigation control module can complete speed control, steering control, and attitude adjustment of the mobile platform in a confined space environment based on the global reference path and local planning results.

[0133] By outputting the global reference path to the aforementioned module, this embodiment achieves an effective connection between global path search and path execution control, enabling the mobile platform to autonomously navigate along the global reference path in a confined space environment. Simultaneously, the global reference path can also serve as the basis for subsequent path optimization, trajectory replanning, or navigation decisions, thereby improving the continuity, stability, and executability of the overall system navigation process.

[0134] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus.

[0135] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A global path planning method for confined spaces based on multi-step ant colony orientation guidance, characterized in that, Includes the following steps: S1. Acquire environmental map data, perform two-dimensional raster modeling on the environmental map data, and expand the obstacle boundaries in combination with the external dimensions of the mobile platform and the preset safety distance to construct a two-dimensional raster map containing safety boundary constraints. S2. Establish a Hybrid A* search space based on the starting state, target state, and kinematic constraints of the mobile platform, wherein the starting state and target state are both represented by position and heading angle. A node evaluation function is established for the nodes in the search process. The node evaluation function includes the true cost and the heuristic cost. The true cost is used to characterize the cumulative path cost from the starting state to the current node, and the heuristic cost is used to estimate the remaining cost from the current node to the target state. S3. During the node expansion phase, multiple candidate motion primitives are generated based on the current node's state information and preset discrete control inputs. A multi-step ant colony direction guidance mechanism is used to calculate the direction guidance weight of the candidate motion primitives, and the candidate motion primitives are subjected to direction bias constraints, probabilistic screening and priority expansion sorting based on the direction guidance weight to obtain the expansion direction set. S4. Expand the current node according to the set of expansion directions to generate a new node; Based on the two-dimensional grid map containing safety boundary constraints, collision constraint detection and safety distance constraint detection are performed on the new node, and the comprehensive evaluation value of the new node is calculated based on the node evaluation function; new nodes that meet the constraint conditions and search update conditions are added to the open list, and the current nodes that have been expanded are added to the closed list to complete the iterative update of the search process; S5. When the search process in S4 finds the target state or meets the preset target neighborhood arrival conditions, backtracking and path reconstruction are performed based on the parent-child relationship between nodes to generate a global reference path that satisfies the kinematic constraints of the mobile platform; otherwise, steps S3 and S4 are executed. S6. Output the global reference path for path tracking, local planning, or navigation control of the mobile platform in a confined space environment.

2. The global path planning method for confined space based on multi-step ant colony orientation guidance according to claim 1, characterized in that, S1 includes the following steps: S11. Obtain environmental map data and extract obstacle information and passable area information from the environmental map data; S12. Perform two-dimensional raster modeling on the environmental map data, discretize the environment into multiple raster units, and generate an initial two-dimensional raster map according to the occupancy status of each raster unit. Raster units occupied by the obstacle information are marked as obstacle rasters, and raster units not occupied by the obstacle information are marked as passable rasters. S13. Combining the external dimensions of the mobile platform and the preset safety distance, perform dilation processing on the obstacle grid boundaries in the initial two-dimensional grid map to generate a two-dimensional grid map containing safety boundary constraints.

3. The global path planning method for confined space based on multi-step ant colony orientation guidance according to claim 1, characterized in that, S2 includes the following steps: S21. Obtain the starting state and target state of the mobile platform, and represent the starting state and target state as pose states containing position and heading angle information respectively; S22. A Hybrid A* search space is established based on the kinematic constraints of the mobile platform. The state transitions in the search space are obtained by discretizing the kinematic equations corresponding to the preset discrete control inputs. S23. Based on the preset discrete control input and state transition method, the current node is state-advanced; S24. Establish a node evaluation function for the nodes in the search process. The node evaluation function includes the true cost and the heuristic cost. The true cost includes at least the length cost, turning cost, obstacle cost and reversing cost. The heuristic cost includes at least the distance term, heading term and curvature term. S25. Calculate the comprehensive evaluation value of the node based on the actual cost and the heuristic cost, so as to use it for node priority evaluation in the subsequent search process.

4. The global path planning method for confined space based on multi-step ant colony orientation guidance according to claim 1, characterized in that, S3 includes the following steps: S31. At the current node, generate multiple candidate motion elements according to the preset discrete control input, and extract the direction information, pose advancement results and relative relationship information between each candidate motion element and the target state. S32. Based on the pose state of the current node, the target state, the orientation information of the candidate motion primitives, the pose advancement result, the relative relationship information between the current node and the target state, and the obstacle distribution information in the two-dimensional grid map containing safety boundary constraints, construct the orientation guidance weights corresponding to each candidate motion primitive. S33. A multi-step ant colony directional guidance mechanism is used to iteratively update the directional guidance weights of each candidate motion primitive, so that the candidate motion primitives with higher consistency with the target state direction, better travel trend and less interference from obstacles are given higher expansion priority. S34. Based on the updated directional guidance weights, perform directional bias constraints and probabilistic screening on the candidate motion primitives, and prioritize and sort the candidate motion primitives according to the screening results. S35. Based on the sorting results, retain the candidate motion primitives with higher priority to form the set of expansion directions corresponding to the current node, which will be used for subsequent node expansion and search.

5. The global path planning method for confined space based on multi-step ant colony orientation guidance according to claim 1, characterized in that, S4 includes the following steps: S41. Perform expansion on the current node according to the set of expansion directions to generate a corresponding new node; S42. Based on the two-dimensional grid map containing safety boundary constraints, perform collision constraint detection and safety distance constraint detection on the new nodes, and remove invalid nodes that collide with obstacle grids or do not meet safety boundary constraints. S43. Based on the node evaluation function, calculate the true cost, heuristic cost, and comprehensive evaluation value for new nodes that meet the constraints, and determine the sorting priority of the new node in the open list based on the comprehensive evaluation value. S44. Add new nodes that meet the search update conditions to the open list, and add current nodes that have completed expansion to the closed list; S45. Select the node with the best comprehensive evaluation value from the open list as the next current node, and repeat the node expansion, constraint detection, cost calculation and list update process until the target state is found, the preset target neighborhood reach condition is met or other preset termination conditions are met.

6. The global path planning method for confined space based on multi-step ant colony orientation guidance according to claim 1, characterized in that, S5 includes the following steps: S51. When the search process in S4 finds the target state or the distance and heading deviation between the current node and the target state meet the preset target neighborhood arrival conditions, the search process is terminated; otherwise, steps S3 and S4 are executed. S52. Take the node that meets the termination condition as the target associated node, and backtrack from the target associated node to the starting node according to the parent-child relationship between the nodes to obtain the discrete path node sequence. S53. Reverse the order of the discrete path node sequence and connect the paths to obtain an initial global path from the starting state to the target state; S54. Perform redundancy removal and optimization processing on the initial global path to generate a global reference path that satisfies the kinematic constraints of the mobile platform.

7. The global path planning method for confined space based on multi-step ant colony orientation guidance according to claim 1, characterized in that, In step S6, the global reference path from step S5 is output to the path tracking module, local planning module, or navigation control module of the mobile platform as a reference input for subsequent trajectory tracking, local planning, and navigation control, so as to guide the mobile platform to complete autonomous passage along the global reference path in a confined space environment.

Images