Robot path planning method and system with local search path optimization

Abstract

The invention belongs to the technical field of path planning, and provides a robot path planning method and system with a local search path optimization function, and the method comprises the steps: determining the positions of a starting point and a terminal point, and the number and positions of obstacles according to a map environment of robot movement, initializing two random trees with the starting point and the terminal point as root nodes, determining the maximum number of iterations and a termination condition; a root node of a third random tree is generated and initialized; according to the position of the obstacle, iteratively utilizing three random trees to alternately carry out node expansion, and in the expansion process, adopting an adjacent point local search strategy, and when the random trees carry out continuous expansion to a certain node, carrying out local search on a newly generated node to seek a better path; and obtaining the current optimal planning path of the robot until the maximum number of iterations or the termination condition is met. According to the method, the trajectory planning efficiency can be remarkably improved, and the trajectory generation stability is improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of path planning, and in particular relates to a robot path planning method and system with local search path optimization. Background Art

[0002] The statements in this section merely provide background information related to the present invention and do not necessarily constitute prior art.

[0003] Robot motion path planning refers to the arrangement of motion sequences designed for robots to complete set tasks and actions while meeting certain physical constraints. At present, significant progress has been made in robot path planning.

[0004] However, for complex application environments, such as those with numerous obstacles and limited workspaces, how to quickly and efficiently plan a collision-free path from the starting point to the end point has been a research hotspot in academia. Traditional path planning methods can be roughly categorized into three types: graph search-based path planning, optimization-based path planning, and sampling-based path planning.

[0005] The advantage of search-based planning algorithms, such as the A* algorithm and the D* algorithm, is that they can find the optimal solution in a limited state space. However, their disadvantage is that they have high computational complexity and are not applicable to high-dimensional spaces.

[0006] Planning algorithms based on optimization, such as ant colony algorithms and genetic algorithms, have advantages such as strong global search capabilities and can be applied to dynamic environments. In theory, they can be applied to high-dimensional spaces, but they still have disadvantages such as high computational complexity and difficulty in parameter setting.

[0007] The most significant characteristic of sampling-based planning algorithms is their wide applicability, applicable to a variety of environments, including dynamic and static environments and high-dimensional spaces. Existing sampling-based planning algorithms primarily include the Rapid Exploration Random Tree (RRT) algorithm and its derivatives. However, these algorithms still suffer from issues such as high randomness and frequent useless searches. This can increase path costs and reduce algorithm efficiency. They can also lead to path redundancy, making it impossible to obtain the optimal path within a limited timeframe.

[0008] To sum up, for robot motion path planning, which is a specific application scenario with many high-dimensional and complex motion scenes, and may even have a small motion space and many obstacles, and requires the robot body or robotic arm to have a fast response speed, traditional path planning methods are not applicable. Summary of the Invention

[0009] In order to solve the above problems, the present invention proposes a robot path planning method and system with local search path optimization. The present invention improves the RRT-Connect algorithm. On the basis of the two random trees initially generated by the RRT-Connect algorithm, a third tree is added, and the adjacent point search and adaptive step size strategy are combined to significantly improve the efficiency of trajectory planning and improve the stability of the robot motion trajectory generation. The present invention can be used for motion path navigation in two-dimensional scenes such as the movement of the robot body, and can also be used for motion path planning and navigation in three-dimensional scenes such as the execution of the robot's manipulator motion task.

[0010] According to some embodiments, the present invention adopts the following technical solutions: A robot path planning method with local search path optimization includes the following steps: According to the map environment of the robot's movement, determine the starting and ending positions, as well as the number and positions of obstacles, initialize two random trees with the starting and ending points as root nodes, and determine the maximum number of iterations and termination conditions; Generate the root node of the third random tree and initialize it; According to the location of the obstacle, the three random trees are used alternately to iteratively expand the nodes. The third random tree is involved in each expansion process. During the expansion process, the local search strategy of adjacent points is adopted. When the random tree continuously expands to a node, the newly generated nodes are locally searched to find a better path. Until the maximum number of iterations or the termination condition is met, the current optimal planning path of the robot is obtained.

