Mobile robot robust path planning method for unknown obstacle environment

Abstract

The invention discloses a mobile robot robust path planning method oriented to an unknown obstacle environment. The method adopts a cooperative scheme of a two-way inert search strategy, a structure sensing path repair mechanism and an ELCTRE multi-target decision-making mechanism. The method comprises the following steps: generating a high-quality initial path through two-way inert expansion of four expansion trees and delayed collision detection; when the path is fractured, constructing a local connectivity compensation area and repairing the path by combining a dynamic sampling radius mechanism; and based on the path length, the steering smoothness and the heuristic cost, an optimal temporary target point is screened through an ELCTRE dominating relation to complete replanning. Experiments show that in a complex scene, the planning success rate reaches 93%-99%, the planning time is shortened by 26%-59%, the path length is reduced by 4%-12%, the method can be widely applied to scenes such as building construction and post-disaster rescue, and the navigation robustness and efficiency of the robot in a dynamic environment are remarkably improved.

Description

Technical Field

[0001] This invention belongs to the field of autonomous navigation and path planning technology for mobile robots, specifically relating to a robust path planning method for mobile robots in environments with unknown obstacles. Background Technology

[0002] With the widespread application of mobile robots in industry, rescue, and exploration, the working environments they face are becoming increasingly complex. Especially in unknown or partially known environments, dynamic obstacle interference, local path failures, and incomplete navigation maps have become key challenges restricting autonomous robot navigation. Path planning, as a core component of autonomous robot navigation, not only needs to generate high-quality paths in prior known environments but also needs robust planning and adaptive reconstruction capabilities in dynamic environments. Currently, mainstream path planning algorithms are mainly divided into two categories:

[0003] I. Graph Search-Based Methods: Represented by Dijkstra's algorithm, A* algorithm, and its incremental version Lifelong Planning A* (LPA*), these algorithms search for the shortest path on discrete grids or graph structures, offering advantages such as strong determinism and clear structure. LPA*, in particular, introduces an incremental update mechanism, enabling efficient path updates when the graph structure changes locally, providing a foundation for dynamic programming algorithms like D*Lite, and performing well in static or semi-static environments where obstacles change slowly. However, when the environmental dimensionality increases and the state space becomes continuous and frequently changing, pure graph search methods suffer from insufficient spatial coverage and a sharp increase in computational overhead, making them unsuitable for highly dynamic and complex environments.

[0004] II. Sampling-Based Methods: To address the needs of path planning in high-dimensional continuous spaces, sampling-based methods have gradually become mainstream, with typical examples including Rapid Random Tree Exploration (RRT) and Probabilistic Road Graph (PRM). RRT constructs a search tree through random sampling and incremental expansion, possessing strong spatial exploration capabilities, but path quality is difficult to guarantee. Subsequent improved RRT algorithms achieve asymptotic optimality through path reconnection operations, Lazy PRM reduces invalid computation through delayed collision detection, Batch Heuristic Tree (BIT*) algorithms utilize batch sampling and global heuristic information to improve search directionality, and Adaptive Heuristic Tree (AIT*) algorithms combine heuristic guidance and reverse search structure optimization to improve cost estimation. To address the asymmetric search structure problem of AIT*, the BiAIT* algorithm proposes a symmetric bidirectional search strategy, simultaneously performing lazy expansion and exchanging heuristic information from both the start and end points, improving adaptability to complex environments.

[0005] Although the above algorithms perform well in the initial path generation stage, the limitations of existing technologies become apparent when the robot encounters unknown obstacles during actual operation, causing the path to break.

[0006] 1. Poor reliability of path repair: Most algorithms rely on local random sampling to repair paths. In narrow channels, sparsely sampled areas, or complex obstacle environments, random sampling is prone to getting stuck in invalid expansions, making it difficult to find feasible connections and leading to repair failure.

[0007] 2. Low replanning efficiency: Some methods require a global replanning restart after the path is interrupted, which has a large computational cost and the new path deviates greatly from the original path, causing drastic path fluctuations;

[0008] 3. Lack of intelligent decision-making: When reconstructing paths, the selection of candidate target points relies solely on single indicators such as distance and cost, lacking global guidance and multi-factor decision-making criteria. This can easily lead to disjointed, circuitous, and lengthy replanned paths, or even failure to reconnect.

[0009] In summary, existing technologies struggle to simultaneously meet the robustness, real-time performance, and consistency requirements of path planning in dynamic, unknown obstacle environments, necessitating a new technical solution to address these pain points. Summary of the Invention

[0010] The purpose of this invention is to overcome the shortcomings of existing technologies in path planning under dynamic unknown obstacle environments. This invention provides a robust path planning method for mobile robots in unknown obstacle environments. This method combines a bidirectional lazy search strategy, a structure-aware path repair mechanism, and a multi-criteria decision-making mechanism, which improves the path generation and recovery capabilities of mobile robots in dynamic environments, enhances the robustness and rapid reconstruction capabilities of mobile robots in path planning under unknown obstacle interference, and provides a new path planning framework that enables robots to maintain the high quality of the initial path while quickly repairing the obstructed path and optimizing subsequent travel routes, thereby significantly improving the reliability, consistency, and efficiency of navigation in dynamic environments.

[0011] This invention proposes a robust path planning method for mobile robots in environments with unknown obstacles, which includes the following steps:

[0012] S1. Initial configuration and tree structure establishment: Set the starting point, ending point and map environment for the mobile robot path planning, configure path planning related parameters, establish a bidirectional search tree structure and forward and reverse expansion trees, and complete the initial guidance sampling and adaptive determination of the connection radius. The connection radius is dynamically adjusted with the number of samples.

[0013] S2, Bidirectional Lazy Collaborative Expansion: The bidirectional search trees start from the beginning and end points respectively, and adopt a heuristic expansion strategy based on lazy LPA* to delay collision detection and exchange heuristic information, thereby accelerating the merging of the two trees to generate initial path candidates.

