Method for improving three-dimensional RRT* path planning effect and control system

Abstract

The invention relates to a method for improving a three-dimensional RRT * path planning effect and a control system, and the method comprises the following steps: collecting environment data, and initializing the number of iterations and a path point set; dynamically adjusting the deviation sampling probability according to the environment data; optimizing an artificial potential field model, combining an RRT * algorithm with a potential field method, and carrying out path point growth under the guidance of a potential field; dynamically adjusting the random sampling and potential field method step length; performing father node reselection and rewiring operation on the generated path nodes, and generating the path nodes through collision detection; and optimizing existing path nodes by using a greedy algorithm. Through test verification, when a path planning task is carried out, iteration time and iteration times consumed when the path is searched can be effectively reduced, and the method is more suitable for path planning in a high-dimensional and complex environment; and meanwhile, the generated path can be optimized, unnecessary redundancy and bending conditions in the path are reduced, the path length is effectively reduced, and the method is more suitable for practical application.

Description

Technical Field

[0001] The present invention relates to the field of motion control planning, in particular to a path planning method based on random sampling, and specifically to a method and a control system for improving the effect of three-dimensional RRT* path planning. Background Art

[0002] In the architecture of mobile robots, the path planning module is the main decision-making and control part. The performance of path planning directly determines the quality of the driving path selected by the robot and the smoothness of its movement. Therefore, solving the challenges in path planning algorithms, that is, how to plan paths quickly, collision-free, and accurately in various scenarios, and giving them the ability to cope with dynamic changes in the environment, has become a key task for path planning algorithms.

[0003] RRT was first proposed by Professor Steven M. LaValle of Iowa State University in 1998. It is an algorithm based on the idea of ​​random growth trees, which aims to achieve fast search in non-convex high-dimensional space. The algorithm is suitable for dealing with scenes with obstacles and differential motion constraints. Its main advantage is that it does not need to model the specific environment, can efficiently cover the search space and quickly explore unknown areas. However, RRT also has the disadvantages of high computational cost and reliance on randomness, so researchers have proposed a variety of improvements to solve these problems.

[0004] In the RRT motion planning based on probability sampling, as the sampling points tend to infinity, the probability of converging to the optimal solution is 0. For this reason, Karaman and other scholars proposed the RRT* algorithm with asymptotic optimality in 2011. Based on the original RRT* algorithm, this algorithm improves the parent node selection method and uses the cost function to select the node with the minimum cost in the neighborhood of the extended node as the parent node, ensuring that the sum of the costs from the node to the starting node through this parent node is the smallest; at the same time, the nodes on the existing tree will be reconnected after each iteration to ensure the asymptotic optimal solution.

[0005] In recent years, scholars have optimized algorithms based on RRT*. In 2013, M. Jordan and A. Perez proposed Bi-RRT*, which accelerates the generation of paths by bidirectionally expanding two RRT* trees. It has good search characteristics, but this method is based on global uniform random sampling, has high path costs, and lacks optimization process. Gammell et al. proposed the Informed-RRT* algorithm. After finding a valid path for the first time, the sampling range is limited to the elliptical area, which speeds up the convergence of the algorithm. Yang Zhen et al. proposed a method for using the heuristic function of the projected sampling point to make an ellipse prior judgment, using geodesics to estimate the current cost of the projected sampling point, enhancing the quality of the ellipse prior and the iteration speed of the algorithm, but the path quality needs to be improved. Xu Wan et al. proposed the SMRS-RRT* algorithm, which finds paths through the PRM method based on grid map simplification and expands random trees by selecting functions, but occupies a large amount of computing resources in high-dimensional complex environments.

[0006] The path planning algorithm based on RRT* has made significant progress and has become one of the important technologies in fields such as robot path planning and autonomous driving. However, it still faces some challenges in terms of computational efficiency, path optimization and real-time performance. In the future, it is expected to further expand the application scenarios through more efficient computing methods and improved technical solutions. Summary of the invention

[0007] In order to solve the problems of low search efficiency and poor adaptability to complex environments in traditional RRT* algorithms in three-dimensional space, resulting in poor path planning efficiency, the present invention provides a method and control system for improving the three-dimensional RRT* path planning effect, reducing the number of extended path points, and reducing the number of iterations and related calculations required for path finding, thereby improving the path planning efficiency in various environments.

