Mobile robot RRT* path planning method and system

Abstract

The invention relates to the technical field of mobile robot path planning, and discloses a mobile robot RRT * path planning method and system, and the method comprises the steps: building an environment model containing a static obstacle in a continuous working space; constructing a local environment complexity factor for any position in the continuous working space; constructing a time factor which is monotonically changed along with the promotion of the number of iterations; obtaining a target bias probability based on the local environment complexity factor and the time factor of the focus node, and determining whether to take an end point as a sampling point or not according to the target bias probability; in each iteration, calculating the complexity of a tree node closest to a sampling point, obtaining an extended step length in combination with a time factor, and generating a new node and a candidate connecting edge; collision detection is carried out, and RRT * reconnection is executed; and when a termination condition is satisfied, extracting a path point sequence from the starting point to the terminal point from the tree structure to form path planning, thereby improving the reachability of the narrow channel, reducing invalid collision expansion and improving the overall convergence efficiency.

Description

Technical Field

[0001] This invention relates to the field of mobile robot path planning and navigation technology, and in particular to a mobile robot RRT* path planning method and system. Background Technology

[0002] The goal of mobile robot path planning is to generate a collision-free feasible path given an environment model, a start point, and an end point, while optimizing the path length, cost, or safety margin as much as possible. Existing mobile robot path planning methods typically include environmental modeling / map representation, feasible space or cost map construction, path search and optimization, and path smoothing and tracking. From an algorithmic perspective, they can be divided into graph search-based planning methods and continuous space-based planning methods. Graph search-based methods usually first rasterize the workspace or construct a topological graph, and then use algorithms such as Dijkstra, A*, and D to search for a global path on the discrete graph. Continuous space-based local obstacle avoidance methods rely more on online feedback (such as artificial potential fields, dynamic windows, etc.) to achieve real-time responses. These discretization / heuristic methods are prone to resolution-computational complexity conflicts or decreased search efficiency in bottleneck areas in high-dimensional continuous spaces or scenarios with complex obstacle shapes and a high proportion of narrow passages. To improve global search efficiency in continuous spaces, path planning methods based on random sampling have emerged, such as Probabilistic Landmark Maps (PRM) and Rapid Expanding Random Trees (RRT). Among them, RRT and its improved algorithm RRT* achieve global search through random sampling and tree structure expansion. RRT* further possesses asymptotic optimality and is widely used in complex environments and high-dimensional spaces.

[0003] However, traditional RRT* often has the following problems in practical applications: Firstly, target bias strategies typically employ a fixed probability, meaning that the endpoint is directly used as the sampling point with a fixed probability in each iteration. While a fixed bias can accelerate convergence in open regions, it can easily lead to over-concentration of sampling in narrow channels, areas with dense obstacles, or regions with restricted directions. This can cause repeated collisions or failures at bottlenecks, thereby reducing the effective sampling rate.

[0004] Secondly, the expansion step size is usually set to a fixed value. A fixed step size may be too small in open areas, resulting in low search efficiency, while it may be too large in complex areas, leading to crossing narrow gaps, connecting along edges, or generating a high collision rate, resulting in a large number of invalid expansions and failed reconnections.

[0005] Third, the robustness of planned connections near obstacle boundaries is poor. Even if the path is geometrically collision-free, if the connection is close to the obstacle boundary, it is prone to collisions when there are positioning errors, control tracking errors, or map modeling errors, affecting actual usability.

[0006] Therefore, a planning method is needed that can adaptively adjust the sampling strategy and expansion step size according to the difficulty of the local environment, so that the algorithm is more cautious in complex areas and more efficient in open areas, while taking into account both early exploration and later convergence. Summary of the Invention

[0007] To address the aforementioned technical problems, this invention provides a mobile robot RRT* path planning method and system, thereby improving narrow channel accessibility, reducing invalid collision propagation, and enhancing overall convergence efficiency.

[0008] The first aspect of this invention provides a mobile robot RRT* path planning method, comprising: A mobile robot RRT* path planning method includes the following steps: Environmental modeling and safety expansion: An environmental model containing static obstacles is established in a continuous workspace. Based on the robot's equivalent radius and safety margin, the obstacles are expanded outward to obtain the expanded obstacle region and determine the free space. Construction of local environment complexity factor: The local environment complexity factor is constructed for any location within the continuous workspace. The local environment complexity factor is obtained by fusing the net space metric and the orientation constraint metric. Time factor construction: Using the number of iterations in RRT* as the time variable, construct a time factor that changes monotonically with the number of iterations; Dynamic target bias sampling: In each iteration, a focus candidate set is constructed in the tree structure of RRT*. The focus candidate set consists of multiple nodes in the tree structure with the smallest Euclidean distance to the destination. The focus node is determined based on the Euclidean distance from the candidate node to the destination and the path cost from the starting point to the candidate node. The target bias probability is obtained based on the local environment complexity factor and time factor of the focus node. The destination is then used as the sampling point based on the target bias probability. Adaptive step-size expansion: In each iteration, the complexity of the tree node closest to the sampling point is calculated, and the expansion step size is obtained by combining the time factor; new nodes and candidate edges are generated from the nearest node toward the sampling point according to the expansion step size; Collision detection and RRT* reconnection: Collision detection is performed on new nodes and candidate edges based on the expanding obstacle region. When there is no collision between the new node and its edges, the new node is added to the tree structure and RRT* reconnection is performed. Path extraction: When the termination condition is met, the path point sequence from the starting point to the ending point is extracted from the tree structure to form a path plan. The termination condition includes reaching the ending point within a preset neighborhood or reaching a preset upper limit of the number of iterations.

[0009] Optional, local environment complexity factor We obtain it from the following formula: ， in, For the measurement of net clearance, This is a directionally constrained metric.

[0010] Optionally, clearance measurement is performed by location. The shortest distance to the boundary of the outward-expanding obstacle is obtained and normalized; The directional constraint metric is obtained based on the free distance of multi-directional ray detection. The free distance of each direction is normalized to obtain the weight of each direction. The uniformity of the distribution of the free distance weights in each direction is obtained based on the weight of each direction. The directional constraint metric is obtained based on the uniformity of the distribution.

[0011] Optional, time factor We obtain it from the following formula: , in, This represents the current iteration number. The upper limit of iterations, For preset shape parameters, .

[0012] Optionally, obtaining the target bias probability includes the following steps: First, by time factor Upper bound of target bias Gradual adjustment is achieved, expressed by the following formula: , in, This is the initial target bias upper bound parameter. This serves as the upper bound parameter for the target bias in the later stages, and ; Combined with the focal node Local environmental complexity factor Obtain the target bias probability As shown in the following formula: , in, The target bias lower bound parameter.

[0013] Optional, expand step size The acquisition includes the following steps: First, by time factor Make the maximum step size boundary Gradual tightening is expressed by the following formula: , in, These are the initial maximum step size boundary parameters. These are the boundary parameters for the maximum step size in the later stages, and ; Minimum step size boundary Combined with the most recent node Local environment complexity factor Get the extended step size It can be expressed by the following formula: , in, This represents the current iteration number. These are the minimum step size boundary parameters.

[0014] Optionally, the focus node is determined through the following steps: Focus nodes are selected using a comprehensive evaluation function. The comprehensive evaluation function considers candidate nodes simultaneously. To the finish line distance and the path cost from the starting point to the candidate node. It also introduces a penalty term that is positively correlated with the number of expansion failures or collision rate. To suppress long-term ineffective candidate nodes from repeatedly becoming focal nodes, a comprehensive evaluation function is constructed accordingly: , in, The weighting coefficients are preset to be of the same order of magnitude and optimized through offline simulation calibration. This is obtained from the statistics of the number of expansion attempts and the number of failures / collisions of the candidate node; Focus Node Determined by the following formula: , in, The focus candidate set is a tree structure. The first one with the smallest Euclidean distance to the endpoint is selected from the options. It consists of nodes. Determined by the following formula: , in, For the current tree structure The total number of nodes, It is the natural logarithm function. Indicates rounding up. and Used to Cut-off restrictions Within the range.

[0015] Optionally, path extraction also includes performing pruning to remove redundant path points. Pruning includes the following steps: Collision detection is performed on candidate straight line segments between any two points in the path point sequence based on the outward obstacle region. If there is no collision between the straight line segments, the intermediate redundant path points are deleted.

[0016] Optionally, pruning also includes complexity constraints: Discretely sample along candidate straight line segments, calculate the average complexity, and only perform the deletion operation if the average complexity does not exceed a preset threshold.

[0017] A second aspect of the present invention provides a mobile robot path planning system, which operates in a continuous workspace containing static obstacles, comprising: The environment modeling module is used to expand the obstacle area based on the robot's equivalent radius and safety margin, thereby obtaining the expanded obstacle area and determining the free space. The local environment complexity generation module is used to generate a local environment complexity factor that integrates clearance measurement and orientation constraint measurement for any location within the workspace. The time factor generation module uses RRT* iteration number as the time variable to generate a time factor that monotonically changes with the iteration number. The dynamic target bias sampling module is used to select multiple nodes with the smallest Euclidean distance to the endpoint from the tree structure in each iteration to form a focus candidate set, and to determine the focus node based on the Euclidean distance from the candidate node to the endpoint and the path cost from the starting point to the node; the target bias probability is calculated based on the local environment complexity factor and time factor of the focus node, and the endpoint is used as the sampling point to determine whether to use the endpoint. The adaptive step-size expansion module is used to calculate the local environment complexity of the tree node closest to the sampling point in each iteration, determine the expansion step size in combination with the time factor, and generate new nodes and candidate edges from the nearest node toward the sampling point according to the step size. The collision detection and tree update module performs collision detection on new nodes and candidate edges based on the expanded obstacle region. If there is no collision, the new node is added to the tree structure and the RRT* reconnection operation is performed. The path extraction and optimization module extracts the path point sequence from the starting point to the ending point from the tree structure when the termination condition is met, forming the final path. The termination condition includes reaching the preset neighborhood range of the ending point or the number of iterations reaching a preset upper limit.

[0018] The technical solution provided by the embodiments of the present invention has the following advantages compared with the prior art: This invention provides a mobile robot RRT* path planning method and system, which involves establishing a dynamic target bias probability decision mechanism based on local environmental complexity factors and time factors in a continuous spatial obstacle environment. The constructed local environmental complexity factor can quantitatively assess the complexity of the environment surrounding the robot's current position (such as narrowness and directional restriction). Closed-loop scheduling is achieved through the local environmental complexity factor: in complex areas, the target bias is automatically reduced and the step size is decreased; in open areas, the target bias is automatically increased and the step size is increased, balancing accessibility and efficiency. The time factor provides opposite progressive adjustments to the upper bound of the target bias probability and the upper bound of the extended step size: as iteration progresses, the upper bound of the target bias gradually widens to promote convergence, while the upper bound of the extended step size gradually tightens to achieve refined expansion, achieving a balance between early exploration and later convergence, reducing premature bottlenecks. Based on the robot's equivalent radius and safety margin, obstacles are expanded outwards to obtain the expanded obstacle region and determine the free space, allowing the obstacle region to be expanded before planning begins. This essentially creates a buffer layer for the robot and its safety margin, with the planning process taking place within this more conservative free space. Subsequent collision detection between new nodes and candidate edges, based on the expanded obstacle region, ensures that the generated path and tree nodes themselves are outside the buffer layer. Employing obstacle safety expansion improves the safety margin of the planning results and significantly reduces the actual collision risk from edge-hugging connections. This fundamentally improves the path's tolerance to localization, control, and map errors. Collision detection based on the expanded region ensures that the tree expansion process itself occurs within a safe corridor.