[0011] As an optional implementation, the process of iteratively expanding nodes by alternately using three random trees according to the obstacle position includes: (1) Set global variables and initialize them; (2) According to the value of the global variable, determine the random trees in the dataset V1 and the dataset V2. At least one of the datasets V1 and V2 is the third lesson random tree. And gradually update the value of the global variable to detect whether the datasets V1 and V2 are connected. If the two random trees are already connected, iterate this step. (3) Generate random points in the map environment , find the random point in the data set V1 Nearest Node ,node Towards Expand a certain step in the direction of the node to generate a new node , determine the new node With node Whether the line segment between the two points passes through an obstacle. If no collision occurs, the node Add to data set V1; if a collision occurs, return to step (2); (4) Find the outlier node in dataset V2 Nearest Node , followed by the node Towards The direction of adaptive expansion step size generates new nodes ; Judgment node With node Whether there is a collision between the two points, if there is a collision, return to step (2), if there is no collision, use the adjacent point search strategy to find Is there a node in the dataset V1 nearby? If so, connect directly to the node. If not, connect to the node Continue to expand along the original direction until it reaches or collides with an obstacle; (5) Determine whether the two random trees Tree1 and Tree2 with the starting point and the end point as root nodes are both connected to the third random tree Tree3. If not, return to step (2); if satisfied, the expansion process ends and a collision-free path from the starting point to the end point is generated.

[0012] As a further implementation manner, the initialization value of the global variable is zero, and when the value of the global variable is gradually updated, the value of the global variable is increased by one each time.

[0013] As an optional implementation, the process of determining the random trees in the dataset V1 and the dataset V2 according to the value of the global variable includes: Let the global variable be i, the data sets be V1 and V2, the two random trees with the starting point and the end point as the root nodes be Tree1 and Tree2, and the third random tree be Tree3; When the remainder of i with respect to 4 is 0, , ;when When the remainder of 4 is 1, , ;when When the remainder of 4 is 2, , ;when When the remainder of 4 is 3, , .

[0014] As an optional implementation method, during the expansion process, an adaptive step size adjustment strategy is adopted to divide the expansion area into a first step size area and a second step size area. The values ​​of the two step sizes are different, the first step size is larger than the second step size, and the distance from the first step size area to the obstacle is greater than the set value, and the distance from the second step size area to the obstacle is less than the set value.

[0015] As a further step, the adaptive step size adjustment strategy is as follows: 、 The direction vector of the line segment is denoted as , when the node Using a greedy strategy to node Before continuous expansion, calculate all obstacles to vector The distance between the obstacle and the obstacle is set, and the area near the obstacle is calculated. The coordinates of the intersection point, the intersection point will be vector It is divided into multiple parts, where the area within a pair of intersection points is recorded as the second step area, and the rest is recorded as the first step area. When the newly generated node extends to the second step area, the step size is shortened. The adaptive step size function is as follows: ; in, Indicates the default step size, represents the second step area, Indicates the first step length area.

[0016] As an optional implementation, the process of generating the root node of the third random tree includes: generating the root node of the third random tree using an obstacle density estimation method, calculating the obstacle density value of a random point in a set range of the map, and selecting a node as the root node of the third tree according to the obstacle density value.

[0017] As a further step, the specific process of generating the root node of the third random tree includes: rasterizing the map environment and representing each cell in the raster map as having an obstacle or not having an obstacle; Obstacle density estimation is performed using the Gaussian kernel function. The obstacle density estimation value of each cell data point is calculated. After all cell data points are calculated, the obstacle density estimation value of the entire map environment is obtained. With the midpoint between the starting point and the end point as the center of the circle, set the generation range of the root node, randomly generate a certain number of points within the range, and select the point with the lowest obstacle density value as the root node of the third tree.

[0018] As an optional implementation, the adjacent point local search strategy is specifically: When the node To the node When expanding, each new node is generated When the node Set the search scope to find datasets within the search scope Nodes in , and then calculate the nodes The distance from these nodes is used to find the nearest node. When the distance between two points is less than a pre-set threshold and there is no collision with obstacles, the two points are connected. If such a point cannot be found, the local search strategy for adjacent points is exited and the next node expansion is continued.

[0019] A robot path planning system with local search path optimization, comprising: The initialization module is configured to determine the starting and ending positions, as well as the number and positions of obstacles based on the map environment in which the robot moves, initialize two random trees with the starting and ending points as root nodes, and determine the maximum number of iterations and termination conditions; The third random tree generation module is configured to generate a root node of the third random tree and initialize it; The robot path planning module is configured to iteratively use three random trees to alternately expand nodes based on the location of obstacles. The third random tree is involved in each expansion process, and during the expansion process, a local search strategy of adjacent points is adopted. When the random tree continuously expands to a certain node, a local search is performed on the newly generated node to seek a better path; until the maximum number of iterations or the termination condition is met, the current optimal planned path of the robot is obtained.