[0008] To achieve the above object, the specific technical solution proposed by the present invention is: a method for improving the effect of three-dimensional RRT* path planning, comprising the following steps: S1 collects environmental data and initializes the number of iterations and the set of path points; S2 dynamically adjusts the biased sampling probability according to the environmental data, generates sampling nodes, and performs collision detection; S3 optimizes the artificial potential field method model, combines the RRT* algorithm with the potential field method, and calculates the repulsive and attractive forces on the sampling nodes; S4 dynamically adjusts the step size of random sampling and potential field method, and generates path nodes under the superposition of random sampling and potential field method; S5 reselects the parent node and rewires the current path node, and generates a path node through collision detection; S6 obtains the complete path by backtracking the nodes and iteratively optimizes the existing path through a greedy algorithm.

[0009] Specifically, in step S1, the environmental data includes the density of obstacles, and the path point set includes setting the starting point ,end , and create a path based on the RRT* algorithm, starting from the starting point Start, end Direction growth.

[0010] Furthermore, in step S2, the density of obstacles is represented by the current environment obstacle influence degree ε, which is expressed as: ; Among them: F R is the repulsive force exerted on the node within the obstacle’s influence range, F G is the gravitational force exerted on the node by the endpoint; The biased sampling probability BSP expression is: ; in: is a probability function based on the first Euler integral, The value elements of must be in the interval [0,1]; is the beta function, z and w are distribution parameters and are both non-negative real numbers; The biased sampling scheme is: ; in: is the sampling point, is the end point, RandomSampling is random sampling in the space of the global environment, and the rand() function can randomly generate a number in [0,1].

[0011] Furthermore, the sampling scheme comprises the following specific steps: S201 search space determines the sampling point by the target probability bias value If the random value is less than the set BSP value, random sampling is performed in the global environment space; otherwise, the end point is As sampling point; S202: the sampling points selected in step S201 To judge, if the sampling point If it is within the obstacle, return to step S201 to resample; if it passes the collision detection, perform subsequent operations.

[0012] Specifically, in step S3, the artificial potential field model includes the repulsive potential field generated by the obstacle on the node q and the gravitational potential field generated by the target point on the node q.

[0013] Furthermore, the repulsive potential field generated by the obstacle on the node q is: ; in: is the magnitude of the repulsive potential field at point q, k is the gain coefficient of the repulsive potential field, is the Euclidean distance from point q to the obstacle, is the range of influence of the obstacle repulsion; The repulsive force generated by the obstacle on node q is: ; in: is the magnitude of the repulsive force on point q, k is the gain coefficient of the repulsive potential field, is the Euclidean distance from point q to the obstacle, is the range of influence of the obstacle repulsion; The gravitational potential field generated by the target point on node q is: ; in: is the magnitude of the gravitational potential field at point q, is the gravitational potential field gain coefficient, is the gravity threshold, To adjust the factor, it needs to be set according to the specific map size; is the Euclidean distance from point q to the end point; d is the gravitational influence distance factor; c is the gravitational gain constant, which is used to adjust the lower limit of the gravitational force; The gravitational force generated by the target point on node q is: ; When the direction of gravity points from point q to the end point, ; in: is the magnitude of the gravitational force on point q; d is the gravitational influence distance factor; is the gravity threshold; is the adjustment factor, which needs to be set according to the specific map size; c is the gravity gain, which is used to adjust the lower limit of gravity.

[0014] Furthermore, in step S4, the growth step length formula under the action of random sampling and potential field force is: ; Among them: stepsize is the growth step size; is the average value of the minimum distance from the sampling point to all obstacles; is the distance threshold; is the maximum step size, which generally depends on the Euclidean distance from the starting point to the end point; k is the precision coefficient. The larger the k value, the smaller the stepsize, the higher the success rate of tree node growth, the finer the path, but the number of iterations and the number of path points increase; β1 and β2 are the step size coefficients corresponding to random sampling and potential field method, respectively. The weight can be changed according to the effect of random sampling or potential field method; if a dynamic obstacle appears within the influence range of the obstacle, then , , the local path planning operation is performed separately by the artificial potential field method; is the vector from the sampling point to the neighboring node, is the distance from the sampling point to the neighboring node, It is the combined force of repulsion and attraction on the current sampling point.

[0015] In step S4, the path point growth process is: S401 Search for the distance sampling points in the set of paths The nearest node , and then from this node Go to the sampling point with step size stepsize , which is the vector A of random sampling growth; S402 defines the gravitational force, repulsive force coefficient, and obstacle influence range, establishes a gravitational potential field at the target point, and a repulsive potential field at the obstacle, and calculates the gravitational force F on the sampling node. G and repulsive force F R , and determine the resultant force ; S403 According to the combined force And the randomly sampled growth vector A is obtained , get the path and new node ; S404 determines the path status. If the path passes through an obstacle, return to step S2 for resampling; if the path passes the collision detection, execute the subsequent steps.