[0019] The method and system of this invention are applicable to global path generation of indoor mobile robots, unmanned vehicles, and drones under known maps. They can also serve as an upper-level planning module to provide reference paths for downstream control or learning-type tracking modules. The target bias probability and expansion step size of RRT* are adaptively adjusted using local environmental complexity factors, and the adjustment boundary is gradually adjusted in combination with time factors, thereby improving narrow channel accessibility, reducing invalid collision expansion, and improving overall convergence efficiency. Attached Figure Description

[0020] Figure 1 A flowchart of a mobile robot RRT* path planning method provided in the first part of the embodiments of the present invention; Figure 2 This is a schematic diagram illustrating the construction of the local environment complexity factor provided in an embodiment of the present invention; Figure 3 This is a closed-loop scheduling diagram of dynamic target bias and adaptive step size provided in an embodiment of the present invention; Figure 4 This is a schematic diagram of path pruning provided in an embodiment of the present invention; Figure 5This is a flowchart of a mobile robot RRT* path planning method provided in the second part of the embodiments of the present invention. Detailed Implementation

[0021] The following detailed description of a specific embodiment of the present invention is provided in conjunction with the accompanying drawings. However, it should be understood that the scope of protection of the present invention is not limited to the specific embodiment.

[0022] In the description of this invention, it should be understood that the terms "center," "longitudinal," "lateral," "length," "width," "thickness," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," "outer," "axial," "radial," and "circumferential" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are only for the convenience of describing the technical solution of this invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on this invention.

[0023] The present invention will be described below through several specific embodiments. To keep the following description of the embodiments clear and concise, detailed descriptions of known functions and components may be omitted. When any component of an embodiment of the present invention appears in more than one drawing, the component may be represented by the same reference numerals in each drawing.

[0024] like Figure 1 As shown, the first embodiment of the present invention provides a mobile robot RRT* path planning method, including the following steps: Environmental Modeling and Safety Expansion: An environmental model containing static obstacles is established within a continuous workspace. Based on the robot's equivalent radius and safety margin, the obstacles are expanded outwards to obtain the expanded obstacle region and determine the free space. Specifically: A mobile robot working environment model is established within a continuous workspace, containing several sets of static obstacles. Set the starting point and the finish line Based on the robot's equivalent radius With preset safety margin Expand the obstacle outward to obtain the expanded obstacle area. The outer radius satisfies , and define the free space by extending the area of ​​obstacles; Construction of local environment complexity factor: For any location within a continuous workspace Constructing local environment complexity factor The local environmental complexity factor is obtained by fusing the clearance metric and the directional constraint metric, and is used to characterize the degree of obstacle congestion near the location and the degree of restriction on the passable direction; Time factor construction: Using the number of iterations in RRT* as the time variable, construct a time factor that monotonically changes with the number of iterations. It is used to gradually adjust the boundary parameters of target bias and expansion step size to balance early exploration and later convergence. Dynamic target bias sampling: In each iteration, a focus candidate set is constructed in the tree structure of RRT*. The focus candidate set consists of multiple nodes in the tree structure with the smallest Euclidean distance to the destination. The focus node is determined based on the Euclidean distance from the candidate node to the destination and the path cost from the starting point to the candidate node. The target bias probability is obtained based on the local environment complexity factor and time factor of the focus node. The destination is then used as the sampling point based on the target bias probability. Adaptive step-size expansion: In each iteration, the complexity of the tree node closest to the sampling point is calculated, and the expansion step size is obtained by combining the time factor; new nodes and candidate edges are generated from the nearest node toward the sampling point according to the expansion step size; Collision Detection and RRT* Reconnection: Collision detection is performed between new nodes and candidate edges based on the expanding obstacle region. When a new node... and its connecting edges When there is no collision, the new node is added to the tree structure, and RRT* reconnection is performed. That is, according to the RRT* rules, the parent node is selected in the neighborhood and rewire is performed to update the path cost. Specifically, obstacle safety expansion: let the robot's equivalent radius be... The safety margin is The outer radius is .in, It can be determined according to the proportion of the robot's equivalent radius, that is , For safety, a factor of 0.05 to 0.30 is preferred. The outward expansion of the obstacle area is then used to obtain the expanded obstacle region. During the planning process, a collision is determined if a node falls into the extended region or its edge intersects with the extended region, thus ensuring that the planning result maintains a safe distance outside the actual obstacle boundary. This effectively alleviates the problem of geometric feasibility but vulnerability near the edge.

[0025] Path extraction: When the termination condition is met, the path point sequence from the starting point to the ending point is extracted from the tree structure to form a path plan. The termination condition includes reaching the ending point within a preset neighborhood or reaching a preset upper limit of the number of iterations.

