Mobile robot path planning method in strongly restricted environment based on improved RRT-STAR

Abstract

The invention discloses a mobile robot path planning method in a strongly restricted environment based on improved RRT-STAR, and belongs to the technical field of intelligent robot navigation and path planning, and the method mainly comprises the steps: 1, initializing, and constructing a spatial index structure; 2, realizing efficient path planning through a density perception adjustment mechanism and a hybrid sampling strategy; and step 3, optimizing the path generated in the step 2. Local obstacle density and environmental risk are evaluated in real time through a density sensing and risk degree field weighted sampling mechanism, sampling bias probability and extended step length are adaptively adjusted, dynamic weighted fusion is performed among three sampling modes of target priority, local adaptive and global uniformity, and the balance of global exploration and local optimization is realized; adjustable curvature continuity and jerk amplitude limiting soft constraint are introduced, local optimization is carried out by means of Clothoid segmented interpolation, and executable track improving path smoothness and overall quality meeting the dynamic requirements of Ccontinuity, speed, minimum turning radius and the like are generated.

Description

Technical Field

[0001] The present invention belongs to the technical field of intelligent robot navigation and path planning, and in particular relates to a path planning method for a mobile robot in a strongly constrained environment based on an improved RRT-STAR. Background Art

[0002] Path planning algorithms have been widely used in robot navigation, unmanned driving, and industrial automation. Among them, the rapidly expanding random tree (RRT) and its improved algorithms (such as ) plays an important role in high-dimensional, continuous space path planning tasks. RRT constructs a tree structure from the starting point to the target point through random sampling, which has the ability to quickly generate feasible paths. Therefore, it has obvious advantages in scenarios with high real-time requirements. However, due to the randomness of its sampling, the path quality generated by this algorithm is often low, and the path may be uneven and lengthy. To address this shortcoming, By introducing path reconnection and optimization mechanisms, the asymptotic optimality of the path is improved, but it still performs poorly in complex environments.

[0003] In complex environments (such as dense obstacles or complex and changing scenes), RRT and The main reason for this is that the random sampling strategies of these algorithms tend to focus on irrelevant areas, resulting in the generation of a large number of redundant nodes and significantly increasing search time. Furthermore, these algorithms rely on uniform sampling strategies, which are difficult to adapt to the dynamic changes in obstacle density in the environment, resulting in insufficient exploration of key areas, further reducing the efficiency and effectiveness of the algorithms. These problems are particularly prominent in high-dimensional or high-complexity scenarios, becoming a significant factor limiting the performance of the algorithms.

[0004] In order to overcome the above problems, further improved algorithms such as Informed RRT and RRT-Connect have been proposed. Informed RRT uses heuristic methods to narrow the sampling range, significantly accelerating the convergence speed of the algorithm, while RRT-Connect adopts a bidirectional expansion method to further improve the search efficiency of the algorithm. However, these improved algorithms still have some shortcomings that cannot be ignored when facing narrow channels or dynamic obstacle environments. For example, the heuristic sampling strategy is highly dependent on the environment and has difficulty in coping with dynamic changes; although bidirectional expansion can accelerate the search, it is easy to fall into local optimal solutions in complex scenarios. In addition, these algorithms generally lack the ability to intelligently explore key areas, the sampling strategy is fixed, and it is difficult to dynamically adjust according to the distribution of obstacles, resulting in a waste of computing resources and the path quality is still not smooth enough.

[0005] In summary, in complex environments with dense obstacles, traditional algorithms tend to generate a large number of redundant nodes, resulting in low search efficiency and excessive computational overhead. Furthermore, the resulting paths are often uneven and lengthy, making them difficult to meet the efficiency and quality requirements of practical applications. Furthermore, fixed sampling strategies cannot be dynamically adjusted to environmental changes, limiting the algorithm's adaptability in complex and changing scenarios. Summary of the Invention

[0006] In order to solve the problems raised by the above background technology, the purpose of the present invention is to provide a path planning method for mobile robots in strongly constrained environments based on improved RRT-STAR, which evaluates local obstacle density and environmental risks in real time through density perception and risk field weighted sampling mechanism, adaptively adjusts sampling bias probability and expansion step size, and dynamically weighted fusion between the three sampling modes of target priority, local adaptation and global uniformity to achieve a balance between global exploration and local optimization; on this basis, adjustable curvature continuity and jerk limit soft constraints are introduced, and local optimization is performed with the help of Clothoid segmented interpolation to generate executable trajectories that meet dynamic requirements such as C² continuity, speed, and minimum turning radius; at the same time, incremental KD-tree / R-tree indexing is used to accelerate nearest neighbor retrieval and collision detection, and multi-layer parent node candidate screening and multi-objective evaluation significantly reduce computational overhead, improve path smoothness and overall quality. Simulation and experimental results show that this method is superior to traditional methods in terms of path length, smoothness, convergence speed and computational efficiency. The existing improved algorithms have been significantly improved.