[0016] Furthermore, in step S4, the path point growth process is: S401 Search for the distance sampling points in the set of paths The nearest node , and then from this node Go to the sampling point with step size stepsize , which is the vector A of random sampling growth; S402 defines the gravitational force, repulsive force coefficient, and obstacle influence range, establishes a gravitational potential field at the target point, and a repulsive potential field at the obstacle, and calculates the gravitational force F on the sampling node. G and repulsive force F R , and determine the resultant force ; S403 According to the combined force And the randomly sampled growth vector A is obtained , get the path and new node ; S404 determines the path status. If the path passes through an obstacle, return to step S2 for resampling; if the path passes the collision detection, execute the subsequent steps.

[0017] Furthermore, in step S5, the process of reselecting the parent node and generating the path node of the rewiring operation is as follows: S501 is in the newly generated node Find "neighbors" within the defined radius r as candidates to replace the parent node; S502 calculates the path cost from the "neighbor" node to the starting point and adds the cost of the new node The path cost to each "neighbor"; S503 If a little can make the starting point To the new node is shorter, then replace the point with The parent node of S504 determines the cost of connecting the random path nodes, reroutes the random path, and then Determine for the parent node whether other node connections within the radius range r have less path cost, so as to ensure that the sum of the path costs from the node to the starting point is minimized, so that the random path after generating the new node reduces redundant paths; S505 will generate a new node Put it into the path set E and update the connection method of the path; S506 repeats the process from step S2 to step S5, and gradually builds the path by updating the set E of paths until the newly generated new node of the path To the end When the Euclidean distance is less than the set distance threshold Thr and there are no obstacles blocking the connection, the new node is directly connected. and end point , get the complete set of path points E, and continue to execute subsequent steps.

[0018] Furthermore, in step S6, a complete path is obtained, and the greedy algorithm optimization iterative process is specifically as follows: S601 from the end Go back to the starting point through the parent node , get a complete feasible path; S602 sets the initial path node set to , where P0 is the starting point, P n is the end point, and the simplified path node set is ; S603 initializes the path, and sets the starting point P0 and the end point P n Put into the simplified path node set, that is , ; S604 In the inner loop, try to connect to the current node P i To the end point P n The further node P j ;when If the collision detection passes, let k=k+1 and continue iterating until j=i+k fails the collision detection. Then the node that succeeded in the last detection is replaced. Update to the simplified path node set, that is, , and point Continue to repeat the above iterative operation as a new node; S605 repeats the above steps until P j =P n The iteration ends when , and the simplified path node set is obtained. The path nodes are connected in sequence to obtain the final path.

[0019] Also disclosed is a control system for improving the three-dimensional RRT* path planning effect, which is used to run the above-mentioned method for improving the three-dimensional RRT* path planning effect, including a data terminal, a visual camera, an operating platform, a control unit, a driving device, and a motion terminal; The operating platform is a user-friendly platform equipped with tools for robot simulation and visualization; The data terminal is used to realize data processing, and the operating platform is connected with the data terminal by signal, and is used to realize the development and control of the motion terminal through the data terminal; The visual camera is connected to the data terminal signal and is used to transmit the acquired environmental information to the data terminal; The data terminal is connected to the control unit by signal, and is used to complete data processing, motion planning and send control commands to the execution structure; The control unit is connected to the drive device signal, and is used to transmit the control signal to the drive device, and transmit the information fed back by the drive device to the data terminal; The driving device is connected to the motion end signal, and the motion end is an actuator for executing control commands.

[0020] Beneficial effects of the present invention: 1. RRT* is combined with the potential field method, and the sampling strategy and growth step size are dynamically adjusted, which reduces the number of extended path points and effectively reduces the number of iterations and time required for path finding, simplifies the relevant calculation amount, and is more suitable for path planning in high-dimensional environments; 2. Using the greedy algorithm to iterate the path can effectively reduce unnecessary redundancy in the path and reduce the total path length, making the path more suitable for practical applications. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. The drawings are used to provide a further understanding of the present invention and constitute a part of the specification. Together with the embodiments of the present invention, they are used to explain the present invention and do not constitute a limitation of the present invention. For those skilled in the art, other drawings can be obtained based on the drawings without paying creative work.

[0022] Figure 1 It is a flow chart of the present invention; Figure 2 It is a schematic diagram of combining the RRT* algorithm and the potential field method of the present invention; Figure 3 This is a flow chart of the operation process of an embodiment of the present invention; Figure 4 A simple environment map view of an embodiment of the present invention; Figure 5 This is the path planning result diagram of the traditional RRT* algorithm in a simple environment; Figure 6 This is a graph showing the path planning results in a simple environment according to an embodiment of the present invention; Figure 7 A complex environment map view according to an embodiment of the present invention; Figure 8 This is the path planning result diagram of the traditional RRT* algorithm in a complex environment; Fig. 9 A diagram showing the path planning results in a complex environment according to an embodiment of the present invention; Fig.10 This is a schematic diagram of the iterative path of the greedy algorithm according to an embodiment of the present invention; Fig.11 This is a diagram showing the effect of the iterative path of the greedy algorithm according to an embodiment of the present invention; Fig.12 Schematic diagram of the control system structure of an embodiment of the present invention. DETAILED DESCRIPTION

[0023] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0024] refer to Figure 1 As shown, the specific steps of a method for improving the three-dimensional RRT* path planning effect of the present invention include the following steps: S1 collects environmental data and initializes the number of iterations and the set of path points; S2 dynamically adjusts the biased sampling probability according to the environmental data, generates sampling nodes, and performs collision detection; S3 optimizes the artificial potential field method model, combines the RRT* algorithm with the potential field method, and calculates the repulsive and attractive forces on the sampling nodes; S4 dynamically adjusts the step size of random sampling and potential field method, and generates path nodes under the superposition of random sampling and potential field method; S5 reselects the parent node and rewires the current path node, and generates a path node through collision detection; S6 obtains the complete path by backtracking the nodes and iteratively optimizes the existing path through a greedy algorithm.

[0025] In step S1, the environment data includes the density of obstacles, and the path point set includes setting the starting point ,end , and create a path based on the RRT* algorithm, starting from the starting point Start, end Direction growth.

[0026] In step S2, the density of obstacles is represented by the current environment obstacle influence degree ε, which is expressed as: ; Among them: F R is the repulsive force exerted on the node within the obstacle’s influence range, F G is the gravitational force exerted on the node by the endpoint; The biased sampling probability BSP in step S2 is expressed as: ; in: is a probability function based on the first Euler integral, The value elements of must be in the interval [0,1]; is the beta function, z and w are distribution parameters and are both non-negative real numbers; The biased sampling scheme is: ; in: is the sampling point, is the end point, RandomSampling is random sampling in the space of the global environment, and the rand() function can randomly generate a number in [0,1].

[0027] The specific steps of the sampling plan are: S201 search space determines the sampling point by the target probability bias value If the random value is less than the set BSP value, random sampling is performed in the global environment space; otherwise, the end point is As sampling point; S202: the sampling points selected in step S201 To judge, if the sampling point If it is within the obstacle, return to step S201 to resample; if it passes the collision detection, perform subsequent operations.

[0028] In step S3, the artificial potential field model includes the repulsive potential field generated by the obstacle on the node q and the gravitational potential field generated by the target point on the node q; The repulsive potential field generated by the obstacle on node q is expressed as: ; in: is the magnitude of the repulsive potential field at point q, k is the gain coefficient of the repulsive potential field, is the Euclidean distance from point q to the obstacle, is the range of influence of the obstacle repulsion; The repulsive force generated by the obstacle on node q is: ; in: is the magnitude of the repulsive force on point q, k is the gain coefficient of the repulsive potential field, is the Euclidean distance from point q to the obstacle, is the range of influence of the obstacle repulsion; The gravitational potential field generated by the target point on node q is: ; in: is the magnitude of the gravitational potential field at point q, is the gravitational potential field gain coefficient, is the gravity threshold, To adjust the factor, it needs to be set according to the specific map size; is the Euclidean distance from point q to the end point; d is the gravitational influence distance factor; c is the gravitational gain constant, which is used to adjust the lower limit of the gravitational force; The gravitational force generated by the target point on node q is: ; When the direction of gravity points from point q to the end point, ; in: is the magnitude of the gravitational force on point q; d is the gravitational influence distance factor; is the gravity threshold; is the adjustment factor, which needs to be set according to the specific map size; c is the gravity gain, which is used to adjust the lower limit of gravity.

[0029] In step S4, the growth step length formula under random sampling and potential field force is: ; Among them: stepsize is the growth step size; is the average value of the minimum distance from the sampling point to all obstacles; is the distance threshold; is the maximum step size, which generally depends on the Euclidean distance from the starting point to the end point; k is the precision coefficient. The larger the k value, the smaller the stepsize, the higher the success rate of tree node growth, the finer the path, but the number of iterations and the number of path points increase; β1 and β2 are the step size coefficients corresponding to random sampling and potential field method, respectively. The weight can be changed according to the effect of random sampling or potential field method; if a dynamic obstacle appears within the influence range of the obstacle, then , , the local path planning operation is performed separately by the artificial potential field method; is the vector from the sampling point to the neighboring node, is the distance from the sampling point to the neighboring node, It is the combined force of repulsion and attraction on the current sampling point.

