CN110275528B - Improved path optimization method for RRT algorithm - Google Patents
Improved path optimization method for RRT algorithm Download PDFInfo
- Publication number
- CN110275528B CN110275528B CN201910482275.6A CN201910482275A CN110275528B CN 110275528 B CN110275528 B CN 110275528B CN 201910482275 A CN201910482275 A CN 201910482275A CN 110275528 B CN110275528 B CN 110275528B
- Authority
- CN
- China
- Prior art keywords
- waypoint
- newi
- new
- goal
- init
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 49
- 238000005457 optimization Methods 0.000 title claims abstract description 18
- 238000001914 filtration Methods 0.000 claims abstract description 7
- 101100273916 Schizosaccharomyces pombe (strain 972 / ATCC 24843) wip1 gene Proteins 0.000 claims description 29
- 101100460203 Schizosaccharomyces pombe (strain 972 / ATCC 24843) new2 gene Proteins 0.000 claims description 24
- 238000009499 grossing Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 3
- 230000007547 defect Effects 0.000 description 3
- 230000003044 adaptive effect Effects 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 230000004888 barrier function Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000002457 bidirectional effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000037361 pathway Effects 0.000 description 1
- 230000002035 prolonged effect Effects 0.000 description 1
- 238000004904 shortening Methods 0.000 description 1
- 230000002123 temporal effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
- G05D1/0289—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling with means for avoiding collisions between vehicles
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
- G05D1/0291—Fleet control
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
The invention provides an improved path optimization method aiming at an RRT algorithm. The method introduces a filtering method and a self-adaptive expansion method of invalid waypoints into the RRT algorithm, effectively and quickly fills the local minimum value, prevents excessive searching of the configuration space, continuously improves reachable space information by reconstructing boundary points in the joint space, avoids repeated expansion of invalid waypoints, and thus can improve searching efficiency and shorten time. The method can enable the path planning algorithm to jump out of the local minimum area and to approach the target area quickly.
Description
Technical Field
The invention belongs to the field of path planning of industrial robots, and particularly relates to a path optimization method which introduces a filtering method and a self-adaptive expansion method of invalid waypoints aiming at an RRT algorithm.
Background
With the development of modern manufacturing industry, industrial robots have been widely used in many fields, such as integrated circuits, automobile 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, the conventional online and offline programming methods require a lot of time and effort, and cannot ensure satisfactory results; therefore, autonomous road force planning for industrial robots has become a current urgent need.
For many years, robot path planning has gained much attention in robot research, and its related algorithms are also comprehensive. Related planning methods include a probabilistic roadmap method, an artificial potential field method, a fast-exploration random tree (RRT) and the like. In contrast to trajectory optimization, path planning is a geometric description of the robot motion, for which it 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 to an undetected area in a large area and finally reaches a target state. It has probabilistic completeness, is easy to implement and is suitable for solving high-dimensional problems. However, the RRT algorithm slowly searches the configuration space uniformly. Some improved algorithms based on the RRT algorithm may improve the search efficiency to some extent, but they are easily trapped in local minima. Therefore, when the robot manipulator arm faces a relatively narrow operating environment, the original RRT algorithm does not solve the road stiffness planning problem well. Most of the research based on the RRT algorithm focuses on the algorithm itself, which improves the efficiency of the algorithm to varying degrees. However, they do not integrate particularly tightly with specific application environments and process characteristics. For example, path planning for robotic manipulators in complex environments.
In a path planning method for improving the RRT algorithm disclosed in 2018, 8, 24 and 8, CN 108444489A, a random tree is randomly expanded when meeting an obstacle in the expansion process; when the obstacle is not met, introducing a target gravity strategy to correct the expansion direction of the random tree; and introducing a bidirectional expansion method, and respectively expanding from a starting point and a target point. The method has the following defects:
(1) the problem that the random tree is repeated to the same waypoint for many times is not considered in the expansion process of the random tree, so that the path expansion efficiency is greatly reduced;
(2) whether the waypoint collides with an obstacle in the future expansion process is not considered in the expansion process of the random tree, and the path expansion time is prolonged if the waypoint collides with the obstacle;
in a three-dimensional space multi-target path planning method combining an RRT and an ant colony algorithm disclosed in 2019, 1, 15, chinese patent application publication (CN 109211242a), a linear distance between target points is used as an initial path cost, a multi-target path planning problem in a three-dimensional space is converted into a traveler problem of a known path, and then the traveler problem is optimized and solved by an ant colony algorithm. The method has the following defects: efficiency in the expansion process is not considered, and a large number of invalid waypoints are expanded to reduce the efficiency of path planning.
Therefore, the improved path optimization method aiming at the RRT algorithm has important research significance and application value.
Disclosure of Invention
Aiming at the defects of the RRT algorithm, the invention provides a filtering method and a self-adaptive expansion method for introducing invalid waypoints to optimize the method, and aims to improve the efficiency of the RRT algorithm.
The invention aims to realize the purpose, the invention provides an improved path optimization method aiming at an RRT algorithm, and a filtering method and a self-adaptive expansion method of invalid waypoints are introduced into the RRT algorithm to improve the path searching efficiency, and the specific steps are as follows:
step 1, parameter setting
Initializing the robot to expand step length p 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, 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 route point with the minimum Euclidean distance is recorded as the minimum route point X neari 1,2, n, said 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 the connecting line A has an obstacle, the step returns to the step 2.1, and the exploration waypoint X is reselected 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 conflict, return to 2.3, and reselect a new waypoint X newi ；
If the eta value 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 ) The 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 waypoint with waypoint X by a straight line new(j-1) Obtaining a smooth path (X) init ，X new1 ，X new2 ，...，X new(j-1) ) (ii) a M is a positive integer, and m is less than or equal to n;
step 4.2, first from the path queue (X) init ，X new1 ，X new2 ，...，X newn ，X goal ) The smooth path (X) obtained in step 4.1 is deleted init ，X new1 ，X new2 ，...，X new(j-1) ) Obtaining a remnant 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.
Preferably, the threshold τ is P/3.
Preferably, the euclidean distance is a straight line distance between two waypoints, and the calculation formula is as follows:
wherein D (p) 1 ，p 2 ) For any waypoint p 1 And an arbitrary waypoint p 2 Euclidean distance of (x) 1 ，y 1 ) For any waypoint p 1 The plane coordinates of (a); (x) 2 ，y 2 ) Arbitrary waypoint p ２ The plane coordinates of (a).
Compared with the prior art, the invention has the beneficial effects that:
the invention can effectively and quickly fill the local minimum value and prevent excessive searching of the configuration space, and can continuously improve the reachable space information and avoid repeated expansion of invalid waypoints by reconstructing boundary points in the joint space, thereby improving the searching efficiency and shortening the time. The method can enable the path planning algorithm to jump out of the local minimum area and to approach the target area quickly.
Drawings
Fig. 1 is a flow chart of the improved path optimization method for the RRT algorithm of the present invention.
Fig. 2 is a schematic diagram of invalid waypoints in the path expansion of the present invention.
Detailed Description
The technical scheme of the invention is clearly and completely described below with reference to the accompanying drawings.
Fig. 1 is a flow chart of the improved path optimization method for the RRT algorithm of the present invention. As can be seen from fig. 1, the present invention provides an improved path optimization method for an RRT algorithm, where a filtering method of invalid waypoints and an 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 robot to expand step length p and setting an initial waypoint X init Destination waypoint X goal And a threshold τ. The threshold value tau is P/3.
Step 2, setting a random tree T to expand to a target waypoint X goal The required expansion times are n, that is, the random tree T is expanded to the target waypoint X by 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 ，i＝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 way point X in the random tree T randi The route point with the minimum Euclidean distance is recorded as the minimum route point X neari I =1, 2,. n, the free space C free The robot is provided with a safe space without obstacles in an obstacle avoidance environment.
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 line a, step 2.3 is performed.
Step 2.3, randomly selecting a waypoint mark at any position on the connecting line AIs a new waypoint X newi 。
Step 2.4, judge the new waypoint X newi Whether the filtering condition of the invalid waypoint is satisfied. Specifically, the new waypoint X is recorded 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 Euclidean distance D (X) therebetween 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 Is a valid waypoint, step 2.5 is performed.
Step 2.5, judge the new waypoint X newi Whether the conditions for adaptive expansion are satisfied. In particular, 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 will occur with future extensions, step 2.6 is performed.
The collision index is an index that causes the robot manipulator arm to collide with all edges connected to the waypoint. New waypoint X new1 The smaller the kappa value of, the new waypoint X new1 The greater the likelihood of successful expansion. In the present invention, let κ be 0.8.
The state value of the waypoint is a parameter for measuring whether the waypoint can be freely expanded or not. In the expansion process of the random tree T, if the initial values of the state values η of all the waypoints are all 0, it means that all the waypoints can be freely expanded. Supposing that r edges are connected to a waypoint q currently, if the sub-waypoint subjected to collision detection q collides with an obstacle, the eta value of the waypoint q is increased
If the next sub-waypoint does not collide with the barrier through collision detection, the eta value of the q waypoint is reduced
In the present invention, a new waypoint X is set newi Is 0, i.e. the new waypoint X newi Can be freely expanded.
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 ) The 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 at the i +1 th time p(i+1) Substituting the current parent waypoint X pi And performing next expansion of the random tree T.
(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 proceeds to 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 ) And carrying out smoothing treatment.
Step (ii) of4.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, and m is less than or equal to n.
Step 4.2, first from the path queue (X) init ，X new1 ，X new2 ，...，X newn ，X goal ) The smooth path (X) obtained in step 4.1 is deleted 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, a successive attempt is made to connect the remnant path queues (X) as per step 4.1 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.
In the above step, the euclidean distance is a linear distance between two waypoints, and the calculation formula is as follows:
wherein D (p) 1 ，p 2 ) For any waypoint p 1 And an arbitrary waypoint p 2 Euclidean distance of (x) 1 ，y 1 ) For any waypoint p 1 The plane coordinates of (a); (x) 2 ，y 2 ) Arbitrary waypoint p 2 The plane coordinates of (a).
Fig. 2 is a schematic diagram of invalid waypoints in the path expansion of the present invention. As can be seen in FIG. 2, from the current parent waypoint X pi Starting from, passing through the minimum waypoint X neari Find out new waypoint X newi The pathway of (1). Because D (X) newi ，X pi )＞D(X newi ，X neari ) So the new waypoint is an invalid waypoint. Also, with 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) 1 ，p 2 ) For any waypoint p 1 And an arbitrary waypoint p 2 Euclidean distance of (x) 1 ，y 1 ) For any waypoint p 1 The plane coordinates of (a); (x) 2 ，y 2 ) Arbitrary waypoint p 2 The plane coordinates of (a).
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910482275.6A CN110275528B (en) | 2019-06-04 | 2019-06-04 | Improved path optimization method for RRT algorithm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910482275.6A CN110275528B (en) | 2019-06-04 | 2019-06-04 | Improved path optimization method for RRT algorithm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110275528A CN110275528A (en) | 2019-09-24 |
| CN110275528B true CN110275528B (en) | 2022-08-16 |
Family
ID=67961956
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910482275.6A Active CN110275528B (en) | 2019-06-04 | 2019-06-04 | Improved path optimization method for RRT algorithm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110275528B (en) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111506073B (en) * | 2020-05-07 | 2022-04-19 | 安徽理工大学 | Mobile robot path planning method based on fusion membrane calculation and RRT (remote distance transform) |
| CN111752281A (en) * | 2020-07-13 | 2020-10-09 | 浪潮软件股份有限公司 | Mobile robot path planning method and system based on improved RRT algorithm |
| CN112987799B (en) * | 2021-04-16 | 2022-04-05 | 电子科技大学 | Unmanned aerial vehicle path planning method based on improved RRT algorithm |
| US12104910B2 (en) | 2022-02-23 | 2024-10-01 | Toyota Research Institute, Inc. | Systems and methods for informable multi-objective and multi-direction rapidly exploring random tree route planning |
| CN115950439B (en) * | 2023-03-15 | 2023-06-02 | 季华实验室 | Bidirectional RRT path planning method and device, electronic equipment and storage medium |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2017104775A1 (en) * | 2015-12-14 | 2017-06-22 | Mitsubishi Electric Corporation | Method for controlling vehicle and control system of vehicle |
| CN107883961A (en) * | 2017-11-06 | 2018-04-06 | 哈尔滨工程大学 | A kind of underwater robot method for optimizing route based on Smooth RRT algorithms |
| CN108507575A (en) * | 2018-03-20 | 2018-09-07 | 华南理工大学 | A kind of unmanned boat sea paths planning method and system based on RRT algorithms |
| CN108762270A (en) * | 2018-06-01 | 2018-11-06 | 上海理工大学 | The two-way rapidly-exploring random tree modified two-step method planning algorithm of changeable probability |
| CN109445444A (en) * | 2018-12-25 | 2019-03-08 | 同济大学 | A kind of barrier concentrates the robot path generation method under environment |
| CN109542117A (en) * | 2018-10-19 | 2019-03-29 | 哈尔滨工业大学（威海） | Based on the submarine navigation device Rolling Planning algorithm for improving RRT |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108444489A (en) * | 2018-03-07 | 2018-08-24 | 北京工业大学 | A kind of paths planning method improving RRT algorithms |
-
2019
- 2019-06-04 CN CN201910482275.6A patent/CN110275528B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2017104775A1 (en) * | 2015-12-14 | 2017-06-22 | Mitsubishi Electric Corporation | Method for controlling vehicle and control system of vehicle |
| CN107883961A (en) * | 2017-11-06 | 2018-04-06 | 哈尔滨工程大学 | A kind of underwater robot method for optimizing route based on Smooth RRT algorithms |
| CN108507575A (en) * | 2018-03-20 | 2018-09-07 | 华南理工大学 | A kind of unmanned boat sea paths planning method and system based on RRT algorithms |
| CN108762270A (en) * | 2018-06-01 | 2018-11-06 | 上海理工大学 | The two-way rapidly-exploring random tree modified two-step method planning algorithm of changeable probability |
| CN109542117A (en) * | 2018-10-19 | 2019-03-29 | 哈尔滨工业大学（威海） | Based on the submarine navigation device Rolling Planning algorithm for improving RRT |
| CN109445444A (en) * | 2018-12-25 | 2019-03-08 | 同济大学 | A kind of barrier concentrates the robot path generation method under environment |
Non-Patent Citations (1)
| Title |
|---|
| 基于RRT算法的非完整移动机器人运动规划;李加东;《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》;20140930;第33-64页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110275528A (en) | 2019-09-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110275528B (en) | Improved path optimization method for RRT algorithm | |
| CN110509279B (en) | Motion path planning method and system of humanoid mechanical arm | |
| CN111546347B (en) | Mechanical arm path planning method suitable for dynamic environment | |
| CN107234617B (en) | Obstacle avoidance path planning method guided by artificial potential field irrelevant to obstacle avoidance task | |
| CN109579854B (en) | Unmanned vehicle obstacle avoidance method based on fast expansion random tree | |
| CN108253984A (en) | A kind of method for planning path for mobile robot based on improvement A star algorithms | |
| US9044862B2 (en) | Path planning apparatus and method for robot | |
| CN104155974B (en) | Path planning method and apparatus for robot fast collision avoidance | |
| CN112683278B (en) | Global path planning method based on improved A-algorithm and Bezier curve | |
| US11919170B2 (en) | Fast method for robot path planning with obstacle avoidance | |
| Park et al. | Path planning for a robot manipulator based on probabilistic roadmap and reinforcement learning | |
| CN113858210B (en) | Mechanical arm path planning method based on hybrid algorithm | |
| CN114705196B (en) | Self-adaptive heuristic global path planning method and system for robot | |
| Huh et al. | Efficient sampling with Q-learning to guide rapidly exploring random trees | |
| CN114939872B (en) | MIRRT-Connect algorithm-based intelligent storage redundant mechanical arm dynamic obstacle avoidance motion planning method | |
| CN116652932A (en) | Mechanical arm autonomous obstacle avoidance method based on improved RRT algorithm | |
| Le et al. | Search-based planning and replanning in robotics and autonomous systems | |
| CN115179287A (en) | Path planning method of mechanical arm | |
| CN117124335B (en) | Improved RRT path planning method based on path marking backtracking strategy | |
| CN117260735A (en) | Path planning method for robot deep frame grabbing | |
| CN117029845A (en) | Improved bidirectional RRT path planning method based on greedy algorithm | |
| CN114995391B (en) | 4-order B spline curve path planning method for improving A-algorithm | |
| CN115922716A (en) | Bidirectional RRT-connect algorithm fused with process knowledge realizes rapid path planning of industrial robot | |
| Kim et al. | Efficient path planning for high-DOF articulated robots with adaptive dimensionality | |
| Jianning et al. | Beetle Antennae Search guided RRT* for path planning of GIS inspection and maintenance robot |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |