CN115946117B - Three-dimensional space path planning method - Google Patents
Three-dimensional space path planning method Download PDFInfo
- Publication number
- CN115946117B CN115946117B CN202211628076.XA CN202211628076A CN115946117B CN 115946117 B CN115946117 B CN 115946117B CN 202211628076 A CN202211628076 A CN 202211628076A CN 115946117 B CN115946117 B CN 115946117B
- Authority
- CN
- China
- Prior art keywords
- node
- new
- new node
- preselected
- path
- 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 41
- 238000012797 qualification Methods 0.000 claims abstract description 10
- 230000012010 growth Effects 0.000 claims abstract description 9
- 238000005070 sampling Methods 0.000 claims description 23
- 230000009191 jumping Effects 0.000 claims description 5
- 238000001514 detection method Methods 0.000 claims description 4
- 238000004422 calculation algorithm Methods 0.000 abstract description 50
- 230000033001 locomotion Effects 0.000 abstract description 17
- 230000008569 process Effects 0.000 abstract description 4
- 230000002457 bidirectional effect Effects 0.000 abstract description 3
- 238000013138 pruning Methods 0.000 abstract description 2
- 238000004088 simulation Methods 0.000 description 11
- 238000003801 milling Methods 0.000 description 10
- 238000010586 diagram Methods 0.000 description 8
- 230000006870 function Effects 0.000 description 8
- 230000002035 prolonged effect Effects 0.000 description 4
- 230000001133 acceleration Effects 0.000 description 3
- 230000008859 change Effects 0.000 description 3
- 238000011161 development Methods 0.000 description 3
- 108091028140 FREP Proteins 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 2
- 238000012217 deletion Methods 0.000 description 2
- 230000037430 deletion Effects 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 230000001788 irregular Effects 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 101100273916 Schizosaccharomyces pombe (strain 972 / ATCC 24843) wip1 gene Proteins 0.000 description 1
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 239000012636 effector Substances 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000007773 growth pattern Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 230000001681 protective effect Effects 0.000 description 1
Classifications
-
- 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
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
The invention discloses a three-dimensional space path planning method, a system and application, wherein the method comprises the following steps: obtaining a preselected new node according to the expansion direction of the double tree; according to the preselected new node, parent node reselection is carried out; according to the parent node reselection result, respectively growing new nodes on parent nodes of the double-tree preselected new nodes to obtain new nodes; and judging the qualification and termination of the new node to obtain a final path. The invention establishes a probability target threshold value to control the expansion direction of the new node, so that the searching process has a certain guidance; secondly, combining the idea of double trees, starting point unidirectional search is changed into starting point and ending point bidirectional search, so that the searching efficiency of the algorithm is improved; on the basis, the growth direction of the new node is enabled to avoid the obstacle and be further close to the target point through the guidance of the artificial potential field idea on the growth mode of the new node; and finally, pruning the planned path, deleting redundant points, and performing path smooth fitting by using a cubic B-spline curve, so that vibration of the mechanical arm during movement is reduced, and the path movement is more stable.
Description
Technical Field
The invention relates to a three-dimensional space path planning method, a three-dimensional space path planning system and application, and belongs to the technical field of motion planning.
Background
With the continuous development of intelligent manufacturing technology, the mechanical arm is widely applied to industries such as industrial development, space exploration, medical assistance and the like, reduces the influence of human factors on work, improves the working efficiency and brings a lot of convenience to manufacturing industry. However, the automation and the intelligent degree of the current mechanical arm still develop slowly, and in most of work occasions with narrow environments, manual teaching is needed, and workers are difficult to directly participate in the environments, so that the development of the mechanical arm is limited. Therefore, the mechanical arm is autonomous, and a feasible path is efficiently planned to become a current research hotspot.
The mechanical arm path planning refers to searching a feasible path from the initial pose to the final pose while avoiding the obstacle. Scholars have made a great deal of research on mechanical arm path planning algorithms, and the ideas of the algorithms are mainly divided into three categories. Firstly, path planning is carried out by using a heuristic algorithm, mainly comprising an A-type algorithm, an ant colony algorithm, a particle swarm algorithm and the like, wherein the heuristic algorithm is relatively intuitive and easy to modify, and can give an optimal solution in a short time, but has instability, and the performance depends on specific problems and experience of a designer; secondly, planning a potential field idea, mainly a manual potential field method, is characterized by a feedback control strategy, has certain robustness on control and sensing errors, but is easy to fall into a local minimum value and does not necessarily obtain an optimal solution; thirdly, the sampling idea is used for planning, and the method is mainly applicable to a high-dimensional space by a rapid random tree algorithm (Rapidly-Exploring Random Trees) and a probability map method (Probailistic Roadmap Method), and the completeness is replaced by probability completeness, so that the searching efficiency is improved, but the path tortuosity degree is larger and is not necessarily an optimal value. Therefore, it is necessary to design a good-quality planning method to realize three-dimensional space path planning.
Disclosure of Invention
The invention provides a three-dimensional space path planning method, a three-dimensional space path planning system and application, which realize three-dimensional space path planning and can be further effectively used for planning a work path of a manipulator for placing a workpiece.
The technical scheme of the invention is as follows:
According to an aspect of the present invention, there is provided a three-dimensional space path planning method including: obtaining a preselected new node according to the expansion direction of the double tree; the double Tree comprises a starting point Tree1 and a final point Tree2; according to the preselected new node, parent node reselection is carried out; according to the parent node reselection result, respectively growing new nodes on parent nodes of the double-tree preselected new nodes to obtain new nodes; and judging the qualification and termination of the new node to obtain a final path.
The method for obtaining the pre-selected new node according to the expansion direction of the double tree comprises the following steps: when the first iteration N=1, if the random probability point is larger than or equal to the target bias threshold value, performing one step expansion by taking the first node of the opposite side tree as the sampling direction point x rand to obtain an Nth preselected new node x new; otherwise, randomly searching a sampling direction point x rand in the initialized three-dimensional space to perform a step expansion to obtain an Nth preselected new node x new; when the iteration number N is greater than 1, if the random probability point is greater than or equal to the target bias threshold value, performing one step expansion by taking the N new node of the opposite side tree as the sampling direction point x rand to obtain the N pre-selected new node x new; otherwise, randomly searching a sampling direction point x rand in the initialized three-dimensional space to perform a step expansion to obtain an Nth preselected new node x new.
And the parent node reselection is performed according to the pre-selected new node, which comprises the following steps: respectively taking a preselected new node x new of a starting point Tree1 and a preselected new node x new of a destination point Tree2 as a circle center, and taking r as a radius to make a circle, and finding out all points existing in the my Tree in the circle; and (4) parent node reselection is carried out according to the points found in the circle: if the number of points found in the circle is greater than 2, constructing a reselection father node set; judging whether the path cost from any point in the parent node set to the preselected new node is smaller than the path cost from the original parent node to the preselected new node; if any point exists to make the path cost small, taking the point with the small path cost as a reselection father node of the preselected new node; if not, the parent node of the preselected new node is unchanged; designating the reselection parent node with the smallest path as a minimum parent node x min, and connecting the minimum parent node with a preselected new node; the node in the reselection father node set is a point which is found in a circle and is except a starting point and a preselected new node original father node; otherwise, the parent node of the preselected new node is unchanged.
According to the parent node reselection result, respectively carrying out new node growth on the parent nodes of the double-tree preselected new nodes to obtain new nodes, including: the method comprises the steps of introducing an artificial potential field to a father node, calculating an attractive force vector F att, a repulsive force vector F rep and a vector F rand pointing to a preselected new node of the father node, calculating a sum vector F of the attractive force vector and the repulsive force vector, summing the vector F with a vector F rand pointing to the preselected new node to obtain a new vector F1, expanding the father node along the new vector direction F1 by a step length to obtain an Nth new node Xnew, and replacing the preselected new node x new with the new node Xnew.
The step of judging the qualification and termination of the new node to obtain a final path comprises the following steps: judging whether a connection line between a father node of the new node and the new node Xnew collides with an obstacle or not according to the generated new node Xnew: if collision occurs, deleting the new node and jumping to the 'according to the expansion direction of the double tree, obtaining a preselected new node' until a new node Xnew without collision is found, otherwise, adding the new node into the point set of each tree; judging whether the new nodes generated by the two trees are the same, if so, ending the iteration, finding a final path, otherwise, adding 1 to the iteration times, and then executing the step of obtaining a preselected new node according to the expansion direction of the double trees.
According to another aspect of the present invention, there is provided a three-dimensional space path planning system including: the first obtaining module is used for obtaining a preselected new node according to the expansion direction of the double tree; the double Tree comprises a starting point Tree1 and a final point Tree2; the reselection module is used for reselecting the father node according to the preselected new node; the second obtaining module is used for respectively growing new nodes on the father nodes of the double-tree preselected new nodes according to the father node reselection result to obtain the new nodes; and the third obtaining module is used for judging the qualification and termination of the new node to obtain a final path.
According to another aspect of the present invention, there is provided a processor for running a program, wherein the program when run performs any one of the above three-dimensional space path planning methods.
According to another aspect of the present invention, there is provided an application of a three-dimensional space path planning method for planning a work path of a robot arm for placing a workpiece, including any one of the above three-dimensional space path planning methods, further including: deleting redundant points according to the final path to obtain a shortest path; performing curve fitting according to the shortest path to obtain a gentle path; and performing collision detection according to the gentle curve path to obtain a collision-free path.
And deleting redundant points according to the final path to obtain a shortest path, wherein the method comprises the following steps of: according to the final path, using a redundant point continuous deleting strategy, and if a point T k (k > 1) exists, so that the connection line between T 1 and T k is not intersected with the obstacle, and the connection line between T 1 and T k+1 is intersected with the obstacle, removing all points in the middle of T 1 and T k, and directly connecting the points T 1 and T k; and continuing to delete the redundant points from the T k until all the path points are traversed, and removing all the redundant points.
And performing three times of uniform B spline curve fitting on the path.
The beneficial effects of the invention are as follows: the invention establishes a probability target threshold value to control the expansion direction of the new node, so that the searching process has a certain guidance; secondly, combining the idea of double trees, starting point unidirectional search is changed into starting point and ending point bidirectional search, so that the searching efficiency of the algorithm is improved; on the basis, the growth direction of the new node is enabled to avoid the obstacle and be further close to the target point through the guidance of the artificial potential field idea on the growth mode of the new node; and finally, pruning the planned path, deleting redundant points, and performing path smooth fitting by using a cubic B-spline curve, so that vibration of the mechanical arm during movement is reduced, and the path movement is more stable. In addition, path planning experiments of different algorithms under different environments are compared for a plurality of times, the algorithm of the invention is proved to be efficient, a simulation experiment platform for placing a workpiece by the mechanical arm is built according to a real physical scene, the path planning simulation experiment of the mechanical arm improvement algorithm is carried out, and the experimental result proves the applicability of the algorithm of the invention.
Drawings
FIG. 1 is a schematic flow chart of an embodiment of the present invention;
FIG. 2 is a bi-directional expansion diagram;
FIG. 3 is a diagram of a redrawing rerouting process;
FIG. 4 is a diagram of a new node growth pattern;
FIG. 5 is a comparison of a first layout environment algorithm;
FIG. 6 is a comparison of a second layout environment algorithm;
FIG. 7 is a comparison of a third layout environment algorithm;
FIG. 8 is a schematic diagram of a redundant point deletion policy;
FIG. 9 is a robot arm placement work path of the present invention;
Fig. 10 is a graph showing the change in the angle of each joint, the angular velocity of each joint, and the angular acceleration of each joint.
Detailed Description
The invention will be further described with reference to the drawings and examples, but the invention is not limited to the scope.
Example 1: a three-dimensional spatial path planning method, comprising: obtaining a preselected new node according to the expansion direction of the double tree; the double Tree comprises a starting point Tree1 and a final point Tree2; according to the preselected new node, parent node reselection is carried out; according to the parent node reselection result, respectively growing new nodes on parent nodes of the double-tree preselected new nodes to obtain new nodes; and judging the qualification and termination of the new node to obtain a final path.
Optionally, the obtaining a pre-selected new node according to the expansion direction of the dual tree includes: when the first iteration N=1, if the random probability point is larger than or equal to the target bias threshold value, performing one step expansion by taking the first node of the opposite side tree as the sampling direction point x rand to obtain an Nth preselected new node x new; otherwise, randomly searching a sampling direction point x rand in the initialized three-dimensional space to perform a step expansion to obtain an Nth preselected new node x new; when the iteration number N is greater than 1, if the random probability point is greater than or equal to the target bias threshold value, performing one step expansion by taking the N new node of the opposite side tree as the sampling direction point x rand to obtain the N pre-selected new node x new; otherwise, randomly searching a sampling direction point x rand in the initialized three-dimensional space to perform a step expansion to obtain an Nth preselected new node x new.
Optionally, the performing parent node reselection according to the pre-selected new node includes: respectively taking a preselected new node x new of a starting point Tree1 and a preselected new node x new of a destination point Tree2 as a circle center, and taking r as a radius to make a circle, and finding out all points existing in the my Tree in the circle; and (4) parent node reselection is carried out according to the points found in the circle: if the number of points found in the circle is greater than 2, constructing a reselection father node set; judging whether the path cost from any point in the parent node set to the preselected new node is smaller than the path cost from the original parent node to the preselected new node; if any point exists to make the path cost small, taking the point with the small path cost as a reselection father node of the preselected new node; if not, the parent node of the preselected new node is unchanged; designating the reselection parent node with the smallest path as a minimum parent node x min, and connecting the minimum parent node with a preselected new node; the node in the reselection father node set is a point which is found in a circle and is except a starting point and a preselected new node original father node; otherwise, the parent node of the preselected new node is unchanged.
Optionally, according to the parent node reselection result, growing new nodes on parent nodes of the dual-tree preselected new nodes respectively to obtain new nodes, including: the method comprises the steps of introducing an artificial potential field to a father node, calculating an attractive force vector F att, a repulsive force vector F rep and a vector F rand pointing to a preselected new node of the father node, calculating a sum vector F of the attractive force vector and the repulsive force vector, summing the vector F with a vector F rand pointing to the preselected new node to obtain a new vector F1, expanding the father node along the new vector direction F1 by a step length to obtain an Nth new node Xnew, and replacing the preselected new node x new with the new node Xnew.
Optionally, the determining the eligibility and the ending property of the new node to obtain a final path includes: judging whether a connection line between a father node of the new node and the new node Xnew collides with an obstacle or not according to the generated new node Xnew: if collision occurs, deleting the new node and jumping to the 'according to the expansion direction of the double tree, obtaining a preselected new node' until a new node Xnew without collision is found, otherwise, adding the new node into the point set of each tree; judging whether the new nodes generated by the two trees are the same, if so, ending the iteration, finding a final path, otherwise, adding 1 to the iteration times, and then executing the step of obtaining a preselected new node according to the expansion direction of the double trees.
Example 2: a three-dimensional space path planning system, comprising: the first obtaining module is used for obtaining a preselected new node according to the expansion direction of the double tree; the double Tree comprises a starting point Tree1 and a final point Tree2; the reselection module is used for reselecting the father node according to the preselected new node; the second obtaining module is used for respectively growing new nodes on the father nodes of the double-tree preselected new nodes according to the father node reselection result to obtain the new nodes; and the third obtaining module is used for judging the qualification and termination of the new node to obtain a final path.
Example 3: a processor for running a program, wherein the program when run performs the three-dimensional space path planning method of any one of the above.
Example 4: as shown in fig. 1 to 10, an application of a three-dimensional space path planning method is used for planning a working path of a workpiece placed by a mechanical arm, and the three-dimensional space path planning method includes any one of the above steps: deleting redundant points according to the final path to obtain a shortest path; performing curve fitting according to the shortest path to obtain a gentle path; and performing collision detection according to the gentle curve path to obtain a collision-free path.
Optionally, deleting the redundant point according to the final path to obtain the shortest path, including: according to the final path, using a redundant point continuous deleting strategy, and if a point T k (k > 1) exists, so that the connection line between T 1 and T k is not intersected with the obstacle, and the connection line between T 1 and T k+1 is intersected with the obstacle, removing all points in the middle of T 1 and T k, and directly connecting the points T 1 and T k; and continuing to delete the redundant points from the T k until all the path points are traversed, and removing all the redundant points.
Optionally, three uniform B-spline curve fits are performed on the path.
Further, the following alternative implementation procedure is given:
(1) Initializing a three-dimensional space, establishing an obstacle information point Obs, initializing a starting point position x start, adding a starting point Tree1 as a first node of the starting point Tree1, initializing a target position x goal, adding an end point Tree2 as a first node of the end point Tree2, iterating a step length x stepsize, a target bias threshold value x value, an artificial potential field gravitation coefficient xi=0.01, a repulsive force coefficient xi r =200, a repulsive force field acting radius d min =100, and an artificial potential field iterating step length lambda=3; the number of initialization iterations n=1 and N takes a positive integer.
(2) The method comprises the steps of simultaneously guiding the expansion direction of the double Tree by adopting a probability bias mode for a starting point Tree1 and a terminal point Tree 2:
when iterating for the first time, the target bias probability formula of the starting point Tree1 sampling point x rand1 is as follows:
the target bias probability formula for the end Tree2 sampling point xrand is:
And when the iteration number is more than or equal to 2, replacing sampling target points of the two trees. When the starting point Tree1 is sampled, when the random probability point prandom is larger than or equal to the bias threshold value, changing the target sampling point into the latest node of the Tree2 point set; the terminal Tree Tree2 is the same, when the random probability point is larger, the target sampling point is the latest node in the Tree1 point set; the formula is:
(3) Respectively taking a preselected new node x new of a starting point Tree1 and a preselected new node x new of a destination point Tree2 as a circle center, and taking r as a radius to make a circle, and finding out all points existing in the my Tree in the circle; and (4) parent node reselection is carried out according to the points found in the circle: r is typically 2 to 3 times the step size, in this example 2 times the step size.
If the number of points found in the circle is greater than 2, constructing a reselection father node set; judging whether the path cost from any point in the parent node set to the preselected new node is smaller than the path cost from the original parent node to the preselected new node; if any point exists to make the path cost small, taking the point with the small path cost as a reselection father node of the preselected new node; if not, the parent node of the preselected new node is unchanged; designating the reselection parent node with the smallest path as a minimum parent node x min, and connecting the minimum parent node with a preselected new node; the node in the reselection father node set is a point which is found in a circle and is except a starting point and a preselected new node original father node; it should be noted that, for any point with small path cost, it refers to a point with minimal path cost selected after all points in the parent node set are judged, if after judgment, the path cost is greater than or equal to 2 compared with the node data with small path cost from the point to the preselected new node through the original parent node, then a node is selected as the parent node; otherwise, if the number of points found in the circle is equal to 2, the parent node of the preselected new node is unchanged; taking a tree as an example, taking a preselected new node x new as a circle center and r as a radius to make a circle, finding out 4 points existing in all the my trees in the circle, constructing a reselection father node set (the total of 2 points except for a starting point and the original father node of the preselected new node, which are found in the circle, as nodes in the reselection father node set), and according to the calculation, x min in the graph enables the path cost to be smaller than the other nodes, so that the reselection father node with the minimum path is named as a minimum father node x min, and connecting the minimum father node with the preselected new node.
(4) The method comprises the steps of introducing an artificial potential field to a father node, calculating an attractive force vector F att, a repulsive force vector F rep and a vector F rand pointing to a preselected new node of the father node, calculating a sum vector F of the attractive force vector and the repulsive force vector, summing the vector F with a vector F rand pointing to the preselected new node to obtain a new vector F1, expanding the father node along the new vector direction F1 by a step length to obtain an Nth new node Xnew, and replacing the preselected new node x new with the new node Xnew.
The artificial potential field potential function is:
Uatt(x)＝1/2ξd2(x,xgoal)
The artificial potential field attraction function is:
Fatt(x)＝▽Uatt(x)＝ξd(x,xgoal)
Wherein: ζ is the gravitational constant; x and x goal are the starting point position and the target position in the prescribed region, respectively; d (x, x goal) represents the euclidean distance between two positions, which is a set distance threshold.
The repulsive potential function of the artificial potential field is:
the repulsive force function of the artificial potential field is:
Wherein: xi r is the repulsive force constant; d min is the repulsive force field action radius; Acting radius for repulsive force potential field.
The resultant force potential function of the artificial potential field method is as follows:
U(x)＝Uatt(x)+Urep(x)
the sum force function of the artificial potential field method is:
F(x)＝Fatt(x)+Frep(x)
the growth formula of the new node Xnew is:
Xnew＝xmin+xstepsize(F/||F||+λFrand/||Frand||)
Wherein: x min is the minimum parent node, x stepsize is the iteration step, λ is the iteration step of the artificial potential field, and F is the artificial potential field and vector; f rand is a pointing vector, pointing to a preselected new node x new by a parent node;
As shown in fig. 4, x new is a point obtained by expanding the point x min by one step in the x min—xrand direction; vector Frep is the repulsive force of the obstacle to the gravity parent node x min, vector Fatt is the attractive force of the target point x goal to the point x min, and F is the artificial potential field and vector of the point x min in the search area; f rand represents a vector pointing from point x min to x new. F1 is the sum vector of F and F rand; point X new represents a new node that point X min iterates once in the direction of vector F1.
(5) Judging whether a connection line between a father node of the new node and the new node Xnew collides with an obstacle or not according to the generated new node Xnew: if a collision occurs, deleting the new node and jumping to the step (2) until a new collision-free node Xnew is found, otherwise, adding the new node to the point set of the respective tree; judging whether new nodes generated by the two trees are the same, if so, ending iteration, namely x new1＝xnew2, finding a final path, otherwise, adding 1 to the iteration times, and then executing the step (2);
(6) The invention provides three different high-density abnormal obstacle avoidance environment simulation experiments by verifying the high efficiency of an algorithm. The first environment is an octagonal sphere large obstacle, the layout mode describes the situation that the large obstacle occupies four corners in a six-face search area, the fusion of the algorithm of the large obstacle existing at the starting point and the ending point is analyzed, and a simulation result diagram is shown in fig. 5; the second environment is a regular diagonal serial obstacle, the layout mode selects smaller obstacles on the straight line of the start and end connection points, and the condition that the artificial potential field easily oscillates on the connecting line of the center of the obstacle and the start and end points in the fusion algorithm is considered, and a simulation result diagram is shown in fig. 6. The third environment is an irregular layout obstacle simulation experiment, and a simulation result diagram is shown in fig. 7. Setting the search area size to [01000 ] using a MATLAB2021b platform; 01000;01000, a grid map is built for the search area, the starting point is the origin [000], the target point is [100010001000], and the obstacle color is light gray. In the following table, the invention provides comparison of four algorithms, namely a traditional fast random tree algorithm (RRT), a target probability bias bidirectional progressive optimal fast random tree algorithm (Goal-bias BRRT x), a fast random tree algorithm (APF-RRT) for guiding node growth by artificial potential field and a method (GB-APFB-RRT) of the invention; experimental results show that under four user-defined parameters, the algorithm of the invention has optimal planning time, iteration step length, node number and path length, and the accuracy and the high efficiency of the algorithm of the invention are proved.
Specifically, for the eight-corner sphere large obstacle layout, x stepsize＝5,xvalue =0.5 is taken as a motion planning chart in table 1, as shown in fig. 5, the data in table 1 are analyzed, the planning time of the algorithm is reduced by 83%, the iteration times are reduced by 97%, the number of path nodes is reduced by 39%, and the path length is reduced by 31% compared with the traditional RRT algorithm; compared with the Goal-bias BRRT algorithm, the comparison indexes are respectively reduced by 75%,94%,35% and 22%; compared with the APF-RRT algorithm, the comparison indexes are respectively reduced by 75%,94%,35% and 22%.
For comparison of different parameters of the algorithm, when the iteration step size x stepsize =5 is unchanged and the target bias threshold value x value is increased from 0.5 to 0.8, the planning time is prolonged by 58%, and the path length is increased by 15%. Keeping the target bias threshold x value unchanged, the iteration step x stepsize increases from 5 to 10, reducing the programming time by 24%, but increasing the path length by 3%.
Table 1 lays out an environmental algorithm simulation experiment result:
Specifically, for the diagonal string type obstacle layout, x stepsize＝5,xvalue =0.5 is taken as a motion planning chart in table 2, as shown in fig. 6, the data in table 2 is analyzed, the planning time of the algorithm is reduced by 82% compared with the traditional RRT algorithm, the iteration times are reduced by 91%, the number of path nodes is reduced by 32%, and the path length is reduced by 33%; compared with the Goal-bias BRRT algorithm, the comparison indexes are respectively reduced by 81%,85%,11% and 25%; compared with the APF-RRT algorithm, the comparison indexes are respectively reduced by 60%,90%, 30%.
For comparison of different parameters of the algorithm, when the iteration step size x stepsize =5 is unchanged and the target bias threshold value x value is increased from 0.5 to 0.8 formula, the planning time is prolonged by 30%, and the path length is increased by 10%. Keeping the target bias threshold x value unchanged, the iteration step x stepsize increases from 5 to 10, reducing the programming time by 39%, but increasing the path length by 4%.
Table 2 layout two environment algorithm simulation experiment results:
specifically, for irregular initial diagonal obstacle layout, x stepsize＝5,xvalue =0.5 is taken as a motion planning chart in table 3, as shown in fig. 7, the data in table 3 are analyzed, the planning time of the algorithm is reduced by 95%, the iteration times are reduced by 97%, the number of path nodes is reduced by 28%, and the path length is reduced by 33% compared with the traditional RRT algorithm; compared with the Goal-bias BRRT algorithm, the comparison indexes are respectively reduced by 75%,86%,20% and 24%; compared with the APF-RRT algorithm, the comparison indexes are respectively reduced by 53%,80%,35% and 9%;
For comparison of different parameters of the algorithm, when the iteration step size x stepsize =5 is unchanged and the target bias threshold value x value is increased from 0.5 to 0.8 formula, the planning time is prolonged by 59%, and the path length is increased by 9%. Keeping the target bias threshold x value =0.5 unchanged, the iteration step x stepsize increases from 5 to 10, reducing the planning time by 39%, but with a 1% increase in path length.
Table 3 layout three environment algorithm simulation experiment results:
In summary, according to analysis of simulation experiment results of three layout environments, compared with other three algorithms, the algorithm provided by the invention has the advantages that the planning time, the iteration times, the node number and the path length are all optimal, the path of the experiment diagram is relatively smooth, the degree of the path is higher, and the expansion of useless branch and leaf nodes is less. The programming time is the optimal value, and the iteration times are the next time, so that the running memory can be effectively reduced and the searching efficiency is improved under the same computer configuration.
For the optimal custom parameter analysis of the algorithm, when the iteration step length x stepsize =5 is unchanged, the target bias threshold is improved, the planning time is prolonged, and the path length is increased. When the target bias threshold value x value =0.5 is unchanged, the iteration step length is increased, so that the planning time can be effectively reduced, but the path length is increased, the number of inflection points is increased, the iteration step length x stepsize =5 is selected based on the optimal performance of the algorithm path, and the target bias threshold value x value =0.8.
(7) After finding the final path, there are few redundant points in the path, using a redundant point continuous deletion strategy, the path point t= { T 1,T2…Tn }, if there is a point T k (k > 1), so that the connection line between T 1 and T k does not intersect with an obstacle, and the connection line between T 1 and T k+1 intersects with an obstacle, then all points in the middle of T 1 and T k are removed, and the connection line between the points T 1 and T k is directly connected. And continuing to delete the redundant points from the T k until all the path points are traversed, and removing all the redundant points. As shown in fig. 8.
(8) After the redundant points are deleted, in order to reduce mechanical impact and terminal shake of the mechanical arm in the motion process, a path planned by an algorithm needs to be fitted with a B spline curve for three times to realize smooth processing, and the B spline curve is one or spline curves developed on the basis of a Bezier curve, so that inconvenience brought by the overall performance of the Bezier curve is overcome.
The K-th order B-spline curve equation is:
Wherein: k represents the number of times of B-spline, K+1 represents the order of curve, i represents the number of B-spline and the number of control points di is n+1.Ni, K (u) B represent i K-th B spline basis functions, and are obtained by using a Cox-deBoor recurrence formula;
Thus, the cubic non-uniform B-spline basis function is:
The cubic non-uniform B-spline curve is defined as:
p0,3(u)＝d0*N0,3(u)+d1*N1,3(u)+d2*N2,3(u)+d3*N3,3(u)u∈[0,1]
When the control point di is given, a gentle curve point of the cubic B pattern can be obtained by using the above formula.
(9) And building a motion planning scene on the Matlab platform. The starting point is the position of a manipulator grabbing actuator of a manipulator end effector, and the target point is the workpiece clamping and positioning center of a workbench of the numerical control milling machine. The obstacle environment is a numerical control milling machine protective baffle, a workbench, a clamp and a milling cutter. And (3) performing collision detection on the mechanical arm and the obstacle in a bounding box mode, and obtaining a collision-free path from a starting point to an end point by sampling and planning the ERF-900 mechanical arm in a joint space after the path obtained in the step (8). Six running poses of the mechanical arm in a path planned by the algorithm are recorded, wherein the six running poses are respectively an initial pose (a), and the mechanical arm is in a zero state; turning to a milling machine pose (b), enabling the mechanical arm to start turning movement, and enabling the tail end of the mechanical arm to point to the front of the milling machine through movement; facing the pose (c) of the milling machine protection plate, finishing steering movement of the mechanical arm, and facing the milling machine fixture at the tail end; preparing to enter a pose (d) of a protection plate of the milling machine, and starting to carry out placing movement by the mechanical arm; the mechanical arm is in a motion state of the milling machine protection plate through the milling machine protection plate pose (f); and (e) final position and posture, the mechanical arm reaches the position where the clamp clamps the workpiece, and the placement of the workpiece is completed, as shown in fig. 9.
(10) Fig. 10 shows the joint angle, joint angular velocity and joint angular acceleration change curves obtained by the motion planning of the workpiece placed by the mechanical arm. From the graph, a smooth joint angle change curve is obtained after three times of B spline curve fitting is carried out on the tail end track of the mechanical arm, and the speed and the acceleration are not suddenly changed. And a gentle track is obtained at the tail end, the shaking degree of the machine is reduced, and the smooth running and successful obstacle avoidance of the mechanical arm on a path obtained by motion planning are ensured.
The foregoing embodiment numbers of the present invention are merely for the purpose of description, and do not represent the advantages or disadvantages of the embodiments.
In the foregoing embodiments of the present invention, the descriptions of the embodiments are emphasized, and for a portion of this disclosure that is not described in detail in this embodiment, reference is made to the related descriptions of other embodiments.
While the present invention has been described in detail with reference to the drawings, the present invention is not limited to the above embodiments, and various changes can be made without departing from the spirit of the present invention within the knowledge of those skilled in the art.
Claims (6)
1. A three-dimensional space path planning method, comprising:
Obtaining a preselected new node according to the expansion direction of the double tree; the double Tree comprises a starting point Tree1 and a final point Tree2;
According to the preselected new node, parent node reselection is carried out;
According to the parent node reselection result, respectively growing new nodes on parent nodes of the double-tree preselected new nodes to obtain new nodes;
judging the qualification and termination of the new node to obtain a final path;
The method for obtaining the pre-selected new node according to the expansion direction of the double tree comprises the following steps:
When the first iteration N=1, if the random probability point is larger than or equal to the target bias threshold value, performing one step expansion by taking the first node of the opposite side tree as the sampling direction point x rand to obtain an Nth preselected new node x new; otherwise, randomly searching a sampling direction point x rand in the initialized three-dimensional space to perform a step expansion to obtain an Nth preselected new node x new;
When the iteration number N is greater than 1, if the random probability point is greater than or equal to the target bias threshold value, performing one step expansion by taking the N new node of the opposite side tree as the sampling direction point x rand to obtain the N pre-selected new node x new; otherwise, randomly searching a sampling direction point x rand in the initialized three-dimensional space to perform a step expansion to obtain an Nth preselected new node x new;
and the parent node reselection is performed according to the pre-selected new node, which comprises the following steps:
respectively taking a preselected new node x new of a starting point Tree1 and a preselected new node x new of a destination point Tree2 as a circle center, and taking r as a radius to make a circle, and finding out all points existing in the my Tree in the circle; and (4) parent node reselection is carried out according to the points found in the circle:
If the number of points found in the circle is greater than 2, constructing a reselection father node set; judging whether the path cost from any point in the parent node set to the preselected new node is smaller than the path cost from the original parent node to the preselected new node; if any point exists to make the path cost small, taking the point with the small path cost as a reselection father node of the preselected new node; if not, the parent node of the preselected new node is unchanged; designating the reselection parent node with the smallest path as a minimum parent node x min, and connecting the minimum parent node with a preselected new node; the node in the reselection father node set is a point which is found in a circle and is except a starting point and a preselected new node original father node;
Otherwise, the parent node of the preselected new node is unchanged;
according to the parent node reselection result, respectively carrying out new node growth on the parent nodes of the double-tree preselected new nodes to obtain new nodes, including:
introducing an artificial potential field idea on a father node, calculating an attractive force vector F att, a repulsive force vector F rep and a vector F rand pointing to a preselected new node of the father node, calculating a sum vector F of the attractive force vector and the repulsive force vector, summing the vector F with a vector F rand pointing to the preselected new node to obtain a new vector F1, and expanding the father node along the new vector direction F1 by a step length to obtain an Nth new node Xnew, and replacing the preselected new node x new with the new node Xnew;
the step of judging the qualification and termination of the new node to obtain a final path comprises the following steps:
Judging whether a connection line between a father node of the new node and the new node Xnew collides with an obstacle or not according to the generated new node Xnew: if collision occurs, deleting the new node and jumping to the 'according to the expansion direction of the double tree, obtaining a preselected new node' until a new node Xnew without collision is found, otherwise, adding the new node into the point set of each tree; judging whether the new nodes generated by the two trees are the same, if so, ending the iteration, finding a final path, otherwise, adding 1 to the iteration times, and then executing the step of obtaining a preselected new node according to the expansion direction of the double trees.
2. A three-dimensional space path planning system, comprising:
the first obtaining module is used for obtaining a preselected new node according to the expansion direction of the double tree; the double Tree comprises a starting point Tree1 and a final point Tree2;
The reselection module is used for reselecting the father node according to the preselected new node;
the second obtaining module is used for respectively growing new nodes on the father nodes of the double-tree preselected new nodes according to the father node reselection result to obtain the new nodes;
the third obtaining module is used for judging the qualification and termination of the new node to obtain a final path;
The method for obtaining the pre-selected new node according to the expansion direction of the double tree comprises the following steps:
When the first iteration N=1, if the random probability point is larger than or equal to the target bias threshold value, performing one step expansion by taking the first node of the opposite side tree as the sampling direction point x rand to obtain an Nth preselected new node x new; otherwise, randomly searching a sampling direction point x rand in the initialized three-dimensional space to perform a step expansion to obtain an Nth preselected new node x new;
When the iteration number N is greater than 1, if the random probability point is greater than or equal to the target bias threshold value, performing one step expansion by taking the N new node of the opposite side tree as the sampling direction point x rand to obtain the N pre-selected new node x new; otherwise, randomly searching a sampling direction point x rand in the initialized three-dimensional space to perform a step expansion to obtain an Nth preselected new node x new;
and the parent node reselection is performed according to the pre-selected new node, which comprises the following steps:
respectively taking a preselected new node x new of a starting point Tree1 and a preselected new node x new of a destination point Tree2 as a circle center, and taking r as a radius to make a circle, and finding out all points existing in the my Tree in the circle; and (4) parent node reselection is carried out according to the points found in the circle:
If the number of points found in the circle is greater than 2, constructing a reselection father node set; judging whether the path cost from any point in the parent node set to the preselected new node is smaller than the path cost from the original parent node to the preselected new node; if any point exists to make the path cost small, taking the point with the small path cost as a reselection father node of the preselected new node; if not, the parent node of the preselected new node is unchanged; designating the reselection parent node with the smallest path as a minimum parent node x min, and connecting the minimum parent node with a preselected new node; the node in the reselection father node set is a point which is found in a circle and is except a starting point and a preselected new node original father node;
Otherwise, the parent node of the preselected new node is unchanged;
according to the parent node reselection result, respectively carrying out new node growth on the parent nodes of the double-tree preselected new nodes to obtain new nodes, including:
introducing an artificial potential field idea on a father node, calculating an attractive force vector F att, a repulsive force vector F rep and a vector F rand pointing to a preselected new node of the father node, calculating a sum vector F of the attractive force vector and the repulsive force vector, summing the vector F with a vector F rand pointing to the preselected new node to obtain a new vector F1, and expanding the father node along the new vector direction F1 by a step length to obtain an Nth new node Xnew, and replacing the preselected new node x new with the new node Xnew;
the step of judging the qualification and termination of the new node to obtain a final path comprises the following steps:
Judging whether a connection line between a father node of the new node and the new node Xnew collides with an obstacle or not according to the generated new node Xnew: if collision occurs, deleting the new node and jumping to the 'according to the expansion direction of the double tree, obtaining a preselected new node' until a new node Xnew without collision is found, otherwise, adding the new node into the point set of each tree; judging whether the new nodes generated by the two trees are the same, if so, ending the iteration, finding a final path, otherwise, adding 1 to the iteration times, and then executing the step of obtaining a preselected new node according to the expansion direction of the double trees.
3. A processor for running a program, wherein the program when run performs the three-dimensional space path planning method of claim 1.
4. The application of the three-dimensional space path planning method is characterized by being used for planning a work path for placing a workpiece by a mechanical arm, and comprising the following steps: the three-dimensional space path planning method of claim 1, further comprising:
Deleting redundant points according to the final path to obtain a shortest path;
Performing curve fitting according to the shortest path to obtain a gentle path;
And performing collision detection according to the gentle curve path to obtain a collision-free path.
5. The method for three-dimensional space path planning according to claim 4, wherein said deleting redundant points according to the final path to obtain the shortest path comprises:
According to the final path, using a redundant point continuous deleting strategy, and if a point T k (k > 1) exists, so that the connection line between T 1 and T k is not intersected with the obstacle, and the connection line between T 1 and T k+1 is intersected with the obstacle, removing all points in the middle of T 1 and T k, and directly connecting the points T 1 and T k; and continuing to delete the redundant points from the T k until all the path points are traversed, and removing all the redundant points.
6. The use of the three-dimensional space path planning method according to claim 4, wherein three uniform B-spline curve fits are performed on the path.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211628076.XA CN115946117B (en) | 2022-12-16 | 2022-12-16 | Three-dimensional space path planning method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211628076.XA CN115946117B (en) | 2022-12-16 | 2022-12-16 | Three-dimensional space path planning method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115946117A CN115946117A (en) | 2023-04-11 |
| CN115946117B true CN115946117B (en) | 2024-06-11 |
Family
ID=87286830
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211628076.XA Active CN115946117B (en) | 2022-12-16 | 2022-12-16 | Three-dimensional space path planning method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115946117B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118372256B (en) * | 2024-06-21 | 2024-08-20 | 贵州大学 | Real-time obstacle avoidance and terminal pose adjustment method for redundant mechanical arm |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101339480B1 (en) * | 2012-12-14 | 2013-12-10 | 고려대학교 산학협력단 | Trajectory planning method for mobile robot using dual tree structure based on rrt |
| CN108981704A (en) * | 2018-07-13 | 2018-12-11 | 昆明理工大学 | A kind of two-way RRT paths planning method of target gravitation based on dynamic step length |
| CN110083165A (en) * | 2019-05-21 | 2019-08-02 | 大连大学 | A kind of robot paths planning method under complicated narrow environment |
| CN113084811A (en) * | 2021-04-12 | 2021-07-09 | 贵州大学 | Mechanical arm path planning method |
| CN114161416A (en) * | 2021-12-06 | 2022-03-11 | 贵州大学 | Robot path planning method based on potential function |
-
2022
- 2022-12-16 CN CN202211628076.XA patent/CN115946117B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101339480B1 (en) * | 2012-12-14 | 2013-12-10 | 고려대학교 산학협력단 | Trajectory planning method for mobile robot using dual tree structure based on rrt |
| CN108981704A (en) * | 2018-07-13 | 2018-12-11 | 昆明理工大学 | A kind of two-way RRT paths planning method of target gravitation based on dynamic step length |
| CN110083165A (en) * | 2019-05-21 | 2019-08-02 | 大连大学 | A kind of robot paths planning method under complicated narrow environment |
| CN113084811A (en) * | 2021-04-12 | 2021-07-09 | 贵州大学 | Mechanical arm path planning method |
| CN114161416A (en) * | 2021-12-06 | 2022-03-11 | 贵州大学 | Robot path planning method based on potential function |
Non-Patent Citations (3)
| Title |
|---|
| Biased Sampling Potentially Guided Intelligent Bidirectional RRT∗ Algorithm for UAV Path Planning in 3D Environment;Xiaojing Wu;Mathematical Problems in Engineering;20191119;第2019卷;全文 * |
| 六轴机械臂路径规划及孪生实现;尹庆文;中国优秀硕士学位论文全文数据库;20240415(第4（2024）期);I140-693 * |
| 动态约束采样RRT*-Connect算法的移动机器人路径规划;陈忠;机械科学与技术;20231225;12 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115946117A (en) | 2023-04-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Zhang et al. | Path planning of industrial robot based on improved RRT algorithm in complex environments | |
| CN109571466B (en) | Seven-degree-of-freedom redundant mechanical arm dynamic obstacle avoidance path planning method based on rapid random search tree | |
| Jiang et al. | Path planning for robotic manipulator in complex multi-obstacle environment based on improved_RRT | |
| CN110962130B (en) | Heuristic RRT mechanical arm motion planning method based on target deviation optimization | |
| CN108356819B (en) | Industrial mechanical arm collision-free path planning method based on improved A-x algorithm | |
| CN110231824B (en) | Intelligent agent path planning method based on straight line deviation method | |
| Sudhakara et al. | Trajectory planning of a mobile robot using enhanced A-star algorithm | |
| Dai et al. | Novel potential guided bidirectional RRT* with direct connection strategy for path planning of redundant robot manipulators in joint space | |
| CN111761582A (en) | Mobile mechanical arm obstacle avoidance planning method based on random sampling | |
| CN114147708B (en) | Mechanical arm obstacle avoidance path planning method based on improved longhorn beetle whisker search algorithm | |
| CN113858205A (en) | Seven-axis redundant mechanical arm obstacle avoidance algorithm based on improved RRT | |
| CN114939872B (en) | MIRRT-Connect algorithm-based intelligent storage redundant mechanical arm dynamic obstacle avoidance motion planning method | |
| US20210323154A1 (en) | Disassembly based assembly planning | |
| Ji et al. | E-RRT*: Path Planning for Hyper-Redundant Manipulators | |
| CN115946117B (en) | Three-dimensional space path planning method | |
| CN116572244A (en) | Mechanical arm obstacle avoidance path planning method based on RRT-FN algorithm | |
| Shao et al. | Rrt-goalbias and path smoothing based motion planning of mobile manipulators with obstacle avoidance | |
| JP2003280710A (en) | Generation and control method of working track of robot hand | |
| He et al. | Robot path planning using improved rapidly-exploring random tree algorithm | |
| CN114986501A (en) | Mechanical arm path planning method and system and mechanical arm | |
| CN117260735A (en) | Path planning method for robot deep frame grabbing | |
| Jarvis | Robot path planning: complexity, flexibility and application scope | |
| CN116852367A (en) | Six-axis mechanical arm obstacle avoidance path planning method based on improved RRTstar | |
| CN115533912A (en) | Mechanical arm motion planning method based on rapid expansion random tree improvement | |
| Zhang et al. | Robot motion planning with orientational constraints based on offline sampling datasets |
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 |