Robot path planning method and system

Abstract

The invention provides a robot path planning method and system based on an improved RRT * algorithm, and relates to the field of path optimization, and the method comprises the steps: initializing a free space, a starting point and a target point of a two-dimensional mobile robot; improving an RRT * algorithm through target constraint sampling, target bias expansion and path smoothing processing; and performing path planning on the two-dimensional robot through an improved RRT * algorithm to obtain an optimal path, and performing visual display. According to the technical scheme, the node utilization rate is improved, the number of iterations is reduced, the operation time is shortened, the planning efficiency is improved, and the finally obtained path is shorter, smoother and better in effect.

Description

Technical Field

[0001] The present application relates to the field of path optimization, and in particular to a robot path planning method and system. Background Art

[0002] RRT and PRM are sampling-based path planning algorithms. Since they do not require accurate modeling of the obstacle environment and have probabilistic completeness, they are widely used in robot path planning. Among them, the asymptotically optimal rapidly expanding random tree algorithm (RRT*) is an optimal path planning algorithm improved on the RRT algorithm. It has both probabilistic completeness and asymptotic optimality, so it is often used in optimal path planning problems.

[0003] Although the RRT* algorithm has probabilistic completeness and asymptotic optimality, it still has the following defects in some special environments: 1) The sampling points have poor target orientation. Since RRT* uses uniform sampling, the sampling points are evenly distributed in the entire free space, which will lead to a large number of invalid samplings that deviate from the target; 2) New node expansion is blind. The expansion direction of new nodes in the RRT* algorithm is completely determined by the sampling point. If the sampling point deviates too much from the target point, the expanded new node will also deviate from the path optimization area, causing the robot to spend a lot of time to expand the search; 3) Lack of effective path post-processing, the path is not feasible. The original path planned by the basic RRT* algorithm has large curvature at corners, which makes the path not smooth enough and does not meet the requirements of robot motion stability. Summary of the invention

[0004] The purpose of the present invention is to provide a robot path planning method and system in order to solve the problems of poor target orientation of sampling points, blind expansion of new nodes and lack of effective path post-processing in the existing generation path planning method based on RRT* algorithm.

[0005] The above-mentioned purpose of the present application is achieved through the following technical solutions: S1: Initialize the free space, starting point and target point of the two-dimensional mobile robot; S2: Improve the RRT* algorithm through target constraint sampling, target bias extension and path smoothing; S3: The improved RRT* algorithm is used to plan the path of the two-dimensional robot, obtain the optimal path and visualize it.

[0006] Optionally, step S2 includes: The specific steps to improve the RRT* algorithm through target constrained sampling include: Sampling is performed through the target bias strategy to obtain the sampling points as follows: Set a target bias probability , generates a probability value according to uniform probability ; The target bias sampling formula is:

[0007] in, represents the random sampling function, represents the target point, represents the sampling point, Indicates that a sampling point is generated in free space; If the probability value Less than target bias probability , then the target point As sampling point ; If the probability value Greater than or equal to target bias probability , then a sampling point is generated in the free space; The position constraints are imposed on the sampling points generated in the free space, specifically: Set the starting point to the first sampling point; Set the judgment condition: If the distance between the sampling point and the target point in the X direction or Y direction is less than the distance between the previous sampling point and the target point in the X direction or Y direction, the judgment condition is met; If the judgment condition of the sampling point generated in the free space is met, the sampling is considered successful. If the judgment condition of the sampling point generated in the free space is not met, the target bias strategy is continued to perform cyclic sampling until the judgment condition is met.

[0008] Optionally, step S2 further includes: The specific improvements to the RRT* algorithm through target bias extension include: New Node The generation formula of the bias target is as follows:

[0009] in, The distance sampling point The nearest neighbor node, To expand the step size, Target point The direction weight of is the unit vector in the direction of the target point, is the unit vector of the sampling point direction, and the expressions are: , .

[0010] Optionally, step S2 further includes: The specific process of using cubic B-spline curve to smooth the path generated by RRT* algorithm includes: Features Control Vertices , , the cubic B-spline curve is expressed as:

[0011] In the formula, is a cubic canonical B-spline basis function, whose highest degree is 3; is the knot vector of the spline function.

[0012] Optionally, step S2 further includes: the B-spline basis function is obtained by a Cox-deBoor recursive formula.

[0013] A robot path planning system, the system comprising: a data acquisition module, a data processing module, and a display module; The data acquisition module is used to obtain the free space, starting point and target point of the two-dimensional mobile robot; The data processing module is used to initialize the free space, starting point and target point of the two-dimensional mobile robot; The data processing module is also used to improve the RRT* algorithm through target constraint sampling, target bias extension and path smoothing; The data processing module is also used to plan the path of the two-dimensional robot through the improved RRT* algorithm to obtain the optimal path; The display module is used to visualize the optimal path.

[0014] An electronic device includes a processor, a memory, a user interface and a network interface, wherein the memory is used to store instructions, the user interface and the network interface are used to communicate with other devices, and the processor is used to execute the instructions stored in the memory so that the electronic device executes a robot path planning method.

[0015] A computer-readable storage medium stores instructions, and when the instructions are executed, a robot path planning method is executed.

[0016] The beneficial effects of the technical solution provided by this application are: The technical solution of the present application improves the sampling method, new node expansion and path post-processing in three aspects, solving the problems of poor target orientation of sampling points in the existing RRT* algorithm, blind expansion of new nodes, and large curvature of the original path that does not meet the requirements of robot motion stability. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] The present application will be further described below with reference to the accompanying drawings and embodiments, in which: Figure 1 is a flow chart in an embodiment of the present application; Figure 2 is a schematic diagram of sampling constraints in an embodiment of the present application; Figure 3 is a schematic diagram of target bias expansion in an embodiment of the present application; Figure 4 is a comparison diagram of the path before and after smoothing in the embodiment of the present application; Figure 5 It is the planning result diagram of environment 1 in the embodiment of the present application; Figure 6 It is the planning result diagram of environment 2 in the embodiment of the present application; Figure 7 It is the planning result diagram of environment 3 in the embodiment of the present application; Figure 8 It is the planning result diagram of environment 4 in the embodiment of the present application; Fig. 9 It is a schematic diagram of the structure of an electronic device in an embodiment of the present application. DETAILED DESCRIPTION

[0018] In order to have a clearer understanding of the technical features, purposes and effects of the present application, the specific implementation methods of the present application are now described in detail with reference to the accompanying drawings.

[0019] An embodiment of the present application provides a robot path planning method.

[0020] Please refer to Figure 1 , Figure 1 : is a flow chart of a robot path planning method in an embodiment of the present application, comprising: S1: Initialize the free space, starting point and target point of the two-dimensional mobile robot; S2: Improve the RRT* algorithm through target constraint sampling, target bias extension and path smoothing; S3: The improved RRT* algorithm is used to plan the path of the two-dimensional robot, obtain the optimal path and visualize it.

[0021] The present application provides an embodiment as follows: RRT* (Rapidly-exploring Random Trees Star) algorithm is an algorithm for efficient path planning, which is particularly suitable for complex or constrained environments. As an improved version of RRT (Rapidly-exploring Random Trees), RRT not only inherits the efficient exploration properties of RRT, but also introduces the function of optimizing the path, so that the final path cost tends to be optimal.

[0022] Step S2 includes: The specific steps to improve the RRT* algorithm through target constrained sampling include: Sampling is performed through the target bias strategy to obtain the sampling points as follows: Set a target bias probability , generates a probability value according to uniform probability ; The target bias sampling formula is:

[0023] in, represents the random sampling function, represents the target point, represents the sampling point, Indicates that a sampling point is generated in free space; If the probability value Less than target bias probability , then the target point As sampling point ; If the probability value Greater than or equal to target bias probability , then a sampling point is generated in the free space; The position constraints are imposed on the sampling points generated in the free space, specifically: Set the starting point to the first sampling point; Set the judgment condition: If the distance between the sampling point and the target point in the X direction or Y direction is less than the distance between the previous sampling point and the target point in the X direction or Y direction, the judgment condition is met; If the judgment condition of the sampling point generated in the free space is met, the sampling is considered successful. If the judgment condition of the sampling point generated in the free space is not met, the target bias strategy is continued to perform cyclic sampling until the judgment condition is met.

[0024] The present application provides an embodiment as follows. The constraint idea is to randomly generate a sampling point in the free space, that is, to judge its position. The judgment condition is that the sampling point is closer to the target point than the previous sampling point in the X direction or the Y direction. If the condition is met, the sampling is considered successful; otherwise, the sampling is continued in a loop until the condition is met.

[0025] The present application provides an embodiment as follows, a sampling constraint diagram, such as Figure 2 As shown. This application adopts the above technical solution. The existing sampling methods of the RRT* improved algorithm are mainly: uniform sampling, target bias sampling, intelligent sampling and elliptical subset sampling, etc. Although they can overcome the blindness of random sampling, the target orientation is still not strong enough. Therefore, this application intends to combine the target bias strategy and position constraint to solve this sampling problem.