[0014] S3. Path Collision Repair and Optimization: Collision detection is performed on the initial candidate paths. For collided path segments, local reconnection or structure-aware local connectivity compensation sampling methods are used to repair the paths. Then, through pruning, smoothing, curvature optimization, and constraint sampling iterative optimization, feasible optimized paths are obtained; and

[0015] S4. Dynamic Reconnection Decision and Path Reconstruction: When the robot travels along the optimized path, if the path is detected to be broken due to an unknown obstacle, a set of candidate temporary target points is constructed with the break position as the current node. The optimal temporary target point is selected by multi-index evaluation and ELECTRE dominance relationship ranking. Based on the target point, the path replanning is completed to achieve robust path navigation in the environment of unknown obstacles.

[0016] Preferably, the bidirectional search tree in step S1 includes a forward lazy search tree rooted at the starting point and a backward lazy search tree rooted at the ending point; the path planning related parameters include expansion step size, connection threshold, heuristic weight, sampling strategy, maximum number of iterations, and safety margin; the formula for calculating the connection radius is:

[0017]

[0018] Where q represents the number of sampling points, η is the adjustment parameter, and n is the spatial dimension. For the free region measure, ζ n Let be the volume of an n-dimensional unit sphere.

[0019] Preferably, the bidirectional inertial cooperative expansion described in step S2 satisfies the sufficient rendezvous condition |xx g |+|xx s |≤(1+ε)·C lower , where |xx g |、|xx s | represents the lower bound of the Euclidean distance from the current node to the target point and the starting point, respectively, C. lower is the lower bound of the optimal path cost from the starting point to the ending point, and is the error coefficient; the forward lazy search tree passes the heuristic value from the starting point to the backward expansion tree, and the backward lazy search tree passes the heuristic value from the ending point to the forward expansion tree.

[0020] Further preferably, any candidate node x within the forward lazy search tree i A step forward inspired RHS for This expression represents node x i heuristic value The cumulative cost of neighboring nodes With edge cost The sum and decision; the one-step prospect of the node within the reverse lazy search tree is inspired by... The cumulative estimated cost in the destination direction. For the cost estimation of an edge, this formula represents the cost of node x. i The heuristic value is the sum of the cumulative costs of its neighbors added to x. i The edge cost is determined by taking the minimum value as the lower bound of the estimate.

[0021] Preferably, the structure-aware local connectivity compensation sampling in step S3 introduces a path bend-driven dynamic sampling radius mechanism, with a sampling distance of [missing information]. Where θ fse The path at point x s The angle at the point is d0, the basic sampling radius is d0, and κ and γ are adjustment factors, k∈(0,1], γ≥1.

[0022] More preferably, the process of constructing the local connectivity compensation region is as follows:

