A path planning method

Abstract

This invention provides a path planning method belonging to the field of robot path planning, comprising: constructing a non-uniform dynamic partitioning strategy based on the obstacle distribution density within the workspace, prioritizing the selection of key areas that the path may traverse, and reducing the generation of redundant nodes and edges; constructing a weighted sampling strategy based on the comprehensive distance between sampling points and obstacles, increasing the sampling density of obstacle boundaries and narrow passage areas, and reducing invalid sampling in free space; constructing an adaptive variable step-size connection strategy based on the regional obstacle distribution characteristics, dynamically adjusting the node connection step size to enhance the connectivity of the route graph and reduce redundant edges; and performing path querying using a graph search algorithm based on the constructed probabilistic route graph, and performing cubic spline interpolation smoothing on the generated original path to obtain a smooth final path. This method is applicable to path planning in two-dimensional and three-dimensional spaces and for multi-degree-of-freedom robotic arms, and can effectively shorten path length and reduce planning time.

Description

Technical Field

[0001] This invention belongs to the field of robot motion planning technology, specifically relating to a path planning method, which is particularly suitable for efficient collision-free path planning in two-dimensional and three-dimensional workspaces and multi-degree-of-freedom robotic arm joint spaces containing dense obstacle areas and narrow passages. Background Technology

[0002] Path planning is a core technology for autonomous robot operation, aiming to generate a collision-free, low-cost, and executable motion path within a constrained space. Mainstream methods include graph search, artificial potential field methods, and sampling-based algorithms. Among these, probabilistic path graphs (PRMs) are widely used in robotic arms, mobile robots, and other scenarios due to their fast exploration speed and strong adaptability to high-dimensional spaces.

[0003] Traditional PRM has the following drawbacks: random sampling distribution, insufficient sampling points in narrow channels and obstacle boundaries, which easily leads to planning failure; uniform sampling across the entire domain, with excessive redundant sampling and edges in open areas, which greatly increases the time required for graph construction; fixed connection step size, where too small a step size leads to insufficient connectivity, and too large a step size introduces a large number of redundant edges; and a sharp increase in computational burden in high-dimensional spaces (such as a seven-DOF robotic arm), making it difficult to balance efficiency and success rate.

[0004] Existing improved methods such as Lazy-PRM, OBPRM, and Gaussian sampling PRM either omit collision detection during the mapping stage, leading to a decrease in path reliability, or rely on complex geometric calculations that are difficult to generalize, or fail to achieve coordinated optimization of the sampling region and the connection step size. In complex and narrow channel scenarios, they still cannot simultaneously achieve high success rate, short path, and low time consumption.

[0005] To address this, this invention proposes an improved PRM algorithm that integrates dynamic partitioning, weighted adaptive sampling, and adaptive variable step-size connections, systematically improving planning performance from three levels: sampling space, sampling distribution, and node connections. Summary of the Invention

[0006] In view of this, the present invention provides a path planning method that can effectively solve the sampling problem in narrow channels and improve the efficiency of constructing highly connected route maps, including:

[0007] The workspace of the robotic arm is divided into non-uniform sector partitions based on obstacle density, and the region with the fewest partitions from the starting point to the target point is selected as the effective sampling region.

[0008] Within the effective sampling area, the sampling weight is calculated based on the distance between the sampling point and the obstacle;

[0009] The node connection step size is dynamically adjusted according to the sampling weight of the obstacle corresponding to the sampling point. The adjustment of the node connection step size includes: using a small step size connection for obstacle boundary areas in the effective sampling area, and / or using a large step size connection for free open areas.

[0010] Construct a probabilistic roadmap based on a binary tree structure algorithm;

[0011] The initial path of the robotic arm is obtained from the probabilistic path graph using a graph search algorithm;

[0012] After smoothing the initial path, the collision-free optimal path of the robotic arm is obtained.

[0013] In the above method, the step of dividing the workspace of the robotic arm into non-uniform sector partitions based on obstacle density and selecting the region with the fewest partitions from the starting point to the target point as the effective sampling region includes:

[0014] The workspace is divided into several sector-shaped areas with the center of the workspace as the center.

[0015] Based on the obstacle's core information, determine the sector to which the obstacle belongs within the sector area, count the number of obstacles in each sector sector, and calculate the obstacle density in each sector sector. The calculation formula is as follows:

[0016]

[0017] Among them, For the first The number of obstacles within each sector area For the first The area or volume of a sector;

[0018] If the density of obstacles is higher than a preset density threshold, the angle of the sector is decreased; if the density of obstacles is lower than the preset density threshold, the angle of the sector is increased. The formula for calculating the density threshold is:

[0019]

[0020] in, The average density of the obstacles. The standard deviation of density, These are empirical parameters;

[0021] Calculate the number of partitions along the clockwise and counterclockwise directions from the starting point to the target point, and select the area corresponding to the direction with fewer partitions as the effective sampling area.

[0022] In the above method, within the effective sampling area, the sampling weight is calculated based on the distance between the sampling point and the obstacle, including:

[0023] For three-dimensional space, the sampling point is defined as:

[0024]

[0025] obstacles Construct the cube envelope surface; the boundary of the cube envelope surface is:

[0026]

[0027] Based on the sampling points and the three-dimensional envelope, calculate the distance between each sampling point and the obstacle. axis, shaft and The distance component on the axis is calculated using the following formula:

[0028]

[0029] in, , , These are non-linear adjustment coefficients set according to the size of the obstacle;

[0030] Based on the calculated distance components, the combined distance between the sampling point and the obstacle is calculated using the following formula:

[0031]

[0032] in, A small constant used to suppress numerical oscillations;

[0033] Based on the combined distance between the sampling point and the obstacle, the initial weight of a single obstacle is calculated using the following formula:

[0034]

[0035] in, This is the distance offset. This is the parameter for exponential decay adjustment;

[0036] The initial weights of all obstacles are weighted and summed to calculate the total weight. The sampling points are then filtered based on this total weight. The formula for calculating the total weight is as follows:

[0037]

[0038] in, To deal with obstacles Relevant proportional coefficients.

[0039] In the above method, the step of dynamically adjusting the node connection step size according to the sampling weight of the obstacle corresponding to the sampling point includes:

[0040] The node connection step size is calculated based on the sampling weight of the obstacle corresponding to the sampling point. The calculation formula is as follows:

[0041]

[0042] in, and These are the preset minimum and maximum step sizes, respectively.

[0043] The above method involves constructing a probabilistic roadmap based on a binary tree structure algorithm, including:

[0044] The KD-Tree binary tree algorithm is used to establish a spatial index for the sampling points. Based on the spatial index, the search complexity of the neighborhood nodes is reduced from... Reduce to And under the constraint of node connection step size, the collision-free edge is constructed to obtain the probabilistic roadmap.

[0045] In the above method, the graph search algorithm is either Dijkstra's algorithm or the A* heuristic search algorithm;

[0046] The initial path is smoothed by employing a cubic spline interpolation algorithm. Attached Figure Description

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

[0048] Figure 1 This is a flowchart illustrating the path planning method provided in an embodiment of the present invention;

[0049] Figure 2 This is a schematic diagram of a non-uniform sector partition in an embodiment of the present invention;

[0050] Figure 3 This is a comparison chart of uniform random sampling and weighted sampling methods in this embodiment of the invention;

[0051] Figure 4 This is a comparison diagram of the fixed step size and adaptive step size methods in the embodiments of the present invention. Specific Implementation

[0052] To better understand the technical solution of the present invention, the embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0053] It should be understood that the described embodiments are only some embodiments of the present invention, and not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without inventive effort are within the scope of protection of the present invention.

[0054] The improved PRM method described in this invention mainly improves the learning phase of the standard PRM algorithm. When constructing an undirected route graph, the standard PRM can represent the route graph as follows:

[0055]

[0056] in, This represents the set of nodes obtained from sampling. This represents the set of undirected edges between nodes. In the learning phase, the standard PRM first randomly samples the entire space, then attempts to establish edges between nodes according to a fixed connection scale, and removes edges that intersect with obstacles through collision detection; in the query phase, it searches for the optimal path between the starting point and the target point on the generated route map.

[0057] This invention provides a path planning method; please refer to [the relevant documentation]. Figure 1 This is a flowchart illustrating the improved PRM path planning method based on dynamic partitioning and adaptive sampling provided in this embodiment of the invention. The method includes the following steps:

[0058] Step 101 proposes a non-uniform sector partitioning strategy based on obstacle density. By analyzing the distribution density of obstacles in the spatial environment, sampling areas are dynamically divided, and sampling and route maps are constructed only within key sectors. This effectively reduces the connection of redundant edges, thereby shortening the planning time. Please refer to [reference needed]. Figure 2 This is a schematic diagram of a non-uniform sector partition in an embodiment of the present invention.

