CN110275528B - Improved path optimization method for RRT algorithm - Google Patents

Abstract

The present invention provides an improved path optimization method for the RRT algorithm. This method introduces the filtering method of invalid waypoints and the adaptive expansion method in the RRT algorithm, which effectively and quickly fills the local minimum and prevents excessive search of the configuration space, and continuously improves the reachable space information by reconstructing the boundary points in the joint space. , to avoid repeated expansion of invalid waypoints, which can improve the search efficiency and shorten the time. It can make the path planning algorithm jump out of the local minimum area faster and approach the target area faster.

Description

Improved Path Optimization Method for RRT Algorithm

technical field

The invention belongs to the field of path planning of industrial robots, in particular to a path optimization method which introduces an invalid waypoint filtering method and an adaptive expansion method for the RRT algorithm.

Background technique

With the development of modern manufacturing, industrial robots have been widely used in many fields, such as integrated circuits, automotive food and other automated production lines. However, due to the wide range of applications, the programming technology of industrial robots faces many new challenges; for many complex tasks, traditional online programming and offline programming methods require a lot of time and effort, and cannot ensure satisfactory results; therefore, The autonomous road strength planning of industrial robots has become an urgent need at present.

Over the years, robot path planning has gained a lot of attention in robotics research, and its related algorithms are also very comprehensive. Related planning methods include probabilistic roadmap method, artificial potential field method, rapid exploration random tree (RRT) and so on. In contrast to trajectory optimization, path planning is a geometric description of the robot's motion, and should be defined as the generation of a geometric path without mentioning any specific temporal regularity. In most cases, path optimization precedes trajectory optimization. The pioneering form of the RRT algorithm randomly expands the tree over large areas into the unexplored region and eventually reaches the target state. It is probabilistically complete, easy to implement and suitable for solving high-dimensional problems. However, the RRT algorithm slowly performs a uniform search of the configuration space. Some improved algorithms based on the RRT algorithm can improve the search efficiency to a certain extent, but they are easily trapped in local minima. Therefore, the original RRT algorithm cannot solve the problem of road strength planning well when the robot manipulator arm faces a relatively narrow operating environment. Most of the research based on RRT algorithms focuses on the algorithms themselves, which improve the efficiency of the algorithms to varying degrees. However, they are not particularly tightly integrated with specific application environments and process characteristics. For example, for path planning of robotic manipulators in complex environments.

In a path planning method for an improved RRT algorithm disclosed in the Chinese invention patent document (CN 108444489A) on August 24, 2018, the random tree is expanded randomly when an obstacle is encountered during the expansion process; when no obstacle is encountered , the target gravity strategy is introduced to correct the expansion direction of the random tree; the bidirectional expansion method is introduced to expand from the starting point and the target point respectively. This method has the following shortcomings:

(1) The problem of repeating to the same waypoint multiple times is not considered in the expansion process of the random tree, which greatly reduces the efficiency of path expansion;

(2) In the expansion process of the random tree, it is not considered whether the waypoint will collide with the obstacle in the future expansion process. If a collision occurs, the path expansion time will be increased;

Chinese invention patent document (CN 109211242A) disclosed on January 15, 2019 in a three-dimensional space multi-objective path planning method integrating RRT and ant colony algorithm, the straight-line distance between each target point is used as the initial path cost, and the The multi-objective path planning problem in three-dimensional space is transformed into a traveling salesman problem with known paths, and then the ant colony algorithm is used to solve the traveling salesman problem optimally. The shortcomings of this method are: the efficiency problem in the expansion process is not considered, and a large number of invalid waypoints are expanded to reduce the efficiency of path planning.

Therefore, an improved path optimization method for RRT algorithm has important research significance and application value.

SUMMARY OF THE INVENTION

Aiming at the shortcomings of the RRT algorithm, the invention proposes a filtering method and an adaptive expansion method that introduce invalid waypoints to optimize it, and the purpose is to improve the efficiency of the RRT algorithm.

The object of the present invention is achieved in this way. The present invention provides a path optimization method for RRT algorithm improvement. In the RRT algorithm, a filtering method of invalid waypoints and an adaptive expansion method are introduced to improve the path search efficiency. The specific steps are as follows:

Step 1, parameter setting

Initialize the robot expansion step p, set the starting waypoint X _init , the target waypoint X _goal and the threshold τ;

Step 2, let the number of expansions required to expand the random tree T to the target waypoint X _goal is n, that is, through n expansions, the random tree T is expanded to the target waypoint X _goal , and n new waypoints are obtained. Any one of the new waypoints is denoted as a new waypoint X _newi , i=1, 2, . . . , n; taking the starting waypoint X _init as the parent waypoint of the first expansion, perform n times on the random tree T Expansion; denote any one of the n expansions as expansion i, and the parent waypoint in expansion i is the current parent waypoint X _pi , i=1, 2, .. , and the steps of one expansion are as follows:

Step 2.1, expand from the current parent waypoint X _pi , randomly select an exploration waypoint X _randi in the free space C _free , i=1, 2, .., n, and then find one in the random tree T and explore The waypoint with the smallest Euclidean distance between waypoints X _randi is denoted as the smallest waypoint X _neari , i=1, 2, .., n, the free space C _free is the safe space without obstacles in the robot obstacle avoidance environment ;

Step 2.2, make a connection between the exploration waypoint X _randi obtained in step 2.1 and the minimum waypoint X _neari and record it as connection A, and determine whether there is an obstacle on the connection A:

When there is an obstacle on the connection line A, return to step 2.1, and re-select the exploration waypoint X _randi ;

When there is no obstacle on connection A, go to step 2.3;

Step 2.3, randomly select a waypoint at any position on the connection line A and record it as a new waypoint X _newi ;

Step 2.4, record the Euclidean distance between the new waypoint _Xnewi and the current parent waypoint _Xpi as D( _Xnewi , _Xpi ), and the Euclidean distance between the new waypoint _Xnewi and the minimum waypoint _Xneari as D( X _newi , X _neari ), make the following judgments:

If D(X _newi , X _pi )>D(X _newi , X _neari ), the new waypoint X _newi is an invalid waypoint, return to step 2.3, and reselect the new waypoint X _newi ;

If D(X _newi , X _pi )≤D(X _newi , X _neari ), the new waypoint X _newi is a valid waypoint, and step 2.5 is executed;

Step 2.5, according to the relationship between the state value η of the new waypoint X _newi and the collision index κ of the new waypoint X _newi , the following judgments are made:

If η>κ, the future expansion of the new waypoint X _newi will conflict, return to 2.3, and reselect the new waypoint X _newi ;

If the value of η≤κ, the future expansion of the new waypoint X _new1 will not conflict, and step 2.6 is executed;

In step 2.6, the Euclidean distance between the new waypoint X _newi obtained in step 2.5 and the target waypoint X _goal is recorded as the Euclidean distance D(X _newi , X _goal ), and the Euclidean distance D(X _newi , X _goal ) and the set Compared with the predetermined threshold τ, it is judged whether the random tree T has been expanded to the target waypoint X _goal , that is, whether the expansion goal has been reached:

(1) If D(X _newi , X _goal )>τ, the expansion goal is not reached, update the random tree T to obtain the parent waypoint X _p(i+1) of the i+1th expansion, X _{p(i+1 )} =X _newi ;

Return to step 2.1, and substitute the i+1 th parent waypoint Xp _(i+1) into the current parent waypoint _Xpi for the next expansion of the random tree T;

(2) If D(X _newi , X _goal )≤τ, the expansion goal has been reached, the expansion of the random tree T ends, the new waypoint X _newi is recorded, and step 3 is entered;

Step 3, record the n new waypoints obtained by step 2, and take the starting waypoint X _init as the starting point and the target waypoint X _goal as the end point, and connect the n new waypoints with the starting waypoint X _init and the target waypoint. Connect the points X _goal sequentially to get the path queue (X _init , X _new1 , X _new2 ....X _newn , X _goal );

Step 4, smoothing the path queue (X _init , X _new1 , X _new2 , . . . X _newn , X _goal );

Step 4.1, let the starting point X _init be the starting point, and try to connect each way point in the path queue (X _init , X _new1 , X _new2 , . . . , X _newn , X _goal ) in turn, if in the connection process If it is found that a waypoint in the path queue (X _init , X _new1 , X _new2 , ..., X _newn , X _goal ) is not on the same straight line with the two adjacent way points before the way point, then record this way The point is an unreachable waypoint X _newj , a straight line is used to connect the starting waypoint and the waypoint X _{new (j-1)} to obtain a smooth path (X _init , X _new1 , X _new2 ,..., X _{new (j- 1)} ); j=1,2...m,m is a positive integer, m≤n;

Step 4.2, first delete the smooth path ( _Xinit , _Xnew1 , _Xnew2 ,...,X) obtained in step 4.1 from the path queue ( _Xinit , _Xnew1 , _Xnew2 ,..., _Xnewn , _Xgoal ) _new(j-1) ), to get a remaining path queue (X _new(j-1) , X _newj , ..., X _newn , X _goal ), and then take the waypoint X _new(j-1) as the starting point , try to connect each waypoint in the remaining path queue (X _new(j-1) , X _newj , ..., X _newn , X _goal ) in turn according to the method of step 4.1, and obtain a smooth path again; and so on, When the starting point is the target waypoint x _goal , a total of several smooth paths are obtained, that is, the path queue (X _init , X _new1 , X _new2 , ... X _newn , X _goal ) is smoothed into several connected straight lines, and the path optimization ends.

Preferably, the threshold τ=P/3.

Preferably, the Euclidean distance is the straight-line distance between two waypoints, and the calculation formula is as follows:

where D(p ₁ , p ₂ ) is the Euclidean distance between any waypoint p ₁ and any waypoint p ₂ , (x ₁ , y ₁ ) is the plane coordinate of any way point p ₁ ; (x ₂ , y ₂ ) is arbitrary The plane coordinates of waypoint _p2 .

Compared with the prior art, the beneficial effects of the present invention are:

The invention can effectively and quickly fill the local minimum value and prevent excessive searching of the configuration space, and by reconstructing the boundary points in the joint space, the reachable space information can be continuously improved, and the repeated expansion of invalid waypoints can be avoided, so that the search efficiency can be improved and the shortening time can be shortened. time. It can make the path planning algorithm jump out of the local minimum area faster and approach the target area faster.

Description of drawings

FIG. 1 is a flow chart of the path optimization method improved by the present invention for the RRT algorithm.

FIG. 2 is a schematic diagram of an invalid waypoint in the path extension of the present invention.

Detailed ways

The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings.

FIG. 1 is a flow chart of the path optimization method improved by the present invention for the RRT algorithm. As can be seen from Figure 1, the present invention provides a path optimization method for RRT algorithm improvement, and introduces the filtering method of invalid waypoints and the adaptive expansion method in the RRT algorithm to improve the path search efficiency, and the specific steps are as follows:

Step 1, parameter setting

Initialize the robot expansion step p, set the starting waypoint X _init , the target waypoint X _goal and the threshold τ. The threshold τ=P/3.

Step 2, let the number of expansions required to expand the random tree T to the target waypoint X _goal is n, that is, through n expansions, the random tree T is expanded to the target waypoint X _goal , and n new waypoints are obtained. Any one of the new waypoints is denoted as a new waypoint X _newi , i=1, 2, . . . , n.

Taking the starting waypoint X _init as the parent waypoint of the first expansion, the random tree T is expanded n times; any one of the n expansions is expanded as expansion i, and the parent waypoint in expansion i is the current parent way Point X _pi , i=1, 2, .., n, the steps of one expansion are as follows:

Step 2.1, expand from the current parent waypoint X _pi , randomly select an exploration waypoint X _randi in the free space C _free , i=1, 2, .., n, and then find one in the random tree T and explore The waypoint with the smallest Euclidean distance between the waypoints X _randi is denoted as the smallest waypoint X _neari , i=1, 2, .., n, the free space C _free is the safe space without obstacles in the robot obstacle avoidance environment .

Step 2.2, make a connection between the exploration waypoint X _randi obtained in step 2.1 and the minimum waypoint X _neari and record it as connection A, and determine whether there is an obstacle on the connection A:

When there is an obstacle on the connection line A, return to step 2.1, and re-select the exploration waypoint X _randi ;

When no obstacle is encountered on connection A, go to step 2.3.

Step 2.3, randomly select a waypoint at any position on the connection line A and record it as a new waypoint X _newi .

Step 2.4, determine whether the new waypoint X _newi satisfies the filtering condition of the invalid waypoint. Specifically, denote the Euclidean distance between the new waypoint X _newi and the current parent waypoint X _pi as D(X _newi , X _pi ), and the Euclidean distance between the new waypoint X _newi and the minimum waypoint X _neari D(X _newi , X _neari ), make the following judgments:

If D(X _newi , X _pi )>D(X _newi , X _neari ), the new waypoint X _newi is an invalid waypoint, return to step 2.3, and reselect the new waypoint X _newi ;

If D(X _newi , X _pi )≤D(X _newi , X _neari ), the new waypoint X _newi is a valid waypoint, and step 2.5 is executed.

Step 2.5, determine whether the new waypoint X _newi satisfies the condition of adaptive expansion. Specifically, according to the relationship between the state value η of the new waypoint X _newi and the collision index κ of the new waypoint X _newi , the following judgments are made:

If η>κ, the future expansion of the new waypoint X _newi will conflict, return to 2.3, and reselect the new waypoint X _newi ;

If the value of η≤κ, the future expansion of the new waypoint X _new1 will not conflict, and step 2.6 is executed.

The collision index is the index that causes the robot manipulator arm to collide with all edges connected to the waypoint. The smaller the κ value of the new waypoint _Xnew1 , the greater the possibility of the successful expansion of the new waypoint _Xnew1 . In the present invention, let κ=0.8.

若经碰撞检测下一个子路点没有与障碍物发生碰撞时，q路点的η值减

在本发明中，设新路点X_newi的状态值η的初始值为0，即新路点X_newi可以自由扩展。The state value of the waypoint is a parameter to measure whether the waypoint can be freely expanded. In the expansion process of the random tree T, if the initial value of the state value η of all waypoints is 0, it means that all waypoints can be expanded freely. Assuming that there are currently r edges connected to waypoint q, if the sub-waypoint of q collides with an obstacle after collision detection, the η value of q waypoint increases

If the next sub-waypoint does not collide with the obstacle after collision detection, the η value of the q-waypoint decreases

In the present invention, the initial value of the state value η of the new waypoint X _newi is set to 0, that is, the new waypoint X _newi can be freely expanded.

In step 2.6, the Euclidean distance between the new waypoint X _newi obtained in step 2.5 and the target waypoint X _goal is recorded as the Euclidean distance D(X _newi , X _goal ), and the Euclidean distance D(X _newi , X _goal ) and the set Compared with the predetermined threshold τ, it is judged whether the random tree T has been expanded to the target waypoint X _goal , that is, whether the expansion goal has been reached:

(1) If D(X _newi , X _goal )>τ, the expansion goal is not reached, update the random tree T to obtain the parent waypoint X _p(i+1) of the i+1th expansion, X _{p(i+1 )} =X _newi ;

Return to step 2.1, and substitute the i+1 th parent waypoint Xp _(i+1) into the current parent waypoint _Xpi to perform the next expansion of the random tree T.

(2) If D(X _newi , X _goal )≤τ, the expansion goal has been reached, the expansion of the random tree T is completed, the new waypoint X _newi is recorded, and step 3 is entered.

Step 3, record the n new waypoints obtained by step 2, and take the starting waypoint X _init as the starting point and the target waypoint X _goal as the end point, and connect the n new waypoints with the starting waypoint X _init and the target waypoint. The points X _goal are sequentially connected to obtain the path queue (X _init , X _new1 , X _new2 , . . . , X _newn , X _goal ).

Step 4: Smooth the path queues (X _init , X _new1 , X _new2 , . . . , X _newn , X _goal ).

Step 4.1, let the starting point X _init be the starting point, and try to connect each way point in the path queue (X _init , X _new1 , X _new2 , . . . , X _newn , X _goal ) in turn, if in the connection process If it is found that a waypoint in the path queue (X _init , X _new1 , X _new2 , ..., X _newn , X _goal ) is not on the same straight line with the two adjacent way points before the way point, then record this way The point is an unreachable waypoint X _newj , and a straight line is used to connect the starting waypoint _Xinit and the waypoint Xnew _(j-1) to obtain a smooth path (Xinit, _Xnew1 , _Xnew2 ,..., _{Xnew ( j-1)} ); j=1,2...m,m is a positive integer, m≤n.