[0023] Let the current path segment E be... fail =(x s ,x e The edge is determined to be impassable, and its endpoints are the forward connect points x and x. s and backward connection point x e Let the upstream and downstream nodes of the connection point be x and x respectively. f x b , where: x f The upstream connector at the beginning of the path segment is connected to x. s ;x b For the downstream connection point at the end of the path segment, connected to x e ;

[0024] Connect the upstream connection point x f With forward connection point x s Connect the neighboring points of x to form a set of forward sector intervals; similarly, connect x... b With x e Connecting the neighboring points, the geometric intersection of the two sectors yields the intersection constraint point x. dcp1 x dcp2 , will x f x b x dcp1 x dcp2 Projecting these lines onto the horizontal and vertical directions respectively creates a set of orthogonal boundary lines, which ultimately enclose a rectangular area, known as the local connectivity compensation region.

[0025] Preferably, the constraint sampling iterative optimization in step S3 refers to constructing a path with the starting point and the ending point as the focus, which includes the current optimal path length C. best Elliptical region:

[0026]

[0027] Where x s With x g These are the starting point and the ending point, respectively. s ‖+‖xx g || represents the sum of distances from point x to the two foci; high-density sampling and local reconnection are performed within the elliptical region, and C is updated if the new path has a better cost. best The elliptical domain is then narrowed, and the path quality is iteratively optimized.

[0028] Preferably, the source of the candidate temporary target point set in step S4 includes the original reverse search tree nodes near the breakpoint, locally newly sampled nodes, and path skeleton screening nodes; the multi-index evaluation indexes include path length, turning smoothness, and heuristic cost.

[0029] More preferably, the steering smoothness is determined by the current heading angle α. cur Calculation of the angle between the current heading angle and the candidate point's orientation angle, and the current heading angle α. cur The corresponding unit direction vector is v cur =[cosψ cur ,sinψ cur Candidate point q i =(x i ,y i ); Current position is q cur =(x cur ,y cur If the current position points to the candidate point, then the unit direction vector is... The angle between the two is the steering angle θ. i =arccos(clip(v cur ·v i ,-1,1)),θ i ∈[0,π].; The heuristic cost is obtained through the cost-to-go value of the reverse search tree.

[0030] Preferably, the ELECTRE dominance ranking in step 4 includes the following steps:

[0031] Construct a pairwise consistent matrix C and a non-consistent matrix D, C ij Statistical candidate node q i Better than q j The total number of indicators, D ij Take q i Inferior to q j The maximum index difference value;

[0032] Set a consistency threshold λ and a non-consistency threshold δ, if C ij ≥λ and D ij If ≤δ, then determine q. i dominate qj ;

[0033] Construct a dominant directed graph, select the node with the largest out-degree as the optimal temporary target point, and when the out-degrees are the same, use the closest distance as the secondary index to determine the target point.

[0034] Compared with the prior art, the beneficial results of the present invention are as follows:

[0035] (1) High-quality initial path and efficient generation: Bidirectional lazy LPA* search and delayed collision detection are adopted, which enables the initial path to quickly converge to a high-quality path candidate without performing too many invalid collision calculations. While ensuring the asymptotic optimality of the path, the computational overhead is greatly reduced and the planning efficiency is improved.

[0036] (2) Significantly improved path repair success rate: Through a structure-aware local connectivity compensation sampling strategy, when the initial path or replanned path encounters obstacles leading to breakage, the sampling of key areas can be selectively increased to find alternative routes. Compared with the traditional method of blindly expanding locally, this invention can maintain the global connectivity of the path in difficult scenarios such as narrow passages and complex obstacle areas, and is less prone to planning failure.

[0037] (3) Enhanced Decision Intelligence and Path Coherence: This invention introduces the ELECTRE multi-objective decision-making method to comprehensively evaluate and rank candidate reconnection nodes, avoiding the bias that may arise from selecting target points based on a single indicator (such as nearest distance or nearest cost). By comprehensively considering factors such as path length, turning angle, and connectivity stability, and employing an explicit merit-based comparison mechanism, this invention can select the reconnection scheme with the most comprehensive advantages, resulting in a smoother and lower-cost replanned path. In particular, compared to simple greedy strategies or Pareto front screening, the ELECTRE method can clearly distinguish the merit order of candidate schemes, thus improving the reliability and accuracy of dynamic replanning decisions. Attached Figure Description

[0038] The accompanying drawings are included to provide a further understanding of the embodiments and are incorporated in and constitute a part of this specification. The drawings illustrate embodiments and, together with the description, serve to explain the principles of the invention. Other embodiments and many anticipated advantages of the embodiments will be readily recognized as they become better understood through reference to the following detailed description. Elements in the drawings are not necessarily to scale. The same reference numerals refer to corresponding similar parts.

[0039] Figure 1 This is a flowchart illustrating a robust path planning method for mobile robots according to an embodiment of the present invention.

[0040] Figure 2This is a schematic diagram of the overall process of the robust path planning method for mobile robots according to an embodiment of the present invention;

[0041] Figure 3 This is a schematic diagram of the structure of the local connectivity compensation region according to an embodiment of the present invention;

[0042] Figure 4 This is a comparison diagram of the path planning effects under a simplified diagram according to an embodiment of the present invention;

[0043] Figure 5 This is a comparison chart of the path planning performance under a multi-obstacle map according to an embodiment of the present invention;

[0044] Figure 6 This is a comparison chart of the path planning effects under a slot map according to an embodiment of the present invention;

[0045] Figure 7 This is a comparison chart of the path planning effects of the present invention on complex maze maps;

[0046] Figure 8 This is a bar chart comparing the average path length of each algorithm in four environments according to embodiments of the present invention;

[0047] Figure 9 This is a bar chart comparing the success rates of various algorithms in four environments according to embodiments of the present invention;

[0048] Figure 10 This is a bar chart comparing the average time of each algorithm in four environments according to embodiments of the present invention;

[0049] Figure 11 This is an example of a directed graph illustrating the dominance relationship of ELECTRE in an embodiment of the present invention;

[0050] Figure 12 This is a schematic diagram illustrating the process by which a robot avoids unknown obstacles according to an embodiment of the present invention;

[0051] Figure 13 (a)-(e) in the figure are schematic diagrams of the specific process of the robot avoiding U-shaped obstacles in the embodiments of the present invention. Detailed Implementation

[0052] The present invention will now be described in further detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and not intended to limit it. Furthermore, it should be noted that, for ease of description, only the parts relevant to the invention are shown in the accompanying drawings.

[0053] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0054] Figure 1 This invention illustrates a robust path planning method for mobile robots in environments with unknown obstacles, as disclosed in an embodiment of the present invention. Figure 1 As shown, the method includes the following steps:

[0055] S1. Initial configuration and tree structure establishment: Set the starting point, ending point and map environment for the mobile robot path planning, configure path planning related parameters, establish a bidirectional search tree structure and forward and reverse expansion trees, and complete the initial guidance sampling and adaptive determination of the connection radius. The connection radius is dynamically adjusted with the number of samples.

[0056] S2, Bidirectional Lazy Collaborative Expansion: The bidirectional search trees start from the beginning and end points respectively, and adopt a heuristic expansion strategy based on lazy LPA* to delay collision detection and exchange heuristic information, thereby accelerating the merging of the two trees to generate initial path candidates.

[0057] S3. Path Collision Repair and Optimization: Collision detection is performed on the initial candidate paths. For collided path segments, local reconnection or structure-aware local connectivity compensation sampling methods are used to repair the paths. Then, through pruning, smoothing, curvature optimization, and constraint sampling iterative optimization, feasible optimized paths are obtained; and

[0058] S4. Dynamic Reconnection Decision and Path Reconstruction: When the robot travels along the optimized path, if the path is detected to be broken due to an unknown obstacle, a set of candidate temporary target points is constructed with the break position as the current node. The optimal temporary target point is selected by multi-index evaluation and ELECTRE dominance relationship ranking. Based on the target point, the path replanning is completed to achieve robust path navigation in the environment of unknown obstacles.

[0059] Specifically, given the shortcomings of existing technologies in path planning under dynamic, unknown obstacle environments, the technical problem to be solved by the embodiments of this invention is to improve the robustness and rapid reconstruction capability of mobile robots in path planning under interference from unknown obstacles. Specifically, this invention aims to address the problems of low path reconnection success rate, discontinuous or detour-like paths, and lack of multi-factor decision-making in existing methods when the robot path is interrupted by sudden obstacles. The purpose of this invention is to provide a new path planning framework that enables the robot to quickly repair the obstructed path and optimize subsequent travel routes while maintaining the high quality of the initial path, thereby significantly improving the reliability, consistency, and efficiency of navigation in dynamic environments.

[0060] To address the aforementioned technical problems, this invention provides a robust path planning method for dynamic, unknown obstacle environments. This method combines a bidirectional lazy search strategy, a structure-aware path repair mechanism, and a multi-criteria decision-making mechanism, improving the path generation and recovery capabilities of mobile robots in dynamic environments. Specifically, the method of this invention includes the following steps, and the overall flowchart is as follows: Figure 2 As shown:

[0061] Initialization: At the beginning of path planning, the start point, end point, and map environment are set, and parameters such as expansion step size, connection threshold, heuristic weight, sampling strategy, maximum iteration, and safety margin are configured. Simultaneously, four expansion trees are established: a forward lazy search tree and a forward expansion tree rooted at the start point, and a backward lazy search tree and a backward expansion tree rooted at the end point. Initial guiding sampling and adaptive estimation of the connection radius are completed, providing a basis for subsequent nearest neighbor search and expansion decisions. The connection radius is dynamically adjusted according to the number of samples, and its calculation formula is:

[0062]

[0063] Where q represents the number of sampling points, η is the adjustment parameter, and n is the spatial dimension. For the free region measure, ζ n Let be the volume of an n-dimensional unit sphere.

[0064] Forward Lazy Expansion: An incremental optimal expansion strategy based on lazy LPA* is adopted in the forward lazy search tree. Starting from the starting point, candidate nodes are sorted and expanded preferentially according to heuristic cost. To reduce computational overhead, collision detection of connections is temporarily suspended during the expansion phase. The expansion results and estimated costs are recorded first, and geometric verification is performed only when necessary, thereby accelerating the tree to grow towards the meeting region and forming connection candidates. When the distance between the node pair and the node pair of the reverse lazy search tree falls within a preset threshold, potential connection edges are recorded to form candidate connection paths, and this process is continued until several candidate connections appear. At the same time, the starting-side heuristic value generated by the forward lazy search tree is provided to the reverse expansion tree to guide it to tend towards the region with lower cost during sampling and expansion.

[0065] Within the forward lazy search tree, incremental optimal expansion is performed starting from the initial node x. i Its next step, inspired by the prospect (rhs), takes the minimum cost sum in the neighborhood:

[0066]

[0067] This expression represents node x i heuristic value The cumulative cost of neighboring nodes With edge cost The decision.

[0068] Reverse lazy expansion: Starting from the endpoint in the reverse lazy search tree, the same lazy LPA* strategy is used for expansion. The cost is dynamically corrected by using dual estimation of expansion heuristic and connectivity heuristic combined with consistency judgment. Collision detection is also delayed to improve expansion efficiency, so that the tree can construct connected candidates towards the starting point more quickly. When a node pair with the forward lazy search tree enters the connection threshold range, the potential connection edge is recorded synchronously and included in the candidate set. The terminal-side heuristic value generated by the reverse lazy search tree will be used by the forward expansion tree as the lower bound information of its expansion priority cost, thereby significantly improving the merging efficiency of the forward and reverse sides.

[0069] Within a reverse lazy search tree, lazy expansion is performed symmetrically starting from the endpoint, and its one-step look-through is inspired by:

[0070]

[0071] The cumulative estimated cost in the destination direction. For cost estimation of the edge, the formula is expressed as: node x i The heuristic value is the sum of the cumulative costs of its neighbors added to x. i The edge cost is determined, and the minimum value is taken as the lower bound of the estimate. The two sides perform heuristic propagation upon meeting to share information on the lower bound of the cost, for example:

[0072]

[0073] The "cost-to-go lower bound on the endpoint side" calculated by the backward lazy search tree is passed to the forward search tree, enabling the forward expansion to utilize more accurate endpoint information during sampling and sorting. Simultaneously, the cost estimate from the forward search tree is also fed back to the backward tree. Through this interoperability, both expansion trees can expand based on more reliable lower bounds, thereby improving search convergence speed and path quality.

[0074] Bidirectional Alternation and Mutual Inspiration: During path planning, the forward and backward expansion trees expand alternately, guided by heuristics from the forward lazy search tree and the backward lazy search tree. The forward expansion tree generates new nodes under the guidance of the terminal-side heuristics provided by the backward lazy search tree, and optimizes the accumulated cost through parent node selection and local rewiring, performing collision detection when necessary to ensure path feasibility. The backward expansion tree expands under the guidance of the starting-side heuristics provided by the forward lazy search tree, similarly completing parent node selection, local rewiring, and collision detection, gradually transforming the heuristics on the terminal side into the actual path structure.

[0075] Through this bidirectional alternation and mutual inspiration mechanism, the four trees work together to focus the search on potential meeting regions, accelerating the connection between preceding and following trees. To ensure the quality of the meeting regions, the following sufficient condition can be adopted:

[0076]

[0077] in, These are the lower bounds of the Euclidean distances from the current node to the target point and the starting point, respectively. This is the lower bound of the optimal path cost from the start point to the end point. This condition ensures that bidirectional searches meet in the middle region without deviating from the optimal lower bound, thereby improving meeting efficiency and path quality.

[0078] Path segment collision detection and local connectivity compensation: Collision detection is performed segment by segment on the initial path candidate; if there is no collision in the entire segment, it is output as the initial feasible path; if there is a collision segment, local reconnection is first performed in the neighborhood of the two ends of the segment to attempt direct obstacle bypass replacement; if local reconnection fails, a local connectivity compensation region is constructed based on the sampling of the key region.

