CN108274465A - Based on optimization A*Artificial Potential Field machinery arm, three-D obstacle-avoiding route planning method - Google Patents
Based on optimization A*Artificial Potential Field machinery arm, three-D obstacle-avoiding route planning method Download PDFInfo
- Publication number
- CN108274465A CN108274465A CN201810022436.9A CN201810022436A CN108274465A CN 108274465 A CN108274465 A CN 108274465A CN 201810022436 A CN201810022436 A CN 201810022436A CN 108274465 A CN108274465 A CN 108274465A
- Authority
- CN
- China
- Prior art keywords
- node
- mechanical arm
- minimum
- tables
- 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.)
- Pending
Links
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
- B25J9/1666—Avoiding collision or forbidden zones
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
The present invention relates to one kind based on optimization A*Artificial Potential Field machinery arm, three-D obstacle-avoiding route planning method include the following steps：Establish three-dimensional avoidance environment and mechanical arm initialization；Use A*While avoiding Artificial Potential Field Method from being absorbed in local minimum, A is improved by minimum y-bend heapsort*The efficiency for searching for least estimated cost, mechanical arm, three-D obstacle-avoiding route planning is carried out with this；Inverse Kinematics Solution is asked to the path cooked up, obtains each joint angle of mechanical arm, thus plans the mechanical arm tail end path of an avoiding obstacles.Using minimum Binary Heap sort method to A*Artificial Potential Field Method path search algorithm in OPEN tables carry out minimum y-bend heapsort, more fast search can go out the node of estimate cost minimum and improve the efficiency of algorithm so as to quickly find the avoidance path of least estimated cost.Smoothing processing is done to avoidance path using least square fitting, makes gained avoidance path smooth, manipulator motion is avoided to shake.
Description
Technical field
The present invention relates to a kind of mechanical arm obstacle-avoiding route planning technologies, more particularly to a kind of to be based on y-bend heap optimization A*'s
Artificial Potential Field machinery arm, three-D obstacle-avoiding route planning method.
Background technology
Mechanical arm obstacle-avoiding route planning is referred in given environment and obstacle condition, starting point pose and target point pose
In the case of, to plan a path from starting point to target point so that mechanical arm can one paths of collisionless planning.
There is the obstacle-avoiding route planning of mobile robot in two-dimensional environment, there is the avoidance path of mechanical arm to advise in three-dimensional environment
It draws.The method of traditional path planning has simulated annealing, Artificial Potential Field Method etc., preferable path search algorithm to have
Fallback algorithms, Floyd algorithms, dijkstra's algorithm, A*Algorithm etc..
The algorithm structure of Artificial Potential Field Method is simple, and arithmetic speed is fast, but is easy to be absorbed in local minizing point, A*It is a kind of complete
The optimal planning algorithm of office, can be effectively prevented from Artificial Potential Field Method and be absorbed in local minizing point.But in three-dimensional space environment
During lower searching route, every time from A*OPEN tables in take out estimate cost minimum node be used for calculate next time, deposit
In the problem that arithmetic speed is slow.
Invention content
The problem of the present invention be directed in three-dimensional environment with the presence of the obstacle-avoiding route planning of mechanical arm, it is proposed that one kind is based on
Optimize A*Artificial Potential Field machinery arm, three-D obstacle-avoiding route planning method, using minimum Binary Heap sort method to A*Artificial gesture
OPEN tables in the method searching route algorithm of field carry out minimum y-bend heapsort, more can go out estimate cost minimum by fast search
Node improves the efficiency of algorithm so as to quickly find the avoidance path of least estimated cost.
The technical scheme is that：One kind is based on optimization A*Artificial Potential Field machinery arm, three-D obstacle-avoiding route planning side
Method,
The first step：The model that barrier is established in three-dimensional space environment, using sphere envelope barrier, and to mechanical arm
The initial joint angle of the initial point position of end effector, aiming spot, each joint is initialized；
Second step：Use minimum y-bend heap optimization A*Artificial Potential Field Method plan the three-dimensional avoidance path of mechanical arm, it is fixed
Adopted subordinate ordered array memory node information defines OPEN tables and is used for storing having generated and node to be investigated, CLOSED tables are used for
The node accessed is stored, using minimum Binary Heap to OPEN list sortings, finds least estimated cost node avoidance path,
Satisfactory node is stored in OPEN tables by the case where according to each step decision node, after often having run the node,
The node is stored in CLOSED tables from OPEN tables again, what is stored inside each node is location information, and uses minimum Binary Heap
OPEN tables to being inserted into new node i+1 are ranked up, and ensure that the corresponding heuristic function f (n) of root node of update OPEN tables is minimum,
F (n)=g (n)+h (n), f (n) indicate optimal path cost the sum of of the mechanical arm tail end from start node to destination node, g (n)
It indicates to indicate the estimate cost from present node n to destination node from start node to the actual cost of present node n, h (n),
Each make a move of mechanical arm tail end is both needed to calculate the value of the heuristic function f (n), and the node of gained f (n) minimums is exactly mechanical arm end
The position that end reaches in next step；
Third walks：Judge that the mechanical arm tail end three-dimensional avoidance path for cooking up to come in second step whether there is Inverse Kinematics
Solution：Solution of inverting is to determine each link joint angle of mechanical arm by the coordinate dot position information of mechanical arm tail end, determines that mechanical arm is
It is no to there is the avoidance path that run, if there are inverse kinetics solution, execute the 4th step；If kinematic solution of inverting fails, explanation
Working space of the path point beyond robot arm end effector that mechanical arm reaches unusual limit or solved, then return to second step
Again planning path；
4th step：By the robot inverse solution function in the robot tool case Robotics Toolbox of MATLAB
Ikine () seeks Inverse Kinematics Solution to the mechanical arm tail end three-dimensional avoidance path cooked up, obtains 8 groups of different joint angles,
And select joint angle of the one group of wherein optimal solution as each joint of required mechanical arm；
5th step：Barrier collision detection：Using based on the obstacle detection method for surrounding ball, gone out according to four-step calculation
At a distance from the barrier centre of sphere, whether detection mechanical arm collides with barrier for each joint angle of mechanical arm and mechanical arm, if hair
Raw collision returns to the 4th step and reselects one group of feasible suboptimal solution progress collision detection；If not colliding, the 4th step institute
It is optimal joint angle to seek joint angle, and the path planned meets the requirement of mechanical arm avoidance planning, successfully plans avoidance
Mechanical arm tail end path, planning terminate.
The step second step specifically comprises the following steps：
S1：Empty OPEN tables and CLOSED tables are created, step-length stepL is defined；
S2：Starting point START is inserted into OPEN tables, calculates heuristic function f (n)=g (n)+h (n), f (n) indicates mechanical arm
Optimal path cost the sum of of the end from start node to destination node, g (n) are indicated from start node to the reality of present node n
Border cost, h (n) indicate the estimate cost from present node n to destination node,
For starting point START, wherein actual cost g (n)=0, estimate cost
Estimate cost function h (n) used herein is
Mechanical arm tail end point position (xn、yn、zn) and target point (xd、yd、zd) between Euclidean distance, then starting point STRAT is inserted
Enter CLOSED tables, note present node is i=START；
S3：Collision detection is carried out to present node i, if colliding, planning failure, stop planning；If not touching
It hits, then executes next step S4；
S4：Whether be destination node, if not destination node, thens follow the steps S5 if it is corresponding to detect present node i；If
Destination node thens follow the steps S9；
S5：Repulsion, gravitation, resultant force of the target point to mechanical arm tail end are calculated using Artificial Potential Field Method, further according to occlusometer
The stressing conditions of next node i+1 are calculated, the wherein actual cost g (n) of present node i is the actual cost of a upper node plus step
Long stepL；
S6：Judge whether present node i is minimum point and whether the suffered resultant force of present node i mechanical arms is zero, if not
It is minimum point, then executes next step S7；If minimum point then executes IV,
IV specific steps include：
SS1：Carrying out up, down, left, right, before and after to present node i, totally 6 directions are extended with step-length stepL；
SS2：Descendant node next is obtained by extension, calculates the cost f (next) of descendant node NEXT；
SS3：Descendant node next is judged whether in OPEN tables, if executing SS4；If not existing, SS5 is executed；
SS4：The f (n) of f (next) minimum root nodes corresponding with present node i is compared, minimum Binary Heap is used
Root node in ranking replacement OPEN tables is minimum father node f (n)；
SS5：Descendant node next is judged whether in CLOSED tables, if executing SS6；If not existing, subsequent section
Point next is inserted into OPEN tables；
SS6：Judge whether OPEN tables are empty, if not empty, then execute SS7；If it is empty, then terminate to plan；
SS7：OPEN tables are ranked up with minimum Binary Heap method, update minimum root node f (n), updated minimum
Local minizing point has been jumped out in the next node i+1 that root node is planned as avoidance, at this time avoidance planning；
SS8：Whether mechanical arm collides at decision node i+1, if colliding, chooses the f of time minimum node
(n) it is used as next node i+1, and goes to SS7 and rejudges；If not colliding, SS9 is executed；
SS9：Next node i+1 is inserted into CLOSED tables, and using minimum Binary Heap method to the CLOSED list sortings, assignment
I=i+1, i.e., using next node i+1 as present node i, it is present node i to update satisfactory next node i+1；
SS10：Judge updated node i whether within the scope of barrier influences, if so, go to IV again into
Row extension；If it is not, then going to II rejudges whether updated present node i is destination node； S7：By artificial gesture
The next node i+1 that field method calculates is inserted into OPEN tables and CLOSED tables, and using minimum Binary Heap to being inserted into new node i+1's
OPEN tables are ranked up, and ensure that the root node f (n) of update OPEN tables is minimum, are similarly arranged using minimum Binary Heap CLOSED tables
Sequence updates, then present node i is saved as father node, preserves using the OPEN tables and CLOSED after minimum Binary Heap ranking replacement
Table；
S8：By next node i+1 as present node i, goes to step S3 and carry out collision detection；
S9：Present node i is destination node, and note destination node is END, then illustrates that mechanical arm searches out avoidance path, from
END is reversely returned and is sought to START, the avoidance path planned；
S10：Using least square method, the avoidance path obtained to planning is fitted, and keeps the path more smooth；
S11：Planning terminates.
The beneficial effects of the present invention are：The present invention is based on optimization A*Artificial Potential Field machinery arm, three-D obstacle-avoiding route planning
Method, minimum y-bend heapsort improve A*Artificial Potential Field Method searching route hunting speed, use least square fitting
Smoothing processing is done to avoidance path, makes gained avoidance path smooth, manipulator motion is avoided to shake.
Description of the drawings
Fig. 1 is Artificial Potential Field machinery arm, three-D obstacle-avoiding route planning method main flow chart of the present invention；
Fig. 2 is minimum y-bend heap optimization A in the method for the present invention*Artificial Potential Field Method flow chart；
Fig. 3 is Binary Heap A when Artificial Potential Field Method is absorbed in local minizing point in the method for the present invention*Jump out local minimum
The flow chart of point；
Fig. 4 is the method for the present invention minimum Binary Heap to A*OPEN list sorting embodiments flow chart；
Fig. 5 implements illustration to be resequenced with minimum Binary Heap after the method for the present invention delete operation；
Fig. 6 implements illustration to be resequenced with minimum Binary Heap after the method for the present invention insertion operation.
Specific implementation mode
Binary Heap is that completely or approximately complete binary tree, this method use minimum y-bend heap optimization A*Artificial Potential Field
The characteristics of method, minimum Binary Heap：Father node is always not more than child node, after minimum y-bend heapsort, the root node
Value is minimum.So using minimum Binary Heap to OPEN list sortings so that the root node of OPEN tables is the node of least estimated cost,
It can accelerate the speed in searching least estimated cost node avoidance path.
This method embodiment, Artificial Potential Field Method machinery arm, three-D obstacle-avoiding route planning method main flow step as shown in Figure 1
Including：
The first step：The model that barrier is established in three-dimensional space environment, using sphere envelope barrier, and to mechanical arm
The initial joint angle of the initial point position of end effector, aiming spot, each joint is initialized；
Second step：Use minimum y-bend heap optimization A*Artificial Potential Field Method plan the three-dimensional avoidance path of mechanical arm；
Third walks：Judge that the mechanical arm tail end three-dimensional avoidance path for cooking up to come in second step whether there is Inverse Kinematics
Solution.Solution of inverting is to determine each link joint angle of mechanical arm etc. by the coordinate dot position information of mechanical arm tail end, determines mechanical arm
With the presence or absence of the avoidance path that can be run.If there are inverse kinetics solution, the 4th step is executed；If kinematic solution of inverting fails, say
Bright mechanical arm reaches unusual limit, or the path point solved exceeds the working space of robot arm end effector, then returns to the
Two steps planning path again；
4th step：By the robot inverse solution function in the robot tool case Robotics Toolbox of MATLAB
Ikine () seeks Inverse Kinematics Solution to the mechanical arm tail end three-dimensional avoidance path cooked up, obtains 8 groups of different joint angles,
And select joint angle of the one group of wherein optimal solution as each joint of required mechanical arm；
5th step：Barrier collision detection.Using based on the obstacle detection method for surrounding ball, gone out according to four-step calculation
At a distance from the barrier centre of sphere, whether detection mechanical arm collides with barrier for each joint angle of mechanical arm and mechanical arm, if hair
Raw collision, the 4th step of return reselects one group of feasible suboptimal solution (when seeking Inverse Kinematics Solution, can find out optimal automatically
Solution, suboptimal solution, excellent solution ... of third, etc. is only preferably to select that optimal group every time as being solved, it is optimal that
When group does not meet avoidance motion planning, suboptimal solution can be just selected), if not colliding, joint angle is required by the 4th step
Optimal joint angle, the path planned meet the requirement of mechanical arm avoidance planning, and planning terminates.
It is as shown in Figures 2 and 3 to the detailed process of Fig. 1 second steps, minimum y-bend heap optimization A*Artificial Potential Field Method and artificial
With Binary Heap A when potential field method is absorbed in local minizing point*The flow chart of local minizing point is jumped out, is used for this method embodiment
A*While algorithm avoids Artificial Potential Field Method from being absorbed in local minizing point, using minimum Binary Heap to OPEN list sortings, it can accelerate
The speed in least estimated cost node avoidance path is found, including：
S1：Empty OPEN tables and CLOSED tables are created, step-length stepL is defined；
S2：Starting point START is inserted into OPEN tables, calculates heuristic function f (n)=g (n)+h (n), f (n) indicates machinery
Optimal path cost the sum of of the arm end from start node to destination node, g (n) are indicated from start node to present node n's
Actual cost, h (n) indicates the estimate cost from present node n to destination node, for starting point START, wherein practical generation
Valence g (n)=0, estimate cost
Estimate cost function h (n) used herein is
Mechanical arm tail end point position (xn、yn、zn) and target point (xd、yd、zd) between Euclidean distance, then starting point STRAT is inserted
Enter CLOSED tables, note present node is i=START；
S3：Collision detection is carried out to present node i, if colliding, planning failure stops planning；If not colliding,
Then execute next step S4；
S4：Whether it is destination node that it is corresponding to detect present node i, if not destination node, thens follow the steps S5, if
Destination node thens follow the steps S9；
S5：Target point is calculated to (mechanical arm tail ends such as the repulsion, gravitation, resultant force of mechanical arm tail end using Artificial Potential Field Method
Next node is often moved to, gravitation, repulsion, resultant force etc. suffered by calculating machine arm end are required for, passes through the automatic guiding of resultant force
Mechanical arm tail end reaches next node position), further according to resultant force calculate next node i+1 (under the guiding of article market, machine
Tool arm is moved towards target point, moves a step-length stepL every time, next section is known that plus step-length by the direction of resultant force
Point) stressing conditions, the wherein actual cost g (n) of present node i be a upper node actual cost add step-length stepL；
S6：Judge whether present node i is minimum point and whether the suffered resultant force of present node i mechanical arms is zero, if not
It is minimum point, then executes next step S7, if minimum point, then executes IV；
S7：The next node i+1 calculated by Artificial Potential Field Method is inserted into OPEN tables, and (OPEN tables are used for storing with CLOSED tables
It has generated and node to be investigated, CLOSED tables are used for storing the node accessed.Originally the OPEN tables and CLOSED tables defined
All it is empty, the case where according to each step decision node, satisfactory node is stored in OPEN tables, has often run the node
Afterwards, then the node from OPEN tables CLOSED tables are stored in.What is stored inside each node is location information), and use minimum two
Fork heap is ranked up the OPEN tables for being inserted into new node i+1, and (root node is minimum y-bend to the root node of guarantee update OPEN tables
One node of heap the top) f (n) minimums, similarly to CLOSED tables using minimum Binary Heap ranking replacement, then present node
I saves as father node (father node is a upper node for presently described node), preserves using minimum Binary Heap ranking replacement
OPEN tables afterwards and CLOSED tables；
S8：By next node i+1 as present node i, goes to S3 and carry out collision detection；
S9：Present node i is destination node, and note destination node is END, then illustrates that mechanical arm searches out avoidance path, from
END is reversely returned and is sought to START, the avoidance path planned；
S10：Using least square method, the avoidance path obtained to planning is fitted, and keeps the path more smooth；
S11：Planning terminates.
In preceding step S6, if minimum point, then IV is executed, IV specific steps include：
SS1：Carrying out up, down, left, right, before and after to present node i, totally 6 directions are extended with step-length stepL；
SS2：Descendant node next is obtained by extension, calculates the cost f (next) of descendant node NEXT；
SS3：Descendant node next is judged whether in OPEN tables, if executing SS4, if not existing, executing SS5；
SS4：The f (n) of f (next) minimum root nodes corresponding with present node i is compared, minimum Binary Heap is used
Root node in ranking replacement OPEN tables is minimum father node f (n)；
SS5：Descendant node next is judged whether in CLOSED tables, if SS6 is being executed, if not existing, subsequent section
Point next is inserted into OPEN tables；
SS6：Judge whether OPEN tables are empty, if not empty, then execute SS7, if it is empty, then terminate to plan；
SS7：OPEN tables are ranked up with minimum Binary Heap method, update minimum root node f (n), updated minimum
Local minizing point has been jumped out in the next node i+1 that root node is planned as avoidance, at this time avoidance planning；
SS8：Whether mechanical arm collides at decision node i+1, if colliding, chooses the f of " secondary minimum node "
(n) it is used as next node i+1, and goes to SS7 and rejudges, if not colliding, executes SS9；
SS9：Next node i+1 is inserted into CLOSED tables, and using minimum Binary Heap method to the CLOSED list sortings, assignment
I=i+1, i.e., using next node i+1 as present node i, it is present node i to update satisfactory next node i+1；
SS10：Judge updated node i whether within the scope of barrier influences, if so, go to IV again into
Row extension, if it is not, then going to II rejudges whether updated present node i is destination node.
This method implements such as attached drawing 4, using minimum Binary Heap to A*OPEN list sortings, which specifically wraps
It includes：
STEP1：The OPEN tables are ranked up with minimum Binary Heap sort method, obtain the minimum that root node is minimum value
Binary Heap list, and can resequence after carrying out insert or delete operation to the table after sequence, keep root node minimum；
STEP2：If be inserted into new node into the minimum Binary Heap list, STEP4 is executed, if being arranged to the minimum Binary Heap
Minimum value node is deleted in table, that is, executes STEP3 when deleting root node；
STEP3：Deletion of node operation is executed to the minimum Binary Heap list, minimum Binary Heap is used after this method delete operation
Such as attached drawing 5, step are implemented in rearrangement：
STEP31：Root node f (n) is deleted, that is, deletes the node of the least estimated cost value of the Binary Heap list, makes at this
For vacancy；
STEP32：Then the node of the Binary Heap list end is put into and deletes root node empty place, constitute one it is new
Heap list；
STEP33：Operation is ranked up using the method for minimum y-bend heapsort to the new heap list, is saved towards father is made
The new node is exchanged position by the point direction sequencing small unlike child node when the new node is more than child node with child node, is protected
Card is small never than child node with updated father node；
STEP34：When all father nodes are small unlike child node, then the small y-bend heapsort of the heap list is completed,
The newer root node f (n) of gained, to delete the node for representing least estimated cost value after new node is resequenced；
STEP4：It is executed to the minimum Binary Heap list and is inserted into new node operation, with minimum y-bend after this method insertion operation
Such as attached drawing 6, step are implemented in heap rearrangement：
STEP41：New node is inserted in the end of the minimum Binary Heap list, obtains the heap list for being inserted into new node；
STEP42：Then the heap list of insertion new node, it is ranked up operation using the method for minimum y-bend heapsort,
Towards the direction sequencing for keeping father node small unlike child node, the node is exchanged with father node when the new node is less than father node
Position；
STEP43：When all father nodes are small unlike child node, then the small y-bend heapsort of the heap list is completed,
The newer root node f (n) of gained, to be inserted into after new node rearrangement the node for representing least estimated cost value.
Claims (2)
1. one kind is based on optimization A*Artificial Potential Field machinery arm, three-D obstacle-avoiding route planning method, which is characterized in that
The first step：The model that barrier is established in three-dimensional space environment, using sphere envelope barrier, and to mechanical arm tail end
The initial joint angle of the initial point position of actuator, aiming spot, each joint is initialized；
Second step：Use minimum y-bend heap optimization A*Artificial Potential Field Method plan the three-dimensional avoidance path of mechanical arm, definition is orderly
Storage of array nodal information defines OPEN tables and is used for storing having generated and node to be investigated, CLOSED tables are used for storing visit
The node asked finds least estimated cost node avoidance path using minimum Binary Heap to OPEN list sortings,
Satisfactory node is stored in OPEN tables by the case where according to each step decision node, after often having run the node, then
The node is stored in CLOSED tables from OPEN tables, and what is stored inside each node is location information, and using minimum Binary Heap to inserting
The OPEN tables for entering new node i+1 are ranked up, and ensure that the corresponding heuristic function f (n) of root node of update OPEN tables is minimum, f (n)
=g (n)+h (n), f (n) indicate that optimal path cost the sum of of the mechanical arm tail end from start node to destination node, g (n) indicate
The estimate cost from present node n to destination node, machinery are indicated from start node to the actual cost of present node n, h (n)
Each make a move in arm end is both needed to calculate the value of the heuristic function f (n), and the node of gained f (n) minimums is exactly under mechanical arm tail end
The position that one step reaches；
Third walks：Judge that the mechanical arm tail end three-dimensional avoidance path for cooking up to come in second step whether there is Inverse Kinematics Solution：It asks
Inverse solution is to determine each link joint angle of mechanical arm by the coordinate dot position information of mechanical arm tail end, determines that mechanical arm whether there is
If the avoidance path that can be run executes the 4th step there are inverse kinetics solution；If kinematic solution of inverting fails, illustrate mechanical arm
Working space of the path point beyond robot arm end effector for reaching unusual limit or being solved, then return to second step and advise again
Draw path；
4th step：By the robot inverse solution function ikine () in the robot tool case Robotics Toolbox of MATLAB,
Inverse Kinematics Solution is asked to the mechanical arm tail end three-dimensional avoidance path cooked up, obtains 8 groups of different joint angles, and select wherein
Joint angle of the one group of optimal solution as each joint of required mechanical arm；
5th step：Barrier collision detection：Using based on the obstacle detection method for surrounding ball, machinery is gone out according to four-step calculation
Each joint angle of arm and mechanical arm are at a distance from the barrier centre of sphere, and whether detection mechanical arm collides with barrier, if touching
It hits, returns to the 4th step and reselect one group of feasible suboptimal solution progress collision detection；If not colliding, the 4th step is required to close
Section angle is optimal joint angle, and the path planned meets the requirement of mechanical arm avoidance planning, successfully plans the machinery of an avoidance
Arm terminal end path, planning terminate.
2. being based on optimization A according to claim 1*Artificial Potential Field machinery arm, three-D obstacle-avoiding route planning method, feature exists
In the step second step specifically comprises the following steps：
S1：Empty OPEN tables and CLOSED tables are created, step-length stepL is defined；
S2：Starting point START is inserted into OPEN tables, calculates heuristic function f (n)=g (n)+h (n), f (n) indicates mechanical arm tail end
The sum of optimal path cost from start node to destination node, g (n) indicate the practical generation from start node to present node n
Valence, h (n) indicate the estimate cost from present node n to destination node,
For starting point START, wherein actual cost g (n)=0, estimate cost
Estimate cost function h (n) used herein is mechanical arm tail end point position (xn、yn、zn) and target point (xd、yd、zd) between
Starting point STRAT, is then inserted into CLOSED tables by Euclidean distance, and note present node is i=START；
S3：Collision detection is carried out to present node i, if colliding, planning failure, stop planning；If not colliding,
Execute next step S4；
S4：Whether be destination node, if not destination node, thens follow the steps S5 if it is corresponding to detect present node i；If target
Node thens follow the steps S9；
S5：Repulsion, gravitation, resultant force of the target point to mechanical arm tail end are calculated using Artificial Potential Field Method, under being calculated further according to resultant force
The stressing conditions of one node i+1, wherein the actual cost g (n) of present node i are that the actual cost of a upper node adds step-length
stepL；
S6：Judge whether present node i is minimum point and whether the suffered resultant force of present node i mechanical arms is zero, if not pole
Small value point, then execute next step S7；If minimum point then executes IV,
IV specific steps include：
SS1：Carrying out up, down, left, right, before and after to present node i, totally 6 directions are extended with step-length stepL；
SS2：Descendant node next is obtained by extension, calculates the cost f (next) of descendant node NEXT；
SS3：Descendant node next is judged whether in OPEN tables, if executing SS4；If not existing, SS5 is executed；
SS4：The f (n) of f (next) minimum root nodes corresponding with present node i is compared, minimum y-bend heapsort is used
It updates the root node in OPEN tables or is minimum father node f (n)；
SS5：Descendant node next is judged whether in CLOSED tables, if executing SS6；If not existing, descendant node
Next is inserted into OPEN tables；
SS6：Judge whether OPEN tables are empty, if not empty, then execute SS7；If it is empty, then terminate to plan；
SS7：OPEN tables are ranked up with minimum Binary Heap method, update minimum root node f (n), updated least root section
Local minizing point has been jumped out in the next node i+1 that point is planned as avoidance, at this time avoidance planning；
SS8：Whether mechanical arm collides at decision node i+1, if colliding, chooses f (n) conducts of time minimum node
Next node i+1, and go to SS7 and rejudge；If not colliding, SS9 is executed；
SS9：Next node i+1 is inserted into CLOSED tables, and using minimum Binary Heap method to the CLOSED list sortings, assignment i=i
+ 1, i.e., using next node i+1 as present node i, it is present node i to update satisfactory next node i+1；
SS10：Updated node i is judged whether within the scope of barrier influences, if so, going to IV re-starts expansion
Exhibition；If it is not, then going to II rejudges whether updated present node i is destination node；
S7：The next node i+1 calculated by Artificial Potential Field Method is inserted into OPEN tables and CLOSED tables, and uses minimum Binary Heap pair
The OPEN tables for being inserted into new node i+1 are ranked up, and are ensured that the root node f (n) of update OPEN tables is minimum, are similarly made to CLOSED tables
Father node is saved as with minimum Binary Heap ranking replacement, then present node i, is preserved using after minimum Binary Heap ranking replacement
OPEN tables and CLOSED tables；
S8：By next node i+1 as present node i, goes to step S3 and carry out collision detection；
S9：Present node i is destination node, and note destination node is END, then illustrates that mechanical arm searches out avoidance path, anti-from END
It is sought to returning to START, the avoidance path planned；
S10：Using least square method, the avoidance path obtained to planning is fitted, and keeps the path more smooth；
S11：Planning terminates.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810022436.9A CN108274465A (en) | 2018-01-10 | 2018-01-10 | Based on optimization A*Artificial Potential Field machinery arm, three-D obstacle-avoiding route planning method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810022436.9A CN108274465A (en) | 2018-01-10 | 2018-01-10 | Based on optimization A*Artificial Potential Field machinery arm, three-D obstacle-avoiding route planning method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN108274465A true CN108274465A (en) | 2018-07-13 |
Family
ID=62803463
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810022436.9A Pending CN108274465A (en) | 2018-01-10 | 2018-01-10 | Based on optimization A*Artificial Potential Field machinery arm, three-D obstacle-avoiding route planning method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108274465A (en) |
Cited By (19)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109470249A (en) * | 2018-11-07 | 2019-03-15 | 河海大学 | A kind of optimum path planning of submarine navigation device and obstacle avoidance design method |
| CN109711638A (en) * | 2019-01-16 | 2019-05-03 | 中国大恒(集团)有限公司北京图像视觉技术分公司 | A kind of industrial machinery arm transport path planing method based on time-varying digraph |
| CN109857110A (en) * | 2019-02-13 | 2019-06-07 | 广州视源电子科技股份有限公司 | Motion planning method, device, equipment and computer readable storage medium |
| CN110207708A (en) * | 2019-06-25 | 2019-09-06 | 重庆邮电大学 | A kind of unmanned aerial vehicle flight path device for planning and method |
| CN110487295A (en) * | 2019-09-06 | 2019-11-22 | 中国计量大学 | A kind of time-optimized smooth A* algorithm |
| CN111168675A (en) * | 2020-01-08 | 2020-05-19 | 北京航空航天大学 | Dynamic obstacle avoidance motion planning method for mechanical arm of household service robot |
| CN111844007A (en) * | 2020-06-02 | 2020-10-30 | 江苏理工学院 | Pollination robot mechanical arm obstacle avoidance path planning method and device |
| CN112264989A (en) * | 2020-06-17 | 2021-01-26 | 华中科技大学 | Two-mechanical-arm cooperative obstacle avoidance method based on neighborhood traversal |
| CN112595324A (en) * | 2020-12-10 | 2021-04-02 | 安徽工程大学 | Optimal node wheel type mobile robot path planning method under optimal energy consumption |
| CN113414761A (en) * | 2021-02-03 | 2021-09-21 | 中国人民解放军63920部队 | Method for optimizing motion trail of redundant mechanical arm |
| CN113485388A (en) * | 2021-07-27 | 2021-10-08 | 天津城建大学 | AUV local obstacle avoidance method based on collision detection model and artificial potential field method |
| CN113547523A (en) * | 2021-08-05 | 2021-10-26 | 河北工业大学 | Redundancy mechanical arm inversion solution method based on artificial potential field method and gray wolf algorithm |
| CN113608531A (en) * | 2021-07-26 | 2021-11-05 | 福州大学 | Unmanned vehicle real-time global path planning method based on dynamic window of safety A-guiding point |
| CN113710431A (en) * | 2019-04-17 | 2021-11-26 | 株式会社途伟尼 | Path planning method using optimal tree based on sampling, recording medium storing program for implementing the method, and computer program stored in the medium to implement the method |
| CN113799141A (en) * | 2021-10-14 | 2021-12-17 | 福州大学 | Six-degree-of-freedom mechanical arm obstacle avoidance path planning method |
| CN113960996A (en) * | 2020-07-20 | 2022-01-21 | 华为技术有限公司 | Planning method and device for obstacle avoidance path of driving device |
| CN114559435A (en) * | 2022-03-23 | 2022-05-31 | 杭州电子科技大学 | Mechanical arm track planning method based on sphere envelope and optimal performance target |
| CN115648220A (en) * | 2022-11-15 | 2023-01-31 | 华侨大学 | Mechanical arm joint space obstacle avoidance path planning method based on minimum cost reduction |
| CN116872212A (en) * | 2023-08-14 | 2023-10-13 | 山东工商学院 | Double-mechanical-arm obstacle avoidance planning method based on A-Star algorithm and improved artificial potential field method |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101213632B1 (en) * | 2010-04-30 | 2012-12-18 | 동국대학교 산학협력단 | Method and apparatus of planning trajectory using multi-step configuration space |
| CN104029203A (en) * | 2014-06-18 | 2014-09-10 | 大连大学 | Path planning method for implementation of obstacle avoidance for space manipulators |
| CN105955254A (en) * | 2016-04-25 | 2016-09-21 | 广西大学 | Improved A* algorithm suitable for robot path search |
| CN106166750A (en) * | 2016-09-27 | 2016-11-30 | 北京邮电大学 | A kind of modified model D* mechanical arm dynamic obstacle avoidance paths planning method |
-
2018
- 2018-01-10 CN CN201810022436.9A patent/CN108274465A/en active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101213632B1 (en) * | 2010-04-30 | 2012-12-18 | 동국대학교 산학협력단 | Method and apparatus of planning trajectory using multi-step configuration space |
| CN104029203A (en) * | 2014-06-18 | 2014-09-10 | 大连大学 | Path planning method for implementation of obstacle avoidance for space manipulators |
| CN105955254A (en) * | 2016-04-25 | 2016-09-21 | 广西大学 | Improved A* algorithm suitable for robot path search |
| CN106166750A (en) * | 2016-09-27 | 2016-11-30 | 北京邮电大学 | A kind of modified model D* mechanical arm dynamic obstacle avoidance paths planning method |
Non-Patent Citations (2)
| Title |
|---|
| 程国秀: "基于六自由度机械臂的避障路径规划研究", 《中国优秀硕士学位论文全文数据库》 * |
| 陈暄: "一种改进的A*算法在路径规划中的研究", 《电脑知识与技术》 * |
Cited By (26)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109470249A (en) * | 2018-11-07 | 2019-03-15 | 河海大学 | A kind of optimum path planning of submarine navigation device and obstacle avoidance design method |
| CN109711638B (en) * | 2019-01-16 | 2020-12-04 | 中国大恒(集团)有限公司北京图像视觉技术分公司 | Industrial mechanical arm carrying path planning method based on time-varying directed graph |
| CN109711638A (en) * | 2019-01-16 | 2019-05-03 | 中国大恒(集团)有限公司北京图像视觉技术分公司 | A kind of industrial machinery arm transport path planing method based on time-varying digraph |
| CN109857110A (en) * | 2019-02-13 | 2019-06-07 | 广州视源电子科技股份有限公司 | Motion planning method, device, equipment and computer readable storage medium |
| CN113710431A (en) * | 2019-04-17 | 2021-11-26 | 株式会社途伟尼 | Path planning method using optimal tree based on sampling, recording medium storing program for implementing the method, and computer program stored in the medium to implement the method |
| CN113710431B (en) * | 2019-04-17 | 2023-12-22 | 株式会社途伟尼 | Path planning method using sample-based optimal tree and recording medium |
| CN110207708A (en) * | 2019-06-25 | 2019-09-06 | 重庆邮电大学 | A kind of unmanned aerial vehicle flight path device for planning and method |
| CN110487295A (en) * | 2019-09-06 | 2019-11-22 | 中国计量大学 | A kind of time-optimized smooth A* algorithm |
| CN111168675B (en) * | 2020-01-08 | 2021-09-03 | 北京航空航天大学 | Dynamic obstacle avoidance motion planning method for mechanical arm of household service robot |
| CN111168675A (en) * | 2020-01-08 | 2020-05-19 | 北京航空航天大学 | Dynamic obstacle avoidance motion planning method for mechanical arm of household service robot |
| CN111844007A (en) * | 2020-06-02 | 2020-10-30 | 江苏理工学院 | Pollination robot mechanical arm obstacle avoidance path planning method and device |
| CN112264989A (en) * | 2020-06-17 | 2021-01-26 | 华中科技大学 | Two-mechanical-arm cooperative obstacle avoidance method based on neighborhood traversal |
| CN113960996A (en) * | 2020-07-20 | 2022-01-21 | 华为技术有限公司 | Planning method and device for obstacle avoidance path of driving device |
| CN112595324A (en) * | 2020-12-10 | 2021-04-02 | 安徽工程大学 | Optimal node wheel type mobile robot path planning method under optimal energy consumption |
| CN113414761A (en) * | 2021-02-03 | 2021-09-21 | 中国人民解放军63920部队 | Method for optimizing motion trail of redundant mechanical arm |
| CN113608531A (en) * | 2021-07-26 | 2021-11-05 | 福州大学 | Unmanned vehicle real-time global path planning method based on dynamic window of safety A-guiding point |
| CN113608531B (en) * | 2021-07-26 | 2023-09-12 | 福州大学 | Unmanned vehicle real-time global path planning method based on safety A-guidance points |
| CN113485388A (en) * | 2021-07-27 | 2021-10-08 | 天津城建大学 | AUV local obstacle avoidance method based on collision detection model and artificial potential field method |
| CN113485388B (en) * | 2021-07-27 | 2022-07-29 | 天津城建大学 | AUV local obstacle avoidance method based on collision detection model and artificial potential field method |
| CN113547523B (en) * | 2021-08-05 | 2022-04-15 | 河北工业大学 | Redundancy mechanical arm inversion solution method based on artificial potential field method and gray wolf algorithm |
| CN113547523A (en) * | 2021-08-05 | 2021-10-26 | 河北工业大学 | Redundancy mechanical arm inversion solution method based on artificial potential field method and gray wolf algorithm |
| CN113799141A (en) * | 2021-10-14 | 2021-12-17 | 福州大学 | Six-degree-of-freedom mechanical arm obstacle avoidance path planning method |
| CN114559435A (en) * | 2022-03-23 | 2022-05-31 | 杭州电子科技大学 | Mechanical arm track planning method based on sphere envelope and optimal performance target |
| CN114559435B (en) * | 2022-03-23 | 2023-08-29 | 杭州电子科技大学 | Mechanical arm track planning method based on sphere envelope and under optimal performance target |
| CN115648220A (en) * | 2022-11-15 | 2023-01-31 | 华侨大学 | Mechanical arm joint space obstacle avoidance path planning method based on minimum cost reduction |
| CN116872212A (en) * | 2023-08-14 | 2023-10-13 | 山东工商学院 | Double-mechanical-arm obstacle avoidance planning method based on A-Star algorithm and improved artificial potential field method |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108274465A (en) | Based on optimization A*Artificial Potential Field machinery arm, three-D obstacle-avoiding route planning method | |
| CN106166750B (en) | A kind of modified D* mechanical arm dynamic obstacle avoidance paths planning method | |
| CN109976350B (en) | Multi-robot scheduling method, device, server and computer readable storage medium | |
| CN110228069A (en) | A kind of online avoidance motion planning method of mechanical arm | |
| CN105955254B (en) | A kind of improved A* algorithm suitable for robot path search | |
| CN110231824B (en) | Intelligent agent path planning method based on straight line deviation method | |
| CN111844007B (en) | Obstacle avoidance path planning method and device for mechanical arm of pollination robot | |
| CN109945873A (en) | A kind of mixed path planing method for indoor mobile robot motion control | |
| CN111679679B (en) | Robot state planning method based on Monte Carlo tree search algorithm | |
| CN114161416B (en) | Robot path planning method based on potential function | |
| JP2017529631A (en) | Method and system for determining a path of an object that moves from a start state to an end state set while avoiding one or more obstacles | |
| CN113359718B (en) | Method and equipment for fusing global path planning and local path planning of mobile robot | |
| CN115079705A (en) | Routing planning method for inspection robot based on improved A star fusion DWA optimization algorithm | |
| CN112229419B (en) | Dynamic path planning navigation method and system | |
| CN114510056A (en) | Stable moving global path planning method for indoor mobile robot | |
| CN107357295B (en) | Path searching method and chip based on grid map and robot | |
| US20220203534A1 (en) | Path planning method and biped robot using the same | |
| van den Berg | Path planning in dynamic environments | |
| JP2020004421A (en) | Methods and systems for determining a path of an object moving from an initial state to final state set while avoiding one or more obstacle | |
| CN110967015A (en) | Path planning method and system | |
| CN114199270A (en) | Robot path planning method integrating bidirectional search mechanism and improved A-algorithm | |
| CN114939872B (en) | MIRRT-Connect algorithm-based intelligent storage redundant mechanical arm dynamic obstacle avoidance motion planning method | |
| CN111664851B (en) | Robot state planning method and device based on sequence optimization and storage medium | |
| CN114489040A (en) | Hybrid path planning method based on improved A-star algorithm and artificial potential field algorithm | |
| CN110275528A (en) | For the method for optimizing route of RRT algorithm improvement |
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 | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20180713 |