Step 4.2, first delete the smooth path ( _Xinit , _Xnew1 , _Xnew2 ,...,X) obtained in step 4.1 from the path queue ( _Xinit , _Xnew1 , _Xnew2 ,..., _Xnewn , _Xgoal ) _new(j-1) ), get a remaining path queue (X _new(j-1) , X _newj , ..., X _newn , X _goal ), and then take the waypoint X _new(j-1) as the starting point, According to the method of step 4.1, try to connect each waypoint in the remaining path queue (X _new(j-1) , X _newj , ..., X _newn , X _goal) in turn, and get a smooth path; and so on, when When the starting point is the target waypoint x _goal , a total of several smooth paths are obtained, that is, the path queue (X _init , X _new1 , X _new2 , ... X _newn , X _goal ) is smoothed into several connected straight lines, and the path optimization ends.

In the above steps, the Euclidean distance is the straight-line distance between two waypoints, and the calculation formula is as follows:

where D(p ₁ , p ₂ ) is the Euclidean distance between any waypoint p ₁ and any waypoint p ₂ , (x ₁ , y ₁ ) is the plane coordinate of any way point p ₁ ; (x ₂ , y ₂ ) is arbitrary Planar coordinates of waypoint _p2 .

FIG. 2 is a schematic diagram of an invalid waypoint in the path extension of the present invention. As can be seen from FIG. 2 , starting from the current parent waypoint X _pi , through the minimum waypoint X _neari , a way to find the new waypoint X _newi . Since D(X _newi , X _pi )>D(X _newi , X _neari ), the new waypoint is an invalid waypoint. Also by the present invention, the path queue has been smoothed into several connected straight lines.

Claims (3)

1. A path optimization method aiming at RRT algorithm improvement is characterized in that an invalid waypoint filtering method and a self-adaptive expansion method are introduced into the RRT algorithm to improve the path search efficiency, and the specific steps are as follows:

step 1, parameter setting

Initializing the expansion step length p of the robot and setting an initial waypoint X _init Destination waypoint X _goal And a threshold τ;

step 2, setting a random tree T to expand to a target waypoint X _goal The required expansion times are n, namely the random tree T is expanded to the target waypoint X through n times of expansion _goal And obtaining n new waypoints, and marking any one of the n new waypoints as a new waypoint X _newi 1,2, ·, n; starting with the waypoint X _init As a father node of the first expansion, expanding the random tree T for n times; recording any expansion of the n expansions as an expansion i, wherein a father node in the expansion i is a current father node X _pi 1,2, n, then the step of once expanding is as follows:

step 2.1, from the current parent waypoint X _pi Begin to expand in free space C _free Randomly selecting an exploration waypoint X _randi I 1,2, n, then find a corresponding search waypoint X in the random tree T _randi The waypoint with the minimum Euclidean distance between the two waypoints is marked as the minimum waypoint X _neari ，i＝1，2，..N, the free space C _free A safe space without obstacles in the obstacle avoidance environment of the robot is provided;

step 2.2, search waypoint X obtained in step 2.1 _randi And minimum waypoint X _neari And connecting the lines and recording as a connecting line A, and judging whether an obstacle exists on the connecting line A:

when there is an obstacle on the connection line A, the step returns to the step 2.1 to reselect the exploration waypoint X _randi ；

When no obstacle is encountered on the connecting line A, executing the step 2.3;

step 2.3, randomly selecting a waypoint at any position on the connecting line A and marking the waypoint as a new waypoint X _newi ；

Step 2.4, recording new waypoint X _newi With the current parent waypoint X _pi Has a Euclidean distance D (X) between _newi ，X _pi ) New waypoint X _newi And minimum waypoint X _neari Has a Euclidean distance D (X) between _newi ，X _neari ) The following judgment is made:

if D (X) _newi ，X _pi )＞D(X _newi ，X _neari ) New waypoint X _newi If it is an invalid waypoint, returning to step 2.3 to reselect a new waypoint X _newi ；