[0059] Specifically, assume there are multiple obstacles in the workspace. Each obstacle is represented by several boundary planes or boundary surfaces. (Based on the center of the workspace) First, divide the entire workspace into circles, with the circle as the center. An initial sector-shaped region. For a 2D scene, this division is represented as a planar sector; for a 3D scene, it is represented as a spherical sector or a region division under a spherical projection. The initial angle of each sector-shaped region is:

[0060]

[0061] After completing the initial division, count the number of obstacles in each sector area. And calculate the area or volume of the corresponding region. Thus, the area obstacle density is obtained:

[0062]

[0063] To enable the partition range to adapt to changes in environmental complexity, this invention further introduces an obstacle density threshold. This threshold is determined based on the mean and standard deviation of obstacle density in each zone:

[0064]

[0065] in, This represents the average obstacle density of each sector. The standard deviation of obstacle density This is an empirical coefficient, preferably set to 1.0. When the obstacle density in a certain area exceeds a threshold... When the obstacle density is below the threshold, the angle of the sector corresponding to the region is reduced to improve the resolution of subsequent sampling and mapping; when the obstacle density is below the threshold, the angle of the region is appropriately increased to reduce the computational input for simple regions.

[0066] After completing the partition angle redistribution, the starting point is determined respectively. and target point The shortest path typically traverses fewer partitions. Therefore, this invention compares the number of regions traversed from the starting partition to the target partition in clockwise and counterclockwise directions, and selects the side that traverses fewer partitions as the final sampling area. This restricts sampling and mapping to a planning subspace where effective paths are more likely to be formed.

[0067] Step 102: After determining the sampling area through a non-uniform partitioning strategy, the present invention further optimizes the distribution of sampling points and proposes a weighted sampling strategy. Based on the comprehensive distance index, the density of sampling points in narrow channels is increased, while the number of sampling points in open areas of free space is reduced.

[0068] Specifically, for any candidate sampling point Relative to obstacles bounding box The distance components in the three directions are defined as follows:

[0069]

[0070] in, , and The nonlinear adjustment constant used to balance boundary distance sensitivity is preferably set according to the scale of the obstacle in the corresponding direction, for example... , and This can be determined in the same way. Based on the distance components in the three directions mentioned above, the comprehensive distance between the sampling point and the obstacle is constructed:

[0071]

[0072] in, For small bias, the preferred value is... This is used to avoid numerical fluctuations when the distance approaches zero. Based on this, the sampling point is defined relative to the obstacle. Weight function :

[0073]

[0074] in, For distance offset, the preferred option is... ; The value of the exponential decay control parameter is preferably in the range of 1.5 to 3.0. The larger the value, the faster the sampling weight decays as the distance increases.

[0075] When there are multiple obstacles in the workspace, the total weight of the sampling points is obtained by summing the effects of each obstacle:

[0076]

[0077] in, Obstacles The corresponding scaling factor can be dynamically adjusted based on the relative positional relationship between the sampling point and the obstacle. Through the above weight adjustment method, sampling points closer to obstacle boundaries and narrow passages have a higher retention probability, while the retention probability of sampling points in open areas is relatively lower, thus achieving a more reasonable distribution of sample points in complex environments.

[0078] In actual sampling, the number of samples in each sub-region can be allocated according to the regional angle ratio within the selected sector. Then, candidate sample points are randomly generated in each region. Finally, a weighting function is used to filter or resample the candidate points. This strategy can significantly improve the effective sample ratio in key areas.

[0079] Step 103: The present invention adopts an adaptive variable step size connection strategy, which dynamically adjusts the connection step size between the obstacle boundary region and the open area according to the degree of influence of the sampling point on the obstacle.

[0080] Specifically, based on the aforementioned weighting function Define the node connection step size as follows:

[0081]

[0082] in, This indicates the preset minimum step size, preferably a smaller value, used for fine connections near obstacle boundaries; This represents the preset maximum step size, used for fast edge connections in free space. As can be seen from the formula above, when the sampling point is close to the obstacle boundary... When the value is large, the connection step size tends to be small. When the sampling point is in an open area, When the step size is small, the connection step size gradually approaches... .

[0083] To improve the efficiency of neighbor node search, this invention preferably uses a KD-Tree data structure to establish the sample point index. For each sample point, an adaptive step size is used. Using the search radius, the set of neighboring nodes is quickly found in the KD-Tree, and then collision detection is performed on each candidate edge. If the connecting line segment does not intersect with the obstacle, the edge is added to the route graph; if a collision occurs, the edge is discarded. In this way, the connectivity of the graph can be improved while significantly reducing the number of edges and the collision detection overhead.

