A robot path planning method and system with local search path optimization

Abstract

The present invention belongs to the field of path planning technology, and provides a robot path planning method and system with local search path optimization. According to the map environment of the robot movement, the starting point and the end point are determined, as well as the number and position of obstacles, two random trees with the starting point and the end point as the root nodes are initialized, and the maximum number of iterations and the termination condition are determined; the root node of the third random tree is generated and initialized; according to the obstacle position, the three random trees are iteratively used to alternately expand the nodes, and in the expansion process, the adjacent point local search strategy is adopted. When the random tree is continuously expanded 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. The present invention can significantly improve the efficiency of trajectory planning and improve the stability of trajectory generation.

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 satisfying certain physical constraints. At present, significant progress has been made in robot path planning.

[0004] However, for some complex application environments, such as those with many obstacles and small working spaces, how to quickly and efficiently plan a collision-free path from the starting point to the end point has always been a research hotspot in the academic community. Since the development of this field, traditional path planning methods can be roughly divided into three categories: path planning based on graph search, path planning based on optimization, and path planning based on sampling.

[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 the advantages of 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 biggest feature of sampling-based planning algorithms is their wide applicability, which can be applied to various environments, including dynamic, static environments, and high-dimensional spaces. Existing sampling-based planning algorithms mainly include the Rapid Exploration Random Tree (RRT) algorithm and its various derivative algorithms, but they still have problems such as strong randomness and many useless searches, which to a certain extent will increase the path cost and reduce the algorithm's operating efficiency. There may also be path redundancy, and the optimal path cannot be obtained within a limited time.

[0008] In summary, 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's 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 initially generating two random trees by the RRT-Connect algorithm, a third tree is added, and the adjacent point search and adaptive step size strategy are combined. The efficiency of trajectory planning can be significantly improved, and the stability of the robot motion trajectory generation is improved. The present invention can be used for motion path navigation in two-dimensional scenes such as robot body movement, and can also be used for motion path planning and navigation in three-dimensional scenes such as the robot's mechanical arm motion task execution.

[0010] According to some embodiments, the present invention adopts the following technical solutions:

[0011] A robot path planning method with local search path optimization comprises the following steps:

[0012] 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;

[0013] Generate the root node of the third random tree and initialize it;

[0014] 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.

[0015] Until the maximum number of iterations or the termination condition is met, the current optimal planning path of the robot is obtained.

[0016] As an optional implementation, according to the obstacle position, the process of iteratively using three random trees to alternately expand the nodes includes:

[0017] (1) Set global variables and initialize them;

[0018] (2) Determine the data set V based on the value of the global variable 1 and dataset V 2 Random trees in the dataset V 1 and dataset V 2 At least one of the datasets is a random tree from Lesson 3, and the value of the global variable is updated gradually to detect the dataset V. 1 and V 2 Whether they are connected. If the two random trees are already connected, this step is executed iteratively;

[0019] (3) Generate random points in the map environment , find the dataset V 1 Random point 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 dataset V 1 If a collision occurs, return to step (2).

[0020] (4) In the dataset V 2 Find the outliers 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 dataset V nearby? 1 If the node in the , if it exists, it is directly connected to the node, if it does not exist, it is connected to the node Continue to expand along the original direction until it reaches or collide with an obstacle;

[0021] (5) Determine the two random trees with the starting point and the end point as the root nodes 1 、Tree 2 Are they all related to the third random tree? 3 If not, return to step (2); if satisfied, the expansion process ends and a collision-free path from the start point to the end point is generated.

[0022] 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.

[0023] As an optional implementation, the data set V is determined according to the value of the global variable. 1 and dataset V 2 The process within the random tree consists of:

[0024] Let the global variable be i and the data set be V 1 and V 2 , the two random trees with the starting point and the end point as the root nodes are Tree 1、Tree 2 , the third random tree is Tree 3 ;

[0025] 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, , .

[0026] 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 a set value, and the distance from the second step size area to the obstacle is less than a set value.

[0027] As a further step, 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:

[0028] ;

[0029] in, represents the default step size, represents the second step length area, Indicates the first step length area.

[0030] 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 by 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.

[0031] 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 having no obstacle;

[0032] 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.

[0033] 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.

[0034] As an optional implementation, the adjacent point local search strategy is specifically:

[0035] 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.

[0036] A robot path planning system with local search path optimization, comprising:

[0037] 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;

[0038] A third random tree generation module is configured to generate a root node of the third random tree and initialize it;

[0039] 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.

[0040] Compared with the prior art, the present invention has the following beneficial effects:

[0041] The present invention improves on the basic RRT-Connect by adding a third random tree using the obstacle density estimation method. The tree explores the starting point and the end point at the same time, and the other two trees also approach 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 motion scenes and complex motion scenes where the robot's body or mechanical arm moves, and still has good path quality even in scenes with many obstacles.

[0042] 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

[0043] The accompanying drawings in the specification, 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.

[0044] Figure 1 A flowchart of a robot path planning method with local search path optimization according to an embodiment;

[0045] Figure 2 A flowchart of selecting the third random tree root node according to an embodiment;

[0046] Figure 3 A two-dimensional environment obstacle density map of an embodiment;

[0047] Figure 4 A schematic diagram of an adaptive step size strategy according to an embodiment;

[0048] Figure 5 Schematic diagram of an adjacent point search strategy of an embodiment, wherein (a) is a schematic diagram of an adjacent point search process, and (b) is a schematic diagram of a generated path;

[0049] Figure 6 Schematic diagram of the operation results of an embodiment, wherein (a)-(d) are the operation results under four different obstacle environments respectively.

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

[0051] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

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

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

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

[0055] Embodiment 1

[0056] 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:

[0057] Step S1: Initialize the robot according to the map environment, including determining the starting and ending positions, and initializing a random tree with the starting and ending points as root nodes. 1 With Tree 2 , initialize the obstacle position, determine the maximum number of iterations and termination conditions;

[0058] 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;

[0059] Step S2, performing path planning;

[0060] Specifically include:

[0061] Step S2.1: Generate the root node of the third random tree and initialize the random tree Tree 3 ;

[0062] Specifically, the obstacle density estimation method is used to generate the root node of the third random tree, such as Figure 2 As shown, 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.

[0063] 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;

[0064] (2) Select the Gaussian kernel function for obstacle density estimation. The 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:

[0065] ;

[0066] in is the kernel function, For data points The obstacle density estimate of

[0067] (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;

[0068] (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 the range, and the point with the lowest obstacle density value is selected as the root node of the third tree.

[0069] Step S2.2: Setup It is a global variable, initialized to 0;

[0070] 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, , ;

[0071] Step S2.4: Perform an add 1 operation;

[0072] Step S2.5: Detection and Are they connected? If the two trees are connected, return to step S2.3;

[0073] Step S2.6: Generate random points within the map environment ,turn up Random point The nearest node ;

[0074] Step S2.7: Towards The direction expansion step length , generating a new node ;

[0075] 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;

[0076] Step S2.9: Find the The nearest node ,then Towards The direction of adaptive expansion step size generates new nodes ;

[0077] Specifically, the adaptive step size strategy is as follows: Figure 4 As shown, , The direction vector of a line segment between two points is , , are the coordinates of the 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, 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:

[0078] ;

[0079] 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 in the line segment, forming a matrix ; is the distance from each coordinate to the line segment, forming a matrix . The vicinity of obstacles, such as Figure 4 The dotted line in , when the newly generated node expands to the vicinity of the obstacle, the step length needs to be shortened. Figure 4 The line segment AB in is the short step length area, where R and d1 are known, and the coordinates of points A and B can be obtained using the Pythagorean theorem; the other parts of the line segment keep a certain distance from the obstacle and belong to the long step length area;

[0080] The adaptive step size function is as follows:

[0081] ;

[0082] in, represents the default step size, represents the short step length region, Indicates the long step length area;

[0083] 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 any nearby If the node in the , if it exists, it is directly connected to the node, if it does not exist, it is connected to the node Continue to expand along the original direction until it reaches or collide with an obstacle;

[0084] Specifically, the adjacent point search strategy in the present invention is as follows: Towards When expanding, each new node is generated When 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 , and then calculate The distance between the two points is smaller than the preset threshold and there is no collision with obstacles, so a shorter path is found. If no such point is found, the search for adjacent points is exited and the next node expansion is continued.

[0085] like Figure 5As 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;

[0086] Step S2.11: Determine Tree 1 、Tree 2 Are they all related to Tree 3 If not, return to step S2.3; if satisfied, the algorithm ends and generates a collision-free path from the start point to the end point.

[0087] The above method is applied to path planning in robot mobile navigation. The operation results 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).

[0088] 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 figures 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 have high quality and good robustness.

[0089] 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.

[0090] 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 figures 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 in the path planning process. The results show that the paths generated by the present invention in different three-dimensional environments have better quality and fewer invalid nodes.

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

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

[0093] Embodiment 2

[0094] A robot path planning system with local search path optimization, comprising:

[0095] 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;

[0096] A third random tree generation module is configured to generate a root node of the third random tree and initialize it;

[0097] 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.

[0098] It should be understood by those skilled in the art that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of an all-hardware embodiment, an all-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.).

[0099] 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 flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, 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 flowchart and / or block diagram. 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.

[0100] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate 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 A function specified in one or more boxes.

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

[0102] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made by those skilled in the art within the spirit and principle of the present invention without creative labor shall be included in the protection scope 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 to expand nodes alternately. In each expansion process, the third random tree is involved. 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; The process of generating the root node of the third random tree includes: generating the root node of the third random tree by 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; 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; 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. Generate random points within 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 ; When the data set Generate new nodes , find the outlier nodes in dataset V2 The nearest node , followed by the node Towards The direction of adaptive expansion step size generates new nodes .

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 third random tree. 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 1, 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.

7. 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. In each expansion process, the third random tree is involved, and in the expansion process, the adjacent point local search strategy is adopted. When the random tree continuously expands to a 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; The process of generating the root node of the third random tree includes: generating the root node of the third random tree by 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; 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; 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. Generate random points within 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 ; When the data set Generate new nodes , find the outlier nodes in dataset V2 The nearest node , followed by the node Towards The direction of adaptive expansion step size generates new nodes .

Images