[0007] To achieve the above object, the present invention provides the following technical solution: a path planning method for a mobile robot in a strongly constrained environment based on an improved RRT-STAR, characterized in that it comprises the following steps: S1. Initialization S101, input path planning parameters; S103, constructing a starting point tree and an end point tree, and establishing a corresponding spatial index structure for each tree based on the multi-dimensional space partitioning principle to accelerate efficient nearest neighbor search of path nodes; S105. Construct an R-tree corresponding to the obstacle list for fast obstacle collision detection; S107. Perform Euclidean distance transform (EDT) on obstacles based on the prior map to construct a “risk field” representing the distance between each point in the free space and the nearest obstacle. S2. Path Planning S202, path planning loop limit check, determine whether the maximum number of iterations has been reached, if not, continue iteration; S204, switching between the starting tree and the end tree, alternately selecting the starting tree or the end tree for path expansion, thereby improving search efficiency and accelerating the speed of finding a feasible path; S206. Calculate the bias probability based on the local obstacle density and select a sampling method to perform sampling node sampling. S208, find the nearest node to the sampling node, generate a new node along the direction of the sampling node, expand the tree, and dynamically adjust the step size according to the local obstacle density; S210, check whether the path between the new node and the nearest node has no collision; if there is no collision, add the new node to the tree and update the spatial index structure of the tree; S212, parent node selection and reconnection, select the nearest neighbor node and its ancestor node as candidate parent nodes, optimize the connection path; update the tree structure to maintain the global optimality; S220, connecting the starting point tree and the ending point tree, checking the connection status of the new node, and generating a complete path; S3. Optimize the path generated in step 2 Furthermore, the path planning parameters of S101 include: starting point, end point, obstacle list, local perception range, original step size, number of iterations, turning radius, speed, maximum number of iterations, and bias probability.

[0008] Preferably, the spatial index structure described in step S103 is a KD tree index structure. The KD tree index structure is a binary tree structure that organizes data points by recursively partitioning the space. The structure is constructed by alternately partitioning in each dimension to balance the height of the tree, thereby improving search efficiency. The construction of the KD tree index structure includes the following steps: S1031Start with a data set and select a dimension; S1033 divides the data points into two parts according to the median of the dimension, and the point where the median is located becomes the root node of the KD tree; S1035 For each part, recursively select the next dimension and repeat the above segmentation process until all data points are assigned to leaf nodes.

[0009] Furthermore, when a new node is inserted, the KD tree index is updated in real time to ensure that the new node can immediately participate in the nearest neighbor search; at the same time, for deleted or adjusted nodes, the index is efficiently updated through local reconstruction to maintain query efficiency and data consistency.

[0010] Furthermore, the node sampling methods described in step S206 include target-priority sampling, local adaptive sampling, global uniform sampling, and risk field weighted sampling. The target-priority sampling assigns higher sampling weights to sampling points within a predetermined target area based on the current distance to the target, the real-time sampling success rate, and the risk field information, ensuring that the search always converges quickly toward the target area. The local adaptive sampling uses the R-tree incremental index to collect real-time statistics on obstacle distribution, node density, spatial continuity, and other information for the current expansion area, and dynamically adjusts the sampling probability and expansion step size in combination with the risk field to achieve fine detection and optimization of local complex and narrow sections. The global uniform sampling performs uniform random sampling in the entire feasible space to maintain global coverage and prevent the algorithm from falling into a local optimum. The risk field weighted sampling linearly superimposes the risk field with the original sampling weights during the target-priority and local adaptive sampling stages, and prioritizes sampling within the risk range that maintains a safety margin (stays away from obstacles) while fully utilizing the channel space (not too far from obstacles), thereby generating a smoother and safer path corridor.

[0011] Furthermore, the method for selecting a candidate parent node in S212 includes the following steps: S2122 After the new node is generated, the KD tree index structure is used to quickly retrieve several candidate nodes with the shortest distance, and its ancestors are traversed upward to construct a set of candidate parent nodes. The query depth is limited to 3 to 5 layers of the nearest neighbor nodes. S2124 performs a multi-objective cost evaluation on candidate parent nodes, comprehensively considering path length, collision risk, and dynamic feasibility, and ultimately selects the node with the lowest cost and the most security as the parent node, and updates the tree structure. At the same time, it maintains the KD tree index through an incremental update strategy to ensure that the overall path continues to tend towards the optimal.

[0012] Furthermore, the path optimization in S3 includes the following steps: S31 Redundant Node Determination Redundant nodes are identified based on geometric continuity and steering transition smoothness. Variable thresholds for curvature continuity and curvature change rate are introduced as soft constraints to ensure C² continuity, which means that both the path curvature κ(s) and its rate of change along the arc length, dκ / ds, are continuous. This avoids sudden turns and acceleration jumps, ensuring smoothness and executable motion. When local environmental constraints prevent simultaneous thresholds from being met, geometric continuity is prioritized to ensure that an executable path is always generated. S33 prunes the generated path The pruning window size is adaptively adjusted according to the local curvature distribution. Fine pruning and resampling are performed in high curvature sections, while coarse pruning is maintained in low curvature sections to balance smoothness and algorithm efficiency.

