A robot path planning method based on an improved RRT algorithm

Abstract

The application provides a robot path planning method based on an improved RRT algorithm, and comprises the following steps: initializing a planning environment of a robot, setting a step length L of the planning robot, a starting point X of the robot, a target point X of the robot, a sampling parameter sigma, and a path tree Tree of the robot, wherein the path tree Tree comprises a path from the starting point X to the target point X; randomly generating a sampling point Q, finding a sampling point Q closest to the sampling point Q in the path tree Tree, connecting the sampling point Q and the sampling point X, obtaining a reference direction V, and generating a Gaussian distribution model with the reference direction V as a center and the sampling parameter sigma as a variance. start goal start goal new new near near goal The application has the beneficial effects that the Gaussian distribution model based on an angle is introduced into a bias function of the RRT algorithm, the efficiency of the bias of the RRT algorithm is improved, the growing tree can grow towards the target point while avoiding the influence of the obstacles, the target point is approached in the exploration, and the path length and the planning time are shortened.​​​​​​​​

Description

Technical Field

[0001] This invention belongs to the field of robot path planning, and in particular relates to a robot path planning method based on an improved RRT algorithm. Background Technology

[0002] Motion planning refers to the process of connecting the initial and target states of a robot within a reachable environment, while avoiding the influence of obstacles, with the key focus on finding a continuous feasible path. Motion planning is a core component of autonomous motion for various robots, including drones, unmanned vehicles, robotic arms, and humanoid robots. Solutions to motion planning problems are mainly categorized into sampling-based methods, search-based methods, and optimization-based methods. Search-based path planning methods include the A* algorithm, artificial potential field method, genetic algorithms, etc.; however, these algorithms typically suffer from slow convergence speeds and low efficiency. The most commonly used sampling-based path planning method, RRT, does not require description of obstacles in the configuration space and can perform motion planning in spaces of arbitrary dimensions, making it suitable for all robots. Furthermore, due to the efficiency of sampling, this algorithm is well-suited for complex high-dimensional spaces, such as the motion planning of robotic arms. Therefore, in recent years, many researchers have proposed different methods to improve upon the existing problems of this algorithm. The RRT algorithm proposed by Professor Lavalle has been widely used in the field of path planning. However, the RRT algorithm itself also has some drawbacks. For example, the expansion is not directional and the search is highly random, which leads to the planned path being relatively long and requiring a lot of time. On the other hand, the success rate of the algorithm is significantly reduced when facing narrow and complex environments with many obstacles.

[0003] In traditional RRT algorithms, when a collision between an extended point and an obstacle is detected, the extended point is abandoned and resampled. This method discards useful information about the collision location between the robot and the obstacle, resulting in a significant waste of information and reducing the efficiency of RRT algorithm expansion.

[0004] Therefore, it is essential to design a path planning algorithm based on an improved RRT algorithm. Summary of the Invention

[0005] In view of this, the present invention aims to propose a robot path planning method based on an improved RRT algorithm to at least solve one of the problems in the background art.

[0006] To achieve the above objectives, the technical solution of the present invention is implemented as follows:

[0007] A robot path planning method based on an improved RRT algorithm includes the following steps:

[0008] Initialize the robot's planning environment, set the robot's step size L, and the robot's starting point X. start The robot's target point X goal Sampling parameters Σ, the robot's path tree, where the path tree includes paths from the starting point X. start To target point X goal The path;

[0009] Randomly generate sampling points Q new Find the path tree tree that corresponds to the sampling point Q. new The most recent sampling point Q near Connect sampling point Q near and sampling point X goal Thus, the reference direction V is obtained;

[0010] A Gaussian distribution model centered on the reference direction V and with the sampling parameter Σ as the covariance matrix is ​​generated. Then, the Gaussian distribution model is truncated and normalized within the range of -π to +π in each dimension. Finally, the direction θ is obtained by sampling according to the new probability density function.

[0011] Expanding along the sampling direction θ and using the robot's step size L as the expansion step size, a new point Q is obtained. ext ;

[0012] For the new point Q ext Perform collision detection; if the new point Q... ext If there is no collision, then the new point Q will be... ext Included in the path tree;