[0026] Step S2 also includes: The specific improvements to the RRT* algorithm through target bias extension include: New Node The generation formula of the bias target is as follows:

[0027] in, The distance sampling point The nearest neighbor node, To expand the step size, Target point The direction weight of is the unit vector in the direction of the target point, is the unit vector of the sampling point direction, and the expressions are: , .

[0028] The present application provides an embodiment in which the target bias extension corresponds to function, compared to the RRT* algorithm function, The only change is to New node generation function in becomes .

[0029] The present application provides an embodiment as follows: in the new node expansion of the existing algorithm, the expansion direction is simply determined by the sampling point, so the expanded nodes are likely to deviate from the path optimization area, resulting in an increase in search time. In order to overcome the blindness of the new node expansion, the present application draws on the idea of ​​the gravitational field in the artificial potential field, and assigns different weights to the sampling point direction and the target point direction, so that the expansion direction of the new point is no longer simply determined by the sampling point, but is jointly determined by the sampling point and the target point. In this way, each expansion can be closer to the target point, thereby speeding up the search. Its principle diagram, as shown in the figure Figure 3 shown.

[0030] Step S2 also includes: The specific process of using cubic B-spline curve to smooth the path generated by RRT* algorithm includes: Features Control Vertices , , the cubic B-spline curve is expressed as:

[0031] In the formula, is a cubic canonical B-spline basis function, whose highest degree is 3; is the knot vector of the spline function.

[0032] Step S2 also includes: the B-spline basis function is obtained by the Cox-deBoor recursive formula.

[0033] The present application provides an embodiment in which a path smoothing function is written in MATLAB , embedded into the RRT* algorithm and the improved algorithm. Taking the improved RRT* algorithm as an example, the optimal path is planned in a simple environment with obstacles and smoothed with a third-order B-spline. Figure 4 shown. Figure 4 In the figure, S is the starting point of the path, G is the target point of the path, the green line is the final expansion tree, the blue line is the initial path before smoothing, and the red line is the final path after smoothing. It can be seen that after the original path is smoothed three times with B-spline, it becomes smoother, the corners are eliminated, and the overall shape trend remains almost unchanged.

[0034] In summary, by improving RRT* from the above three aspects, the target orientation of the sampling points can be improved, thereby reducing memory usage, speeding up the expansion search speed, and further improving the path quality. The improved RRT* algorithm pseudocode mainly includes four parts: target constraint sampling function, target bias expansion function, path connection function and path smoothing function.

[0035] First, create an initial random tree and give the target bias probability , target point direction weight and target point , and at the same time as the first reference sampling point; then call the target constraint sampling function Generate constrained sampling points; then call the target bias expansion function Expand the new node and update the reference sampling point; after the loop expansion, return the complete random tree and call the path connection function Generate the shortest path; finally use the path smoothing function The shortest path is smoothed to obtain the final path output. The detailed flow chart of the improved algorithm GCSE-RRT* is as follows: Figure 1 shown.

[0036] The present application provides an embodiment as follows. Existing path post-processing technologies mainly include path pruning and path smoothing. Although path pruning can optimize the path quality to a certain extent, it is still not smooth and continuous enough. Therefore, many smoothing technologies are developed for path smoothing. B-spline curves have geometric invariance and local support, and can modify the path locally without affecting the basic shape characteristics of the entire path. Therefore, they are widely used in path smoothing and trajectory smoothing.

[0037] A robot path planning system, the system comprising: a data acquisition module, a data processing module, and a display module; The data acquisition module is used to obtain the free space, starting point and target point of the two-dimensional mobile robot; The data processing module is used to initialize the free space, starting point and target point of the two-dimensional mobile robot; The data processing module is also used to improve the RRT* algorithm through target constraint sampling, target bias extension and path smoothing; The data processing module is also used to plan the path of the two-dimensional robot through the improved RRT* algorithm to obtain the optimal path; The display module is used to visualize the optimal path.

[0038] This application provides a simulation experiment as follows: Two-dimensional plane robots are often used to compare and verify the performance and advantages of path planning algorithms. Since we only focus on the position of the robot on the plane and the obstacle environment, we can regard it as a particle moving on the plane. Therefore, in order to verify the performance advantage of the improved algorithm GCSE-RRT* compared with the original RRT* algorithm in planning paths, this paper designed four special environments for simulation and analysis. In the experiment, considering the size of the robot itself, the obstacles in each environment were expanded and the robot was treated as a particle. The simulation experiment platform and configuration are: MATLAB R2018b, 64-bit Windows 10, processor Intel(R) Core(TM) i5-4200H, CPU main frequency 2. 8GHz, memory 8GB.