[0084] Step 104: After completing the route map construction, the present invention connects the starting point and the target point to the route map, and uses Dijkstra's algorithm, A* algorithm, or other heuristic graph search algorithms to query the optimal feasible path from the starting point to the target point. Since the searched path is usually a series of nodes directly connected to form a polyline, it may have problems such as sharp corners and large curvature changes. In order to improve the executability of the path, the present invention preferably uses the cubic spline interpolation method to smooth the discrete path points.

[0085] By fitting spline curves to the original polyline path, the abrupt changes in path curvature can be reduced while maintaining the overall trend of the path and the feasibility of collision-free operation, making the generated results more suitable for execution by mobile robots or robotic arms.

[0086] To verify the effectiveness of the weight adjustment sampling strategy based on the method provided in the embodiments of the present invention, the uniform random sampling used in the standard PRM is compared with the weight adjustment sampling proposed in this invention. Please refer to the experimental results. Figure 3 Experimental results show that, under the condition of the same number of sampling points, the method of the present invention significantly increases the number of sampling points in narrow channels, while relatively reducing invalid samples in open areas, indicating that the method can effectively improve the sampling coverage of key areas.

[0087] Table 1 Comparison of four algorithms in complex obstacle scenarios

[0088]

[0089] To verify the effectiveness of the adaptive variable step size connection strategy, edge connections were made using both fixed and adaptive step sizes in the same environment. Please refer to the experimental results. Figure 4 The results show that when the fixed step size is small, the route graph contains multiple disconnected connected components, making it difficult to form a complete feasible path; when the fixed step size is large, although the graph connectivity can be improved, a large number of locally redundant edges will be generated. The adaptive variable step size strategy of this invention can significantly reduce the number of edges while ensuring the overall connectivity of the graph.

[0090] Furthermore, in complex obstacle scenarios, the methods of this invention were compared using standard PRM, Lazy-PRM, OBPRM, and the method of this invention, respectively. Experimental data are shown in Table 1. Under the same sampling point density, the method of this invention has significantly fewer sampling points and edges than standard PRM, Lazy-PRM, and OBPRM, while also achieving shorter path lengths and lower planning times. For example, with a sampling point density of 0.030, the path length of the method of this invention is 148.751, and the planning time is 1.922s, significantly better than the 192.750 and 2.489s of standard PRM. The experimental data demonstrate that this invention can consistently achieve shorter paths and lower planning times even with fewer sampling points and edges, indicating that its overall performance in complex environments is superior to existing methods.

[0091] Table 2 Experimental results of different PRM methods in a seven-DOF robotic arm scenario

[0092]

[0093] To further verify the applicability of this invention in high-dimensional systems, the method of this invention was applied to a path planning experiment of a seven-DOF Franka Emika robotic arm. Seven static obstacles were set up in the experiment. The robotic arm gripped an object and moved it from the starting point to the target point. The planning methods of standard PRM, Lazy-PRM, OBPRM, and the method of this invention were compared and contrasted.

[0094] As shown in Table 2, the method of this invention achieves the shortest path length and lowest planning time in the robotic arm scenario. Compared with the standard PRM, the path length is reduced from 88.01 cm to 73.23 cm, a reduction of approximately 16.79%; the planning time is reduced from 4.299 s to 3.823 s, a reduction of approximately 11.07%. This demonstrates that the method of this invention can provide more efficient collision-free path planning results for robotic arms in complex obstacle spaces.

[0095] The technical solutions of the embodiments of the present invention have the following beneficial effects:

[0096] This invention proposes an improved PRM path planning method based on dynamic partitioning and adaptive sampling. By introducing a non-uniform partitioning strategy based on obstacle density, a weight-adjusted sampling strategy, and an adaptive variable step-size connection strategy during the standard PRM learning phase, it effectively solves the problems of insufficient sampling in narrow channels, excessive redundant edges in the entire space, and the difficulty in balancing connectivity and efficiency with a fixed step size. Experimental results show that this invention can achieve shorter paths and lower planning time while reducing the number of sampling points and edges in two-dimensional, three-dimensional, and multi-degree-of-freedom robotic arm path planning tasks, demonstrating good applicability, robustness, and promotional value.

[0097] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

[0098] The contents not described in detail in this specification are common knowledge to those skilled in the art.

Claims