[0079] Let the current path segment E be... fail =(x s ,x e The edge is determined to be impassable, and its endpoints are the forward connect points x and x. s and backward connection point x e Let the upstream and downstream nodes of the connection point be x and x respectively. f x b , where: x f The upstream connector at the beginning of the path segment, connected to x. s ;x b : The downstream connection point at the end of the path segment, connected to x e .

[0080] To avoid blindly expanding the search space, a set of directional auxiliary sampling points is generated in the four-way neighborhood of these two endpoints, constituting their local directional neighborhood points. To improve the geometric adaptability of neighborhood sampling, this embodiment of the invention introduces a dynamic sampling radius mechanism driven by path bends:

[0081] When generating neighboring points, if the current path segment is at node x s The angle at the point (i.e., the angle formed by the path segment (x) f ,x s ,x e If the defined angle is close to straight (the included angle approaches π), a larger sampling radius is allowed; if the angle is small, the sampling range automatically shrinks to avoid invalid expansion caused by entering high curvature regions. The adjustment function for the sampling distance d is defined as:

[0082]

[0083] Where θ fse The path at point x sThe angle at the bend is defined by d0 as the base sampling radius, and k and γ are adjustment factors, where k∈(0,1] and γ≥1. This mechanism ensures large-scale exploration on smooth segments of the path and maintains local constraints at bends, thereby achieving a coordinated match between local sampling density and geometry.

[0084] Connect the upstream connection point x f With forward connection point x s Connecting the points in the domain forms a set of forward sector intervals; similarly, connecting x... b With x e Connecting the neighboring points of the two sectors, the geometric intersection of the two sectors is defined as the Directional Constraint Points (DCPs), denoted as x and x' respectively. dcp1 ,x dcp2 x f ,x b ,x dcp1 ,x dcp2 Projecting these lines onto the horizontal and vertical directions respectively creates a set of orthogonal boundary lines, ultimately enclosing a rectangular region, which is the local connectivity compensation region, as shown in the image. Figure 3 As shown.

[0085] High-density sampling is performed within the local connectivity compensation region, and the newly generated nodes are used to construct feasible alternative segments for collision path segments, thereby improving the overall recoverability and local connectivity of the path.

[0086] Path connection and merging: After generating a new node in any expanded tree, check whether its connection with the nearest neighbor in the opposite expanded tree satisfies the connection condition; when a newly generated node x i With the nearest neighbor node x in the opposite expansion tree j The Euclidean distance between them is less than a preset threshold τ, and the edge(x) connecting them is less than the preset threshold τ. i ,x j If two trees do not intersect with obstacles, then they can be connected, i.e., ||x i -x j ||≤τ and If there is no collision, candidate connections are registered and their identifiers are merged. When the candidate set is not empty, the one with the minimum total cost is selected to form the initial path candidate. Minimum, where g forward This represents the cumulative cost from the starting point to the new node. Indicates the connection edge (x) i ,x j The estimated cost, h reverse Indicates connection from node x jHeuristic cost to the destination. If necessary, skip connections can be made through guiding regions or auxiliary nodes to bypass local narrow or high-cost areas. After a successful connection, a unified tree identifier is used and the management of nodes and edges is merged.

[0087] Path Extraction and Optimization: After connecting the forward and backward expansion trees, backtracking the parent chains on both sides from the connection point and concatenating them yields the complete initial path. Subsequently, triangular inequality pruning, node position perturbation optimization, and obstacle safety fine-tuning are performed sequentially. Smoothing and curvature optimization are then performed as needed to generate an optimized path with shorter length, smaller turning angles, and sufficient safety margin. To further improve path quality, this invention introduces a constraint sampling mechanism similar to Informed-RRT* during the optimization phase: using the start and end points as focal points, a path containing the current optimal path length C is constructed. best The elliptical region is defined as:

[0088]

[0089] Where x s With x g These are the starting point and the ending point, respectively. s ‖+‖xx g ‖ represents the sum of the distances from point x to the two foci. High-density sampling is performed within this elliptical region, and local reconnection and reconnection are performed using the existing tree structure; if the cost of the newly generated path is better than the current optimal path, then C is updated. best And correspondingly reduce the elliptical sampling domain.

[0090] Through this iterative shrinking method, the search space gradually focuses on the region containing the optimal path, thereby continuously improving the path quality and ultimately obtaining a feasible path that is approximately globally optimal. The final result is shown in the image below. Figures 4-7 As shown, the results are illustrated in simple map, multi-obstacle map, slot map, and complex maze map.

[0091] Each algorithm was run independently 50 times in each environment, and the average was used as the comparison data. The performance comparison of the six algorithms in Environment 1 (simple map) is shown in Table 1 below; the performance comparison of the six algorithms in Environment 2 (multi-obstacle map) is shown in Table 2 below; the performance comparison of the six algorithms in Environment 3 (slit map) is shown in Table 3 below; and the performance comparison of the six algorithms in Environment 4 (complex maze map) is shown in Table 4 below.

[0092] Table 1 shows the performance comparison of the six algorithms in Environment 1.

[0093]

[0094]

[0095] Table 2 shows the performance comparison of the six algorithms in Environment 2.

[0096]

[0097] Table 3 shows the performance comparison of the six algorithms in Environment 3.

[0098]

[0099] Table 4 shows the performance comparison of the six algorithms in Environment 4.

[0100]

[0101] Figure 8 This is a bar chart comparing the average path length of each algorithm in four environments according to embodiments of the present invention; Figure 9 This is a bar chart comparing the success rates of various algorithms in four environments according to embodiments of the present invention; Figure 10 This is a bar chart comparing the average time of each algorithm in four environments according to embodiments of the present invention.

[0102] As can be seen from Tables 1-4 above, the EGAR* algorithm of this invention significantly outperforms traditional methods in all four environments, showing the best performance in terms of success rate, average path length, and planning time. Particularly in narrow passage environments, the success rate is improved by 5%–55%, planning time is reduced by 26%–59%, and path length is reduced by 4%–12%.

[0103] Furthermore, in this embodiment, when the robot is traveling along the initial path, if the sensor detects that a segment of the path is blocked by a newly appeared obstacle, it is determined that the path has broken. At this time, the broken position is used as the current node to construct a set of candidate temporary target points.

[0104] Q cand ={q1,q2,…,q n}

[0105] Candidate points are sourced from several nodes in the original reverse search tree near the breakpoint, newly sampled nodes within the local region, and relevant nodes selected from the previously recorded path skeleton. These methods form a set of candidate nodes, providing potential reconnection targets for path replanning.

[0106] Candidate point attribute evaluation: For each node in the candidate node set, calculate three types of evaluation indicators:

[0107] ① Path length index, representing the straight-line distance d from the current node to the candidate node. i =‖q cur -q i ||;

[0108] ② The steering smoothness index represents the angle required to turn from the current heading to the candidate node, reflecting the smoothness of the path's turn. The current heading angle is , and the corresponding unit direction vector is v. cur =[cosψ cur ,sinψ cur Candidate point q i =(x i ,y i ); Current position is q cur =(x cur ,y cur If the current position points to the candidate point, then the unit direction vector is... The angle between the two is the steering angle θ. i =arccos(clip(v cur ·v i ,-1,1)),θ i ∈[0,π].

[0109] ③ Heuristic cost metric: This represents the heuristic cost estimate from the candidate node to the final target point, typically obtained using the cost-to-go value of the backward search tree. This yields a set of candidate point attributes, providing a quantitative basis for subsequent multi-objective decision-making.

[0110] ELECTRE Domination Relationship Construction: Based on the consistency and inconsistency matrices, consistency and inconsistency thresholds are set. If a candidate node's consistency exceeds the threshold on most metrics, and no inconsistency exceeds the tolerance threshold on any metric, then this node is determined to dominate another node, and a directed edge is added to the domination graph to represent its dominance. The final result is a directed domination graph among candidate nodes, visually displaying the relative superiority / inferiority relationships between nodes. Specifically, this includes:

[0111] Step 1: Input Preparation and Candidate Point Definition: Given the break point (current position) where a collision occurs on the current path as a node, a set of candidate target nodes is generated near the Structured Key Sampling Region (SCSR) or local path skeleton through resampling or existing sampling.

[0112] Q cand ={q1,q2,…,q n}

[0113] For each candidate node q i Calculate the following three specific evaluation indicators:

[0114] Path length L i From the current node q cur to node q i The Euclidean distance;

[0115] Steering angle θ iFrom the current robot heading to node q i Required steering angle (in degrees);

[0116] Heuristic cost h i From node q i The estimated cost to the end of the path or the target point (cost-to-go value obtained through a reverse tree).

[0117] Output: Candidate point attribute set V = {(L i ,θ i ,h i )∣q i ∈Q cand}

[0118] The second step is to construct a pair of consistent matrix C and inconsistent matrix D.

[0119] For any two distinct candidate nodes q i ,q j ∈Q cand Compare paired indicators:

[0120] Consistency index C ij If the k-th index satisfies v ik ≤v jk If a value is consistent, it is recorded as consistent on that indicator. The total number of consistent indicators is C. ij .

[0121] Inconsistency index D ij Defined as the maximum single indicator difference. If a certain indicator has a difference of v... ik -v jk >δ k If the threshold for significant disadvantage is not met, then it is considered inconsistent, and the largest difference among all values ​​satisfying the condition is taken as D. ij .

[0122] Finally, we obtain two n*n matrices: C = [C ij ] n×n ,D=[D ij ] n×n .

[0123] Step 3: Determine the dominance relationships and construct the ELECTRE dominance diagram.

[0124] Setting threshold parameters: (1) Consistency threshold λ: candidate point q i To control q j (1) Minimum number of consistent elements required; (2) Inconsistency threshold ξ: Maximum degree of inconsistency tolerated.

[0125] If a pair of candidate nodes (q) i ,q j)satisfy:

[0126] C ij ≥λ

[0127] D ij ≤ξ

[0128] Then add a directed edge q to the dominating graph G = (V, E). i →q j .

[0129] Condition (1) ensures q i It is not weaker than q in most metrics. j Condition (2) guarantees q i There are no serious disadvantages.

[0130] Output: Dominant graph G = (V, E), where nodes are candidate vertices and edges are dominance relations.

[0131] Step 4: Determine the optimal temporary target node: In the dominance graph G, calculate the out-degree of each node:

[0132] out(q i )=|{q j ∣(q i ,q j )∈E}|

[0133] Select the node with the largest out-degree as the optimal temporary target point:

[0134]

[0135] If multiple nodes have the same out-degree, then a secondary metric (such as the closest one) is used to determine the unique optimal solution.

[0136] like Figure 11 As shown, suppose the robot's path to its current position breaks during obstacle avoidance navigation. In this case, the robot needs to select a temporary target node within the neighborhood of its current position to repair and continue the path. Assuming the current set of candidate temporary target nodes is {S1, S2, S3, S4, S5}, the robot uses the ELECTRE method to evaluate the merits of these nodes based on multiple metrics, including the path length to the target, the turning angle, and the heuristic cost.

[0137] First, based on the index function of each node, an initial judgment matrix is ​​established, and the dominance relationship between each pair of candidate nodes is calculated, thereby constructing the directed graph of dominance relationships in the ELECTRE method. The specific steps are as follows:

[0138] 1) Indicators and Judgment Matrix