[0013] S35 path local optimization Apply Clothoid interpolation to the pruned path segments, taking advantage of the fact that the curvature changes linearly along the arc length. The expression is as follows: ; Curvature at the starting point and the end point The curvature transition is smooth, the acceleration and its rate of change jerk are limited, and the optimal C² smooth curve of the path with continuous curvature is constructed. The expression is as follows: ; Furthermore, the method for calculating the bias probability based on the local obstacle density includes the following steps: S2061 Calculate local obstacle density A predefined radius around each node Count the number of obstacles Let the total number of obstacles in the workspace be , then the local obstacle density Defined as: in, The value range of is from 0 to 1. The larger the value, the denser the obstacles in the area. S2063 adjusts the bias probability based on local obstacle density In order to balance the exploration in complex environments (random sampling) and the utilization in open areas (target priority sampling), the basic target sampling probability is defined as and the local density factor is , the value is usually between 0 and 1, adjusted according to the actual situation), then the bias probability The calculation formula is: When the local density When it is higher, It decreases accordingly, thereby increasing the random sampling ratio, which helps to obtain more complete exploration in areas with dense obstacles; on the contrary, in areas with fewer obstacles, The higher the probability, the faster the expansion to the target. Through the above two-step method, the bias probability can be dynamically adjusted according to the local obstacle density, ensuring a better balance between exploration and utilization in the path planning of mobile robots in strongly constrained environments, thereby improving the efficiency and adaptability of path planning.

[0014] Furthermore, the greedy algorithm operation steps are as follows: S2062 random probability assignment, the expression is as follows: ; Generate compliance interval Uniformly distributed random numbers , and subsequently by and bias probability Compare and decide on the sampling mode to be adopted; S2064 Target Priority Sampling ) executes the following algorithm: ; Close to the target node or starting point The area is sampled to increase the probability of the two trees being connected; at the same time, the bias sampling probability is dynamically adjusted through the formula, where is the local density factor; S2066 Local Adaptive Sampling ) is executed: randomly select a node from the current active tree , and within its radius Generate new samples ; This mode helps improve local connectivity and is suitable for narrow channels or areas with dense obstacles; S2068 global uniform sampling Execute at: Throughout the workspace Uniform sampling ensures global exploration, avoids falling into local optimal solutions, and improves the global exploration ability of the algorithm. in, represents the target sampling bias probability (i.e., the probability of performing target sampling in a mixed sampling strategy); Indicates the basic target sampling probability (the default target sampling probability in an obstacle-free or low-obstacle environment); Local density factor (used to control the impact of obstacle density on target sampling probability, the value range is generally 0 to 1); Indicates the local obstacle density (the density of obstacles within a predefined radius, ranging from 0 to 1, with larger values ​​indicating denser obstacles).

[0015] Furthermore, the tree expansion in step S208 includes the following steps: After determining the nearest neighbor node, S2082 generates a new node along the sampling direction in combination with the robot's own dynamic constraints, which are the maximum steering angle and linear speed limits, to ensure that the expanded path is feasible in terms of both kinematics and dynamics. S2084 uses an adaptive step size based on obstacle distribution characteristics. In sparse areas, the step size is appropriately increased to improve search efficiency. In areas with dense obstacles or narrow passages, the step size is reduced and steering constraints are combined to ensure feasibility and safety. S2086 incorporates both dynamic constraints and local environment distribution into the expansion process, which can further avoid the risk of path deviation or collision during actual execution.

[0016] Compared with the prior art, the present invention has the following beneficial effects: 1. The present invention uses a density-aware adjustment mechanism to dynamically adjust the sampling rate and step size based on the local obstacle density, optimizing exploration efficiency. Furthermore, the algorithm adopts a more detailed exploration strategy in areas with dense obstacles, while a more extensive exploration strategy can be adopted in areas with sparse obstacles, thereby improving overall exploration efficiency.

[0017] 2. The hybrid sampling strategy proposed in this invention combines risk field weighted sampling, target priority sampling, local adaptive sampling and global uniform sampling to achieve a balance between global exploration and local optimization; it breaks through the limitations of traditional single or fixed weighted hybrid sampling, and fully utilizes the prior information of environmental geometry and safety margin, achieving multiple improvements in path smoothness, safety and search efficiency.

[0018] 3. This invention introduces adjustable curvature continuity and jerk clipping soft constraints, and uses Clothoid piecewise interpolation for local optimization to generate executable trajectories that meet dynamic requirements such as C² continuity, speed, and minimum turning radius. It also uses incremental KD-tree / R-tree indexing to accelerate nearest neighbor retrieval and collision detection, and significantly reduces computational overhead, improves path smoothness, and overall quality through multi-layer parent node candidate screening and multi-objective evaluation. Simulation and experimental results show that this method is superior to traditional methods in terms of path length, smoothness, convergence speed, and computational efficiency. The existing improved algorithms have been significantly improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 It is a schematic diagram of the overall process of the present invention; Figure 2 This is the path planning effect diagram in a simple environment of the present invention; Figure 3 This is the effect diagram of the path planning in a dense obstacle environment of the present invention; Figure 4 This is the path planning effect diagram of the present invention in a narrow channel environment; Figure 5 Schematic diagram of the KD tree structure of the present invention; Figure 6 Schematic diagram of the parent node selection strategy of the present invention; Figure 7 This is an effect diagram of the simulation test path planning method of the present invention; DETAILED DESCRIPTION The present invention will be further described below with reference to the embodiments.

[0020] The following examples are intended to illustrate the present invention but are not intended to limit the scope of protection of the present invention. The conditions in the examples may be further adjusted according to specific conditions. Simple improvements to the method of the present invention within the scope of the present invention are also within the scope of protection claimed in the present invention.

[0021] Example 1 See also Figure 1-6 The present invention provides a path planning method for a mobile robot in a strongly constrained environment based on an improved RRT-STAR, which is characterized by comprising the following steps: S1. Initialization S101, input path planning parameters; S103, constructing a starting point tree and an end point tree, and establishing a corresponding spatial index structure for each tree based on the multi-dimensional space partitioning principle to accelerate efficient nearest neighbor search of path nodes; S105. Construct an R-tree corresponding to the obstacle list for fast obstacle collision detection; S107. Perform Euclidean distance transform (EDT) on obstacles based on the prior map to construct a “risk field” representing the distance between each point in the free space and the nearest obstacle. S2. Path Planning S202, path planning loop limit check, determine whether the maximum number of iterations has been reached, if not, continue iteration; S204, switching between the starting tree and the end tree, alternately selecting the starting tree or the end tree for path expansion, thereby improving search efficiency and accelerating the speed of finding a feasible path; S206. Calculate the bias probability based on the local obstacle density and select a sampling method to perform sampling node sampling. S208, find the nearest node to the sampling node, generate a new node along the direction of the sampling node, expand the tree, and dynamically adjust the step size according to the local obstacle density; S210, check whether the path between the new node and the nearest node has no collision; if there is no collision, add the new node to the tree and update the spatial index structure of the tree; S212, parent node selection and reconnection, select the nearest neighbor node and its ancestor node as candidate parent nodes, optimize the connection path; update the tree structure to maintain the global optimality; S220, connecting the starting point tree and the ending point tree, checking the connection status of the new node, and generating a complete path; S3. Optimize the path generated in step 2 like Figure 2-4As shown, the algorithm provided by the present invention can adopt a more detailed exploration strategy in areas with dense obstacles through the density-aware adjustment mechanism, and a more extensive exploration strategy in areas with sparse obstacles, thereby improving the overall exploration efficiency; the hybrid sampling strategy proposed in the present invention combines risk field weighted sampling, target priority sampling, local adaptive sampling and global uniform sampling to achieve a balance between global exploration and local optimization; it breaks through the limitations of traditional single or fixed weighted hybrid sampling, and makes full use of the prior information of environmental geometry and safety margin, achieving multiple improvements in path smoothness, safety and search efficiency.

[0022] In a preferred embodiment, the path planning parameters of S101 include: starting point, end point, obstacle list, local perception range, original step size, number of iterations, turning radius, speed, maximum number of iterations, and bias probability.

[0023] As attached Figure 5 As shown, in a preferred embodiment, the spatial index structure described in step S103 is a KD tree index structure. The KD tree index structure is a binary tree structure that organizes data points by recursively partitioning the space. The structure is constructed by alternately partitioning in each dimension to balance the height of the tree, thereby improving search efficiency. The construction of the KD tree index structure includes the following steps: S1031Start with a data set and select a dimension; S1033 divides the data points into two parts according to the median of the dimension, and the point where the median is located becomes the root node of the KD tree; S1035 For each part, recursively select the next dimension and repeat the above segmentation process until all data points are assigned to leaf nodes.

[0024] Attachment Figure 5 The left picture shows the hierarchical structure of the KD tree: P6 (root node) is the root node of the KD tree and is located at the top of the tree; P2 and P3 (first-level child nodes) are two child nodes of P6, located at the lower left and lower right of P6 respectively; P1, P4, and P5 (second-level child nodes) are child nodes of P2 and P3, where P1 is the left child node of P2, P4 and P5 are the right child nodes of P2, and P3 has no child nodes. This hierarchical structure demonstrates the principle of KD tree to organize data points by recursively dividing the space, each node represents a spatial region, and its child nodes represent more subdivided regions; Figure 5 The right picture shows the spatial division of the KD tree: Nodes P1, P2, P4, P5, and P6 represent data points in the KD tree, and their positions in the two-dimensional space are determined by the horizontal and vertical coordinates; The node P3 is located to the right of P2, which means there is no data point in the area to the right of P2, or P3 is the representative point of the area; The vertical dotted lines represent the partitioning of the KD tree in different dimensions. Further, the vertical dotted line between P2 and P6 represents the partitioning on the x-axis, and the vertical dotted line between P4 and P5 also represents the partitioning on the x-axis.

[0025] The parallel dashed lines indicate the segmentation on the y-axis; The KD tree is constructed by alternating splits along each dimension. Specifically, starting from the root node P6, the first split is performed along the x-axis (P2 and P3), followed by splits along the y-axis (P4 and P5). This alternating split helps balance the height of the tree, thereby improving search efficiency. In general, the KD tree recursively splits the space along different dimensions, organizing multidimensional data points into a binary tree structure. This makes fast searches and nearest neighbor searches possible in multidimensional space.

[0026] In a preferred embodiment, when a new node is inserted, the KD tree index is updated in real time to ensure that the new node can immediately participate in the nearest neighbor search; at the same time, for deleted or adjusted nodes, the index is also efficiently updated through local reconstruction to maintain query efficiency and data consistency.

[0027] In a preferred embodiment, the node sampling methods described in step S206 include target-priority sampling, local adaptive sampling, global uniform sampling, and risk field weighted sampling. The target-priority sampling assigns higher sampling weights to sampling points within a predetermined target area based on the current distance to the target, the real-time sampling success rate, and the risk field information, ensuring that the search always converges rapidly toward the target area. The local adaptive sampling uses R-tree incremental indexing to collect real-time statistics on obstacle distribution, node density, spatial continuity, and other information for the current expansion area, and dynamically adjusts the sampling probability and expansion step size in combination with the risk field to achieve precise detection and optimization of local complex and narrow sections. The global uniform sampling performs uniform random sampling within the entire feasible space to maintain global coverage and prevent the algorithm from falling into a local optimum. The risk field weighted sampling linearly superimposes the risk field with the original sampling weights during the target-priority and local adaptive sampling stages, prioritizing sampling within a risk range that maintains a safety margin (stays away from obstacles) while fully utilizing the channel space (not excessively far from obstacles), thereby generating a smoother and safer path corridor.

[0028] As attached Figure 6As shown, in a preferred embodiment, the method for selecting a candidate parent node in S212 includes the following steps: S2122 After the new node is generated, the KD tree index structure is used to quickly retrieve several candidate nodes with the shortest distance, and its ancestors are traversed upward to construct a set of candidate parent nodes. The query depth is limited to 3 to 5 layers of the nearest neighbor nodes. S2124 performs a multi-objective cost evaluation on candidate parent nodes, comprehensively considering path length, collision risk, and dynamic feasibility, and ultimately selects the node with the lowest cost and the most security as the parent node, and updates the tree structure. At the same time, it maintains the KD tree index through an incremental update strategy to ensure that the overall path continues to tend towards the optimal.

[0029] q_new is a newly generated node, which is obtained by sampling and expanding from a node in the existing tree; q_parent1 and q_parent2 are nodes in the existing tree that are potential parent nodes of q_new. The parent nodes are the direct predecessors of the new node in the tree. q_near1, q_near2, q_near3, q_near4 are the neighboring nodes of q_new, which are the set of nodes in the tree that are closest to q_new; these neighboring nodes are used for parent node selection and path optimization.

[0030] X_obs (circular area) is the obstacle area, which indicates the area that needs to be avoided during path planning.

[0031] Near (dashed circle) represents the neighboring search range of q_new, that is, the range of searching for the node closest to q_new in the tree; In RRT or In the algorithm, the selection of the parent node is usually based on distance: q_new will try to connect itself to all neighboring nodes (q_near1, q_near2, q_near3, q_near4) and select the node that can minimize the path cost (such as path length or path cost) as its parent node; the path connecting q_new to q_near4 through the dotted line is optimal, and q_near4 is selected as the parent node of q_new.

[0032] In addition to selecting a parent node, our algorithm also performs path optimization (rewiring). If connecting q_new to a neighboring node (rather than its immediate parent) yields a more optimal path, the algorithm rewires q_new, altering the tree structure. Once a parent node is selected, a path is generated from the starting point to q_new. This path is constructed by connecting q_new to its parent node and then backtracking to the starting point, ensuring that the resulting path not only avoids obstacles but also optimizes path length or cost as much as possible.

[0033] In a preferred embodiment, the path optimization in S3 includes the following steps: S31 Redundant Node Determination Redundant nodes are identified based on geometric continuity and steering transition smoothness. Variable thresholds for curvature continuity and curvature change rate are introduced as soft constraints to meet the C² continuity condition. This means that both the path curvature κ(s) and its rate of change along the arc length dκ / ds are continuous. This avoids sudden turns and acceleration jumps, ensuring smoothness and executable motion. When local environmental constraints prevent simultaneous thresholds from being met, geometric continuity is prioritized to ensure that an executable path is always generated. S33 prunes the generated path The pruning window size is adaptively adjusted according to the local curvature distribution. Fine pruning and resampling are performed in high curvature sections, while coarse pruning is maintained in low curvature sections to balance smoothness and algorithm efficiency.

[0034] S35 path local optimization Apply Clothoid interpolation to the pruned path segments, taking advantage of the fact that the curvature changes linearly along the arc length. The expression is as follows: ; Curvature at the starting point and the end point The curvature transition is smooth, the acceleration and its rate of change jerk are limited, and the optimal C² smooth curve of the path with continuous curvature is constructed. The expression is as follows: .

[0035] In a preferred embodiment, the method for calculating the bias probability based on the local obstacle density includes the following steps: S2061 Calculate local obstacle density A predefined radius around each node Count the number of obstacles Let the total number of obstacles in the workspace be , then the local obstacle density Defined as: in, The value range of is from 0 to 1. The larger the value, the denser the obstacles in the area. S2063 adjusts the bias probability based on local obstacle density In order to balance the exploration in complex environments (random sampling) and the utilization in open areas (target priority sampling), the basic target sampling probability is defined as and the local density factor is , the value is usually between 0 and 1, adjusted according to the actual situation), then the bias probability The calculation formula is: When the local density When it is higher, It decreases accordingly, thereby increasing the random sampling ratio, which helps to obtain more complete exploration in areas with dense obstacles; on the contrary, in areas with fewer obstacles, The higher the probability, the faster the expansion to the target. Through the above two-step method, the bias probability can be dynamically adjusted according to the local obstacle density, ensuring a better balance between exploration and utilization in the path planning of mobile robots in strongly constrained environments, thereby improving the efficiency and adaptability of path planning.

[0036] Furthermore, the greedy algorithm operation steps are as follows: S2062 random probability assignment, the expression is as follows: ; Generate compliance interval Uniformly distributed random numbers , and subsequently by and bias probability Compare and decide on the sampling mode to be adopted; S2064 Target Priority Sampling ) executes the following algorithm: ; Close to the target node or starting point The area is sampled to increase the probability of the two trees being connected; at the same time, the bias sampling probability is dynamically adjusted through the formula, where is the local density factor; S2066 Local Adaptive Sampling ) is executed: randomly select a node from the current active tree , and within its radius Generate new samples ; This mode helps improve local connectivity and is suitable for narrow channels or areas with dense obstacles; S2068 global uniform sampling Execute at: Throughout the workspace Uniform sampling ensures global exploration, avoids falling into local optimal solutions, and improves the global exploration ability of the algorithm. in, represents the target sampling bias probability (i.e., the probability of performing target sampling in a mixed sampling strategy); Indicates the basic target sampling probability (the default target sampling probability in an obstacle-free or low-obstacle environment); Local density factor (used to control the impact of obstacle density on target sampling probability, the value range is generally 0 to 1); Indicates the local obstacle density (the density of obstacles within a predefined radius, ranging from 0 to 1, with larger values ​​indicating denser obstacles).

[0037] In a preferred embodiment, the tree expansion in step S208 includes the following steps: After determining the nearest neighbor node, S2082 generates a new node along the sampling direction in combination with the robot's own dynamic constraints, which are the maximum steering angle and linear speed limits, to ensure that the expanded path is feasible in terms of both kinematics and dynamics. S2084 uses an adaptive step size based on obstacle distribution characteristics. In sparse areas, the step size is appropriately increased to improve search efficiency. In areas with dense obstacles or narrow passages, the step size is reduced and steering constraints are combined to ensure feasibility and safety. S2086 incorporates both dynamic constraints and local environment distribution into the expansion process, which can further avoid the risk of path deviation or collision during actual execution.

[0038] Simulation Experiment Example 2 like Figure 7 To verify the effectiveness of the method of the present invention, the present invention uses Python language to conduct a simulation experiment in a two-dimensional plane environment containing obstacles, as follows: Map settings: The simulation area is a 2D grid map with a size of 50×50 units. Two rectangular obstacles are set at (30, 13) and (30, 37), with a width and height of 20 units, forming a strongly restricted narrow channel. The starting position is (0, 0) and the end position is (50, 50). The overall process of implementing the algorithm is as follows Figure 1 As shown in the figure, a path planning method for mobile robots in a strongly constrained environment based on the improved RRT-STAR mainly includes the following steps: Step 1: Initialization Build two random trees, starting from the starting point and the end point respectively (bidirectional search); Calculate the area and distribution density of obstacles to dynamically adjust the target sampling rate and step size; Step 2: Path Planning A dynamic sampling strategy combining goal-oriented sampling, local optimization sampling, and global uniform sampling is used to improve the algorithm's exploration efficiency in complex scenarios. Use the double-tree expansion method to search alternately from the starting tree and the ending tree, and try to connect the two trees to generate a complete path; Optimize path costs and reduce path redundancy through enhanced parent node selection mechanism and reconnection strategy; Step 3: Path Optimization After the initial path is generated, the path is optimized through the pruning algorithm to remove redundant nodes and make the path smoother.

[0039] Depend on Figure 7 The simulation and experimental results show that this method is superior to traditional methods in terms of path length, smoothness, convergence speed and computational efficiency. The existing improved algorithms have been significantly improved.

[0040] While embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions, and variations may be made to these embodiments without departing from the principles and spirit of the invention, and that the scope of the invention is defined by the appended claims and their equivalents.

Claims

1. A mobile robot path planning method in a strongly constrained environment based on an improved RRT-STAR, characterized in that: The steps include: S1. Initialization S101, input path planning parameters; S103, constructing a starting point tree and an end point tree, and establishing a corresponding spatial index structure for each tree based on the multi-dimensional space partitioning principle to accelerate efficient nearest neighbor search of path nodes; S105. Construct an R-tree corresponding to the obstacle list for fast obstacle collision detection; S107. Perform Euclidean distance transform (EDT) on obstacles based on the prior map to construct a "risk field" representing the distance between each point in the free space and the nearest obstacle. S2. Path Planning S202, path planning loop limit check, determine whether the maximum number of iterations has been reached, if not, continue iteration; S204, switching between the starting tree and the end tree, alternately selecting the starting tree or the end tree for path expansion, thereby improving search efficiency and accelerating the speed of finding a feasible path; S206. Calculate the bias probability based on the local obstacle density and select a sampling method to perform sampling node sampling. S208, find the nearest node to the sampling node, generate a new node along the direction of the sampling node, expand the tree, and dynamically adjust the step size according to the local obstacle density; S210, check whether the path between the new node and the nearest node has no collision; if there is no collision, add the new node to the tree and update the spatial index structure of the tree; S212, parent node selection and reconnection, select the nearest neighbor node and its ancestor node as candidate parent nodes, optimize the connection path; update the tree structure to maintain the global optimality; S220, connecting the starting point tree and the ending point tree, checking the connection status of the new node, and generating a complete path; S3. Optimize the path generated in step 2.

2. A mobile robot path planning method in a strongly constrained environment based on an improved RRT-STAR as claimed in claim 1, characterized in that: The path planning parameters of S101 include: starting point, end point, obstacle list, local perception range, original step size, number of iterations, turning radius, speed, maximum number of iterations, and bias probability.

3. A mobile robot path planning method in a strongly constrained environment based on an improved RRT-STAR as claimed in claim 1, characterized in that: The spatial index structure described in step S103 is a KD tree index structure. The KD tree index structure is a binary tree structure that organizes data points by recursively partitioning the space. The structure is constructed by alternately partitioning in each dimension to balance the height of the tree, thereby improving search efficiency. The construction of the KD tree index structure includes the following steps: S1031Start with a data set and select a dimension; S1033 divides the data points into two parts according to the median of the dimension, and the point where the median is located becomes the root node of the KD tree; S1035 For each part, recursively select the next dimension and repeat the above segmentation process until all data points are assigned to leaf nodes.

4. A mobile robot path planning method in a strongly constrained environment based on an improved RRT-STAR as claimed in claim 3, characterized in that: When a new node is inserted, the KD tree index is updated in real time to ensure that the new node can immediately participate in the nearest neighbor search; at the same time, for deleted or adjusted nodes, the index is efficiently updated through local reconstruction to maintain query efficiency and data consistency.

5. The method for mobile robot path planning in a strongly constrained environment based on improved RRT-STAR as claimed in claim 1, characterized in that: The node sampling methods described in step S206 include target-priority sampling, local adaptive sampling, global uniform sampling, and risk field weighted sampling. The target-priority sampling assigns higher sampling weights to sampling points within a predetermined target area based on the current distance to the target, the real-time sampling success rate, and the risk field information, ensuring that the search always converges quickly toward the target area. The local adaptive sampling uses R-tree incremental indexing to collect real-time statistics on obstacle distribution, node density, spatial continuity, and other information for the current expansion area, and dynamically adjusts the sampling probability and expansion step size in combination with the risk field to achieve precise detection and optimization of local complex and narrow sections. The global uniform sampling performs uniform random sampling within the entire feasible space to maintain global coverage and prevent the algorithm from falling into a local optimum. The risk field weighted sampling linearly superimposes the risk field with the original sampling weights during the target-priority and local adaptive sampling stages, prioritizing sampling within the risk range that maintains a safety margin (stays away from obstacles) while fully utilizing the channel space (not too far from obstacles), thereby generating a smoother and safer path corridor.

6. A mobile robot path planning method in a strongly constrained environment based on an improved RRT-STAR as claimed in claim 3, characterized in that: The method for selecting a candidate parent node in S212 includes the following steps: S2122 After the new node is generated, the KD tree index structure is used to quickly retrieve several candidate nodes with the shortest distance, and its ancestors are traversed upward to construct a set of candidate parent nodes. The query depth is limited to 3 to 5 layers of the nearest neighbor nodes. S2124 performs a multi-objective cost evaluation on candidate parent nodes, comprehensively considering path length, collision risk, and dynamic feasibility, and ultimately selects the node with the lowest cost and the most security as the parent node, and updates the tree structure. At the same time, it maintains the KD tree index through an incremental update strategy to ensure that the overall path continues to tend towards the optimal.