If D (X) _newi ，X _pi )≤D(X _newi ，X _neari ) New waypoint X _newi If the route is a valid route point, executing the step 2.5;

step 2.5, according to the new waypoint X _newi State value of and new waypoint X _newi The following judgment is made for the relationship between the collision indexes κ:

if η > κ, the new waypoint X _newi Will collide, return to 2.3, reselect a new waypoint X _newi ；

If eta is less than or equal to kappa, the new waypoint X _new1 No conflict occurs with the future extension, step 2.6 is executed;

step 2.6, the new waypoint X obtained in the step 2.5 is used _newi And target waypoint X _goal The Euclidean distance therebetween is denoted as Euclidean distance D (X) _newi ，X _goal ) Will be Euclidean distance D(X _newi ，X _goal ) Comparing with a set threshold value tau, and judging whether the random tree T is expanded to the target waypoint X _goal Namely, whether the extension target is reached is judged:

(1) if D (X) _newi ，X _goal ) If the value is more than tau, the expansion target is not reached, the random tree T is updated to obtain the parent waypoint X expanded for the (i + 1) th time _p(i+1) ，X _p(i+1) ＝X _newi ；

Returning to the step 2.1, and enabling the parent waypoint X of the (i + 1) th time _p(i+1) Substituting the current parent waypoint X _pi Expanding the random tree T for the next time;

(2) if D (X) _newi ，X _goal ) Tau is less than or equal to, the expansion target is reached, the expansion of the random tree T is ended, and a new waypoint X is recorded _newi And entering step 3;

step 3, recording n new waypoints obtained in the step 2 and starting waypoint X _init Is a starting point and a target waypoint X _goal As the end point, n new waypoints and the initial waypoint X _init Destination waypoint X _goal Sequential concatenation results in a path queue (X) _init ，X _new1 ，X _new2 ....X _newn ，X _goal )；

Step 4, to the path queue (X) _init ，X _new1 ，X _new2 ，...X _newn ，X _goal ) Carrying out smoothing treatment;

step 4.1, let the starting waypoint X _init As a starting point, the connection path queue (X) is tried in turn _init ，X _new1 ，X _new2 ，...，X _newn ，X _goal ) If a path queue (X) is found during the connection process for each waypoint in the set _init ，X _new1 ，X _new2 ，...，X _newn ，X _goal ) If one of the waypoints is not on the same straight line with the two waypoints adjacent to the waypoint, the waypoint is marked as an unreachable waypoint X _newj Connecting the starting waypoints X by a straight line _init And waypoint X _new(j-1) Obtaining a smooth path (X) _init ，X _new1 ，X _new2 ，...，X _new(j-1) ) (ii) a M is a positive integer, 1,2，m≤n；

Step 4.2, first from the path queue (X) _init ，X _new1 ，X _new2 ，...，X _newn ，X _goal ) Deleting the smooth path (X) obtained in the step 4.1 _init ，X _new1 ，X _new2 ，...，X _new(j-1) ) Obtaining a remaining path queue (X) _new(j-1) ，X _newj ，...，X _newn ，X _goal ) Then by way of waypoint X _new(j-1) As a starting point, the method of step 4.1 is followed in turn to try to connect the remnant path queue (X) _new(j-1) ，X _newj ，...，X _newn ，X _goal ) Obtaining a smooth path for each waypoint in the path; and analogizing in sequence, when the starting point is the target waypoint x _goal Then, several smooth paths are obtained in total, namely a path queue (X) _init ，X _new1 ，X _new2 ，...X _newn ，X _goal ) And smoothing the data into a plurality of connected straight lines, and finishing the path optimization.

2. A method for improved path optimization for RRT algorithms according to claim 1, characterized in that the threshold τ is P/3.

3. The improved path optimization method for the RRT algorithm of claim 1, wherein the euclidean distance is a straight line distance between two waypoints, and the calculation formula is as follows:

wherein D (p) ₁ ，p ₂ ) For any waypoint p ₁ And an arbitrary waypoint p ₂ Euclidean distance of (x) ₁ ，y ₁ ) For any waypoint p ₁ The plane coordinates of (a); (x) ₂ ，y ₂ ) Arbitrary waypoint p ₂ The plane coordinates of (a).

Images