[0139] Candidate set V = {S1, ..., S} nFor each candidate node S i Calculate three types of cost-related indicators (the smaller the better):

[0140] L i From the current fracture location to S i Path segment length; θ i From the current heading to S i The steering angle; h i From S i The heuristic cost to the global goal (given by the cost-to-go from the backslope tree) is organized into a "judgment matrix":

[0141]

[0142] 2) Consistency matrix C

[0143] For any pair of S i ,S j (i≠j), let:

[0144]

[0145] 3) Inconsistent matrix D

[0146] Define the three-dimensional range:

[0147] ΔL=maxL-minL, Δθ=maxθ-minθ, Δh=maxh-minh

[0148] definition:

[0149]

[0150] If S i Compared to S in a certain indicator j If the difference is too great (exceeding the tolerance level), the comparison is rejected.

[0151] 4) Determine the threshold and the control diagram

[0152] In this embodiment of the invention, λ = 0.6 and δ = 0.3 are used. The dominant edge S is defined as follows: C(i,j) ≥ λ and D(i,j) ≤ δ. i →S j After traversing all ordered pairs (i,j), we obtain the dominated directed graph G = (V,E), which guarantees that the dominance decision is stable and robust.

[0153] Furthermore, the initial state expansion graph shows the relative strengths and weaknesses of the nodes. Since S5 is not weaker than S1 in most metrics and has no significant disadvantage, S5 dominates S1. Similarly, node S2 dominates S1, S3, S4, and S5. Therefore, node S2 has an out-degree of 4, while the in-degrees of nodes S1, S3, S4, and S5, which are dominated by S2, increase by 1. If multiple nodes have the same out-degree, a secondary metric (such as minimum heuristic cost) is used to determine the unique optimal solution. Figure 11 In the given information, S2 has an in-degree of 0 and an out-degree of 4, therefore S2 is the final target node selected.