7. The method for mobile robot path planning in a strongly constrained environment based on improved RRT-STAR as claimed in claim 1, characterized in that: The path optimization described in S3 includes the following steps: S31 Redundant Node Determination Redundant nodes are identified based on geometric continuity and steering transition smoothness. Variable thresholds for curvature continuity and curvature change rate are introduced as soft constraints to ensure C² continuity. This means that both the path curvature κ(s) and its rate of change along the arc length, dκ / ds, are continuous. This prevents sudden turns and acceleration jumps, ensuring smoothness and executable motion. When local environmental constraints prevent simultaneous thresholds from being met, geometric continuity is prioritized to ensure that an executable path is always generated. S33 prunes the generated path The pruning window size is adaptively adjusted according to the local curvature distribution. Fine pruning and resampling are performed in high curvature sections, while coarse pruning is maintained in low curvature sections to balance smoothness and algorithm efficiency. S35 path local optimization Apply Clothoid interpolation to the pruned path segments, taking advantage of the fact that the curvature changes linearly along the arc length. The expression is as follows: ； Curvature at the starting point and the end point The curvature transition is smooth, the acceleration and its rate of change jerk are limited, and the optimal C² smooth curve of the path with continuous curvature is constructed. The expression is as follows: 。 8. The method for mobile robot path planning in a strongly constrained environment based on improved RRT-STAR as claimed in claim 5, characterized in that: The method for calculating the bias probability based on the local obstacle density comprises the following steps: S2061 Calculate local obstacle density A predefined radius around each node Count the number of obstacles Let the total number of obstacles in the workspace be , then the local obstacle density Defined as: in, The value range of is from 0 to 1. The larger the value, the denser the obstacles in the area. S2063 adjusts the bias probability based on local obstacle density In order to balance the exploration in complex environments (random sampling) and the utilization in open areas (target priority sampling), the basic target sampling probability is defined as and the local density factor is , the value is usually between 0 and 1, adjusted according to the actual situation), then the bias probability The calculation formula is: When the local density When it is higher, It decreases accordingly, thereby increasing the random sampling ratio, which helps to obtain more complete exploration in areas with dense obstacles; on the contrary, in areas with fewer obstacles, The higher the probability, the faster the expansion to the target. Through the above two-step method, the bias probability can be dynamically adjusted according to the local obstacle density, ensuring a better balance between exploration and utilization in the path planning of mobile robots in strongly constrained environments, thereby improving the efficiency and adaptability of path planning.

9. A mobile robot path planning method in a strongly constrained environment based on an improved RRT-STAR as claimed in claim 8, characterized in that: The greedy algorithm operation steps are as follows: S2062 random probability assignment, the expression is as follows: ； Generate compliance interval Uniformly distributed random numbers , and subsequently by and bias probability Compare and decide on the sampling mode to be adopted; S2064 Target Priority Sampling ) executes the following algorithm: ； Close to the target node or starting point The area is sampled to increase the probability of the two trees being connected; at the same time, the bias sampling probability is dynamically adjusted through the formula, where is the local density factor; S2066 Local Adaptive Sampling ) is executed: randomly select a node from the current active tree , and within its radius Generate new samples ; This mode helps improve local connectivity and is suitable for narrow channels or areas with dense obstacles; S2068 global uniform sampling Execute at: Throughout the workspace Uniform sampling ensures global exploration, avoids falling into local optimal solutions, and improves the algorithm's global exploration capabilities; in, represents the target sampling bias probability (i.e., the probability of performing target sampling in a mixed sampling strategy); Indicates the basic target sampling probability (the default target sampling probability in an obstacle-free or low-obstacle environment); Local density factor (used to control the impact of obstacle density on target sampling probability, the value range is generally 0 to 1); Indicates the local obstacle density (the density of obstacles within a predefined radius, ranging from 0 to 1, with larger values ​​indicating denser obstacles).

10. The method for mobile robot path planning in a strongly constrained environment based on improved RRT-STAR as claimed in claim 1, characterized in that: The tree expansion in step S208 includes the following steps: After determining the nearest neighbor node, S2082 generates a new node along the sampling direction in combination with the robot's own dynamic constraints, which are the maximum steering angle and linear speed limits, to ensure that the expanded path is feasible in terms of both kinematics and dynamics. S2084 uses an adaptive step size based on obstacle distribution characteristics. In sparse areas, the step size is appropriately increased to improve search efficiency. In areas with dense obstacles or narrow passages, the step size is reduced and steering constraints are combined to ensure feasibility and safety. S2086 incorporates both dynamic constraints and local environment distribution into the expansion process, which can further avoid the risk of path deviation or collision during actual execution.

Images