1. A path planning method characterized by, The method includes: The workspace of the robotic arm is divided into non-uniform sector partitions based on obstacle density, and the region with the fewest partitions from the starting point to the target point is selected as the effective sampling region. Within the effective sampling area, the sampling weight is calculated based on the distance between the sampling point and the obstacle; The node connection step size is dynamically adjusted according to the sampling weight of the obstacle corresponding to the sampling point. The adjustment of the node connection step size includes: using a small step size connection for obstacle boundary areas in the effective sampling area, and / or using a large step size connection for free open areas. Construct a probabilistic roadmap based on a binary tree structure algorithm; The initial path of the robotic arm is obtained from the probabilistic path graph using a graph search algorithm; After smoothing the initial path, the collision-free optimal path of the robotic arm is obtained.

2. The method of claim 1, wherein, The robotic arm's workspace is divided into non-uniform sector partitions based on obstacle density, and the region with the fewest partitions from the starting point to the target point is selected as the effective sampling region, including: The workspace is divided into several sector-shaped areas with the center of the workspace as the center. Based on the obstacle's core information, determine the sector to which the obstacle belongs within the sector area, count the number of obstacles in each sector sector, and calculate the obstacle density in each sector sector. The calculation formula is as follows: Among them, For the first The number of obstacles within each sector area For the first The area or volume of a sector; If the density of obstacles is higher than a preset density threshold, the angle of the sector is decreased; if the density of obstacles is lower than the preset density threshold, the angle of the sector is increased. The formula for calculating the density threshold is: in, The average density of the obstacles. The standard deviation of density, These are empirical parameters; Calculate the number of partitions along the clockwise and counterclockwise directions from the starting point to the target point, and select the area corresponding to the direction with fewer partitions as the effective sampling area.

3. The method according to claim 1, characterized in that, Within the effective sampling area, the sampling weight is calculated based on the distance between the sampling point and the obstacle, including: For three-dimensional space, the sampling point is defined as: obstacles Construct the cube envelope surface; the boundary of the cube envelope surface is: Based on the sampling points and the three-dimensional envelope, calculate the distance between each sampling point and the obstacle. axis, shaft and The distance component on the axis is calculated using the following formula: in, , , These are non-linear adjustment coefficients set according to the size of the obstacle; Based on the calculated distance components, the combined distance between the sampling point and the obstacle is calculated using the following formula: in, A small constant used to suppress numerical oscillations; Based on the combined distance between the sampling point and the obstacle, the initial weight of a single obstacle is calculated using the following formula: in, This is the distance offset. This is the parameter for exponential decay adjustment; The initial weights of all obstacles are weighted and summed to calculate the total weight. The sampling points are then filtered based on this total weight. The formula for calculating the total weight is as follows: in, To deal with obstacles Relevant proportional coefficients.

4. The method according to claim 3, characterized in that, The step of dynamically adjusting the node connection step size based on the sampling weight of the obstacle corresponding to the sampling point includes: The node connection step size is calculated based on the sampling weight of the obstacle corresponding to the sampling point. The calculation formula is as follows: in, and These are the preset minimum and maximum step sizes, respectively.

5. The method according to claim 1, characterized in that, Construct a probabilistic roadmap based on a binary tree structure algorithm, including: The KD-Tree binary tree algorithm is used to establish a spatial index for the sampling points. Based on the spatial index, the search complexity of the neighborhood nodes is reduced from... Reduce to And under the constraint of node connection step size, the collision-free edge is constructed to obtain the probabilistic roadmap.

6. The method according to claim 1, characterized in that, The graph search algorithm is either Dijkstra's algorithm or the A* heuristic search algorithm; The initial path is smoothed by employing a cubic spline interpolation algorithm.

7. The method according to claim 1, characterized in that, The method is applicable to collision-free path planning in two-dimensional planar space, three-dimensional solid space, or the joint space of a multi-degree-of-freedom robotic arm.

8. The method according to claim 7, characterized in that, The multi-degree-of-freedom robotic arm is a robotic arm with at least seven degrees of freedom.

9. A path planning device, characterized in that, include: The partitioning module is configured to partition the workspace of the robotic arm into non-uniform sector partitions based on obstacle density, and select the region with the fewest partitions from the starting point to the target point as the effective sampling region. The sampling module is configured to calculate sampling weights based on the distance between the sampling point and the obstacle within the effective sampling area; The connection module is configured to dynamically adjust the node connection step size according to the sampling weight of the obstacle corresponding to the sampling point. The adjustment of the node connection step size includes: using a small step size connection for obstacle boundary areas in the effective sampling area, and / or using a large step size connection for free open areas. The path planning module is configured to construct a probabilistic route map based on a binary tree structure algorithm, obtain the initial path of the robotic arm from the probabilistic route map using a graph search algorithm, and obtain the collision-free optimal path of the robotic arm after smoothing the initial path.

Images