CN116117822A - RRT mechanical arm track planning method based on non-obstacle space probability potential field sampling - Google Patents
RRT mechanical arm track planning method based on non-obstacle space probability potential field sampling Download PDFInfo
- Publication number
- CN116117822A CN116117822A CN202310214664.7A CN202310214664A CN116117822A CN 116117822 A CN116117822 A CN 116117822A CN 202310214664 A CN202310214664 A CN 202310214664A CN 116117822 A CN116117822 A CN 116117822A
- Authority
- CN
- China
- Prior art keywords
- path
- mechanical arm
- node
- sampling
- obstacle
- 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
- 238000005070 sampling Methods 0.000 title claims abstract description 75
- 238000000034 method Methods 0.000 title claims abstract description 52
- 238000001514 detection method Methods 0.000 claims abstract description 17
- 230000009471 action Effects 0.000 claims abstract description 7
- 238000009499 grossing Methods 0.000 claims abstract description 4
- 238000012216 screening Methods 0.000 claims abstract description 3
- 230000001133 acceleration Effects 0.000 claims description 12
- 230000004888 barrier function Effects 0.000 claims description 7
- 238000012217 deletion Methods 0.000 claims description 6
- 230000037430 deletion Effects 0.000 claims description 6
- 238000005381 potential energy Methods 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 6
- 230000008859 change Effects 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 5
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000000750 progressive effect Effects 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000013138 pruning Methods 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
- 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
-
- 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
-
- 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/1628—Programme controls characterised by the control loop
- B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
The invention provides an RRT mechanical arm track planning method based on non-obstacle space probability potential field sampling, which comprises the steps of generating sampling points based on a non-obstacle space, namely screening the sampling points by judging whether the sampling points fall into the obstacle space or not, and avoiding redundant path generation and unnecessary path collision detection; the planning method further comprises a probability potential field greedy sampling strategy, namely, on the premise of guaranteeing that the probability of the algorithm is complete, the algorithm is endowed with target guidance of the preset probability to accelerate path generation, path collision detection is reduced through the action of a gravitational field and a repulsive force field, and the generated path direction is relatively better; the planning method further comprises a redundant node deleting strategy, namely optimizing a path generated by algorithm backtracking, deleting redundant nodes in the path, and enabling the generated path to be better; smoothing the optimized path through an interpolation polynomial, so that the path is more in line with the motion of an actual mechanical arm; the method can generate the smooth path more efficiently, and has very high industrial application value.
Description
Technical Field
The invention relates to the technical field of mechanical arms, in particular to an RRT mechanical arm track planning method based on non-obstacle space probability potential field sampling.
Background
With the continuous development of artificial intelligence and intelligent manufacturing technology, future industrial manufacturing must be highly intelligent, informative and co-operating with human-machine, and the working scenario faced by the mechanical arm must be complex and uncertain. As the application environment of the industrial mechanical arm is more and more complex, the effect of the mechanical arm obstacle avoidance path planning is more and more important, and the mechanical arm obstacle avoidance path planning means that the mechanical arm can plan a collision-free path in a limited time under the condition that an obstacle exists in a working space.
The problems of the prior art and the existing technology are as follows:
1. the track planning algorithm based on random sampling, namely a rapid expansion random tree (RRT), is a track planning method which can be widely applied to two-dimensional and high-dimensional space and has complete probability, and is widely applied to the obstacle avoidance path planning of the mechanical arm.
2. The RRT-Connect algorithm grows two random trees from the initial point and the target point simultaneously, has good search characteristics, and compared with the RRT algorithm, the RRT-Connect algorithm has obviously improved search speed and efficiency. The progressive optimal RRT algorithm guarantees computational complexity and progressive optimal solutions. The inform-RRT algorithm can return to the asymptotically optimal solution faster. The RRT algorithm generates randomness of sampling points, so that blind sampling in a working space is caused, the time cost for planning a feasible path by the algorithm is too high, a large number of redundant nodes are generated, the space cost is high, and the randomness is high.
3. The artificial potential field method (Artificial potential field method, APF) is excellent in robot working spaces without or with few obstacles, but the attraction and repulsion forces applied to the robot in complex obstacle working spaces by the artificial potential field are very easy to cancel each other, so that the robot falls into a local minimum.
4. The method of target bias in the invention patent CN202210671495.5 has a certain purpose in the condition of keeping the probability complete, shortens the search time, and optimizes the path by using the triangular pruning method, but the whole working space is sampled during the sampling, and the obstacle information is not considered in the target bias process, so a large number of invalid sampling points are generated and a large amount of time is consumed for collision detection.
5. The paper "mechanical arm obstacle avoidance planning combining RRT and artificial potential field method" combines RRT algorithm with artificial potential field, when the artificial potential field falls into minimum value, RRT sampling is used to generate virtual target point to jump out local minimum value.
6. The invention patent CN201810008810.X is easy to sink into local minima for the artificial potential field method, and the path planning is completed by adding virtual barriers and improving potential field functions so that the mechanical arm can jump out of the local minima.
7. The patent CN202210973726.8 sets up virtual target points to complete path planning for the problem of local minima, but the path sought by the virtual target points is not optimal, and after the local minima is trapped, the mechanical arm generates high vibration in the area, which causes strong physical impact to the mechanical arm.
Disclosure of Invention
The RRT mechanical arm track planning method based on non-obstacle space probability potential field sampling can generate a smooth path more efficiently, and has high industrial application value.
The invention adopts the following technical scheme.
The RRT mechanical arm track planning method based on non-obstacle space probability potential field sampling is used for a multi-link mechanical arm with multiple joints, and global path planning is carried out in a non-obstacle space by utilizing the combination of RRT and an artificial potential field;
the planning method further comprises a probability potential field greedy sampling strategy, namely, on the premise of guaranteeing that the probability of the algorithm is complete, the algorithm is endowed with target guidance of the preset probability to accelerate path generation, path collision detection is reduced through the action of a gravitational field and a repulsive force field, and the generated path direction is relatively better;
the planning method further comprises a redundant node deleting strategy, namely optimizing a path generated by algorithm backtracking, deleting redundant nodes in the path, and enabling the generated path to be better; and smoothing the optimized path through an interpolation polynomial, so that the path is more in line with the actual mechanical arm motion to reduce the severe motion friction loss.
The planning method comprises the following steps;
step S1: mathematical modeling of the mechanical arm: describing the pose of the mechanical arm, and establishing a mechanical arm connecting rod coordinate system to obtain a D-H model of the mechanical arm;
step S2: initializing to execute global path planning, specifically: initializing a tree T within a workspace, given an initial node q start ＝[x start ，y start ，z start ]Target node q goal ＝[x goal ，y goal ，z goal ]Expanding fixed step length, maximum iteration number max, obstacle space Ω obs A target point error allowance threshold thr;
step S3: and executing a probability potential field greedy sampling strategy, namely adopting non-obstacle space probability potential field sampling to generate sampling points: setting a random probability threshold value as P, and when the generated random probability P is smaller than P, randomly sampling by an algorithm to obtain a random sampling point q rand ＝[x rand ，y rand， z rand ]；
Step S4: collision detection: will q near To q new Performing collision detection on the generated path;
step S5: redundant node deletion policy: carrying out path re-planning on the obtained path by using a redundant node deleting strategy;
step S6: and optimizing the motion trail of the mechanical arm.
The step S3 comprises the following steps;
step S3-1: if randomly sampling point q rand Falls into the obstacle space omega obs Discarding the point, and randomly sampling again;
step S3-2: if q rand Falling within the non-obstacle space omega, according to traversing the nodes q of the whole spanning tree T ＝[x T ，y T ，z T ]Find q T And random sampling point q rand Is closest to node q near ＝[x near ，y nｅar ，z near ]The method comprises the following steps:
step S3-3: to be used for
Generating new nodes for the direction according to a fixed step length, expressed as a formula
q new ＝[x nｅw ，Y new ，z nｅw ]A second formula;
step S3-4: if the random probability P of the sampling point is greater than P, carrying out greedy sampling guided by an artificial potential field by an algorithm; then according to node q traversing the entire spanning tree T ＝[x T ，y T ，z T ]Find q T With target node q goal ＝[x goal ，y goal ，z goal ]Is far from the nearest node q near ＝[x near ，y near ，z near ]The method comprises the following steps:
step S3-5: compute node q near The resultant force potential energy U:
U＝U att +U rep a formula IV;
in the formula, the total attractive force born by the mechanical arm is expressed as the following formula:
U rep the total repulsive force applied to the mechanical arm:
wherein U is att (i) The gravity generated by the barriers in the space is received by each connecting rod of the mechanical arm; u (U) rep (i) Repulsive force generated by the barriers in the space is applied to each connecting rod of the mechanical arm; n is the number of the mechanical arm connecting rod subjected to the repulsive force of the obstacle; d, d i The Euclidean distance between the actual position of the corresponding connecting rod joint point of the mechanical arm and the obstacle; d, d o A repulsive force field action range for each connecting rod; d, d s For the safety distance of each connecting rod, k a Is the gravitational proportionality constant, k r Is the repulsive force proportionality constant;
step S3-6: after the resultant potential energy U is obtained, a fixed step length step is extended in the direction of a vector U to generate a new node q new ＝[x new ，y new ，z new ]If the potential field falls into a local minimum, the generation of the sampling point is abandoned, and random sampling is performed again.
The step S4 comprises the following steps;
step S4-1: route q near q new The equal step length is divided into n points, if one of the points falls into the space range of the obstacle, the collision is represented, ifWill q if no collision occurs new Store q in the random tree T new Considered as q near Is a parent node of (a); discarding q if a collision is detected new Returning to step S3 to regenerate q new ；
Step S4-2: repeating the steps S3-S4 until a new node q is generated new And q goal The Euclidean distance between the two points is smaller than a threshold thr, namely the point where the target point is found in the representative path planning; and backtracking the random tree T in the random tree T according to the father-son relationship of each node to find the planned path.
The step S5 specifically comprises the following steps:
carrying out path re-planning on the obtained path by using a redundant node deleting strategy; storing the initial path node in an array of nums, nums= [ q ] start ,q 2 ,q 3 ,…,q goal ]From an initial point q on a nums basis start To q goal Path re-planning, i.e. computing root node q in the array root To the next node q next Euclidean distance:
d＝||q root -q next formula eight;
if the Euclidean distance d is less than or equal to the original path length l:
l=step x (n-m) formula nine;
n represents q in array num next The subscript at this time, m represents q in the array nums root Subscripts at this time. If its Euclidean distance is smaller than the original path length and the root node q root To the next node q next If the path of (a) does not collide with the obstacle, q is deleted root And q next Redundant nodes between; and q is set to next Updated to q next Is the next node of (a); if the Euclidean distance d is greater than the original path length l or the root node q root To the next node q next Collision of the path of (c) with the obstacle, q root Updated to q next Repeating the above steps until q next Is q goal Then the reprofiling of the path is complete.
The step S6 specifically includes:
solving discrete nodes with redundant nodes deleted from Cartesian space coordinates through inverse kinematics to obtain joint angles [ theta ] of the mechanical arm at each discrete point 1 ,θ 2 ,θ 3 ,θ 4 ,θ 5 ,θ 6 ]The method comprises the steps of carrying out a first treatment on the surface of the Using polynomial interpolation to process path nodes, the polynomial programming function of the joint angles of the track of the mechanical arm is as follows:
θ i (t)＝a 0 +...+a i t i +...+a n t n formula ten;
the function of a is obtained 0 ,…,a i ,…,a n The functional formula may be determined; the function is led to time, and the joint angular velocity expression is obtained as follows:
deriving the time again to obtain a joint angular acceleration expression as follows:
assume that the joint angle of the mechanical arm is theta when the mechanical arm starts to move 0 The angle at which the joint movement stops is theta f ；
Let the whole movement time be t f The initial joint angular velocity is
Ending joint angular velocity of +.>
Initial angular acceleration of +.>
Ending joint angular acceleration of +.>
Obtaining polynomial coefficient a 0 ,…,a i ,…,a n Substituting the angle into a polynomial equation to obtain a smooth mechanical arm joint change angle, and completing final mechanical arm path planning.
The track planning is mechanical arm obstacle avoidance path planning, namely the mechanical arm plans a collision-free path within a limited time range under the condition that an obstacle exists in a working space.
The invention has the beneficial effects that:
(1) The invention provides a non-obstacle space probability potential field sampling-based method, which not only ensures the probability completeness of RRT sampling, but also adopts greedy sampling guided by artificial potential fields to endow an algorithm with a certain probability of target bias. Collision detection and generation of redundant sampling paths are reduced, and path generation time is shortened.
(2) The redundant node deleting strategy is provided, and the length of a path can be shorter and better through deleting the redundant node.
(3) Polynomial interpolation track optimization is provided, so that the generated final track is smoother and is suitable for mechanical arm operation.
Drawings
The invention is described in further detail below with reference to the attached drawings and detailed description:
FIG. 1 is a schematic diagram of a robot arm according to the present invention (for example, an ABB-1200 robot arm, a table in the diagram is a D-H parameter table of the robot arm according to the present invention);
FIG. 2 is a schematic diagram of an overall workspace sampling in accordance with the invention;
FIG. 3 is a schematic illustration of a non-obstacle workspace sampling in accordance with the invention;
FIG. 4 is a schematic diagram of the trace after redundant node deletion according to the present invention;
FIG. 5 is a schematic diagram of a trace after a fifth order polynomial process according to the present invention;
in the figure: 1-a connecting rod; 2-joint.
Detailed Description
As shown in the figure, the RRT mechanical arm track planning method based on non-obstacle space probability potential field sampling is used for a multi-link 1 mechanical arm with multiple joints 2, and global path planning is performed by combining RRT and an artificial potential field in a non-obstacle space, and the planning method comprises the steps of generating sampling points based on the non-obstacle space, namely screening the sampling points by whether the sampling points fall into the obstacle space or not, so as to avoid redundant path generation and unnecessary path collision detection;
the planning method further comprises a probability potential field greedy sampling strategy, namely, on the premise of guaranteeing that the probability of the algorithm is complete, the algorithm is endowed with target guidance of the preset probability to accelerate path generation, path collision detection is reduced through the action of a gravitational field and a repulsive force field, and the generated path direction is relatively better;
the planning method further comprises a redundant node deleting strategy, namely optimizing a path generated by algorithm backtracking, deleting redundant nodes in the path, and enabling the generated path to be better; and smoothing the optimized path through an interpolation polynomial, so that the path is more in line with the actual mechanical arm motion to reduce the severe motion friction loss.
The planning method comprises the following steps;
step S1: mathematical modeling of the mechanical arm: describing the pose of the mechanical arm, and establishing a mechanical arm connecting rod coordinate system to obtain a D-H model of the mechanical arm;
step S2: initializing to execute global path planning, specifically: initializing a tree T within a workspace, given an initial node q start ＝[x start ，y start ，z start ]Target node q goal ＝[x goal ，y goal ，z goal ]Expanding fixed step length, maximum iteration number max, obstacle space Ω obs A target point error allowance threshold thr;
step S3: and executing a probability potential field greedy sampling strategy, namely adopting non-obstacle space probability potential field sampling to generate sampling points: setting a random probability threshold value as P, and when the generated random probability P is smaller than P, randomly sampling by an algorithm to obtain a random sampling point q rand ＝[x rand ，y rand ，z rand ]；
Step S4: collision detection: will q near To q new Performing collision detection on the generated path;
step S5: redundant node deletion policy: carrying out path re-planning on the obtained path by using a redundant node deleting strategy;
step S6: and optimizing the motion trail of the mechanical arm.
The step S3 comprises the following steps;
step S3-1: if randomly sampling point q rand Falls into the obstacle space omega obs Discarding the point, and randomly sampling again;
step S3-2: if q rand Falling within the non-obstacle space omega, according to traversing the nodes q of the whole spanning tree T ＝[x T ，y T ，z T ]Find q T And random sampling point q rand Is closest to node q near ＝[x near ，y near ，z near ]The method comprises the following steps:
step S3-3: to be used for
Generating new nodes for the direction according to a fixed step length, expressed as a formula
q new ＝[x new ，y new ，z new ]A second formula;
step S3-4: if the random probability P of the sampling point is greater than P, carrying out greedy sampling guided by an artificial potential field by an algorithm; then according to node q traversing the entire spanning tree T ＝[x T ，y T ，z T ]Find q T With target node q goal ＝[x goal ，y goal ，z goal ]Is far from the nearest node q near ＝[x near ，y near ，z near ]The method comprises the following steps:
step S3-5: compute node q near The resultant force potential energy U:
U＝U att +U rep a formula IV;
in the formula, the total attractive force born by the mechanical arm is expressed as the following formula:
U rep the total repulsive force applied to the mechanical arm:
wherein U is att (i) The gravity generated by the barriers in the space is received by each connecting rod of the mechanical arm; u (U) rep (i) Repulsive force generated by the barriers in the space is applied to each connecting rod of the mechanical arm; n is the number of the mechanical arm connecting rod subjected to the repulsive force of the obstacle; d, d i The Euclidean distance between the actual position of the corresponding connecting rod joint point of the mechanical arm and the obstacle; d, d o A repulsive force field action range for each connecting rod; d, d s For the safety distance of each connecting rod, k a Is the gravitational proportionality constant, k r Is the repulsive force proportionality constant;
step S3-6: after the resultant potential energy U is obtained, a fixed step length step is extended in the direction of a vector U to generate a new node q new ＝[x new ，y new ，z new ]If the potential field falls into a local minimum, the generation of the sampling point is abandoned,and randomly sampling again.
The step S4 comprises the following steps;
step S4-1: route q near q new The equal step length is divided into n points, if one point falls into the space range of the obstacle, collision is represented, if no collision is generated, q is represented new Store q in the random tree T new Considered as q near Is a parent node of (a); discarding q if a collision is detected new Returning to step S3 to regenerate q new ；
Step S4-2: repeating the steps S3-S4 until a new node q is generated new And q goal The Euclidean distance between the two points is smaller than a threshold thr, namely the point where the target point is found in the representative path planning; and backtracking the random tree T in the random tree T according to the father-son relationship of each node to find the planned path.
The step S5 specifically comprises the following steps:
carrying out path re-planning on the obtained path by using a redundant node deleting strategy; storing the initial path node in an array of nums, nums= [ q ] start ,q 2 ,q 3 ,…,q goal ]From an initial point q on a nums basis start To q goal Path re-planning, i.e. computing root node q in the array root To the next node q next Euclidean distance:
d＝||q root -q next formula eight;
if the Euclidean distance d is less than or equal to the original path length l:
l=step x (n-m) formula nine;
n represents q in array num next The subscript at this time, m represents q in the array nums root Subscripts at this time. If its Euclidean distance is smaller than the original path length and the root node q root To the next node q next If the path of (a) does not collide with the obstacle, q is deleted root And q next Redundant nodes between; and q is set to next Updated to q next Is the next node of (a); if the Euclidean distance d is greater than the original path length l or the root node q root To the next node q next Is a path of (a)If the obstacle collides, q is as follows root Updated to q next Repeating the above steps until q next Is q goal Then the reprofiling of the path is complete.
The step S6 specifically includes:
solving discrete nodes with redundant nodes deleted from Cartesian space coordinates through inverse kinematics to obtain joint angles [ theta ] of the mechanical arm at each discrete point 1 ,θ 2 ,θ 3 ,θ 4 ,θ 5 ,θ 6 ]The method comprises the steps of carrying out a first treatment on the surface of the Using polynomial interpolation to process path nodes, the polynomial programming function of the joint angles of the track of the mechanical arm is as follows:
θ i (t)＝a 0 +...+a i t i +...+a n t n formula ten;
the function of a is obtained 0 ,…,a i ,…,a n The functional formula may be determined; the function is led to time, and the joint angular velocity expression is obtained as follows:
deriving the time again to obtain a joint angular acceleration expression as follows:
assume that the joint angle of the mechanical arm is theta when the mechanical arm starts to move 0 The angle at which the joint movement stops is theta f ；
Let the whole movement time be t f The initial joint angular velocity is
Ending joint angular velocity of +.>
Initial angular acceleration of +.>
Ending joint angular acceleration of +.>
Obtaining polynomial coefficient a 0 ,…,a i ,…,a n Substituting the angle into a polynomial equation to obtain a smooth mechanical arm joint change angle, and completing final mechanical arm path planning.
The track planning is mechanical arm obstacle avoidance path planning, namely the mechanical arm plans a collision-free path within a limited time range under the condition that an obstacle exists in a working space.
Examples:
in the example, step S1 is performed on the ABB-1200 mechanical arm, as shown in FIG. 1, the pose is described by a D-H method, and a mechanical arm connecting rod coordinate system is established, so that a D-H model of the mechanical arm is obtained, as shown in a table in the figure.
In the present example, in the collision detection of step S4, q near To q new The generated path performs collision detection. Wherein collision is detected as a path q near q new The equal step length is divided into 100 points, if one point falls into the space range of the obstacle, collision is represented, if no collision occurs, q is represented new Store q in the random tree T new Considered as q near Is a parent node of (a); discarding q if a collision is detected new Returning to step S3 to regenerate q new . Repeating the steps S3-S4 until a new node q is generated new And q goal The euclidean distance between the two points is smaller than the threshold thr, and the euclidean distance represents that the target point is found in the path planning. And backtracking the random tree T in the random tree T according to the father-son relationship of each node to find the planned path.
In the case of the redundant node deletion strategy in step S5, the obtained path is subjected to path re-planning by using the redundant node deletion strategy. The initial path nodes are stored in an array of nums, nums= [ q ] start ,q 2 ,q 3 ,…,q goal ]From an initial point q on a nums basis start To q goal The re-planning of the path is performed,i.e. compute root node q in the array root To the next node q next Euclidean distance, if the Euclidean distance is smaller than the original path length and the root node q root To the next node q next If the path of (a) does not collide with the obstacle, q is deleted root And q next Redundant nodes between. The specific process is to initialize q root Is q start ,q next Is q 2 If q root And q next The distance between the two paths is smaller than or equal to the original path length, and q is updated without collision next Is q 3 Delete q root To q 2 Nodes between, if q root And q next The distance between the two paths is larger than the original path length or the collision is performed, and then q is updated root Is q 2 Repeating the above steps until q next Is q goal Then the reprofiling of the path is complete.
In the polynomial trajectory optimization of step S6, the discrete nodes with redundant nodes deleted in the cartesian space coordinates are solved by inverse kinematics to obtain the joint angles [ θ ] of the mechanical arm at each discrete point 1 ,θ 2 ,θ 3 ,θ 4 ,θ 5 ,θ 6 ]. Path nodes are processed using polynomial interpolation. The polynomial programming function of the joint angle of the track of the mechanical arm is as follows:
θ i (t)＝a 0 +...+a i t i +...+a n t n
the function of a is obtained 0 ,…,a i ,…,a n The functional can be determined. The function is led to time, and the joint angular velocity expression is obtained as follows:
deriving the time again to obtain a joint angular acceleration expression as follows:
assume that the joint angle of the mechanical arm is theta when the mechanical arm starts to move 0 The angle at which the joint movement stops is theta f . Let the whole movement time be t f The initial joint angular velocity is
Ending joint angular velocity of +.>
Initial angular acceleration of +.>
Ending joint angular acceleration of +.>
The polynomial coefficient a can be obtained 0 ,…,a i ,…,a n And substituting the angle into a polynomial equation to obtain a smooth mechanical arm joint change angle, and completing final mechanical arm path planning. />
Claims (7)
1. The RRT mechanical arm track planning method based on non-obstacle space probability potential field sampling is used for a multi-link mechanical arm with multiple joints, and global path planning is carried out in a non-obstacle space by combining RRT with an artificial potential field, and is characterized in that: generating sampling points based on a non-obstacle space, namely screening the sampling points according to whether the sampling points fall into the obstacle space, and avoiding redundant path generation and unnecessary path collision detection;
the planning method further comprises a probability potential field greedy sampling strategy, namely, on the premise of guaranteeing that the probability of the algorithm is complete, the algorithm is endowed with target guidance of the preset probability to accelerate path generation, path collision detection is reduced through the action of a gravitational field and a repulsive force field, and the generated path direction is relatively better;
the planning method further comprises a redundant node deleting strategy, namely optimizing a path generated by algorithm backtracking, deleting redundant nodes in the path, and enabling the generated path to be better; and smoothing the optimized path through an interpolation polynomial, so that the path is more in line with the actual mechanical arm motion.
2. The RRT robot trajectory planning method based on non-obstacle space probability potential field sampling of claim 1, wherein: the planning method comprises the following steps;
step S1: mathematical modeling of the mechanical arm: describing the pose of the mechanical arm, and establishing a mechanical arm connecting rod coordinate system to obtain a D-H model of the mechanical arm;
step S2: initializing to execute global path planning, specifically: initializing a tree T within a workspace, given an initial node q start ＝[x start ，y start ，z start ]Target node q goal ＝[x goal ，y goal ，z goal ]Expanding fixed step length, maximum iteration number max, obstacle space Ω abs A target point error allowance threshold thr;
step S3: and executing a probability potential field greedy sampling strategy, namely adopting non-obstacle space probability potential field sampling to generate sampling points: setting a random probability threshold value as P, and when the generated random probability P is smaller than P, randomly sampling by an algorithm to obtain a random sampling point q rand ＝[x rand ，y rand ，z rand ]；
Step S4: collision detection: will q near To q new Performing collision detection on the generated path;
step S5: redundant node deletion policy: carrying out path re-planning on the obtained path by using a redundant node deleting strategy;
step S6: and optimizing the motion trail of the mechanical arm.
3. The RRT robot trajectory planning method based on non-obstacle space probability potential field sampling of claim 2, wherein: the step S3 comprises the following steps;
step S3-1: if randomly sampling point q rand Falls into the obstacle space omega obs In the inner part, discard this pointRandomly sampling again;
step S3-2: if q rand Falling within the non-obstacle space omega, according to traversing the nodes q of the whole spanning tree T ＝[x T ，y T ，z T ]Find q T And random sampling point q rand Is closest to node q near ＝[x near ，y near ，z near ]The method comprises the following steps:
step S3-3: to be used for
Generating new nodes for the direction according to a fixed step length, expressed as a formula
q new ＝[x new ，y new ，z new ]A second formula;
step S3-4: if the random probability P of the sampling point is greater than P, carrying out greedy sampling guided by an artificial potential field by an algorithm; then according to node q traversing the entire spanning tree T ＝[x T ，y T ，z T ]Find q T With target node q goal ＝[x goal ，y goal ，z goal ]Is far from the nearest node q near ＝[x near ，y near ，z near ]The method comprises the following steps:
step S3-5: compute node q near The resultant force potential energy U:
U＝U att +U rep a formula IV;
in the formula, the total attractive force born by the mechanical arm is expressed as the following formula:
U rep the total repulsive force applied to the mechanical arm:
wherein U is att (i) The gravity generated by the barriers in the space is received by each connecting rod of the mechanical arm; u (U) rep (i) Repulsive force generated by the barriers in the space is applied to each connecting rod of the mechanical arm; n is the number of the mechanical arm connecting rod subjected to the repulsive force of the obstacle; d, d i The Euclidean distance between the actual position of the corresponding connecting rod joint point of the mechanical arm and the obstacle; d, d o A repulsive force field action range for each connecting rod; d, d s For the safety distance of each connecting rod, k a Is the gravitational proportionality constant, k r Is the repulsive force proportionality constant;
step S3-6: after the resultant potential energy U is obtained, a fixed step length step is extended in the direction of a vector U to generate a new node q new ＝[x new ，y new ，z new ]If the potential field falls into a local minimum, the generation of the sampling point is abandoned, and random sampling is performed again.
4. The RRT robot trajectory planning method based on non-obstacle space probability potential field sampling of claim 3, wherein: the step S4 comprises the following steps;
step S4-1: route q near q new The equal step length is divided into n points, if one of the points falls into the space range of the obstacle, collision is represented,if no collision occurs, q new Store q in the random tree T new Considered as q near Is a parent node of (a); discarding q if a collision is detected new Returning to step S3 to regenerate g new ；
Step S4-2: repeating the steps S3-S4 until a new node q is generated new And q goal The Euclidean distance between the two points is smaller than a threshold thr, namely the point where the target point is found in the representative path planning; and backtracking the random tree T in the random tree T according to the father-son relationship of each node to find the planned path.
5. The RRT robot trajectory planning method based on non-obstacle space probability potential field sampling of claim 4, wherein: the step S5 specifically comprises the following steps:
carrying out path re-planning on the obtained path by using a redundant node deleting strategy; storing the initial path node in an array of nums, nums= [ q ] start ，q 2 ，g 3 ，…，q goal ]From an initial point q on a nums basis start To q goal Path re-planning, i.e. computing root node q in the array root To the next node q next Euclidean distance:
d＝||q root -q next formula eight;
if the Euclidean distance d is less than or equal to the original path length l:
l=step x (n-m) formula nine;
n represents q in array num next The subscript at this time, m represents q in the array nums root Subscripts at this time. If its Euclidean distance is smaller than the original path length and the root node q root To the next node q next If the path of (a) does not collide with the obstacle, q is deleted root And q next Redundant nodes between; and q is set to next Updated to q next Is the next node of (a); if the Euclidean distance d is greater than the original path length l or the root node q root To the next node q next Collision of the path of (c) with the obstacle, q root Updated to q next Repeating the above stepsStep (d) up to q next Is q goal Then the reprofiling of the path is complete.
6. The RRT robot trajectory planning method based on non-obstacle space probability potential field sampling of claim 5, wherein: the step S6 specifically includes:
solving discrete nodes with redundant nodes deleted from Cartesian space coordinates through inverse kinematics to obtain joint angles [ theta ] of the mechanical arm at each discrete point 1 ，θ 2 ，θ 3 ，θ 4 ，θ 5 ，θ 6 ]The method comprises the steps of carrying out a first treatment on the surface of the Using polynomial interpolation to process path nodes, the polynomial programming function of the joint angles of the track of the mechanical arm is as follows:
θ i (t)＝a 0 +...+a i t i +...+a n t n formula ten;
the alpha is found in the function 0 ，…，a i ，…，a n The functional formula may be determined; the function is led to time, and the joint angular velocity expression is obtained as follows:
deriving the time again to obtain a joint angular acceleration expression as follows:
assume that the joint angle of the mechanical arm is theta when the mechanical arm starts to move 0 The angle at which the joint movement stops is theta f The method comprises the steps of carrying out a first treatment on the surface of the Let the whole movement time be t f The initial joint angular velocity is
Ending joint angular velocity of +.>
Initial angular acceleration of +.>
Ending the angular acceleration of the joint to
Obtaining polynomial coefficient alpha 0 ，…，a i ，…，a n Substituting the angle into a polynomial equation to obtain a smooth mechanical arm joint change angle, and completing final mechanical arm path planning.
7. The RRT robot trajectory planning method based on non-obstacle space probability potential field sampling of claim 1, wherein: the track planning is mechanical arm obstacle avoidance path planning, namely the mechanical arm plans a collision-free path within a limited time range under the condition that an obstacle exists in a working space.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310214664.7A CN116117822A (en) | 2023-03-08 | 2023-03-08 | RRT mechanical arm track planning method based on non-obstacle space probability potential field sampling |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310214664.7A CN116117822A (en) | 2023-03-08 | 2023-03-08 | RRT mechanical arm track planning method based on non-obstacle space probability potential field sampling |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116117822A true CN116117822A (en) | 2023-05-16 |
Family
ID=86306423
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310214664.7A Pending CN116117822A (en) | 2023-03-08 | 2023-03-08 | RRT mechanical arm track planning method based on non-obstacle space probability potential field sampling |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116117822A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117260735A (en) * | 2023-11-01 | 2023-12-22 | 广东工业大学 | Path planning method for robot deep frame grabbing |
| CN117773911A (en) * | 2023-11-03 | 2024-03-29 | 广东工业大学 | Obstacle avoidance method for industrial robot in complex environment |
-
2023
- 2023-03-08 CN CN202310214664.7A patent/CN116117822A/en active Pending
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117260735A (en) * | 2023-11-01 | 2023-12-22 | 广东工业大学 | Path planning method for robot deep frame grabbing |
| CN117773911A (en) * | 2023-11-03 | 2024-03-29 | 广东工业大学 | Obstacle avoidance method for industrial robot in complex environment |
| CN117773911B (en) * | 2023-11-03 | 2024-05-17 | 广东工业大学 | Obstacle avoidance method for industrial robot in complex environment |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110228069B (en) | Online obstacle avoidance motion planning method for mechanical arm | |
| CN110962130B (en) | Heuristic RRT mechanical arm motion planning method based on target deviation optimization | |
| CN116117822A (en) | RRT mechanical arm track planning method based on non-obstacle space probability potential field sampling | |
| US9411335B2 (en) | Method and apparatus to plan motion path of robot | |
| Shyam et al. | Improving local trajectory optimisation using probabilistic movement primitives | |
| CN113885535A (en) | Impact-constrained robot obstacle avoidance and time optimal trajectory planning method | |
| Jiang et al. | Path planning for robotic manipulator in complex multi-obstacle environment based on improved_RRT | |
| Liu et al. | Goal-biased bidirectional RRT based on curve-smoothing | |
| Abu-Dakka et al. | A direct approach to solving trajectory planning problems using genetic algorithms with dynamics considerations in complex environments | |
| CN112809665B (en) | Mechanical arm motion planning method based on improved RRT algorithm | |
| CN113276109B (en) | Dual-mechanical-arm decoupling motion planning method and system based on RRT algorithm | |
| CN113687659B (en) | Optimal trajectory generation method and system based on digital twinning | |
| CN114995431B (en) | Mobile robot path planning method for improving A-RRT | |
| CN115723129B (en) | Continuous operation motion planning method for mechanical arm | |
| CN115309161A (en) | Mobile robot path planning method, electronic equipment and storage medium | |
| Zhang et al. | Mobile robots path planning based on improved artificial potential field | |
| CN115416016A (en) | Mechanical arm obstacle avoidance path planning method based on improved artificial potential field method | |
| CN115958590A (en) | RRT-based mechanical arm deep frame obstacle avoidance motion planning method and device | |
| Cheng et al. | Robot arm path planning based on improved RRT algorithm | |
| Liu et al. | Improved RRT path planning algorithm for humanoid robotic arm | |
| Shao et al. | Rrt-goalbias and path smoothing based motion planning of mobile manipulators with obstacle avoidance | |
| Cao et al. | Multi-AUV target searching under ocean current based on PPSO and velocity synthesis algorithm | |
| Liang et al. | PR-RRT*: Motion Planning of 6-DOF Robotic Arm Based on Improved RRT Algorithm | |
| Tang et al. | Coordinated motion planning of dual-arm space robot with deep reinforcement learning | |
| CN117773911A (en) | Obstacle avoidance method for industrial robot in complex environment |
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 |