[0013] If new point Q ext Collisions are handled by oscillating in each dimension, centered on the reference direction V, and increasing the angle by a set value to obtain a new direction θ. Then, based on the obtained direction θ, the robot expands along its step size L to obtain a new point Q. ext Perform collision detection; if no collision is found, use the newly obtained point Q. ext Continuing to grow, if collisions occur, we will continue to try;

[0014] Repeat the above steps until a path is found from the starting point X. start To target point X goal The path may exceed the preset number of iterations.

[0015] Furthermore, the reference direction V is determined by Q. near Pointing to X from the starting point goal The direction of the vector.

[0016] Furthermore, in the path-finding tree, the sampling point Q... new The most recent sampling point Q near The expression is:

[0017] min f=||Q new -Q near ||2

[0018] stQ near ∈Tree.

[0019] Furthermore, the obtained direction θ is a multi-dimensional vector, with a dimension equal to the number of degrees of freedom of the robot minus one. For a robot with n degrees of freedom, n-1 linearly independent direction vectors are selected as reference directions. The components of θ represent the angles of rotation around the n-1 reference direction vectors, specifically expressed as follows:

[0020] θ = {θ1, θ2, ..., θ} n-1}

[0021] Furthermore, the process of using the Gaussian distribution model to truncate and normalize the range from -π to +π in each dimension includes:

[0022] The expression for the original Gaussian distribution is:

[0023]

[0024] Where μ is the reference direction V, and Σ is the (n-1)×(n-1) dimensional covariance matrix;

[0025] Take the integral of the original Gaussian distribution over all dimensions [-π, π]:

[0026]

[0027] The expression for the normalized probability density function is then:

[0028]

[0029] Furthermore, the new point Q ext Collisions are handled by oscillating in each dimension, centered on the reference direction V, and increasing the angle by a set value to obtain a new direction θ. Then, based on the obtained direction θ, the robot expands along its step size L to obtain a new point Q. ext Perform collision detection; if no collision is found, use the newly obtained point Q. ext Growth is underway, and attempts will continue if collisions occur. Specifically, this includes:

[0030] In the state space of an n-DOF robot, the expression for the V direction is:

[0031] V = {V1, V2, ..., V} n-1}

[0032] When the new point Q extUpon encountering a collision, the first degree of freedom is increased by an angle J to obtain a new direction V′ for growth exploration and collision detection. The expression for the new direction is:

[0033] V′={V1,V2,,,V n-1}±{J,0,,,0}={V1±J,V2,,,V n-1}

[0034] If a collision still occurs and growth fails, then start exploring again from the first degree of freedom and expand the J angle, and repeat this cycle.

[0035] If J reaches the limit angle, i.e. |J|>π, then stop exploring and abandon this growth.

[0036] If a growth path is found in a certain direction during the exploration process, that point is added to the path tree.

[0037] Furthermore, this solution discloses an electronic device, including a processor and a memory communicatively connected to the processor and used to store executable instructions of the processor, wherein the processor is used to execute a robot path planning method based on an improved RRT algorithm.

[0038] Furthermore, this solution discloses a server, including at least one processor and a memory communicatively connected to the processor, the memory storing instructions executable by the at least one processor, the instructions being executed by the processor to cause the at least one processor to execute a robot path planning method based on the RRT algorithm.

[0039] Furthermore, this solution discloses a computer-readable storage medium storing a computer program, which, when executed by a processor, implements a robot path planning method based on an improved RRT algorithm.

[0040] Compared with existing technologies, the robot path planning method based on the improved RRT algorithm described in this invention has the following advantages:

[0041] (1) The robot path planning method based on the improved RRT algorithm described in this invention introduces the Gaussian distribution model based on the deflection angle into the bias function of the RRT algorithm, which improves the efficiency of the RRT algorithm bias and enables the growth tree to avoid the influence of obstacles better while growing toward the target point. It continuously moves closer to the target point during exploration, shortening the path length and planning time.

[0042] (2) The robot path planning method based on the improved RRT algorithm described in this invention utilizes a swing-adaptive strategy to reuse collision information, enabling the robot to repeatedly explore paths around obstacles from the original direction when encountering them, thereby obtaining the most efficient path to avoid obstacles. This method improves the efficiency of path planning, especially the utilization rate of collision point information. Attached Figure Description

[0043] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an undue limitation of the invention. In the drawings:

[0044] Figure 1 This is a schematic flowchart of a robot path planning method based on an improved RRT algorithm according to an embodiment of the present invention.