[0030] The path point growth process is: S401 Search for the distance sampling points in the set of paths The nearest node , and then from this node Go to the sampling point with step size stepsize , which is the vector A of random sampling growth; S402 defines the gravitational force, repulsive force coefficient, and obstacle influence range, establishes a gravitational potential field at the target point, and a repulsive potential field at the obstacle, and calculates the gravitational force F on the sampling node. G and repulsive force F R , and determine the resultant force ; S403 According to the combined force And the randomly sampled growth vector A is obtained , get the path and new node ; S404 determines the path status. If the path passes through an obstacle, return to step S2 for resampling; if the path passes the collision detection, execute the subsequent steps.

[0031] In step S5, the process of reselecting the parent node and generating the path node of the rewiring operation is as follows: S501 is in the newly generated node Find "neighbors" within the defined radius r as candidates to replace the parent node; S502 calculates the path cost from the "neighbor" node to the starting point and adds the cost of the new node The path cost to each "neighbor"; S503 If a little can make the starting point To the new node is shorter, then replace the point with The parent node of S504 determines the cost of connecting the random path nodes, reroutes the random path, and then Determine for the parent node whether other node connections within the radius range r have less path cost, so as to ensure that the sum of the path costs from the node to the starting point is minimized, so that the random path after generating the new node reduces redundant paths; S505 will generate a new node Put it into the path set E and update the connection method of the path; S506 repeats the process from step S2 to step S5, and gradually builds the path by updating the set E of paths until the newly generated new node of the path To the end When the Euclidean distance is less than the set distance threshold Thr and there are no obstacles blocking the connection, the new node is directly connected. and end point , get the complete set of path points E, and continue to execute subsequent steps.

[0032] Get the complete path, and the greedy algorithm optimization iterative process is as follows: S601 from the end Go back to the starting point through the parent node , get a complete feasible path; S602 sets the initial path node set to , where P0 is the starting point, P n is the end point, and the simplified path node set is ; S603 initializes the path, and sets the starting point P0 and the end point P n Put into the simplified path node set, that is , ; S604 In the inner loop, try to connect to the current node P i To the end point P n The further node P j ;when If the collision detection passes, let k=k+1 and continue iterating until j=i+k fails the collision detection. Then the node that succeeded in the last detection is replaced. Update to the simplified path node set, that is, , and point Continue to repeat the above iterative operation as a new node; S605 repeats the above steps until P j =P n The iteration ends when , and the simplified path node set is obtained. The path nodes are connected in sequence to obtain the final path.

[0033] Reference Figure 2 As shown in the figure, the improved potential field method model is combined with the RRT* algorithm to find the distance sampling points in the path set for path growth. The nearest node , and then from this node Go to the sampling point with step size stepsize , which is the vector A of random sampling growth; Define the gravitational force, repulsive force coefficient, and obstacle influence range, establish a gravitational potential field at the target point, and a repulsive potential field at the obstacle, and calculate the gravitational force F on the sampling node G and repulsive force F R , and determine the resultant force ; Then according to the combined force And the randomly sampled growth vector A is obtained , get the expanded tree and new nodes ; If the path passes through an obstacle, return to random sampling again; if it passes the collision detection, execute the subsequent steps.

[0034] Reference Figure 3 As shown, an embodiment of the path optimization of the present invention, the first step is to initialize the number of iterations and the path point set, set the starting point ,end , obtain the obstacle environment, create an RRT* tree (that is, create a path through the RRT* algorithm), and start from the starting point Start, end Direction growth; The second step is to obtain the obstacle density ε in the current environment as the input parameter of the incomplete beta function model, and output it as the dynamic bias sampling probability BSP to select sampling points in the sampling space. , select sampling strategies reasonably to reduce the number of iterations. If the sampling point is within the obstacle, return to the second step to re-randomly sample; if the collision detection is passed, execute the third step; The third step is to find the distance sampling points in the tree set for tree growth. The nearest node , and then from this node Go to the sampling point with step size stepsize , which is the vector A of random sampling growth. Define the gravity, repulsion coefficient, and obstacle influence range, establish a gravitational potential field at the target point, and a repulsive potential field at the obstacle, and calculate the gravitational force F on the sampling node G and repulsive force F R , and determine the resultant force Then according to the combined force Superimpose with the randomly sampled growth vector A to obtain the extended vector and new node If the path passes through an obstacle, return to step 2 and re-randomly sample; if it passes the collision detection, execute step 4.

[0035] The fourth step is to create a new node Find the "nearest neighbor" within the defined radius r as a candidate to replace the parent node. Calculate the path cost from the "nearest neighbor" node to the starting point plus the new node The path cost to each "neighbor". If a point can make the starting point To the new node is shorter, then replace the point with To further minimize the cost of connecting random tree nodes, the random tree is rewired. Determine for the parent node whether other nodes within the radius range r have a lower path cost. This ensures that the sum of the path costs from the node to the starting point is minimized, so that the random tree after generating a new node reduces redundant paths; Step 5: Add the new nodes generated in the above steps Put it into the tree set E and update the tree connection method. Repeat the process from step 2 to step 4, gradually build the path by updating the tree set E until the new node generated by the RRT* tree is reached. To the end When the Euclidean distance is less than the set distance threshold Thr and there are no obstacles blocking the connection, the new node is directly connected. and end point , get the complete set of path points E, and execute step 6; Step 6: From the end point Go back to the starting point through the parent node , get a complete feasible path; In the seventh step, a greedy algorithm is used to iterate the feasible paths generated in step six to reduce the number of redundant path nodes, optimize the path quality, and obtain the final path.

[0036] The tree referred to in this embodiment is a path, which is the sum of feasible paths and extended paths.

[0037] The following specific experiments verify the path planning of the RRT* algorithm before and after the introduction of the potential field method and the dynamic adjustment of the sampling strategy: Reference Figures 4 to 6 As shown, there are corresponding effect diagrams of a simple obstacle environment, the path planning results of the traditional RRT* algorithm in a simple obstacle environment, and the RRT* path planning results of a simple obstacle environment with the potential field method introduced and the sampling strategy dynamically adjusted. In a simple obstacle environment, after the potential field method is introduced and the sampling strategy is dynamically adjusted, the number of extended path points is significantly reduced, which can effectively reduce the number of iterations required for path finding, simplify the related calculation amount, and greatly improve the efficiency of path planning in a high-dimensional environment.

[0038] Reference Figures 7 to 9 As shown, there are corresponding effect diagrams of complex obstacle environment, traditional RRT* algorithm path planning results in complex obstacle environment, and RRT* path planning results with the potential field method introduced and the sampling strategy dynamically adjusted in a complex obstacle environment. In a complex obstacle environment, after the potential field method is introduced and the sampling strategy is dynamically adjusted, the number of extended path points is significantly reduced, which can effectively reduce the number of iterations required for path finding and simplify the related calculation amount, greatly improving the efficiency of path planning in a high-dimensional environment.

[0039] Reference Fig.10 and Fig.11 As shown in FIG. 1 , using a greedy algorithm to iterate the path can effectively reduce unnecessary redundancy and bends in the path, and can reduce the total path length, making the path more suitable for practical applications.

[0040] The RRT* path planning algorithm is based on probability sampling and has a certain degree of randomness. To obtain stable experimental conclusions, it is necessary to conduct multiple repeated iterative experiments and calculate the average results.

[0041] Table 1 shows the comparison results of the average number of iterations, average iteration time, average path length of the traditional RRT* method and the average number of iterations, average iteration time, average path length of the present invention under simpler and more complex environments.

[0042] Table 1 Comparative analysis Map environment Average number of iterations of the traditional RRT* method Average iteration time of traditional RRT* method / s Average path length of traditional RRT* method The average number of iterations of the present invention Average iteration time of the present invention / s Average path length of the present invention Simpler 1155.02 0.9372 1932.2643 280.31 0.2987 1819.0551 More complex 1631.32 1.5371 1920.8522 375.72 0.4204 1803.3457 The results show that the present invention optimizes the traditional RRT* path planning method and can effectively reduce the number of iterations and iteration time required in both simpler and more complex environments, while reducing the path length, making it more suitable for practical engineering applications.

[0043] Referring to FIG. 12 , the present invention is to run the above-mentioned method for improving the path planning effect of three-dimensional RRT*, and also discloses a control system for running the path planning effect method of this embodiment, including a data terminal, a visual camera, an operating platform, a control unit, a driving device, and a motion terminal; The operating platform is a user platform equipped with tools for robot simulation and visualization; the data terminal is used to process data; the visual camera is used to obtain environmental information; the data terminal is used to perform tasks such as data processing and motion decision-making, and is the main control of the entire system; the control unit is used to control the actuator (i.e., the motion end); the drive device is used to drive the motion end to execute the motion trajectory and feedback the motion signal; the motion end executes the control command.