[0154] This method effectively avoids the excessive influence of extreme values ​​of a single indicator on the decision-making process by setting consistency and inconsistency thresholds. It also deeply captures the trade-offs between multiple objectives, making it particularly suitable for local repair and continuous planning of navigation paths in complex obstacle environments. Compared to the Pareto ranking method, which only provides a set of non-dominated candidate solutions and lacks further distinguishing ability, ELECTRE can clearly construct the order of merit among candidate nodes based on a comprehensive consideration of the rationality of multiple objectives, thereby significantly improving the accuracy and efficiency of decision-making in the path recovery stage. For the current invention, if an unknown obstacle appears in the forward lazy expansion tree, it is necessary to expand the tree backward to candidate nodes near the current break point to provide a heuristic cost indicator. By selecting candidate nodes multiple times to avoid the obstacle, the effect is as follows: Figure 12 As shown.

[0155] Figure 13 Figures (a)-(e) illustrate the specific process of the robot avoiding the U-shaped obstacle. The dark blue dots represent the robot's current position, and the light blue area represents the robot's visible range. In this simulation, the radius of the visible range was set to 0.5. It can be seen that for a short period initially, the robot follows a pre-planned initial path, such as... Figure 13 As shown in (a) and (b) in the image. When the robot moves to... Figure 13 At position (b) shown in the diagram, an unknown obstacle appears within its detection area and blocks the pre-planned path. In this case, the robot will use a path replanning algorithm to repeatedly select suitable state tree nodes to avoid the unknown obstacle, such as... Figure 13 As shown in (c), (d), and (e) in the figure.

[0156] In summary, the technical solution provided by this invention forms a complete robust path planning process from initial planning to dynamic reconstruction: it efficiently generates the initial path using bidirectional lazy search, and ensures the feasibility of the path by combining structure-aware local sampling; during operation, it intelligently selects the reconnection target through multi-index decision-making, and retreats to the global skeleton path when necessary to ensure the smooth completion of the navigation task.

[0157] The technical solutions of this invention have the following key innovations and technical features to be protected:

[0158] A bidirectional lazy search initial path planning strategy is proposed: a bidirectional lazy LPA* search method is provided, which simultaneously performs heuristic expansion from the start and end points and delays collision detection to quickly obtain an initial feasible path. This strategy ensures high efficiency and high quality in initial path planning and is one of the fundamental technical features for achieving subsequent robust refactoring.

[0159] A local connectivity compensation mechanism based on directional neighborhoods is proposed: A structure-aware path repair method is described. When a path segment collision failure is detected, directional constraint neighborhoods are defined at both ends of the collision segment, and a local connectivity compensation region is constructed. High-density sampling is performed within this region, and alternative connections are attempted, enabling the path interruption caused by obstacles to be effectively repaired. This technique significantly improves the global connectivity of paths in complex terrain and is a key means by which this invention addresses the problems of narrow passages and sparsely sampled regions.

[0160] Dynamic Reconnection Decision Based on ELECTRE Multi-Objective Ranking: This invention introduces a multi-criteria decision-making method for selecting temporary target points when paths are interrupted. For multiple candidate reconnection nodes, considering multiple indicators such as path cost, turning angle, and connectivity stability, an ELECTRE merit-based comparison strategy is used for ranking and filtering to select the optimal reconnection target. This decision-making mechanism endows the invention with the ability to intelligently select replanning schemes in dynamic environments, avoiding poor paths or reconnection failures caused by the lack of global consideration in traditional methods. This is the key innovation that this invention aims to protect.

[0161] The above key technical features work together and are indispensable, jointly achieving the goal of robust and efficient path planning in dynamic unknown obstacle environments. Therefore, these are also the core innovative points claimed by this invention.

[0162] The above description is merely a preferred embodiment of the present invention and an explanation of the technical principles employed. Those skilled in the art should understand that the scope of the invention is not limited to the specific combination of the above-described technical features, but also includes other technical solutions formed by arbitrary combinations of the above-described technical features or their equivalents without departing from the inventive concept. For example, technical solutions formed by substituting the above-described features with (but not limited to) technical features with similar functions disclosed in this invention.

Claims