[0045] Figure 2 This is a schematic diagram comparing the one-dimensional Gaussian distribution bias probability density function described in this embodiment of the invention with the traditional bias probability density function.

[0046] Figure 3 This is a schematic diagram illustrating the generation of a new reference direction with Gaussian distribution bias in two-dimensional space as described in an embodiment of the present invention.

[0047] Figure 4 This is a schematic diagram illustrating the swinging exploration when encountering a collision in two-dimensional space, as described in an embodiment of the present invention. Detailed Implementation

[0048] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other.

[0049] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0050] This method improves upon the traditional RRT algorithm in the bias sampling and growth stages. In the bias sampling stage, the line connecting the current node and the target point is used as the reference direction, and the deflection angle of the new growth direction relative to the reference direction is used as the variable. A Gaussian distribution model is constructed, and the deflection angle is directly sampled. Growth is then explored in the sampled deflection direction. In the growth stage, this method introduces a swing-adaptive expansion strategy, repeatedly attempting larger deflection angles at collision points. This reuses collision point information, continuously improving the determination of reachable space and avoiding repeated exploration of invalid waypoints. This method effectively improves the problems of non-directional expansion and random search in RRT, enhancing the robot's path planning ability in confined spaces while improving growth efficiency.

[0051] In this embodiment of the invention, a two-dimensional planar robot is used for path planning. It has two degrees of freedom and the planned state space is a two-dimensional space. The axis direction perpendicular to the two-dimensional plane is used as the reference direction vector, and its high-dimensional covariance matrix Σ degenerates into a one-dimensional standard deviation σ.

[0052] A robot path planning method based on an improved RRT algorithm includes the following steps:

[0053] Step 1: Initialize the robot's planning environment, plan the step size L, and the starting point X. start Target point X goal Sampling parameter σ, path tree;

[0054] Step 2, randomly sample and generate point Q new Searching for the tree with Q new The most recent sampling point is Q near Connect to Q near and X goal Thus, the reference direction V is obtained;

[0055] Step 3: Using the reference direction V as the center and the Gaussian distribution model with σ as the standard deviation, the range from -π to +π in each dimension is truncated and normalized. Then, sampling is performed according to the new probability density function to obtain the direction θ.

[0056] Step 4: Expand the sampling direction θ as the expansion direction and the robot's step size L as the expansion step size to obtain a new point Q. ext ;

[0057] Step 5: For Q ext Perform collision detection;

[0058] Step 6: If Q ext If there is no collision, add it to the path tree; if there is a collision, proceed to step 7.

[0059] Step 7: Centered on V, swing each dimension sequentially to increase the angle to grow and explore new θ. If there is still a collision, continue to try. If there is no collision, grow that point.

[0060] Step 8: Repeat steps 2 through 6 until a path is found or the maximum number of iterations is exceeded;

[0061] Specifically, the reference direction in step 2 is determined by Q. near Pointing to X from the starting point goal The direction of the vector;

[0062] Specifically, the expression for finding the nearest sampling point in step 2 is as follows:

[0063] min f=||Q new-Q near ||2

[0064] stQ near ∈Tree

[0065] Specifically, in step 3, the dimension of the direction θ is one less than the number of degrees of freedom of the robot. For a 2-DOF robot, the axis perpendicular to the two-dimensional plane is used as the reference direction vector. The components of θ represent the angle of rotation about the axis perpendicular to the plane, and the specific expression is as follows:

[0066] θ={θ1}

[0067] Specifically, for a two-degree-of-freedom robot, the probability normalization process in step 3 includes the following specific steps:

[0068] Step 3-1: Obtain the expression for the original Gaussian distribution as a one-dimensional Gaussian distribution.

[0069]

[0070] Where μ is the reference direction V, and σ is the standard deviation form obtained after the covariance matrix is ​​degenerated to one dimension;

[0071] Step 3-2: Calculate the integral of the original Gaussian distribution over all dimensions [-π, π].

[0072] C=∫p(x|μ,σ)dx x∈[-π,π]

[0073] Step 3-3: The expression for the normalized probability density function is then obtained as follows:

[0074]

[0075] Specifically, step 7 includes the following steps:

[0076] Step 7-1: In the state space of a 2-DOF robot, the expression for the V direction is:

[0077] V = {V1}

