CN113442170A - Method and system for reverse split calculation of redundant node of mechanical arm path - Google Patents
Method and system for reverse split calculation of redundant node of mechanical arm path Download PDFInfo
- Publication number
- CN113442170A CN113442170A CN202110733121.7A CN202110733121A CN113442170A CN 113442170 A CN113442170 A CN 113442170A CN 202110733121 A CN202110733121 A CN 202110733121A CN 113442170 A CN113442170 A CN 113442170A
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- path
- motion
- node
- collision
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 45
- 238000004364 calculation method Methods 0.000 title abstract description 3
- 230000004888 barrier function Effects 0.000 claims abstract description 37
- 238000001514 detection method Methods 0.000 claims abstract description 9
- 238000004422 calculation algorithm Methods 0.000 claims description 46
- 230000007613 environmental effect Effects 0.000 claims description 30
- 238000005457 optimization Methods 0.000 claims description 27
- 230000008569 process Effects 0.000 claims description 16
- 238000010586 diagram Methods 0.000 description 19
- 238000005070 sampling Methods 0.000 description 10
- 238000004590 computer program Methods 0.000 description 7
- 238000012986 modification Methods 0.000 description 5
- 230000004048 modification Effects 0.000 description 5
- 230000006870 function Effects 0.000 description 4
- 239000012636 effector Substances 0.000 description 3
- 238000003860 storage Methods 0.000 description 3
- 230000009977 dual effect Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000005304 joining Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
-
- 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
Abstract
The invention discloses a method and a system for reverse split calculation of redundant nodes of a mechanical arm path, and belongs to the technical field of robot motion planning. The method comprises the following steps: performing motion planning on the mechanical arm R, determining a motion path as a dynamic barrier, and determining a collision-free motion path; removing redundant nodes from the mechanical arm R, recording corresponding time in a motion track, removing redundant nodes from an inorganic collision-free motion path, and recording corresponding time; carrying out reverse splitting operation on the motion path, and determining a collision-free path; performing collision detection, determining the first collision time, and determining the joint configuration at the moment of collision; determining an optimal node track according to the joint configuration; and repeating the reverse splitting operation of the motion paths of the mechanical arm R and the mechanical arm L to obtain the path of the optimal node track of the mechanical arm R and the mechanical arm L until the optimized path of the mechanical arm R and the mechanical arm L does not collide any more, and determining the reverse splitting path.
Description
Technical Field
The invention relates to the technical field of robot motion planning, in particular to a method and a system for calculating reverse splitting of redundant nodes of a mechanical arm path.
Background
In the fields with higher technical requirements, such as assembly operation, rescue and emergency rescue, medical service and the like, the double-arm robot needs to have better coordination and flexibility, and has incomparable advantages compared with a single-arm robot.
The dual-mechanical arm motion planning algorithm can be divided into a centralized type and a decoupling type, wherein the centralized planning algorithm considers two arms as a single arm with more freedom, so that the motion planning algorithm of the single arm can be directly used for the two arms, the integrity and the optimality of the planning algorithm can be generally kept, but the planning efficiency is lower because the freedom of the two arms is obviously higher than that of the single arm.
The decoupling type motion planning algorithm divides two mechanical arms into a main arm and a slave arm, firstly performs motion planning on the main arm, and then plans the motion of the slave arm by taking the planned path of the main arm as a dynamic barrier of the slave arm. The decoupled motion planning algorithm generally cannot guarantee the completeness and the optimality of the planning, but can quickly plan the motion tracks of the two arms in a simple environment.
Sampling-based path planning algorithms such as RRT and PRM algorithms are widely applied to centralized and decoupled motion planning of double mechanical arms. The method has the advantages of no limitation of configuration space dimension and high planning speed in a simple scene, but the movement path calculated by the sampling movement planning algorithm is often too much in turn and very unsmooth, so that the method is difficult to be directly used as a reference track of the controller, and redundant nodes need to be removed under the condition of ensuring no collision.
Disclosure of Invention
The invention provides 1, a method for calculating reverse splitting of redundant nodes of a mechanical arm path, aiming at the problems of excessive and unsmooth motion trail obtained by performing double-mechanical-arm decoupling motion planning by using a sampling motion planning algorithm, wherein the method comprises the following steps:
performing motion planning on the mechanical arm R, determining a motion path of the mechanical arm R, taking the motion path of the mechanical arm R as a dynamic barrier of the mechanical arm L, and determining a collision-free motion path of the mechanical arm L;
removing redundant nodes from the motion path of the mechanical arm R by using a preset algorithm, recording the corresponding time of the joint nodes of the mechanical arm R in the motion trail, removing the redundant nodes from the collision-free motion path of the mechanical arm L, and recording the corresponding time of the joint nodes of the collision-free motion path of the mechanical arm L in the motion trail;
carrying out reverse splitting operation on the motion paths of the mechanical arm R and the mechanical arm L, and determining the motion path of the mechanical arm R and the collision-free path of the mechanical arm L;
carrying out one-to-one collision detection on the motion path time of the mechanical arm R and the mechanical arm L, and determining the moment t of the first collision of the mechanical arm R and the mechanical arm LpDetermining the joint configuration of the mechanical arm R and the mechanical arm L at the moment of collision of the motion paths of the mechanical arms;
determining the optimal node tracks of the motion paths of the mechanical arm R and the mechanical arm L according to the joint configurations of the mechanical arm R and the mechanical arm L at the time of collision of the collision-free path and the motion path;
and repeating the reverse splitting operation of the motion paths of the mechanical arm R and the mechanical arm L to obtain the path of the optimal node track of the mechanical arm R and the mechanical arm L until the optimized path of the mechanical arm R and the mechanical arm L does not collide any more, and determining the reverse splitting path.
Optionally, the preset algorithm is a greedy algorithm, and includes:
the method comprises the following steps: the first node p of the movement path1And the last node pnAre connected in a straight line, and connect the nodes p1And node pnAdding the path node into the optimized path node set, judging whether the mechanical arm collides with an environmental barrier when being positioned at a path point on a straight line, and if not, determining that the optimized path only contains p1And pnOutput optimized { p1，pn}；
Step two: let i equal 2, j equal n judge the node piAnd node pjWhether the straight line connection between the two collides with the environmental barrier or not, and if the straight line connection between the two collides with the environmental barrier, p is determinediAdding the optimized path set to carry out the fourth step, if the node piAnd node pjThe straight line connection between the two parts collides with the environmental barrier, and then the step three is carried out;
step three: judging whether i is smaller than j, if i is smaller than j, making i equal to i +1 and j equal to n and returning to the second step, otherwise, outputting an optimized path set and ending the optimization process;
step four: and judging whether j is greater than 1, if j is greater than 1, making i equal to 1, and if j equal to i, returning to the step two, otherwise, outputting the optimized path set, and ending the optimization process.
Optionally, the movement paths of the mechanical arm R and the mechanical arm L are reversely split, and the distance between adjacent path points is divided by the time interval between adjacent path points to obtain the movement speed of the mechanical arm between the path points, so that the mechanical arm R and the mechanical arm L pass through the path at a constant speed.
Optionally, the motion path is a motion path calculated by using a sampling motion planning algorithm, and is composed of a plurality of path points.
Optionally, the trajectory between adjacent path points is calculated by linear interpolation, so that the mechanical arm moves at a constant speed between the path points.
The invention also provides a system for calculating the reverse split of the redundant node of the mechanical arm path, which comprises the following steps:
determining a motion path unit, performing motion planning on the mechanical arm R, determining a motion path of the mechanical arm R, taking the motion path of the mechanical arm R as a dynamic obstacle of the mechanical arm L, and determining a collision-free motion path of the mechanical arm L;
the motion path recording unit is used for removing redundant nodes from the motion path of the mechanical arm R by using a preset algorithm, recording the corresponding time of the joint nodes of the mechanical arm R in the motion trail, removing the redundant nodes from the collision-free motion path of the mechanical arm L and recording the corresponding time of the joint nodes of the collision-free motion path of the mechanical arm L in the motion trail;
the reverse splitting unit is used for performing reverse splitting operation on the motion paths of the mechanical arm R and the mechanical arm L and determining the motion path of the mechanical arm R and the collision-free path of the mechanical arm L;
a joint configuration unit for performing one-to-one collision detection on the motion path time of the mechanical arm R and the mechanical arm L, and determining the first collision time t of the mechanical arm R and the mechanical arm LpDetermining the joint configuration of the mechanical arm R and the mechanical arm L at the moment of collision of the motion paths of the mechanical arms;
determining an optimal node unit, and determining optimal node tracks of the motion paths of the mechanical arm R and the mechanical arm L according to the joint configurations of the mechanical arm R and the mechanical arm L at the collision-free path and the motion path at the collision moment;
determining a reverse splitting path unit, repeating the reverse splitting operation of the motion paths of the mechanical arm R and the mechanical arm L, obtaining the path of the optimal node track of the mechanical arm R and the mechanical arm L until the optimized path of the mechanical arm R and the mechanical arm L does not collide any more, and determining the reverse splitting path.
Optionally, the preset algorithm is a greedy algorithm, and includes:
the method comprises the following steps: the first node p of the movement path1And the last sectionPoint pnAre connected in a straight line, and connect the nodes p1And node pnAdding the path node into the optimized path node set, judging whether the mechanical arm collides with an environmental barrier when being positioned at a path point on a straight line, and if not, determining that the optimized path only contains p1And pnOutput optimized { p1，pn}；
Step two: let i equal 2, j equal n judge the node piAnd node pjWhether the straight line connection between the two collides with the environmental barrier or not, and if the straight line connection between the two collides with the environmental barrier, p is determinediAdding the optimized path set to carry out the fourth step, if the node piAnd node pjThe straight line connection between the two parts collides with the environmental barrier, and then the step three is carried out;
step three: judging whether i is smaller than j, if i is smaller than j, making i equal to i +1 and j equal to n and returning to the second step, otherwise, outputting an optimized path set and ending the optimization process;
step four: and judging whether j is greater than 1, if j is greater than 1, making i equal to 1, and if j equal to i, returning to the step two, otherwise, outputting the optimized path set, and ending the optimization process.
Optionally, the movement paths of the mechanical arm R and the mechanical arm L are reversely split, and the distance between adjacent path points is divided by the time interval between adjacent path points to obtain the movement speed of the mechanical arm between the path points, so that the mechanical arm R and the mechanical arm L pass through the path at a constant speed.
Optionally, the motion path is a motion path calculated by using a sampling motion planning algorithm, and is composed of a plurality of path points.
Optionally, the system further calculates a trajectory between adjacent path points by linear interpolation, so that the mechanical arm moves at a constant speed between the path points.
The invention ensures that redundant path nodes are removed on the premise that two arms do not collide. Simulation performed in the MATLAB robot kit shows that the node optimization algorithm significantly improves the quality of the path.
Drawings
FIG. 1 is a flow chart of a method of reverse split computation of a redundant node of a robotic arm path of the present invention;
FIG. 2 is a flow chart of a greedy algorithm optimization node of the present invention;
FIG. 3 is a flowchart of a reverse splitting algorithm of the dual robot arms based on a greedy algorithm according to the present invention;
FIG. 4a is an initial node optimization diagram of a greedy algorithm according to the present invention;
FIG. 4b is a timing diagram for extracting a first collision for collision detection in accordance with the present invention;
FIG. 4c is a partial path re-planning diagram of the present invention;
FIG. 5 is a diagram of the dual arm initial state of the present invention;
FIG. 6 is a diagram of a two-arm object state of the present invention;
FIG. 7 is a diagram of the trajectory of the dual-arm collision-free joint before optimization according to the present invention;
FIG. 8 is a diagram showing the movement path of the two arms in the working space before the optimization of the present invention;
FIG. 9 is a diagram of the trajectory of the optimized dual-arm collision-free joint of the present invention;
FIG. 10 is a diagram of the movement path of the optimized double arms in the working space;
FIG. 11 is a block diagram of a system for reverse split computation of redundant nodes of a robotic arm path according to the present invention.
Detailed Description
The exemplary embodiments of the present invention will now be described with reference to the accompanying drawings, however, the present invention may be embodied in many different forms and is not limited to the embodiments described herein, which are provided for complete and complete disclosure of the present invention and to fully convey the scope of the present invention to those skilled in the art. The terminology used in the exemplary embodiments illustrated in the accompanying drawings is not intended to be limiting of the invention. In the drawings, the same units/elements are denoted by the same reference numerals.
Unless otherwise defined, terms (including technical and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs. Further, it will be understood that terms, such as those defined in commonly used dictionaries, should be interpreted as having a meaning that is consistent with their meaning in the context of the relevant art and will not be interpreted in an idealized or overly formal sense.
The invention provides a method for calculating reverse split of a redundant node of a mechanical arm path, which comprises the following steps of:
performing motion planning on the mechanical arm R, determining a motion path of the mechanical arm R, taking the motion path of the mechanical arm R as a dynamic barrier of the mechanical arm L, and determining a collision-free motion path of the mechanical arm L;
removing redundant nodes from the motion path of the mechanical arm R by using a preset algorithm, recording the corresponding time of the joint nodes of the mechanical arm R in the motion trail, removing the redundant nodes from the collision-free motion path of the mechanical arm L, and recording the corresponding time of the joint nodes of the collision-free motion path of the mechanical arm L in the motion trail;
carrying out reverse splitting operation on the motion paths of the mechanical arm R and the mechanical arm L, and determining the motion path of the mechanical arm R and the collision-free path of the mechanical arm L;
carrying out one-to-one collision detection on the motion path time of the mechanical arm R and the mechanical arm L, and determining the moment t of the first collision of the mechanical arm R and the mechanical arm LpDetermining the joint configuration of the mechanical arm R and the mechanical arm L at the moment of collision of the motion paths of the mechanical arms;
determining the optimal node tracks of the motion paths of the mechanical arm R and the mechanical arm L according to the joint configurations of the mechanical arm R and the mechanical arm L at the time of collision of the collision-free path and the motion path;
and repeating the reverse splitting operation of the motion paths of the mechanical arm R and the mechanical arm L to obtain the path of the optimal node track of the mechanical arm R and the mechanical arm L until the optimized path of the mechanical arm R and the mechanical arm L does not collide any more, and determining the reverse splitting path.
Wherein, the preset algorithm is a greedy algorithm, and comprises the following steps:
the method comprises the following steps: the first node p of the movement path1And the last node pnAre connected in a straight line, and connect the nodes p1And node pnAdding the path node into the optimized path node set, and judging whether the mechanical arm is in the environment barrier state or not when the mechanical arm is positioned at the path point on the straight lineCollision of obstacle, if no collision occurs, the optimized path only contains p1And pnOutput optimized { p1，pn}；
Step two: let i equal 2, j equal n judge the node piAnd node pjWhether the straight line connection between the two collides with the environmental barrier or not, and if the straight line connection between the two collides with the environmental barrier, p is determinediAdding the optimized path set to carry out the fourth step, if the node piAnd node pjThe straight line connection between the two parts collides with the environmental barrier, and then the step three is carried out;
step three: judging whether i is smaller than j, if i is smaller than j, making i equal to i +1 and j equal to n and returning to the second step, otherwise, outputting an optimized path set and ending the optimization process;
step four: and judging whether j is greater than 1, if j is greater than 1, making i equal to 1, and if j equal to i, returning to the step two, otherwise, outputting the optimized path set, and ending the optimization process.
The moving paths of the mechanical arm R and the mechanical arm L are reversely split, the distance between adjacent path points is divided by the time interval between the adjacent path points to obtain the moving speed of the mechanical arm between the path points, and the mechanical arm R and the mechanical arm L pass through the path at a constant speed.
The motion path is calculated by using a sampling motion planning algorithm and is composed of a plurality of path points.
The method also adopts linear interpolation to calculate the track between adjacent path points, so that the mechanical arm moves at a constant speed between each path node.
The specific implementation method comprises the following steps:
the method comprises the following steps: the two mechanical arms are called a mechanical arm R and a mechanical arm L, the mechanical arm R is subjected to motion planning by utilizing a sampling motion planning algorithm (RRT, PRM and the like), the planned path of the mechanical arm R is used as a dynamic barrier of the mechanical arm L to plan the motion of a secondary arm, and a collision-free motion path { p is obtainedL(n)}、{pR(n)}，n＝0，1，2，...，N；
The motion path calculated by the sampling motion planning algorithm is composed of a series of pathsThe path point structure needs to plan the motion path in order to enable the mechanical arm to execute the motion path, the planning can enable the path points to be denser and give path time information by interpolating between path nodes, the path between adjacent path points is calculated by adopting simple linear interpolation, the mechanical arm moves at a constant speed between each path node, and the motion paths { p) of the two mechanical arms relative to the time are calculatedL(t)}、{pR(t)}，t∈[tstart，tend]。
Step two: under the condition that collision between the two arms is not considered, the paths of the two mechanical arms are optimized by a greedy algorithm to remove redundant nodes, and the optimized motion paths { p) of the two mechanical arms, which do not collide with the environmental barrier respectively, are obtainedL′(n)}、{pR′(N) }, wherein N is 0, 1, 2,.., N', and the corresponding time { t < t > of the optimized joint node in the motion trail before optimization is recordedL′(n)}、{tR′(N) }, N is 0, 1, 2.., the flow of the N' greedy algorithm is shown in fig. 2, and comprises the following steps:
step two, firstly: the first node p of the path1And the last node pnThe two nodes are added into the optimized path node set, whether the mechanical arm collides with an environmental barrier when positioned at the path point on the straight line is judged, and if the mechanical arm does not collide with the environmental barrier, the optimized path only contains p1And pnThe output optimized path is { p1，pnFinishing the optimization process, otherwise, performing the following steps;
step two: let i equal 2, j equal n judge the node piAnd node pjWhether the straight line connection between the two collides with the environmental barrier or not, and if the straight line connection between the two collides with the environmental barrier, p is determinediAdding the optimized path set to perform the second step and the fourth step, if the node piAnd node pjThe straight line connection between the two parts collides with the environmental barrier, and then the step two and the step three are carried out;
step two and step three: judging whether i is smaller than j, if i is smaller than j, making i equal to i +1 and j equal to n and returning to the second step, otherwise, outputting an optimized path set and ending the optimization process;
step two, four: and judging whether j is greater than 1, if j is greater than 1, making i equal to 1, and if j equal to i, returning to the second step, otherwise, outputting the optimized path set, and ending the optimization process.
Step three: in order to ensure that the two mechanical arms do not collide, the path is subjected to reverse splitting operation, the distance between adjacent path points is divided by the time interval between the adjacent path points to obtain the movement speed of the mechanical arms between the path points, and the mechanical arms pass through the path at a constant speed according to the movement speed. The time interval between adjacent waypoints may be defined by tL′(n)}、{tR′(N) }, N ═ 0, 1, 2. The trajectory planning according to the method can ensure that the time of each node is consistent with the time of the corresponding node on the initial path, and the motion trajectory of the two mechanical arms which do not collide with the environmental barrier is expressed as { p }L′(t)}、{pR′(t)}，t∈[tstart,tend]。
Step four: the tracks of the two mechanical arms are in one-to-one correspondence with time to carry out collision detection, and the first time of collision t is foundpRespectively finding out the joint configuration of the two mechanical arms at the collision occurrence moment from the initial motion track
Step five: then at { pL′(n)}、{pR′(N) }, where N is 0, 1, 2.., and N', the closest node before and after the collision time is found
Are respectively paired
And
three nodes are used for local track planning to obtainReplaces the section of track corresponding to the collision occurrence time, and will use
Joining an optimal node group { pL′(n)}、{pR′(n) } adding tpJoin time set tL′}、{tR′In (c) }.
Step six: and repeating the fourth step and the fifth step until the optimized paths of the two mechanical arms do not collide any more, wherein the flow and the schematic diagram of the reverse splitting path are respectively shown in fig. 3 and fig. 4.
The following examples are used to demonstrate the beneficial effects of the present invention:
the two mechanical arms adopted in the experiment are isomorphic anthropomorphic mechanical arms with degrees of freedom, and the initial configuration and the target configuration of the mechanical arm motion planning are respectively shown in fig. 5 and fig. 6.
Based on a sampling motion planning algorithm, decoupling motion planning is performed on the two mechanical arms to obtain motion paths of the two mechanical arms, path nodes are subjected to linear interpolation, and joint motion tracks given with time information are shown in fig. 7.
The joint angles of the path nodes are mapped to the working space by using the positive kinematic equation of the mechanical arm, and the motion path of the mechanical arm end effector in the working space is obtained as shown in fig. 8.
Next, the node optimization algorithm provided by the invention is used for carrying out node optimization on the initial joint motion curve, and carrying out uniform linear interpolation between the path nodes with redundant nodes removed to obtain a joint motion curve as shown in fig. 9.
The joint angles of the path nodes are mapped to the working space by using the positive kinematic equation of the mechanical arm, the motion path of the mechanical arm end effector in the working space is obtained as shown in fig. 10, and it can be seen that the motion path of the mechanical arm end effector before the node optimization has more turning and longer path, the motion after the node optimization is more continuous, and the path is more efficient.
The present invention further provides a system 200 for calculating a reverse split of a redundant node of a robot arm path, as shown in fig. 11, including:
a motion path determining unit 201, which is used for performing motion planning on the mechanical arm R, determining a motion path of the mechanical arm R, taking the motion path of the mechanical arm R as a dynamic obstacle of the mechanical arm L, and determining a collision-free motion path of the mechanical arm L;
the movement path recording unit 202 is used for removing redundant nodes from the movement path of the mechanical arm R by using a preset algorithm, recording the corresponding time of the joint nodes of the mechanical arm R in the movement track, removing redundant nodes from the collision-free movement path of the mechanical arm L, and recording the corresponding time of the joint nodes of the collision-free movement path of the mechanical arm L in the movement track;
the reverse splitting unit 203 is used for performing reverse splitting operation on the motion paths of the mechanical arm R and the mechanical arm L and determining the motion path of the mechanical arm R and the collision-free path of the mechanical arm L;
the joint configuration unit 204 performs one-to-one collision detection on the motion path time of the mechanical arm R and the mechanical arm L, and determines the time t of the first collision of the mechanical arm R and the mechanical arm LpDetermining the joint configuration of the mechanical arm R and the mechanical arm L at the moment of collision of the motion paths of the mechanical arms;
an optimal node determining unit 205 that determines optimal node trajectories of the motion paths of the mechanical arm R and the mechanical arm L according to the joint configurations of the mechanical arm R and the mechanical arm L at the time of collision between the collision-free path and the motion path;
and a reverse splitting path determining unit 206, which repeats the reverse splitting operation of the motion paths of the mechanical arm R and the mechanical arm L, obtains the path of the optimal node trajectory of the mechanical arm R and the mechanical arm L until the optimized path of the mechanical arm R and the mechanical arm L does not collide with each other, and determines the reverse splitting path.
Wherein, the preset algorithm is a greedy algorithm, and comprises the following steps:
the method comprises the following steps: the first node p of the movement path1And the last node pnAre connected in a straight line, and connect the nodes p1And node pnAdding the path node set after optimization, and judging the position of the mechanical armWhether collision occurs with the environmental barrier or not at the path point on the straight line, if no collision occurs, the optimized path only contains p1And pnOutput optimized { p1，pn}；
Step two: let i equal 2, j equal n judge the node piAnd node pjWhether the straight line connection between the two collides with the environmental barrier or not, and if the straight line connection between the two collides with the environmental barrier, p is determinediAdding the optimized path set to carry out the fourth step, if the node piAnd node pjThe straight line connection between the two parts collides with the environmental barrier, and then the step three is carried out;
step three: judging whether i is smaller than j, if i is smaller than j, making i equal to i +1 and j equal to n and returning to the second step, otherwise, outputting an optimized path set and ending the optimization process;
step four: and judging whether j is greater than 1, if j is greater than 1, making i equal to 1, and if j equal to i, returning to the step two, otherwise, outputting the optimized path set, and ending the optimization process.
The moving paths of the mechanical arm R and the mechanical arm L are reversely split, the distance between adjacent path points is divided by the time interval between the adjacent path points to obtain the moving speed of the mechanical arm between the path points, and the mechanical arm R and the mechanical arm L pass through the path at a constant speed.
The motion path is calculated by using a sampling motion planning algorithm and is composed of a plurality of path points.
The system also adopts linear interpolation to calculate the track between adjacent path points, so that the mechanical arm moves at a constant speed between the path points.
The invention ensures that redundant path nodes are removed on the premise that two arms do not collide. Simulation performed in the MATLAB robot kit shows that the node optimization algorithm significantly improves the quality of the path.
As will be appreciated by one skilled in the art, embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein. The scheme in the embodiment of the application can be implemented by adopting various computer languages, such as object-oriented programming language Java and transliterated scripting language JavaScript.
The present application is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the application. It will be understood that each flow and/or block of the flow diagrams and/or block diagrams, and combinations of flows and/or blocks in the flow diagrams and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
While the preferred embodiments of the present application have been described, additional variations and modifications in those embodiments may occur to those skilled in the art once they learn of the basic inventive concepts. Therefore, it is intended that the appended claims be interpreted as including preferred embodiments and all alterations and modifications as fall within the scope of the application.
It will be apparent to those skilled in the art that various changes and modifications may be made in the present application without departing from the spirit and scope of the application. Thus, if such modifications and variations of the present application fall within the scope of the claims of the present application and their equivalents, the present application is intended to include such modifications and variations as well.
Claims (10)
1. A method of reverse split computation of a robotic arm path redundant node, the method comprising:
performing motion planning on the mechanical arm R, determining a motion path of the mechanical arm R, taking the motion path of the mechanical arm R as a dynamic barrier of the mechanical arm L, and determining a collision-free motion path of the mechanical arm L;
removing redundant nodes from the motion path of the mechanical arm R by using a preset algorithm, recording the corresponding time of the joint nodes of the mechanical arm R in the motion trail, removing the redundant nodes from the collision-free motion path of the mechanical arm L, and recording the corresponding time of the joint nodes of the collision-free motion path of the mechanical arm L in the motion trail;
carrying out reverse splitting operation on the motion paths of the mechanical arm R and the mechanical arm L, and determining the motion path of the mechanical arm R and the collision-free path of the mechanical arm L;
carrying out one-to-one collision detection on the motion path time of the mechanical arm R and the mechanical arm L, and determining the moment t of the first collision of the mechanical arm R and the mechanical arm LpDetermining the joint configuration of the mechanical arm R and the mechanical arm L at the moment of collision of the motion paths of the mechanical arms;
determining the optimal node tracks of the motion paths of the mechanical arm R and the mechanical arm L according to the joint configurations of the mechanical arm R and the mechanical arm L at the time of collision of the collision-free path and the motion path;
and repeating the reverse splitting operation of the motion paths of the mechanical arm R and the mechanical arm L to obtain the path of the optimal node track of the mechanical arm R and the mechanical arm L until the optimized path of the mechanical arm R and the mechanical arm L does not collide any more, and determining the reverse splitting path.
2. The method of claim 1, the preset algorithm being a greedy algorithm comprising:
the method comprises the following steps: the first node p of the movement path1And the last node pnAre connected in a straight line, and connect the nodes p1And node pnAdding the path node into the optimized path node set, judging whether the mechanical arm collides with an environmental barrier when being positioned at a path point on a straight line, and if not, determining that the optimized path only contains p1And pnOutput optimized { p1，pn}；
Step two: let i equal 2, j equal n judge the node piAnd node pjWhether the straight line connection between the two collides with the environmental barrier or not, and if the straight line connection between the two collides with the environmental barrier, p is determinediAdding the optimized path set to carry out the fourth step, if the node piAnd node pjThe straight line connection between the two parts collides with the environmental barrier, and then the step three is carried out;
step three: judging whether i is smaller than j, if i is smaller than j, making i equal to i +1 and j equal to n and returning to the second step, otherwise, outputting an optimized path set and ending the optimization process;
step four: and judging whether j is greater than 1, if j is greater than 1, making i equal to 1, and if j equal to i, returning to the step two, otherwise, outputting the optimized path set, and ending the optimization process.
3. The method according to claim 1, wherein the movement paths of the robot arm R and the robot arm L are reversely split, and the distance between adjacent path points is divided by the time interval between adjacent path points to obtain the movement speed of the robot arm between the path points, so that the robot arm R and the robot arm L pass through the path at a constant speed.
4. The method of claim 1, wherein the motion path is a motion path calculated using a sampled motion planning algorithm and is composed of a plurality of path points.
5. The method of claim 1, further comprising calculating a trajectory between adjacent path points using linear interpolation to allow the robotic arm to move at a constant velocity between each path point.
6. A system for reverse split computation of a robotic arm path redundant node, the system comprising:
determining a motion path unit, performing motion planning on the mechanical arm R, determining a motion path of the mechanical arm R, taking the motion path of the mechanical arm R as a dynamic obstacle of the mechanical arm L, and determining a collision-free motion path of the mechanical arm L;
the motion path recording unit is used for removing redundant nodes from the motion path of the mechanical arm R by using a preset algorithm, recording the corresponding time of the joint nodes of the mechanical arm R in the motion trail, removing the redundant nodes from the collision-free motion path of the mechanical arm L and recording the corresponding time of the joint nodes of the collision-free motion path of the mechanical arm L in the motion trail;
the reverse splitting unit is used for performing reverse splitting operation on the motion paths of the mechanical arm R and the mechanical arm L and determining the motion path of the mechanical arm R and the collision-free path of the mechanical arm L;
a joint configuration unit for performing one-to-one collision detection on the motion path time of the mechanical arm R and the mechanical arm L, and determining the first collision time t of the mechanical arm R and the mechanical arm LpDetermining the joint configuration of the mechanical arm R and the mechanical arm L at the moment of collision of the motion paths of the mechanical arms;
determining an optimal node unit, and determining optimal node tracks of the motion paths of the mechanical arm R and the mechanical arm L according to the joint configurations of the mechanical arm R and the mechanical arm L at the collision-free path and the motion path at the collision moment;
determining a reverse splitting path unit, repeating the reverse splitting operation of the motion paths of the mechanical arm R and the mechanical arm L, obtaining the path of the optimal node track of the mechanical arm R and the mechanical arm L until the optimized path of the mechanical arm R and the mechanical arm L does not collide any more, and determining the reverse splitting path.
7. The system of claim 6, the preset algorithm being a greedy algorithm comprising:
the method comprises the following steps: the first node p of the movement path1And the last node pnAre connected in a straight line, and connect the nodes p1And node pnAdding the path node into the optimized path node set, judging whether the mechanical arm collides with an environmental barrier when being positioned at a path point on a straight line, and if not, determining that the optimized path only contains p1And pnOutput optimized { p1，pn}；
Step two: let i equal 2, j equal n judge the node piAnd node pjWhether the straight line connection between the two collides with the environmental barrier or not, and if the straight line connection between the two collides with the environmental barrier, p is determinediAdding the optimized path set to carry out the fourth step, if the node piAnd node pjThe straight line connection between the two parts collides with the environmental barrier, and then the step three is carried out;
step three: judging whether i is smaller than j, if i is smaller than j, making i equal to i +1 and j equal to n and returning to the second step, otherwise, outputting an optimized path set and ending the optimization process;
step four: and judging whether j is greater than 1, if j is greater than 1, making i equal to 1, and if j equal to i, returning to the step two, otherwise, outputting the optimized path set, and ending the optimization process.
8. The system according to claim 6, wherein the movement paths of the robot arm R and the robot arm L are reversely split, and the distance between adjacent path points is divided by the time interval between adjacent path points to obtain the movement speed of the robot arm between the path points, so that the robot arm R and the robot arm L pass through the path at a constant speed.
9. The system of claim 6, wherein the motion path is a motion path calculated using a sampled motion planning algorithm and is composed of a plurality of path points.
10. The system of claim 6, further comprising calculating a trajectory between adjacent path points using linear interpolation to allow the robotic arm to move at a constant velocity between each path point.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110733121.7A CN113442170B (en) | 2021-06-28 | 2021-06-28 | Method and system for reversely splitting and calculating redundant nodes of mechanical arm path |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110733121.7A CN113442170B (en) | 2021-06-28 | 2021-06-28 | Method and system for reversely splitting and calculating redundant nodes of mechanical arm path |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113442170A true CN113442170A (en) | 2021-09-28 |
| CN113442170B CN113442170B (en) | 2023-12-01 |
Family
ID=77814317
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110733121.7A Active CN113442170B (en) | 2021-06-28 | 2021-06-28 | Method and system for reversely splitting and calculating redundant nodes of mechanical arm path |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113442170B (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116100561A (en) * | 2023-04-10 | 2023-05-12 | 国网浙江省电力有限公司宁波供电公司 | Automatic wiring track control method and system |
| CN117021101A (en) * | 2023-08-24 | 2023-11-10 | 中国矿业大学 | Multi-arm path planning method for belt conveyor dismounting robot |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106737671A (en) * | 2016-12-21 | 2017-05-31 | 西安科技大学 | The bilayer personification motion planning method of seven degrees of freedom copy man mechanical arm |
| CN108656117A (en) * | 2018-05-21 | 2018-10-16 | 武汉理工大学 | The mechanical arm spacing track optimizing method of optimal time under a kind of multi-constraint condition |
| CN108705532A (en) * | 2018-04-25 | 2018-10-26 | 中国地质大学（武汉） | A kind of mechanical arm obstacle-avoiding route planning method, equipment and storage device |
| CN110181515A (en) * | 2019-06-10 | 2019-08-30 | 浙江工业大学 | A kind of double mechanical arms collaborative assembly working path planing method |
| US20190361452A1 (en) * | 2018-05-22 | 2019-11-28 | King Fahd University Of Petroleum And Minerals | Method and system for controlling a vehicle |
| CN110587600A (en) * | 2019-08-20 | 2019-12-20 | 南京理工大学 | Point cloud-based autonomous path planning method for live working robot |
| CN111168675A (en) * | 2020-01-08 | 2020-05-19 | 北京航空航天大学 | Dynamic obstacle avoidance motion planning method for mechanical arm of household service robot |
| CN112549025A (en) * | 2020-11-27 | 2021-03-26 | 北京工业大学 | Coordination diagram double-arm cooperative control method based on fusion of human body kinematic constraints |
-
2021
- 2021-06-28 CN CN202110733121.7A patent/CN113442170B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106737671A (en) * | 2016-12-21 | 2017-05-31 | 西安科技大学 | The bilayer personification motion planning method of seven degrees of freedom copy man mechanical arm |
| CN108705532A (en) * | 2018-04-25 | 2018-10-26 | 中国地质大学（武汉） | A kind of mechanical arm obstacle-avoiding route planning method, equipment and storage device |
| CN108656117A (en) * | 2018-05-21 | 2018-10-16 | 武汉理工大学 | The mechanical arm spacing track optimizing method of optimal time under a kind of multi-constraint condition |
| US20190361452A1 (en) * | 2018-05-22 | 2019-11-28 | King Fahd University Of Petroleum And Minerals | Method and system for controlling a vehicle |
| CN110181515A (en) * | 2019-06-10 | 2019-08-30 | 浙江工业大学 | A kind of double mechanical arms collaborative assembly working path planing method |
| CN110587600A (en) * | 2019-08-20 | 2019-12-20 | 南京理工大学 | Point cloud-based autonomous path planning method for live working robot |
| CN111168675A (en) * | 2020-01-08 | 2020-05-19 | 北京航空航天大学 | Dynamic obstacle avoidance motion planning method for mechanical arm of household service robot |
| CN112549025A (en) * | 2020-11-27 | 2021-03-26 | 北京工业大学 | Coordination diagram double-arm cooperative control method based on fusion of human body kinematic constraints |
Non-Patent Citations (1)
| Title |
|---|
| 白玉昊: "面向双臂协调的运动规划方法研究", 中国优秀硕士论文全文数据库, pages 140 * |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116100561A (en) * | 2023-04-10 | 2023-05-12 | 国网浙江省电力有限公司宁波供电公司 | Automatic wiring track control method and system |
| CN116100561B (en) * | 2023-04-10 | 2023-09-05 | 国网浙江省电力有限公司宁波供电公司 | Automatic wiring track control method and system |
| CN117021101A (en) * | 2023-08-24 | 2023-11-10 | 中国矿业大学 | Multi-arm path planning method for belt conveyor dismounting robot |
| CN117021101B (en) * | 2023-08-24 | 2024-03-19 | 中国矿业大学 | Multi-arm path planning method for belt conveyor dismounting robot |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113442170B (en) | 2023-12-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110228069B (en) | Online obstacle avoidance motion planning method for mechanical arm | |
| CN109343345B (en) | Mechanical arm polynomial interpolation track planning method based on QPSO algorithm | |
| CN113442170A (en) | Method and system for reverse split calculation of redundant node of mechanical arm path | |
| CN110509279B (en) | Motion path planning method and system of humanoid mechanical arm | |
| CN110196590B (en) | Time optimal trajectory planning method for robot path tracking | |
| CN111546347B (en) | Mechanical arm path planning method suitable for dynamic environment | |
| CN102662350B (en) | Track teaching and planning method of master-slave mode multi-robot cooperative system | |
| JP2019517929A (en) | Trajectory planning method of point-to-point movement in robot joint space | |
| Bazaz et al. | Minimum time on-line joint trajectory generator based on low order spline method for industrial manipulators | |
| CN112083727B (en) | Multi-autonomous system distributed collision avoidance formation control method based on speed obstacle | |
| CN109683615A (en) | The speed look-ahead approach and robot controller in the path that robot continuously moves | |
| Snape et al. | Smooth coordination and navigation for multiple differential-drive robots | |
| CN110561419A (en) | arm-shaped line constraint flexible robot track planning method and device | |
| Fallah et al. | Depth-based visual predictive control of tendon-driven continuum robots | |
| Zhao et al. | Trajectory smoothing using jerk bounded shortcuts for service manipulator robots | |
| KR101981641B1 (en) | Method and system for formation control of multiple mobile robots | |
| CN113325852A (en) | Leader follower mode-based control method for formation change of multiple intelligent agents in advancing process | |
| CN113290553A (en) | Trajectory generation device, multi-link system, and trajectory generation method | |
| Tangpattanakul et al. | Optimal trajectory of robot manipulator using harmony search algorithms | |
| Icer et al. | Cost-optimal composition synthesis for modular robots | |
| CN113442142B (en) | Smoothing method for Cartesian space motion trail of six-axis serial industrial robot | |
| CN107016209B (en) | Industrial robot and guide rail collaborative planning method | |
| Jiang et al. | Contact-aware non-prehensile manipulation for object retrieval in cluttered environments | |
| CN113276109B (en) | Dual-mechanical-arm decoupling motion planning method and system based on RRT algorithm | |
| CN114326726A (en) | Formation path planning control method based on A and improved artificial potential field method |
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 |