[0039] In the experiment, the map scale of all environments is [100,100], the path planning starting point S is set to [5,5], and the target point G is set to [95,95]. Since the RRT* algorithm is random, the RRT* and GCSE-RRT* algorithms (i.e. the improved RRT* algorithm) are repeated 1000 times in each environment, and finally the smoothed path is given, and the average value of each indicator is calculated. The planning results in the four environments are shown in Figure 2. Figure 5 , Figure 6 , Figure 7 , Figure 8 shown.

[0040] In the figure above, the red line represents the optimal path after smoothing, and the green line is the branch of the expansion tree, S is the starting point, and G is the target point. Based on the planning results of the four environments, it can be seen that compared with the RRT* algorithm, the path search of the improved algorithm GCSE-RRT* is more purposeful and directional, which greatly reduces invalid expansion searches, and the optimal path obtained is smoother, so it is more in line with the robot's motion stability requirements. The following analyzes the memory usage, planning efficiency, and path optimization effect of the two algorithms.

[0041] For RRT*, the more valid nodes there are and the fewer extended nodes there are, the higher the node utilization rate is and the less memory the algorithm consumes. Therefore, this paper uses the node utilization rate after path search to measure the memory usage of the algorithm. The average node usage of the two algorithms in path search under different environments is shown in Table 1.

[0042] Table 1 Node usage of path search in four environments

[0043] As can be seen from the table above, for the average expansion node, RRT* has an average of 213 in the four environments, while the GCSE-RRT* algorithm has an average of 110, thus reducing 48% of the expansion nodes; for the average effective node, RRT* has an average of 20 in the four environments, while GCSE-RRT* has an average of 22, with the latter being slightly more than the former; for the average node utilization, RRT* has an average of 10% in the four environments, while GCSE-RRT* has an average of 21%. In summary, the node utilization of GCSE-RRT* is about twice that of RRT*, with higher node utilization and less memory usage.

[0044] Specifically, the fewer the number of iterations the algorithm takes to find a successful path and the shorter the running time, the higher the efficiency of the planned path. Therefore, this application measures the planning efficiency by the average number of algorithm iterations and the average running time. The average number of iterations and the average running time of the algorithm under the four environments are shown in Table 2.

[0045] Table 2 Number of iterations and running time of the algorithm under four environments

[0046] As can be seen from the table, the average number of iterations of the RRT* algorithm in the four environments is 432, while the average number of iterations of the GCSE-RRT* algorithm is 307, which is 29% lower than the average number of iterations of the former; at the same time, the average running time of the RRT* algorithm in the four environments is 0.25s, while the average running time of the GCSE-RRT* algorithm is 0.14s, which is 44% shorter than the average running time in the four environments. Therefore, compared with the RRT* algorithm, the GCSE-RRT* algorithm has higher planning efficiency and can plan feasible paths faster.

[0047] The RRT* algorithm itself has asymptotic optimality and can optimize the path length. Therefore, the average path length is used as the main factor, and the path smoothing effect is used as a supplement to comprehensively measure the optimization effect of the algorithm on the path. The average length of the paths planned by the two algorithms in different environments is summarized in Table 3.

[0048] Table 3 Path lengths planned by algorithms under different environments

[0049] From the table above, we can see that the average length of the path obtained by the RRT* algorithm in the four environments is 163.19 mm, while the average length of the path obtained by the GCSE-RRT* algorithm is 160.07 mm, which is 3.12 mm shorter than the former. Figure 5~Figure 8It can be seen that the path obtained by GCSE-RRT* is generally smoother than that of RRT*. Therefore, the path planned by the GCSE-RRT* algorithm is shorter and smoother, and its path optimization effect is better than that of the RRT* algorithm.

[0050] In summary, this section proposes an improved algorithm GCSE-RRT* based on target constraint sampling and target bias extension to solve the problems of RRT* algorithm in path planning in special environments, and uses cubic B-spline curve to smooth the path obtained by the improved algorithm. Simulation experiments based on MATLAB show that compared with the RRT* algorithm, the advantages of the improved algorithm are: 1) the node utilization rate is increased by more than 1 times, and the memory usage is less; 2) the number of iterations is reduced by 29%, the running time is shortened by 44%, and the planning efficiency is higher; 3) the final path is shorter and smoother, and the optimization effect is better.