[0020] Compared with the prior art, the present invention has the following beneficial effects: This invention improves on the basic RRT-Connect by adding a third random tree using the obstacle density estimation method. This tree explores the starting point and the end point simultaneously, while the other two trees also converge towards the third tree. In addition, adjacent point search and adaptive step size strategies are proposed, which greatly reduce the time cost and path cost of robot path planning and improve its stability. It is suitable for high-dimensional and complex motion scenarios where the robot body or robotic arm moves, and still has good path quality even in scenarios with many obstacles.

[0021] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, preferred embodiments are given below and described in detail with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] The accompanying drawings, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0023] Figure 1 A flowchart of a robot path planning method with local search path optimization according to an embodiment; Figure 2 A flowchart of selecting a third random tree root node according to an embodiment; Figure 3 A two-dimensional environment obstacle density map of an embodiment; Figure 4 A schematic diagram of an adaptive step size strategy according to an embodiment; Figure 5 Schematic diagram of an adjacent point search strategy according to an embodiment, wherein (a) is a schematic diagram of the adjacent point search process, and (b) is a schematic diagram of the generated path; Figure 6 Schematic diagram of the operation results of an embodiment, wherein (a)-(d) are the operation results under four different obstacle environments respectively.

[0024] Figure 7 Schematic diagram of path planning results of a seven-degree-of-freedom robot according to an embodiment, where (a) - (b) respectively show the operation results of the seven-degree-of-freedom robot in two different obstacle environments. DETAILED DESCRIPTION

[0025] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0026] It should be noted that the following detailed descriptions are illustrative and intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the art to which the present invention belongs.

[0027] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, they indicate the presence of features, steps, operations, devices, components and / or combinations thereof.

[0028] In the absence of conflict, the embodiments and features in the embodiments of this application can be combined with each other.

[0029] Example 1 A robot path planning method with local search path optimization is referred to as MSLO-RRT (Multi-Source Local Optimization RRT) for ease of description. Figure 1 As shown, the following steps are included: Step S1: Initialize the robot according to the map environment, including determining the starting and ending positions, initializing the random trees Tree1 and Tree2 with the starting and ending points as root nodes, initializing the obstacle positions, and determining the maximum number of iterations and termination conditions. Specifically, the map environment of this embodiment is two-dimensional, and the size of the map is 50cm×30cm. The starting point and the end point in the environment are set to (2, 2) and (49, 24). The initial step length is Take 0.8cm and the maximum number of iterations is 5000; Step S2: perform path planning; Specifically include: Step S2.1: Generate the root node of the third random tree and initialize the random tree Tree3; Specifically, the obstacle density estimation method is used to generate the root node of the third random tree, such as Figure 2 As shown in Figure 1, by calculating the obstacle density value of a random point in a specific range of the map, a suitable point is selected as the root node of the third tree.

[0030] This step includes: (1) rasterizing the two-dimensional map and representing each cell in the raster map as having an obstacle or not having an obstacle; (2) Select Gaussian kernel function for obstacle density estimation. Kernel function is a function used to generate local probability density around each data point. For each data point , the obstacle density estimation method calculates the weighted sum of the kernel function around it, that is, all other data points The Gaussian kernel function value is multiplied by the weight and then summed over all data points, expressed as the following formula: ; in is the kernel function, For data points The obstacle density estimate of (3) Then calculate the obstacle density estimate of each cell data point. After all the cell data points are calculated, the obstacle density estimate of the entire two-dimensional map is obtained, and the obstacle density map of the environment is as follows: Figure 3 As shown; (4) With the midpoint between the starting point and the end point as the center of the circle, the sampling radius of this embodiment is set to 6 cm. Then, 100 points are randomly generated within this range, and the point with the lowest obstacle density value is selected as the root node of the third tree.