1. A robust path planning method for mobile robots in environments with unknown obstacles, characterized in that, The method includes the following steps: S1. Initial configuration and tree structure establishment: Set the starting point, ending point and map environment for the mobile robot path planning, configure path planning related parameters, establish a bidirectional search tree structure and forward and reverse expansion trees, and complete the initial guidance sampling and adaptive determination of the connection radius. The connection radius is dynamically adjusted with the number of samples. S2, Bidirectional Lazy Collaborative Expansion: The bidirectional search trees start from the beginning and end points respectively, and adopt a heuristic expansion strategy based on lazy LPA* to delay collision detection and exchange heuristic information, thereby accelerating the merging of the two trees to generate initial path candidates. S3. Path Collision Repair and Optimization: Collision detection is performed on the initial path candidates. For the collided path segments, the path is repaired by local reconnection or structure-aware local connectivity compensation sampling. Then, through pruning, smoothing, curvature optimization and constraint sampling iterative optimization, a feasible optimized path is obtained. as well as S4. Dynamic Reconnection Decision and Path Reconstruction: When the robot travels along the optimized path, if the path is detected to be broken due to an unknown obstacle, a set of candidate temporary target points is constructed with the break position as the current node. The optimal temporary target point is selected by multi-index evaluation and ELECTRE dominance relationship ranking. Based on the target point, the path replanning is completed to achieve robust path navigation in the environment of unknown obstacles.

2. The robust path planning method for mobile robots according to claim 1, characterized in that, The bidirectional search tree in step S1 includes a forward lazy search tree rooted at the starting point and a backward lazy search tree rooted at the ending point; the path planning parameters include expansion step size, connection threshold, heuristic weight, sampling strategy, maximum number of iterations, and safety margin; the formula for calculating the connection radius is: Where q represents the number of sampling points, η is the adjustment parameter, and n is the spatial dimension. For the free region measure, ζ n Let be the volume of an n-dimensional unit sphere.

3. The robust path planning method for mobile robots according to claim 1, characterized in that, The bidirectional inertial cooperative expansion described in step S2 satisfies the sufficient rendezvous condition |xx g |+|xx s |≤(1+∈)·C lower , where |xx g |、|xx s | represents the lower bound of the Euclidean distance from the current node to the target point and the starting point, respectively, C. lower ∈ represents the lower bound of the optimal path cost from the starting point to the destination, and ∈ represents the error coefficient; the forward lazy search tree passes the heuristic value from the starting point to the backward expansion tree, and the backward lazy search tree passes the heuristic value from the destination to the forward expansion tree.

4. The robust path planning method for mobile robots according to claim 3, characterized in that, Any candidate node x in the forward lazy search tree i A step forward that inspires RHS This expression represents node x i heuristic value The cumulative cost of neighboring nodes With edge cost The sum and decision; the one-step prospect of the node within the reverse lazy search tree is inspired by... The cumulative estimated cost in the destination direction. For cost estimation of an edge, this formula represents the cost of node x. i The heuristic value is the sum of the cumulative costs of its neighbors added to x. i The edge cost is determined by taking the minimum value as the lower bound of the estimate.

5. The robust path planning method for mobile robots according to claim 1, characterized in that, The structure-aware local connectivity compensation sampling described in step S3 introduces a path bend-driven dynamic sampling radius mechanism, with a sampling distance of [missing information]. Where θ fse The path at point x s The angle at the point is d0, which is the basic sampling radius, and k and γ are adjustment factors, where k∈(0,1] and γ≥1.

6. The robust path planning method for mobile robots according to claim 5, characterized in that, The process of constructing the local connectivity compensation region is as follows: Let the current path segment E be... fail =(x s ,x e The edge is determined to be impassable, and its endpoints are the forward connect points x and x. s and backward connection point x e Let the upstream and downstream nodes of the connection point be x and x respectively. f x b , where: x f The upstream connector at the beginning of the path segment is connected to x. s ;x b For the downstream connection point at the end of the path segment, connected to x e ; Connect the upstream connection point x f With forward connection point x s Connect the neighboring points of x to form a set of forward sector intervals; similarly, connect x... b With x e Connecting the neighboring points, the geometric intersection of the two sectors yields the intersection constraint point x. dcp1 x dcp2 , will x f x b x dcp1 x dcp2 Projecting these lines onto the horizontal and vertical directions respectively creates a set of orthogonal boundary lines, which ultimately enclose a rectangular area, known as the local connectivity compensation region.

7. The robust path planning method for mobile robots according to claim 1, characterized in that, The constrained sampling iterative optimization mentioned in step S3 refers to constructing a path with the starting point and the ending point as the focus, which includes the current optimal path length C. best elliptical region Where x s With x g These are the starting point and the ending point, respectively. s ‖+‖xx g || represents the sum of distances from point x to the two foci; high-density sampling and local reconnection are performed within the elliptical region, and C is updated if the new path has a better cost. best The elliptical domain is then narrowed, and the path quality is iteratively optimized.

8. The robust path planning method for mobile robots according to claim 1, characterized in that, The source of the candidate temporary target point set mentioned in step S4 includes the original reverse search tree nodes near the breakpoint, locally newly sampled nodes, and path skeleton screening nodes; the multi-index evaluation indexes include path length, turning smoothness, and heuristic cost.

9. The robust path planning method for mobile robots according to claim 8, characterized in that, The steering smoothness is determined by the current heading angle α. cur Calculation of the angle between the current heading angle and the candidate point's orientation angle, and the current heading angle α. cur The corresponding unit direction vector is v cur =[cosψ cur ,sinψ cur Candidate point q i =(x i ,y i ); Current position is q cur =(x cur ,y cur If the current position points to the candidate point, then the unit direction vector is... The angle between the two is the steering angle θ. i =arccos(clip(v cur ·v i ,-1,1)),θ i ∈[0,π].; The heuristic cost is obtained through the cost-to-go value of the reverse search tree.

10. The robust path planning method for a mobile robot according to claim 1, characterized in that, The ELECTRE dominance ranking described in step 4 includes the following steps: Construct a pairwise consistent matrix C and a non-consistent matrix D, C ij Statistical candidate node q i Better than q j The total number of indicators, D ij Take q i Inferior to q j The maximum index difference value; Set a consistency threshold λ and a non-consistency threshold δ, if C ij ≥λ and D ij If ≤δ, then determine q. i dominate q j ; Construct a dominant directed graph, select the node with the largest out-degree as the optimal temporary target point, and when the out-degrees are the same, use the closest distance as the secondary index to determine the target point.

Images