[0026] This invention provides a mobile robot RRT* path planning method, which involves establishing a dynamic target bias probability decision mechanism based on local environmental complexity factors and time factors in a continuous spatial obstacle environment. The constructed local environmental complexity factor can quantitatively assess the complexity of the environment surrounding the robot's current position (such as narrowness and directional restriction). Closed-loop scheduling is achieved through the local environmental complexity factor: in complex areas, the target bias is automatically reduced and the step size is decreased; in open areas, the target bias is automatically increased and the step size is increased, balancing accessibility and efficiency. The time factor provides opposite progressive adjustments to the upper bound of the target bias probability and the upper bound of the extended step size: as iteration progresses, the upper bound of the target bias gradually widens to promote convergence, while the upper bound of the extended step size gradually tightens to achieve refined expansion, achieving a balance between early exploration and later convergence, reducing premature bottlenecks. Obstacles are expanded outward based on the robot's equivalent radius and safety margin, resulting in an expanded obstacle region and defining free space. This expansion of the obstacle region before planning begins is equivalent to creating a buffer layer for the robot and its safety margin; the planning process takes place within this more conservative free space. Subsequent collision detection between new nodes and candidate edges, based on the expanded obstacle region, ensures that the generated paths and tree nodes themselves are outside the buffer layer. The use of obstacle safety expansion improves the safety margin of the planning results and significantly reduces the actual collision risk caused by edge-connecting. This fundamentally improves the path's tolerance to positioning errors, control errors, and map errors. Collision detection based on the expanded region ensures that the tree expansion process itself takes place within a safe corridor.

[0027] The method of this invention is applicable to global path generation of indoor mobile robots, unmanned vehicles, and drones under a known map. It can also be used as an upper-level planning module to provide reference paths for downstream control or learning-type tracking modules. The target bias probability and expansion step size of RRT* are adaptively adjusted using a local environmental complexity factor, and the adjustment boundary is gradually adjusted in combination with a time factor, thereby improving narrow channel accessibility, reducing invalid collision expansion, and improving overall convergence efficiency.

[0028] To simultaneously reflect proximity to obstacles and whether passage is restricted, the local environmental complexity factor consists of two parts: clearance measurement and directional restriction measurement.

[0029] refer to Figure 2 Local environment complexity factor We obtain it from the following formula: ， in, For the measurement of net clearance, This is a measure of directional restriction. It applies when either the path is closer to an obstacle or the directional restriction becomes more severe. This increases the size of the sample, thus driving the algorithm to adopt a more conservative sampling / expansion behavior at that point.

[0030] Optionally, clearance measurement is performed by location. The shortest distance to the boundary of the outward-expanding obstacle is obtained and normalized; the clearance measurement is based on the shortest distance from the point to the boundary of the outward-expanding obstacle. The smaller the distance, the more crowded the space, and the more conservative the expansion needs to be. To facilitate unified scheduling, the distance is normalized to... Interval obtained The clearance measurement satisfies:

[0031] make For position The shortest distance to the boundary of the outward-expanding obstacle, at a local scale of ,but , in, For position The shortest distance to the boundary of the outward-expanding obstacle; This is a local evaluation scale used to normalize the clearance distance, and is preferably taken as the initial value of the maximum step size boundary, i.e. (Or take its proportionality coefficient); This is a truncation function used to restrict a value to a certain range. Interval.

[0032] The orientation-constrained metric is obtained based on the free distances from multi-directional ray detection. The free distances in each direction are normalized to obtain weights for each direction. The uniformity of the distribution of these weights is then calculated, and the orientation-constrained metric is derived from this uniformity. This metric is used to characterize narrow passageways and corridor structures. (Based on points...) Using a target as the center, perform ray detection in multiple directions to obtain the free distance that is passable in each direction. If only a few directions have a large free distance, while most directions are blocked, it indicates that the environment is significantly directionally restricted (typically a narrow passage or corridor). In this case, the target offset should be reduced and the step size decreased to improve the probability of passage.

[0033] In one embodiment, the directional constraint metric is obtained by calculating and normalizing the entropy based on the normalized weights of the free distances in each direction, so that the open area exhibits a uniform distribution, high entropy, and low constraint metric, while the narrow channel exhibits a concentrated distribution, low entropy, and high constraint metric.

[0034] Ultimately, the two were merged to obtain Furthermore, it employs a fusion method where the complexity increases with any variation, making the identification of complex regions more sensitive.