[0031] Step S2.2: Setup It is a global variable, initialized to 0; Step S2.3: When When the remainder of 4 is 0, , ;when When the remainder of 4 is 1, , ;when When the remainder of 4 is 2, , ;when When the remainder of 4 is 3, , ; Step S2.4: Perform the addition operation; Step S2.5: Detection and Are they connected? If the two trees are connected, return to step S2.3; Step S2.6: Generate random points within the map environment ,turn up Random point in the middle Nearest Node ; Step S2.7: Towards The direction expansion step , generating a new node ; Step S2.8: Determination and Whether the line segment between the two points passes through an obstacle. If no collision occurs, the node Join If a collision occurs, return to step S2.3; Step S2.9: Find the Nearest Node ,then Towards The direction of adaptive expansion step size generates new nodes ; Specifically, the adaptive step size strategy is as follows Figure 4 As shown, 、 The direction vector of the line segment between two points is , 、 are the coordinates of two points, when the node Adopting the “greedy strategy” to the node When performing continuous expansion, calculate the distance from all obstacles to the line connecting the two points. If the projection point of the obstacle center is outside the line segment, then this obstacle will not affect the random tree expansion and no subsequent calculation is required. Calculate the distance from the obstacle to the vector The distance is shown in the following formula: ; in, is the center point of each obstacle, n is the number of obstacles, are the distance from the center of each obstacle to the point The direction vector forms a matrix ,matrix is the projection distance of each direction vector on the line segment, They are the coordinates of each projection point located in the line segment, forming a matrix ; is the distance from each coordinate to the line segment, forming a matrix The area near the obstacle, such as Figure 4 The dotted line in the figure indicates that when the newly generated node is extended to the vicinity of the obstacle, the step size needs to be shortened. Figure 4 The line segment AB in the figure is the short step length area. Given R and d1, the coordinates of points A and B can be calculated using the Pythagorean theorem. The rest of the line segment maintains a certain distance from obstacles and belongs to the long step length area. The adaptive step size function is as follows: ; in, Indicates the default step size, represents the short step length region, Indicates the long step length area; Step S2.10: Determination and Whether there is a collision between the two points, if there is a collision, return to step S2.3, if there is no collision, use the adjacent point search strategy to find Is there one nearby? If the node in the , if exists, then it is directly connected to the node, if not, then Continue to expand along the original direction until it reaches or collides with an obstacle; Specifically, the adjacent point search strategy in the present invention is as follows: Towards When expanding, each new node is generated When calculating The nodes in The distance does not need to be calculated here All nodes in The distance, as The increase of nodes in the algorithm will significantly reduce the efficiency of the algorithm. Set the search scope to find Nodes in , and then calculate The distance between the two points is calculated by the node search, and the nearest node is found. When the distance between the two points is less than the preset threshold and there is no collision with obstacles, the two points are connected, so that a shorter path is found. If such a point cannot be found, the adjacent point search is exited and the next node expansion is continued.

[0032] like Figure 5 As shown in (a), the dotted box in the figure is The pre-set search range can be seen. Generate new nodes after expansion , when expanding again, it found , and the node meets the distance requirement, so the two nodes are connected, and the generated path is as follows Figure 5 As shown in (b), it can be seen that this strategy significantly reduces the path cost and improves the efficiency of the algorithm; Step S2.11: Determine whether Tree1 and Tree2 are both connected to Tree3. If not, return to step S2.3; if so, the algorithm ends and generates a collision-free path from the start point to the end point.

[0033] The above method is applied to path planning in robot mobile navigation. The results of the operation in a two-dimensional environment are as follows: Figure 6 As shown, in this embodiment, the size of the four maps (a)-(d) in the figure is 50cm×30cm, and the starting point and the end point in the environment are set to (2, 2) and (49, 24).

[0034] In the MSLO-RRT algorithm, when the obstacle density estimation method is used to select the root node of the third tree, the sampling range is set to 6 cm. In addition, the original step size of the algorithm is 0.8 cm. Figure 6 The gray graphics in the figure are obstacles in the environment, the green lines represent the random tree with the starting point as the root node, the blue lines represent the random tree with the end point as the root node, the yellow lines are the additional random trees, and the red lines are the obstacle avoidance paths generated by the present invention. Figure 6 (a) and Figure 6 (c) The obstacles are sparse, Figure 6 (b) and Figure 6 The obstacles in (d) are dense. The results show that the paths generated by the present invention in different two-dimensional environments are of high quality and have good robustness.