[0051] The present application also discloses an electronic device. Fig. 9 , Fig. 9 The electronic device 500 may include: at least one processor 501 , at least one network interface 504 , a user interface 503 , a memory 505 , and at least one communication bus 502 .

[0052] The communication bus 502 is used to realize the connection and communication between these components.

[0053] The user interface 503 may include a display screen, and the optional user interface 503 may also include a standard wired interface or a wireless interface.

[0054] The network interface 504 may optionally include a standard wired interface or a wireless interface (such as a WI-FI interface).

[0055] The present application also discloses a computer-readable storage medium, which stores a plurality of instructions, and the instructions are suitable for a processor to load to execute the above-mentioned robot path planning method.

[0056] The above are only exemplary embodiments of the present disclosure and cannot be used to limit the scope of the present disclosure. That is, any equivalent changes and modifications made according to the teachings of the present disclosure are still within the scope of the present disclosure.

[0057] This application is intended to cover any variation, use or adaptation of the present disclosure, which follows the general principles of the present disclosure and includes common knowledge or customary technical means in the art not described in the present disclosure. The description and examples are to be regarded as exemplary only, and the scope and spirit of the present disclosure are defined by the claims.

Claims

1. A robot path planning method, characterized in that: The method comprises the following steps: S1: Initialize the free space, starting point and target point of the two-dimensional mobile robot; S2: Improve the RRT* algorithm through target constraint sampling, target bias extension and path smoothing; S3: The improved RRT* algorithm is used to plan the path of the two-dimensional robot, obtain the optimal path and visualize it.

2. A robot path planning method as claimed in claim 1, characterized in that: Step S2 includes: The specific steps to improve the RRT* algorithm through target constrained sampling include: Sampling is performed through the target bias strategy to obtain the sampling points as follows: Set a target bias probability , generates a probability value according to uniform probability ; The target bias sampling formula is: in, represents the random sampling function, represents the target point, represents the sampling point, Indicates that a sampling point is generated in free space; If the probability value Less than target bias probability , then the target point As sampling point ; If the probability value Greater than or equal to target bias probability , then a sampling point is generated in the free space; The position constraints are imposed on the sampling points generated in the free space, specifically: Set the starting point to the first sampling point; Set the judgment condition: If the distance between the sampling point and the target point in the X direction or Y direction is less than the distance between the previous sampling point and the target point in the X direction or Y direction, the judgment condition is met; If the judgment condition of the sampling point generated in the free space is met, the sampling is considered successful. If the judgment condition of the sampling point generated in the free space is not met, the target bias strategy is continued to perform cyclic sampling until the judgment condition is met.

3. A robot path planning method as claimed in claim 2, characterized in that: Step S2 also includes: The specific improvements to the RRT* algorithm through target bias extension include: New Node The generation formula of the bias target is as follows: in, The distance sampling point The nearest neighbor node, To expand the step size, Target point The direction weight of is the unit vector in the direction of the target point, is the unit vector of the sampling point direction, and the expressions are: ， 。 4. A robot path planning method as claimed in claim 1, characterized in that: Step S2 also includes: The specific process of using cubic B-spline curve to smooth the path generated by RRT* algorithm includes: Features Control Vertices , , the cubic B-spline curve is expressed as: In the formula, is a cubic canonical B-spline basis function, whose highest degree is 3; is the knot vector of the spline function.

5. A robot path planning method as claimed in claim 4, characterized in that: Step S2 also includes: the B-spline basis function is obtained by the Cox-deBoor recursive formula.

6. A robot path planning system, used to implement a robot path planning method as claimed in any one of claims 1 to 5, characterized in that: The system comprises: a data acquisition module, a data processing module, and a display module; The data acquisition module is used to obtain the free space, starting point and target point of the two-dimensional mobile robot; The data processing module is used to initialize the free space, starting point and target point of the two-dimensional mobile robot; The data processing module is also used to improve the RRT* algorithm through target constraint sampling, target bias extension and path smoothing; The data processing module is also used to plan the path of the two-dimensional robot through the improved RRT* algorithm to obtain the optimal path; The display module is used to visualize the optimal path.

7. An electronic device, characterized in that: The electronic device comprises a processor (501), a memory (505), a user interface (503) and a network interface (504), wherein the memory (505) is used to store instructions, the user interface (503) and the network interface (504) are used to communicate with other devices, and the processor (501) is used to execute the instructions stored in the memory (505) so that the electronic device executes the method according to any one of claims 1 to 5.

8. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores instructions, and when the instructions are executed by a computer, the method according to any one of claims 1 to 5 is executed.

Images