[0035] Orientation-constrained metrics are obtained through multi-directional free distances: By location Select evenly around the center one direction ( The maximum length of the launch in each direction is... The ray is used to obtain the free travel distance in that direction. .in, Indicates from Starting in the direction Distance to the nearest intersection point in the outward-expanding obstacle region; if in length If the inner part does not intersect with the outer expanding obstacle, then take... The weights are obtained by normalizing the free distances in each direction:

[0036] , in, The number of ray detection directions; The weights obtained by normalizing the free distances in each direction satisfy the following conditions: and According to the following formula:

[0037] , Calculate information entropy, where, Information entropy is used to characterize location. Weights of free distances in each direction The degree of uniformity of distribution; It is the natural logarithm function. A larger value indicates a more even weight distribution in all directions (a more open environment). A smaller value indicates a more concentrated weight (fewer possible directions, more channel-like characteristics). A directional constraint metric is also defined. for:

[0038] , To reflect the concentration of passable directions, and A larger value indicates that the local direction is more restricted.

[0039] In the early stages, RRT* requires rapid exploration of the coverage space, while in the later stages, it requires more refined expansion around the found feasible pathways to improve quality.

[0040] Therefore, a time factor is introduced. Time factor We obtain it from the following formula: , in, This represents the current iteration number. The upper limit of iterations, For preset shape parameters, Iteration limit The preset stopping threshold can be determined based on the workspace scale, obstacle density, and computational resource limitations, or obtained through offline simulation calibration, and the shape parameters... These are preset adjustment parameters used to control the growth curve of the time factor; a value of 1 to 3 is preferred. A larger time factor results in more significant changes in the later stages of iteration. Iterative progression facilitates a gradual transition from early exploration to later convergence; the time factor is only used to adjust boundary parameters and does not replace the dominant role of complexity in local behavior. Time Factor The value is gradually increased from 0 to 1 as the iteration progresses, used to adjust the upper bound of the target bias. With the maximum step size boundary The time factor is only used to change the boundary, so that the overall strategy transitions from exploration to convergence. However, at any given moment, the local behavior is still determined by the local environment complexity factor, so as to avoid excessive expansion in complex regions due to the passage of time.

[0041] A fixed step size that is too large in complex regions can easily lead to crossing narrow passages, connecting lines close to edges, or an increased collision rate. Therefore, this embodiment of the invention binds the step size to the complexity of the nearest node: the higher the complexity, the smaller the step size; the lower the complexity, the larger the step size. Simultaneously, a time factor is introduced so that the maximum step size boundary gradually decreases with iteration, achieving a gradual transition from coarse to fine. This allows for rapid deployment of the tree structure and traversal of open areas in the early stages, while enabling refinement and expansion near critical paths in the later stages, improving path quality and accessibility.

[0042] In each iteration of RRT*, the focal node is determined in the tree structure. And based on the complexity of the focus node. and time factor Calculate the target bias probability ; Target bias probability Cut off to Generate uniform random numbers ,when season Otherwise in free space Uniform random sampling was performed within the area to obtain This sampling is a random probability decision, not a deterministic rule that selects the candidate points with the highest / lowest probability. It aims to reduce target bias in complex areas, increase target bias in open areas, and achieve gradual adjustment of the bias boundary under the influence of the time factor.

[0043] like Figure 3 As shown, obtaining the target bias probability includes the following steps: First, by time factor Upper bound of target bias Gradual adjustment is achieved, expressed by the following formula: , in, This is the initial target bias upper bound parameter. This serves as the upper bound parameter for the target bias in the later stages, and ; Combined with the focal node Local environmental complexity factor Obtain the target bias probability As shown in the following formula: , in, This is the lower bound parameter for the target bias. The time factor is used to relax / convergence bias boundaries (smaller in the early stages, larger in the later stages to promote convergence); the local environment complexity factor is used to suppress / enhance local loop closure bias (reducing the probability of sampling towards the endpoint in complex regions to increase entry point exploration, and increasing the probability of target sampling in open regions to accelerate convergence). The principle for setting the target bias parameters is: To ensure that sampling traction to the endpoint is always maintained, it can be calculated according to "expected approximately..." The frequency preset for "at least one endpoint sampling per iteration" is preferably 0.01 to 0.10; To explore the upper bound of the target bias in the exploration phase, and to avoid excessive sampling in the obstructed direction in the early stage, it is advisable to take a relatively small value, preferably 0.05 to 0.15; The upper bound of the objective bias during the convergence phase is set to enhance the convergence trend in later stages; preferably, it is 0.20–0.40, and satisfies the following conditions: The above parameters can be calibrated based on the obstacle density of the workspace, the proportion of narrow channels, and the effective expansion rate / convergence speed statistically obtained from offline simulation.

[0044] In tree structure In the above-mentioned sampling points In the computation tree structure and The node with the smallest Euclidean distance is the nearest node. ,Right now For the nearest node computational complexity and combined with time factor Calculate the expansion step size ;Depend on Generate new nodes in the direction of sampling points Complex regions correspond to smaller step sizes, while open regions correspond to larger step sizes. The overall step size boundary monotonically decreases from large to small over time, and new nodes are generated based on the expanded step size. Simultaneously, construct candidate edges. (That is, the line segment between two nodes). Specifically, the step size is expanded. The acquisition includes the following steps:

[0045] First, by time factor Make the maximum step size boundary Gradual tightening is expressed by the following formula: , in, These are the initial maximum step size boundary parameters. These are the boundary parameters for the maximum step size in the later stages, and ; Minimum step size boundary Combined with the most recent node Local environment complexity factor Get the extended step size It can be expressed by the following formula: , in, This represents the current iteration number. This is the minimum step size boundary parameter. In complex regions, the step size is automatically shortened to reduce collision and edge-grabbing risks, while in open regions, the step size is increased to improve coverage efficiency. The time factor only changes the "overall available step size upper bound," achieving a gradual transition from early coarse-grained expansion to later fine-grained expansion. The local environmental complexity factor always dominates the local conservative / aggressive degree; the greater the complexity, the more... The smaller the size and the lower the complexity, the better. The larger the step size, the better. The principle for setting the step size parameter is: The initial maximum step size boundary is used for rapid deployment of tree structures in open areas, optimized for local evaluation scales. Same magnitude setting (e.g., take) or its proportion); This serves as the maximum step size boundary for later refinement and expansion, with the preferred step size being... 0.4 to 0.7, and satisfying ; This is the minimum step size boundary, used to ensure gradual advancement even in narrow channel scenarios. Ideally, it should not exceed the safe expansion radius. This step size is matched to the map resolution and the minimum control step size. These step size parameters can be calibrated through offline simulation, striking a balance between collision rate, convergence speed, and path quality.

[0046] Traditional fixed target biasing is prone to repeated failures in bottleneck regions. This invention binds the target bias probability to the complexity of the focus node. To avoid the focus node falling into dead ends, near points behind walls, or other false fronts that lead to distorted target bias adjustment, instead of directly selecting a single nearest node in each iteration, a tree structure is first used to determine the target bias. The first one with the smallest Euclidean distance to the endpoint is selected from the options. The nodes constitute the focus candidate set. .in, The preset number of candidates is preferably 5 to 30; as the number of tree nodes increases, it can be... The constraint is adaptively determined within this range. Specifically, let the current tree... The total number of nodes is ,but

[0047] , in, It is the natural logarithm function. Indicates rounding up. and Used to Cut-off restrictions Within the range.

[0048] In the candidate set The focus node is selected using a comprehensive evaluation function: , The comprehensive evaluation can simultaneously consider the distance from candidate nodes to the endpoint. and the path cost from the origin to the node. It can optionally introduce a penalty term that is positively correlated with the number of expansion failures or the collision rate. This is to suppress the repeated occurrence of long-term ineffective candidate nodes as focal nodes. Based on this, a comprehensive evaluation function can be constructed:

[0049] , in, These are preset weighting coefficients. It can be calculated directly; For the RRT* tree, from the starting point to the node The cumulative path cost (accumulated by the cost of the parent node's link edge and updated synchronously during insertion / reconnection); The number of attempts and failures / collisions can be obtained from the statistics of the node; the weighting coefficient can be preset to the same order of magnitude and optimized through offline simulation calibration.

[0050] Based on the selected Computational focus complexity The target bias probability is obtained by combining the time factor, thereby suppressing target sampling in complex regions and enhancing target sampling in open regions, while avoiding repeated ineffective expansion in blocked directions. When the complexity around the focus is high, the target bias is reduced, and more uniform exploration is performed to find a passable entry point; when the complexity around the focus is low, the target bias is increased to accelerate convergence.

[0051] At the same time, the introduction of a time factor makes As the iterations gradually increase: Early iterations avoid premature local obstruction at the endpoint, while later iterations enhance the convergence trend. Since the final probability is still multiplied by... Therefore, complex regions consistently maintain a low target bias. (Focus candidate set) With comprehensive evaluation function The strategy is designed to suppress the misleading effect of false frontiers / dead end near points on bias adjustment and improve the stability of closed-loop scheduling.

[0052] To reduce redundant path points and improve path simplicity, pruning is performed after the path point sequence is extracted. The pruning principle is to delete only redundant points, without adding new points, and without performing curve fitting or interpolation, thereby maintaining the subset property of the path point set.

[0053] In this embodiment, as Figure 5 As shown, path extraction also includes performing pruning to remove redundant path points. Pruning includes the following steps: Collision detection is performed on candidate straight line segments connecting any two points in the path point sequence based on the outward obstacle region. If the straight line segment has no collision, redundant intermediate path points are deleted. After finding a path, the path is optimized by removing unnecessary inflection points to make the path smoother and indirectly reduce the risk of segments close to obstacles. Pruning and deleting redundant points improves the simplicity of the path, and complexity constraints can be used to avoid excessive point deletion in complex regions.

[0054] refer to Figure 4 Pruning also includes complexity constraints: Discrete sampling is performed along candidate straight line segments, and the average complexity is calculated. Deletion is only performed if the average complexity does not exceed a preset threshold. Complexity constraint: For two points to be detected in the path point sequence... and ( Construct candidate direct-connection segments Along the candidate straight-line segments at sampling intervals Perform discrete sampling, number of sampling points for:

[0055] , in, The preset sampling interval is preferably set to no more than [value missing]. Half (or no more than) (half of the original); and obtain the sampling points in sequence. : Local environment complexity factor based on sampling points The average complexity of calculating candidate straight-line segments : If and only if the candidate directly connected line segment It does not collide with the outward-expanding obstacle area and meets the following requirements. When the direct connection replacement is deemed valid, the connection located at [location] is deleted. and Redundant path points between Otherwise, keep the original path point sequence unchanged to avoid excessive deletion of points in complex areas, which could lead to edge-connecting or reduced feasibility.

[0056] in, This is the complexity threshold, which can be preset according to the task's safety margin requirements or obtained through offline simulation calibration.

[0057] The second part of the embodiments of the present invention provides a mobile robot path planning system, which operates in a continuous workspace containing static obstacles, including: The environment modeling module is used to expand the obstacle area based on the robot's equivalent radius and safety margin, thereby obtaining the expanded obstacle area and determining the free space. The local environment complexity generation module is used to generate a local environment complexity factor that integrates clearance measurement and orientation constraint measurement for any location within the workspace. The time factor generation module uses RRT* iteration number as the time variable to generate a time factor that monotonically changes with the iteration number. The dynamic target bias sampling module is used to select multiple nodes with the smallest Euclidean distance to the endpoint from the tree structure in each iteration to form a focus candidate set, and to determine the focus node based on the Euclidean distance from the candidate node to the endpoint and the path cost from the starting point to the node; the target bias probability is calculated based on the local environment complexity factor and time factor of the focus node, and the endpoint is used as the sampling point to determine whether to use the endpoint. The adaptive step-size expansion module is used to calculate the local environment complexity of the tree node closest to the sampling point in each iteration, determine the expansion step size in combination with the time factor, and generate new nodes and candidate edges from the nearest node toward the sampling point according to the step size. The collision detection and tree update module performs collision detection on new nodes and candidate edges based on the expanded obstacle region. If there is no collision, the new node is added to the tree structure and the RRT* reconnection operation is performed. The path extraction and optimization module extracts the path point sequence from the starting point to the ending point from the tree structure when the termination condition is met, forming the final path. The termination condition includes reaching the preset neighborhood range of the ending point or the number of iterations reaching a preset upper limit.

[0058] Example 1: Planning Implementation in a Continuous Two-Dimensional Geometric Obstacle Environment (1) Representation of environment and obstacles The workspace is a two-dimensional continuous planar region, and obstacles are composed of geometric shapes such as circles, polygons, or rectangles. Starting point. and the finish line Located in free space.

[0059] (2) Safety Expansion and Collision Detection Calculated based on the robot's equivalent radius and safety margin And by expanding the obstacles outward, we obtain an expanded set of obstacles.

[0060] Collision detection includes two categories: a) Node collision detection: Determine whether a node is located within any outward-expanding obstacle area; b) Edge collision detection: Determines whether a line segment intersects with any outward-expanding obstacle region, or whether it overlaps with any other obstacle region along the line segment at intervals. Discrete sampling is performed, and node collision detection is applied to the sampled points; discrete sampling is an engineering approximation method. To reduce the risk of missing extremely thin obstacles, The preferred value is no greater than the safe outer radius. Half (or no more than the safety margin) (half of the original text is missing). In situations with high safety requirements, analytical geometric intersection detection (precise intersection determination of line segments and outward obstacle areas) is preferred as the edge collision detection method.