[0035] When the above method is used for the mobile path planning and navigation of the robot's manipulator, the operation results in a three-dimensional environment are as follows: Figure 7 As shown, in this embodiment, the robot model adopts a seven-degree-of-freedom collaborative robot arm. Figure 7 (a) is an environment with a single obstacle, Figure 7 (b) is an environment with multiple obstacles.

[0036] In the MSLO-RRT algorithm, when the obstacle density estimation method is used to select the root node of the third tree, the sampling range is set to 30 cm. In addition, the original step size of the algorithm is 0.02 rad. Figure 7 The blue graphics in the figure are obstacles in the environment, the red lines are obstacle avoidance paths generated by the present invention, and the green dots are nodes generated during the path planning process. The results show that the paths generated by the present invention in different three-dimensional environments have good quality and fewer invalid nodes.

[0037] It can be seen that the MSLO-RRT algorithm proposed in the present invention has a faster average running time, fewer average nodes, and a shorter average path length than the original RRT-Connect algorithm. It is also suitable for high-dimensional motion scenarios and complex motion scenarios where the robot body or robotic arm moves. Even in scenarios with many obstacles, it still has good path quality.

[0038] The specific values ​​of the parameters in the above embodiments may be adjusted or changed in other embodiments.

[0039] Example 2 A robot path planning system with local search path optimization, comprising: The initialization module is configured to determine the starting and ending positions, as well as the number and positions of obstacles based on the map environment in which the robot moves, initialize two random trees with the starting and ending points as root nodes, and determine the maximum number of iterations and termination conditions; The third random tree generation module is configured to generate a root node of the third random tree and initialize it; The robot path planning module is configured to iteratively use three random trees to alternately expand nodes based on the location of obstacles. The third random tree is involved in each expansion process, and during the expansion process, a local search strategy of adjacent points is adopted. When the random tree continuously expands to a certain node, a local search is performed on the newly generated node to seek a better path; until the maximum number of iterations or the termination condition is met, the current optimal planned path of the robot is obtained.

[0040] It will be understood by those skilled in the art that embodiments of the present invention may be provided as methods, systems, or computer program products. Thus, the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention may take the form of one or more computer-usable storage media (including but not limited to disk storage, CD - ROM , optical storage, etc.).

[0041] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowcharts and / or block diagrams, as well as combinations of processes and / or blocks in the flowcharts and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowcharts and / or block diagrams. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0042] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0043] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0044] The foregoing description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Those skilled in the art will readily appreciate that various modifications and variations of the present invention are possible. Any modifications, equivalent substitutions, or improvements made by those skilled in the art that fall within the spirit and principles of the present invention and do not require creative effort are intended to be within the scope of protection of the present invention.

Claims

1. A robot path planning method with local search path optimization, characterized in that: The following steps are involved: According to the map environment of the robot's movement, determine the starting and ending positions, as well as the number and positions of obstacles, initialize two random trees with the starting and ending points as root nodes, and determine the maximum number of iterations and termination conditions; Generate the root node of the third random tree and initialize it; According to the location of obstacles, three random trees are used alternately to expand nodes iteratively. The third random tree is involved in each expansion process. In the expansion process, the local search strategy of adjacent points is adopted. When the random tree continuously expands to a node, the newly generated node is searched locally to seek a better path. Until the maximum number of iterations or the termination condition is met, the current optimal planning path of the robot is obtained.

2. A robot path planning method with local search path optimization as claimed in claim 1, characterized in that: According to the obstacle position, the process of iteratively using three random trees to alternately expand the nodes includes: (1) Set global variables and initialize them; (2) According to the value of the global variable, determine the random trees in the data set V1 and the data set V2. At least one of the data sets V1 and V2 is the random tree of the third lesson. And gradually update the value of the global variable to detect whether the data sets V1 and V2 are connected. If the two random trees are already connected, iterate this step; (3) Generate random points in the map environment , find the random point in the data set V1 The nearest node ,node Towards Expand a certain step in the direction to generate new nodes , determine the new node With Node Whether the line segment between the two points passes through an obstacle. If no collision occurs, the node Add to data set V1; if a collision occurs, return to step (2); (4) Find the outlier nodes in data set V2 The nearest node , followed by the node Towards The direction of adaptive expansion step size generates new nodes ; Judgement node With Node Whether there is a collision between the two points. If there is a collision, return to step (2). If there is no collision, use the adjacent point search strategy to find Is there a node in the dataset V1 nearby? If so, connect directly to the node. If not, connect to the node Continue to expand along the original direction until it reaches or collide with an obstacle; (5) Determine whether the two random trees Tree1 and Tree2 with the starting point and the end point as root nodes are both connected to the third random tree Tree3. If not, return to step (2); if satisfied, the expansion process ends and a collision-free path from the starting point to the end point is generated.