[0044] The operating platform is connected to the data terminal by signal, and is used to realize the development and control of the motion terminal through the data terminal; The visual camera is connected to the data terminal signal to transmit the acquired environmental information to the data terminal; The data terminal is connected to the control unit by signal, and is used to complete data processing, motion planning, and send control commands to the execution structure; The control unit is connected to the drive device signal, and is used to transmit the control signal to the drive device, and transmit the information fed back by the drive device to the data terminal; The driving device is connected to the motion end signal, and the motion end is an actuator for executing the control command.

[0045] In this embodiment, the data terminal uses Raspberry Pi, the operating platform uses the ROS platform, the control unit uses the MCU, the driving device uses a motor, and the mobile terminal uses a robotic arm.

[0046] Although the preferred implementation cases of the present invention have been described above in conjunction with the accompanying drawings, the present invention is not limited to the above-mentioned specific implementation methods. The above-mentioned specific implementation methods are merely illustrative and not restrictive. Under the guidance of the present invention, ordinary technicians in this field can also make many forms of specific changes without departing from the scope of protection of the invention spirit and claims, which all fall within the scope of protection of the present invention.

Claims

1. A method for improving the effect of three-dimensional RRT* path planning, characterized in that: The following steps are involved: S1 collects environmental data and initializes the number of iterations and the set of path points; S2 dynamically adjusts the biased sampling probability according to the environmental data, generates sampling nodes, and performs collision detection; S3 optimizes the artificial potential field method model, combines the RRT* algorithm with the potential field method, and calculates the repulsive and attractive forces on the sampling nodes; S4 dynamically adjusts the step size of random sampling and potential field method, and generates path nodes under the superposition of random sampling and potential field method; S5 reselects the parent node and rewires the current path node, and generates a path node through collision detection; S6 obtains the complete path by backtracking the nodes and iteratively optimizes the existing path through a greedy algorithm.

2. The method for improving the three-dimensional RRT* path planning effect according to claim 1, characterized in that: In step S1, the environmental data includes the density of obstacles, and the path point set includes setting the starting point ,end , and create a path based on the RRT* algorithm, starting from the starting point Start, end Direction growth.

3. The method for improving the three-dimensional RRT* path planning effect according to claim 1, characterized in that: In step S2, the density of obstacles is represented by the current environment obstacle influence degree ε, which is expressed as: ； Among them: F R is the repulsive force exerted on the node within the obstacle’s influence range, F G is the gravitational force exerted on the node by the endpoint; The biased sampling probability BSP expression is: ； in: is a probability function based on the first Euler integral, The value elements of must be in the interval [0,1]; is the beta function, z and w are distribution parameters and are both non-negative real numbers; The biased sampling scheme is: ； in: is the sampling point, is the end point, RandomSampling is random sampling in the space of the global environment, and the rand() function can randomly generate a number in [0,1].

4. The method for improving the three-dimensional RRT* path planning effect according to claim 3, characterized in that: The specific steps of the sampling scheme are as follows: S201 search space determines the sampling point by the target probability bias value , if the random value is less than the set BSP value, random sampling is performed in the space of the global environment; Otherwise, the end point As sampling point; S202: the sampling points selected in step S201 To judge, if the sampling point If it is within the obstacle, return to step S201 and resample; If the collision detection is passed, the subsequent operations are performed.

5. The method for improving the three-dimensional RRT* path planning effect according to claim 4, characterized in that: In step S3, the artificial potential field model includes the repulsive potential field generated by the obstacle on the node q and the gravitational potential field generated by the target point on the node q; The repulsive potential field generated by the obstacle on node q is: ； in: is the magnitude of the repulsive potential field at point q, k is the gain coefficient of the repulsive potential field, is the Euclidean distance from point q to the obstacle, is the range of influence of the obstacle repulsion; The repulsive force generated by the obstacle on node q is: ； in: is the magnitude of the repulsive force on point q, k is the gain coefficient of the repulsive potential field, is the Euclidean distance from point q to the obstacle, is the range of influence of the obstacle repulsion; The gravitational potential field generated by the target point on node q is: ； in: is the magnitude of the gravitational potential field at point q, is the gravitational potential field gain coefficient, is the gravity threshold, is the regulating factor, is the Euclidean distance from point q to the end point, d is the gravitational influence distance factor, and c is the gravitational gain constant; The gravitational force generated by the target point on node q is: ； When the direction of gravity points from point q to the end point, ; in: is the magnitude of the gravitational force on point q, d is the gravitational influence distance factor, is the gravity threshold, is the adjustment factor, and c is the gravitational gain.

6. The method for improving the three-dimensional RRT* path planning effect according to claim 5, characterized in that: In step S4, the growth step length formula under the action of random sampling and potential field force is: ； Among them: stepsize is the growth step size; is the average value of the minimum distance from the sampling point to all obstacles; is the distance threshold, is the maximum step size, k is the precision coefficient, the larger the k value, the smaller the stepsize, the higher the success rate of tree node growth, the finer the path, but the number of iterations and path points increases; β1 and β2 are the step size coefficients corresponding to random sampling and potential field method, respectively, and the weight can be changed according to the effect of random sampling or potential field method; if a dynamic obstacle appears within the influence range of the obstacle, then , , the local path planning operation is performed separately by the artificial potential field method; is the vector from the sampling point to the neighboring node, is the distance from the sampling point to the neighboring node, It is the combined force of repulsion and attraction on the current sampling point.

7. The method for improving the three-dimensional RRT* path planning effect according to claim 6, characterized in that: In step S4, the path point growth process is: S401 Search for the distance sampling points in the set of paths The nearest node , and then from this node Go to the sampling point with step size stepsize , which is the vector A of random sampling growth; S402 defines the gravitational force, repulsive force coefficient, and obstacle influence range, establishes a gravitational potential field at the target point, and a repulsive potential field at the obstacle, and calculates the gravitational force F on the sampling node. G and repulsive force F R , and determine the resultant force ; S403 According to the combined force And the randomly sampled growth vector A is obtained , get the path and new node ; S404 determines the path status, and if the path passes through an obstacle, returns to step S2 for resampling; If the collision detection is passed, the subsequent steps are executed.

8. The method for improving the three-dimensional RRT* path planning effect according to claim 7, characterized in that: In step S5, the process of reselecting the parent node and generating the path node of the rewiring operation is as follows: S501 is in the newly generated node Find "neighbors" within the defined radius r as candidates for replacing the parent node; S502 calculates the path cost from the "neighbor" node to the starting point and adds the cost of the new node The path cost to each "neighbor"; S503 If a little can make the starting point To the new node is shorter, then replace the point with The parent node of S504 determines the cost of connecting the random path nodes, reroutes the random path, and then Determine for the parent node whether other node connections within the radius range r have less path cost, so as to ensure that the sum of the path costs from the node to the starting point is minimized, so that the random path after generating the new node reduces redundant paths; S505 will generate a new node Put it into the path set E and update the connection method of the path; S506 repeats the process from step S2 to step S5, and gradually builds the path by updating the set E of paths until the newly generated new node of the path To the end When the Euclidean distance is less than the set distance threshold Thr and there are no obstacles blocking the connection, the new node is directly connected. and end point , get the complete set of path points E, and continue to execute subsequent steps.

9. The method for improving the three-dimensional RRT* path planning effect according to claim 1, characterized in that: In step S6, a complete path is obtained, and the greedy algorithm optimization iterative process is specifically as follows: S601 from the end Go back to the starting point through the parent node , get a complete feasible path; S602 sets the initial path node set to , where P0 is the starting point, P n is the end point, and the simplified path node set is ; S603 initializes the path, and sets the starting point P0 and the end point P n Put into the simplified path node set, that is , ; S604 In the inner loop, try to connect to the current node P i To the end point P n The further node P j ;when If the collision detection passes, let k=k+1 and continue iterating until j=i+k fails the collision detection. Then the node that succeeded in the last detection is replaced. Update to the simplified path node set, that is, , and point Continue to repeat the above iterative operation as a new node; S605 repeats the above steps until P j =P n The iteration ends when , and the simplified path node set is obtained. The path nodes are connected in sequence to obtain the final path.

10. A control system for improving the effect of three-dimensional RRT* path planning, used for running the method for improving the effect of three-dimensional RRT* path planning according to any one of claims 1 to 9, characterized in that: It includes data terminal, visual camera, operating platform, control unit, drive device and motion end; The operating platform is a user-friendly platform equipped with tools for robot simulation and visualization; The data terminal is used to realize data processing, and the operating platform is connected with the data terminal by signal, and is used to realize the development and control of the motion terminal through the data terminal; The visual camera is connected to the data terminal signal and is used to transmit the acquired environmental information to the data terminal; The data terminal is connected to the control unit by signal, and is used to complete data processing, motion planning and send control commands to the execution structure; The control unit is connected to the drive device signal, and is used to transmit the control signal to the drive device, and transmit the information fed back by the drive device to the data terminal; The driving device is connected to the motion end signal, and the motion end is an actuator for executing a control command.

Images