[0078] Step 7-2: When a collision occurs, increase the angle J in the first degree of freedom to obtain a new direction V′. Perform growth exploration and collision detection on this new direction. The expression for the new direction is:

[0079] V′={V1}±{J}={V1±J}

[0080] Step 7-3: If growth still fails, start again from the first degree of freedom and expand the J angle to explore, and repeat this cycle.

[0081] Step 7-4: If J reaches the limit angle, i.e. |J|>π, then stop exploring and abandon this growth.

[0082] Step 7-5: If a growth path is found in a certain direction during the exploration process, add that point to the path tree.

[0083] All techniques not described in detail in this invention are well-known technologies.

[0084] Those skilled in the art will recognize that the units and method steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of both. To clearly illustrate the interchangeability of hardware and software, the components and steps of the various examples have been generally described in terms of functionality in the foregoing description. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementations should not be considered beyond the scope of this invention.

[0085] In the several embodiments provided in this application, it should be understood that the disclosed methods and systems can be implemented in other ways. For example, the division of units described above is merely a logical functional division, and in actual implementation, there may be other division methods. For instance, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. The aforementioned units may or may not be physically separated. The components shown as units may or may not be physical units, that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of the embodiments of the present invention according to actual needs.

[0086] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention, and they should all be covered within the scope of the claims and specification of the present invention.

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

Claims

1. A robot path planning method based on an improved RRT algorithm, characterized in that, Includes the following steps: Initialize the robot's planning environment, set the robot's step size L, and the robot's starting point. The robot's target point Sampling parameters The robot's path tree, where the path tree includes paths from the starting point... To the target point The path; Randomly generate sampling points Find the path tree with the sampling point The nearest sampling point And take the sampling point in the path tree that is closest to the sampling point as the starting point. The endpoint is the robot's target point. Thus, the reference direction V is obtained; For a robot with n degrees of freedom, n-1 linearly independent direction vectors are selected as reference directions to obtain the orientation. It is a multidimensional vector, whose dimension is the number of degrees of freedom of the robot minus one, and The component represents the angle of rotation about n-1 reference direction vectors, and the specific expression is: ； Generate with reference direction V as the center, and The model is a Gaussian distribution model with a covariance matrix, and then the Gaussian distribution model is subjected to internal analysis in each dimension. arrive Range truncation and normalization processing include: The expression for the original Gaussian distribution is: ； in, With reference direction V, for The covariance matrix of dimension; Find the original Gaussian distribution in all dimensions Integrals: ； The expression for the normalized probability density function is then: ； Using the sampling direction θ as the expansion direction and the robot's step size L as the expansion step size, the new point is obtained. Perform collision detection on the new point. If the new point has no collision, add it to the path tree. If the new point collides... The new point The collision is centered on the reference direction V, and each dimension is oscillated sequentially, increasing the set angle to obtain new values. Then, for the obtained The new point is obtained by combining the robot's step size L with the expansion. Perform collision detection; if no collision is found, use the newly obtained point. Growth is underway, and attempts will continue if collisions occur. Specifically, this includes: In the state space of an n-DOF robot, the expression for the V direction is: ； When new points Upon encountering a collision, increase the first degree of freedom. A new direction is obtained from the angle. To explore growth and detect collisions, the expression for the new direction is: ； If a collision still occurs and growth fails, then start expanding again from the first degree of freedom. Explore from different angles, and repeat in this cycle; like If so, stop exploring and abandon that growth attempt; If a growth path is found in a certain direction during the exploration process, then that point is added to the path tree. Repeat the above steps until the starting point is found. To the target point The path may exceed the preset number of iterations.

2. The robot path planning method based on the improved RRT algorithm according to claim 1, characterized in that: The path-finding tree is related to the sampling point The nearest sampling point The expression is: ； 。 3. An electronic device, comprising a processor and a memory communicatively connected to the processor and used for storing processor-executable instructions, characterized in that: The processor is used to execute the robot path planning method based on the improved RRT algorithm as described in claim 1 or 2.

4. A server, characterized in that: It includes at least one processor and a memory communicatively connected to the processor, the memory storing instructions executable by the at least one processor, the instructions being executed by the processor to cause the at least one processor to perform a robot path planning method based on an improved RRT algorithm as described in claim 1 or 2.

5. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by the processor, it implements the robot path planning method based on the improved RRT algorithm as described in claim 1 or 2.

Images