3. A robot path planning method with local search path optimization as claimed in claim 2, characterized in that: The initialization value of the global variable is zero, and when the value of the global variable is gradually updated, the value of the global variable is increased by one each time.

4. A robot path planning method with local search path optimization as claimed in claim 2, characterized in that: According to the value of the global variable, the process of determining the random trees in the data set V1 and the data set V2 includes: Let the global variable be i, the data sets be V1 and V2, the two random trees with the starting point and the end point as the root nodes be Tree1 and Tree2, and the third random tree be Tree3; When the remainder of i with respect to 4 is 0, , ;when When the remainder of 4 is 1, , ;when When the remainder of 4 is 2, , ;when When the remainder of 4 is 3, , .

5. A robot path planning method with local search path optimization as claimed in claim 1 or 2, characterized in that: During the expansion process, an adaptive step size adjustment strategy is adopted to divide the expansion area into a first step size area and a second step size area. The values ​​of the two step sizes are different. The first step size is larger than the second step size, and the distance from the first step size area to the obstacle is greater than the set value, while the distance from the second step size area to the obstacle is less than the set value.

6. A robot path planning method with local search path optimization as claimed in claim 2, characterized in that: During the expansion process, an adaptive step size adjustment strategy is adopted. The adaptive step size adjustment strategy is as follows: , The direction vector of the line segment is denoted by , when the node Using a greedy strategy to node Before continuous expansion, calculate all obstacles to vector The distance between the obstacle and the vector is calculated. The coordinates of the intersection point, the intersection point will be the vector Divide into multiple parts, the area within a pair of intersection points is recorded as the second step area, and the rest is recorded as the first step area. When the newly generated node extends to the second step area, the step length is shortened. The adaptive step length function is as follows: ； in, represents the default step size, represents the second step length area, Indicates the first step length area.

7. A robot path planning method with local search path optimization as claimed in claim 1, characterized in that: The process of generating the root node of the third random tree includes: generating the root node of the third random tree by using the obstacle density estimation method, calculating the obstacle density value of the random point in the set range of the map, and selecting a node as the root node of the third tree according to the obstacle density value.

8. A robot path planning method with local search path optimization as claimed in claim 7, characterized in that: The specific process of generating the root node of the third random tree includes: rasterizing the map environment and representing each cell in the raster map as having an obstacle or not having an obstacle; Obstacle density estimation is performed using the Gaussian kernel function to calculate the obstacle density estimation value of each cell data point. After all cell data points are calculated, the obstacle density estimation value of the entire map environment is obtained. With the midpoint between the starting point and the end point as the center of the circle, set the generation range of the root node, randomly generate a certain number of points within the range, select the point with the lowest obstacle density value, and use it as the root node of the third tree.

9. A robot path planning method with local search path optimization as claimed in claim 2, characterized in that: The adjacent point local search strategy is specifically: When the node To Node When expanding, each new node is generated When the node Set the search scope to find the datasets within the search scope , and then calculate the nodes The distances to these nodes are used to find the nearest node. When the distance between two points is less than a preset threshold and there is no collision with obstacles, the two points are connected. If such a point cannot be found, the local search strategy for adjacent points is exited and the next node expansion is continued.

10. A robot path planning system with local search path optimization, characterized in that it includes: The initialization module is configured to determine the starting point and the end point, as well as the number and positions of obstacles according to the map environment of the robot movement, initialize two random trees with the starting point and the end point as the root nodes, and determine the maximum number of iterations and the termination condition; A third random tree generation module is configured to generate a root node of the third random tree and initialize it; The robot path planning module is configured to iteratively use three random trees to alternately expand nodes according to the location of obstacles. The third random tree is involved in each expansion process, and the adjacent point local search strategy is adopted during the expansion process. When the random tree continuously expands to a certain node, the newly generated node is locally searched to seek a better path; until the maximum number of iterations or the termination condition is met, the current optimal planning path of the robot is obtained.

Images