[0061] (3) Calculation of local environment complexity factor Set local scale With direction number .in, Preset parameters for the number of ray detection directions, used to characterize position. The surrounding area has accessible routes; The constant can be taken as follows, preferably 8 to 32 (e.g., 16). A larger value results in higher directional resolution but also a corresponding increase in computational cost. To ensure uniform directional coverage, we can set... , For any point Calculate the shortest distance to the outward-expanding barrier boundary To obtain the net air volume measurement .

[0062] In direction-constrained metrics, from by Rays are emitted in one direction, and the distance between the nearest intersection point of the ray and the outward-spreading obstacle is the free distance. If it does not intersect with the obstacle, then take The weights are obtained by normalizing the free distance. And based on entropy normalization, we obtain The fusion yielded .

[0063] In open areas, free distances in multiple directions are close. In corridors or narrow passages, the directional constraint metric is relatively small; however, in corridors or narrow passages, only a few directions have a large free distance, resulting in a greater directional constraint metric. Increase.

[0064] (4) Dynamic target bias In each iteration, the tree structure is first selected from the trees with the smallest Euclidean distance to the endpoint. The nodes constitute the focus candidate set. Then according to the comprehensive evaluation function Select the focus node ,calculate And update in conjunction with time factor , obtain the target bias probability .when When the area is large (complex / narrow channel region), This relatively reduces the probability of sampling to the endpoint and increases the uniform sampling ratio, thereby improving the exploration of the channel entrance; when When the area is relatively small (open area), The relative increase increases the probability of sampling to the endpoint, thereby accelerating convergence.

[0065] (5) Adaptive step size For the nearest node Calculate the local environment complexity factor and combined with time factor Update maximum step size boundary Further calculate the extended step size Then by Generate new nodes in the direction of the sampling point by expanding the step size. In this process, complex regions correspond to smaller step sizes to traverse narrow openings and reduce the risk of edge contact, while open regions correspond to larger step sizes to improve coverage efficiency. As time progresses, the overall step size boundary monotonically decreases from large to small, thus allowing for more refined expansion in the later stages to promote convergence.

[0066] (6) RRT* reconnection After a new node is added to the tree structure through collision detection, the parent node with the minimum cost within the neighborhood radius is selected and rewire is performed to continuously improve the path cost and maintain the asymptotic optimality of RRT*.

[0067] (7) Path extraction and pruning When the endpoint neighborhood is reached or the iteration limit is reached, the path point sequence is extracted and pruning is performed. Basic pruning only relies on the direct connection collision-free condition to remove redundant points; in complexity-constrained pruning, the average complexity of the candidate direct connection segment is further calculated and compared with the threshold. Only when the complexity is not high is large-span point deletion allowed, thereby avoiding excessive point deletion in narrow channel areas that leads to direct connections touching the edge.

[0068] Example 2: Parameter Setting Example Number of directions 12 or 16 can be chosen; local scale Can be set on the same order of magnitude as the maximum step size; time factor shape parameter A value of 2 can be selected to make later changes more obvious; the target bias parameter can be set. , , Step size parameter can be set. and The ratio is 1:3 to 1:4. Pick The pruning complexity threshold is 0.4 to 0.7. It can be calibrated according to the scene channel width and safety margin.

[0069] This invention, based on the standard RRT* process, introduces a local environment complexity factor. The local environment complexity factor provides feedback during each expansion and sampling decision: when the local environment is complex, the target bias is reduced and the step size is decreased; when the local environment is open, the target bias is increased and the step size is increased. To balance rapid early exploration with fine-grained later convergence, a time factor is introduced. The upper bound of the target bias and the maximum step size boundary are progressively adjusted to allow the algorithm to gradually transition from graph laying to convergence. However, the final scheduling is still dominated by the local environment complexity factor, thus ensuring that conservative behavior is maintained in complex regions. This invention does not change the tree structure and reconnection rules of RRT*, but introduces a closed-loop scheduling mechanism based on local environment complexity in the two key stages of sampling decision and expansion step size decision. Optional complexity constraint pruning can also be introduced, thus forming an improved implementation of RRT*.

[0070] Furthermore, to enhance safety margins and reduce the risk of edge-bound connections, obstacle safety expansion is employed, merging the robot's geometry and safety margin into the expanded obstacle area for collision detection. Finally, redundant points are pruned on the resulting path, and in one embodiment, complexity constraints are used to control the pruning intensity to avoid excessive point deletion in complex regions.

[0071] The above inventions are merely a few specific embodiments of the present invention. However, the embodiments of the present invention are not limited thereto, and any variations that can be conceived by those skilled in the art should fall within the protection scope of the present invention.

Claims

1. A path planning method for a mobile robot RRT*, characterized in that, Includes the following steps: Environmental modeling and safety expansion: An environmental model containing static obstacles is established in a continuous workspace. Based on the robot's equivalent radius and safety margin, the obstacles are expanded outward to obtain the expanded obstacle region and determine the free space. Construction of local environment complexity factor: A local environment complexity factor is constructed for any location within a continuous workspace. The local environment complexity factor is obtained by fusing the clearance metric and the orientation constraint metric. Time factor construction: Using the number of iterations in RRT* as the time variable, construct a time factor that changes monotonically with the number of iterations; Dynamic target bias sampling: In each iteration, a focus candidate set is constructed in the tree structure of RRT*. The focus candidate set consists of multiple nodes in the tree structure with the smallest Euclidean distance to the destination. The focus node is determined based on the Euclidean distance from the candidate node to the destination and the path cost from the starting point to the candidate node. The target bias probability is obtained based on the local environment complexity factor and time factor of the focus node. The destination is then used as the sampling point based on the target bias probability. Adaptive step-size expansion: In each iteration, the complexity of the tree node closest to the sampling point is calculated, and the expansion step size is obtained by combining the time factor; new nodes and candidate edges are generated from the nearest node toward the sampling point according to the expansion step size; Collision detection and RRT* reconnection: Collision detection is performed on new nodes and candidate edges based on the expanding obstacle region. When there is no collision between the new node and its edges, the new node is added to the tree structure and RRT* reconnection is performed. Path extraction: When the termination condition is met, the path point sequence from the starting point to the ending point is extracted from the tree structure to form a path plan. The termination condition includes reaching the ending point within a preset neighborhood or the number of iterations reaching a preset upper limit.

2. The mobile robot RRT* path planning method as described in claim 1, characterized in that, The local environment complexity factor We obtain it from the following formula: ， in, For the measurement of net clearance, This is a directionally constrained metric.

3. The mobile robot RRT* path planning method as described in claim 2, characterized in that, The clearance measurement is based on location. The shortest distance to the boundary of the outward-expanding obstacle is obtained and normalized; The directional constraint metric is obtained based on the free distance of multi-directional ray detection, and the free distance of each direction is normalized to obtain the weight of each direction. The distribution uniformity of the free distance weights in each direction is obtained based on the weight of each direction, and the directional constraint metric is obtained based on the distribution uniformity.

4. The mobile robot RRT* path planning method as described in claim 1, characterized in that, The time factor We obtain it from the following formula: ， in, This represents the current iteration number. The upper limit of iterations, For preset shape parameters, .

5. The mobile robot RRT* path planning method as described in claim 1, characterized in that, Obtaining the target bias probability includes the following steps: First, by time factor Upper bound of target bias Gradual adjustment is achieved, expressed by the following formula: ， in, This is the initial target bias upper bound parameter. This serves as the upper bound parameter for the target bias in the later stages, and ; Combined with the focal node Local environmental complexity factor Obtain the target bias probability As shown in the following formula: ， in, The target bias lower bound parameter.

6. The mobile robot RRT* path planning method as described in claim 1, characterized in that, The extended step size The acquisition includes the following steps: First, by time factor Make the maximum step size boundary Gradual tightening is expressed by the following formula: ， in, These are the initial maximum step size boundary parameters. These are the boundary parameters for the maximum step size in the later stages, and ; Minimum step size boundary Combined with the most recent node Local environment complexity factor Get the extended step size It can be expressed by the following formula: ， in, This represents the current iteration number. These are the minimum step size boundary parameters.

7. The mobile robot RRT* path planning method as described in claim 1, characterized in that, The focal node is determined through the following steps: Focus nodes are selected using a comprehensive evaluation function. The comprehensive evaluation function simultaneously considers candidate nodes. To the finish line distance and the path cost from the starting point to the candidate node. It also introduces a penalty term that is positively correlated with the number of expansion failures or collision rate. To suppress long-term ineffective candidate nodes from repeatedly becoming focal nodes, a comprehensive evaluation function is constructed accordingly: ， in, The weighting coefficients are preset to be of the same order of magnitude and optimized through offline simulation calibration. This is obtained from the statistics of the number of expansion attempts and the number of failures / collisions of the candidate node; Focus Node Determined by the following formula: ， in, The focus candidate set is a tree structure. The first one with the smallest Euclidean distance to the endpoint is selected from the options. It consists of nodes. Determined by the following formula: ， in, For the current tree structure The total number of nodes, It is the natural logarithm function. Indicates rounding up. and Used to Cut-off restrictions Within the range.

8. The mobile robot RRT* path planning method as described in claim 1, characterized in that, The path extraction also includes performing pruning to delete redundant path points. The pruning includes the following steps: Collision detection is performed on candidate straight line segments between any two points in the path point sequence based on the outward obstacle region. If there is no collision between the straight line segments, the intermediate redundant path points are deleted.

9. The mobile robot RRT* path planning method as described in claim 8, characterized in that, The pruning also includes complexity constraints: Discretely sample along candidate straight line segments, calculate the average complexity, and only perform the deletion operation if the average complexity does not exceed a preset threshold.

10. A mobile robot path planning system, characterized in that, The system operates in a continuous workspace containing static obstacles, including: The environment modeling module is used to expand the obstacle area based on the robot's equivalent radius and safety margin, thereby obtaining the expanded obstacle area and determining the free space. The local environment complexity generation module is used to generate a local environment complexity factor that integrates clearance measurement and orientation constraint measurement for any location within the workspace. The time factor generation module uses RRT* iteration number as the time variable to generate a time factor that monotonically changes with the iteration number. The dynamic target bias sampling module is used to select multiple nodes with the smallest Euclidean distance to the endpoint from the tree structure in each iteration to form a focus candidate set, and to determine the focus node based on the Euclidean distance from the candidate node to the endpoint and the path cost from the starting point to the node; the target bias probability is calculated based on the local environment complexity factor and time factor of the focus node, and the endpoint is used as the sampling point to determine whether to use the endpoint. The adaptive step-size expansion module is used to calculate the local environment complexity of the tree node closest to the sampling point in each iteration, determine the expansion step size in combination with the time factor, and generate new nodes and candidate edges from the nearest node toward the sampling point according to the step size. The collision detection and tree update module performs collision detection on new nodes and candidate edges based on the expanded obstacle region. If there is no collision, the new node is added to the tree structure and the RRT* reconnection operation is performed. The path extraction and optimization module extracts a sequence of path points from the starting point to the ending point from the tree structure when the termination condition is met, forming the final path. The termination condition includes reaching the preset neighborhood range of the ending point or the number of iterations reaching a preset upper limit.

Images