CN115723125A - Mechanical arm repeated operation motion planning method - Google Patents
Mechanical arm repeated operation motion planning method Download PDFInfo
- Publication number
- CN115723125A CN115723125A CN202211350644.4A CN202211350644A CN115723125A CN 115723125 A CN115723125 A CN 115723125A CN 202211350644 A CN202211350644 A CN 202211350644A CN 115723125 A CN115723125 A CN 115723125A
- Authority
- CN
- China
- Prior art keywords
- path
- tree
- mechanical arm
- exploration
- space
- 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 59
- 238000005070 sampling Methods 0.000 claims abstract description 27
- 239000000203 mixture Substances 0.000 claims abstract description 26
- 238000010845 search algorithm Methods 0.000 claims abstract description 7
- 230000008569 process Effects 0.000 claims description 25
- 238000013138 pruning Methods 0.000 claims description 12
- 230000000750 progressive effect Effects 0.000 claims description 10
- 238000001514 detection method Methods 0.000 claims description 9
- 230000003044 adaptive effect Effects 0.000 claims description 7
- 230000003252 repetitive effect Effects 0.000 claims description 7
- 239000012636 effector Substances 0.000 claims description 6
- 230000002457 bidirectional effect Effects 0.000 claims description 3
- 230000006870 function Effects 0.000 claims description 2
- 238000013507 mapping Methods 0.000 claims description 2
- 239000003550 marker Substances 0.000 claims description 2
- 239000011159 matrix material Substances 0.000 claims description 2
- 230000000007 visual effect Effects 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 5
- 230000008859 change Effects 0.000 description 3
- 238000004519 manufacturing process Methods 0.000 description 3
- 230000004075 alteration Effects 0.000 description 2
- 241000282414 Homo sapiens Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000007726 management method Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Images
Landscapes
- Manipulator (AREA)
Abstract
The invention discloses a planning method for repeated operation and movement of a mechanical arm. The method comprises the following steps: determining D-H parameters of the mechanical arm according to a D-H description method and constructing a D-H model of the mechanical arm; determining the initial state and the target state of the mechanical arm in a joint space by adopting an inverse kinematics algorithm; learning a path library by adopting a Gaussian mixture model and constructing a self-adaptive sampler so as to improve the random sampling efficiency; the exploration (experience) of the search space of the previous task is saved in an experience graph, and a graph search algorithm is used for retrieving local useful experience from past experience so as to accelerate the solution of a subsequent planning problem. The motion planning method provided by the invention can learn experience from past successful planning, save search time and improve the execution efficiency of the repeated operation task of the mechanical arm.
Description
Technical Field
The invention relates to the technical field of motion planning of mechanical arms, in particular to a motion planning method for repeated operation of a mechanical arm.
Background
The path planning is one of important guarantees of the safety operation of the mechanical arm, and can plan a collision-free progressive optimal path according to the initial position and the target position of the mechanical arm. In recent years, robots are widely used in work scenarios such as automatic assembly systems, warehouse logistics management, automatic sorting, etc., where the robots usually replace human beings to perform repeated operation tasks, which will lead to similar motion planning. In addition, the potential free collision-free space is changed due to frequent operation of the mechanical arm, and for the above problems, how to design an online efficient motion planner is crucial to improving the production efficiency.
The sampling-based motion planning algorithm is widely applied to the actual production process due to good robustness. The method can be mainly divided into two categories, namely a graph-based motion planning algorithm and a tree-based motion planning algorithm. The method is suitable for the multi-query motion planning problem, but cannot adapt to the dynamic change of the environment. The tree-based motion planning method is characterized in that a given starting point is used as a root node, a space to be searched is explored in an incremental mode until a tree is connected to a target node, the algorithm can be effectively applied to an operation scene with changed environment, for repeated operation tasks, the correlation among the tasks is not considered, for each new task, planning is restarted without fully utilizing priori knowledge, and the efficiency of the tree-based motion planning method is low when the tree is subjected to repeated operation.
Therefore, how to enable the mechanical arm to learn experience from the past successful planning to accelerate the planning and solving of the subsequent task when the mechanical arm faces the repeated operation task is an urgent problem to be solved when the mechanical arm faces the repeated operation task, and meanwhile, the motion planner has certain environment adaptability to deal with the dynamic change of the environment.
Disclosure of Invention
The invention provides a planning algorithm for repeated operation and movement of a mechanical arm, which can enable the mechanical arm to learn experiences from past successful planning and guide the planning of subsequent tasks, and can adapt to tiny dynamic changes of the environment, thereby improving the sampling efficiency and saving the planning time.
In order to achieve the above object, the present invention provides the following solutions:
a method for planning repeated operation and movement of a mechanical arm comprises the following steps:
determining the D-H parameters of the mechanical arm by adopting a D-H description mechanism, and constructing a kinematic model of the mechanical arm;
acquiring an initial coordinate position and a target coordinate position of an object to be operated in a working space by adopting a vision sensor, determining a to-be-grabbed pose and a target placing pose of an end effector of the mechanical arm in the working space, and mapping the initial pose and the target pose of the end effector of the mechanical arm in the working space into an initial joint state and a target joint state of the mechanical arm in the space to be searched by adopting an inverse kinematics algorithm according to the D-H model; (ii) a The space to be searched is a joint space of the mechanical arm;
the motion planner adopted by the invention is a bidirectional rapid exploration random search tree motion planner based on experience; when the motion planner runs for the first time, initializing the experience graph and the path library to be empty; the path library is a path set obtained by planning; the experience graph saves the exploration of the mechanical arm to-be-searched space in the previous planning task;
taking the initial joint state of the mechanical arm in the joint space as the starting point of the space to be searched, and taking the target state of the mechanical arm in the joint space as the target point of the space to be searched; and taking the empirical chart, the path library and the mechanical arm at a starting point and a target point to be searched as input of a planner, and solving a collision-free progressive optimal path of the mechanical arm from the starting point to the target point in a space to be searched, wherein the specific planning process is as follows:
step 1, respectively constructing a starting exploration tree T by the mechanical arm at a starting point and a target point of a space to be searched a And target exploration tree T b (ii) a Respectively taking the starting point and the target point of the mechanical arm in the space to be searched as root nodes to initialize T a And T b Initialization of T a And T b Is empty, initializes T a And T b Initializing the starting exploration tree T with the exploration identifier of true a Is the current spanning tree; the describedThe search marker indicates whether the search tree can continue to search in the search space, if false, the search of the search tree in the space to be searched is stopped;
step 2, learning a path library by adopting a Gaussian mixture model, acquiring a Gaussian probability distribution model of path points, constructing a Gaussian mixture model sampler, and constructing an adaptive sampler by combining the Gaussian mixture model sampler and a uniform sampler;
step 3, determining random sampling points q in the space to be searched by the self-adaptive sampler rand (ii) a By starting exploration tree T a Searching whether the identifier is true for the current expansion tree or not, if so, then sampling a random point q rand Determining an expansion node q of a current expansion tree for a search direction new (ii) a If not, exchanging the initial exploration tree and the target exploration tree, and if the current expansion tree is the initial exploration tree T a If so, switching the target exploration tree T b For the current expansion tree, if the current expansion tree is the target exploration tree T b Then switch the initial exploration tree T a Is the current spanning tree;
step 4, if the current expansion tree is successfully expanded and can successfully return to the expansion node q new Judging whether another exploration tree can be connected with the returned extension node q new Direct collision-free connection, if yes, the initial exploration tree T a And target exploration tree T b The method can be connected, the whole exploring process of the planner is stopped, and a feasible collision-free path from a starting point to a target point of the mechanical arm in the search for silence is obtained by adopting a path reverse retrieval method;
step 5, if the above mentioned initial search tree T a And target exploration tree T b If the current expansion tree can not be directly connected with the experience graph, continuing to judge whether the current expansion tree can be connected with the experience graph, if so, stopping the current expansion tree exploration, and setting a false identifier of the current expansion tree exploration;
step 6, if the exploration tree T is started a And target exploration tree T b Indicates that the starting exploration tree and the target exploration tree are connected to the empirical graph, and the target exploration tree is adopted as a graphSearching a local shortest path from the empirical graph by using a searching algorithm; determining a path with the minimum weight from a starting point to the empirical graph and from a target point to the empirical graph by adopting a path reverse retrieval method, and forming a complete path from the starting position to the target position of the mechanical arm to be searched together with the local shortest path in the graph;
step 7, if the search algorithm of the tree graph fails to search, the initial exploration tree T is used a And target exploration tree T b Resetting the exploration identifier to true, and recovering the exploration states of the two exploration trees to continuously explore in the space to be explored;
step 8, in the time allowed range, if a collision-free path from the starting point to the target point of the mechanical arm in the space to be searched is not found after the steps 3 to 7 are completed, exchanging the starting exploration tree T a And target exploration tree T b If the current spanning tree is T a Then switch the current tree to T b If the current spanning tree is T b Then switch the current tree to T a (ii) a Repeating the step 3 to the step 7 until a collision-free progressive optimal path is found within a time allowed range;
step 9, after the path from the starting point of the space to be searched to the target point of the mechanical arm is found, pruning the road stiffness by adopting a path pruning algorithm, and eliminating redundant path points in the path;
step 10, after the steps 1 to 9 are completed, adding the pruned path into the path library, and updating the experience graph according to the search result of the current task tree; and finally returning the finally obtained collision-free progressive optimal path of the mechanical arm from the initial position to the target position in the joint space.
Optionally, a Gaussian mixture model is adopted to obtain the secondary path library
Learning in the process of the process so as to obtain a discrete probability distribution model of the path points; the path library is a set of path points obtained by previous successful planning
Said heightThe si-hybrid model is expressed by equation (1):
wherein
π k Represents the weight of the k-th Gaussian component, wherein
θ＝θ k And K is a parameter to be solved for the kth Gaussian component, i = 1\8230. Phi (q | theta) k ) The probability density function representing each gaussian component is expressed by equation (2):
in the formula (2) < theta > k ＝(μ k ,Σ k )，μ k Sum sigma k Respectively representing the mean value and covariance matrix of the kth Gaussian component, and d is the dimension of the mechanical arm configuration space, namely the exploration space. Then, iterative solution of Gaussian component parameter mu by adopting EM algorithm k 、Σ k And pi k As shown in equation (3):
the obtained gaussian component parameters can be represented by formula (4):
optionally, the step 3 of combining the random uniform sampler and the adaptive sampler based on the gaussian mixture model sampler includes:
setting a random sampling threshold xi, xi =0.5;
generating a random number rand of 0-1 by adopting a random number generator;
and comparing the random number rand with the threshold xi, if the random number rand is less than or equal to the threshold xi or the Gaussian mixture model is empty, determining a random sampling point in the space to be searched of the mechanical arm by adopting a random sampling strategy, and if the random number rand is greater than the threshold xi, determining a random sampling point in the space to be searched by adopting a sampling strategy based on the Gaussian mixture model.
Optionally, in the step 3 tree expansion process, in q new In the field of q new Reselecting the parent node of the node and aligning to the node located at q new Carrying out pruning operation on nodes in the field to obtain a progressive optimal path; the field is from q new Nearest m nodes, where m is chosen to satisfy m>e (1 + 1/d), d is the dimension of the exploration space, and e is the natural logarithm;
optionally, in the path reverse retrieval process in step 4, the path length of each node and the connected node is calculated as a path weight, and a total path weight minimum path is retrieved from a starting point to a target point of the space to be searched by the mechanical arm as a final required path.
Optionally, in the graph searching process in step 6, firstly, the collision detection condition of the path is not considered, a conventional graph searching algorithm a is adopted to search for a path from the graph, then a lazy collision detection strategy is adopted to test the path, if the whole path does not collide with the obstacle, the obtained path is returned, and if some edges in the path collide with the obstacle, the path is regarded as invalid, and the collision edges are removed from the empirical graph; repeating the A search and the lazy collision detection until a collision-free path is obtained; if no collision-free path can be obtained finally, the graph search fails.
Optionally, in the path pruning process in step 9, a greedy connection strategy is adopted to remove redundant points in the path to obtain a shorter path; the greedy link policy is expressed as: suppose q 1 ,q 2 ,q 3 Three successive path points, q, being a section of collision-free path 1 ,q 3 Can skip q 2 Direct collision-free connection indicates q 2 And eliminating the redundant point of the path segment.
Optionally, in the step 10, in the process of updating the path library and the empirical graph, adding the path obtained according to the current task to the path library; and according to the current tree exploration result, searching adjacent nodes for each node in the tree for connection, and integrating the edge set obtained by connection into the original empirical graph without considering collision detection.
Adopt above-mentioned technical scheme, this patent has following beneficial effect at least:
according to the invention, a Gaussian mixture model is adopted to learn a previously successful planned path so as to find out an expected area where the path may exist, and the adaptive sampler is constructed according to the Gaussian mixture model, so that blind sampling of a sampling zone in an exploration space can be avoided, and the sampling efficiency is improved; the invention adopts the empirical graph to store the exploration process of the previous task to the space to be explored, and performs local empirical retrieval on the subsequent operation task by using a graph search algorithm and a lazy collision detection strategy, thereby accelerating the solution of the subsequent planning problem; the method does not need any environment preprocessing step, accelerates the subsequent solution of similar planning problems by carrying out experience learning in an online mode, and is easy to deploy in an actual production environment.
Drawings
FIG. 1 is a flowchart of an experience-based task for repetitive operation of a robotic arm according to the present invention;
FIG. 2 is a schematic diagram of a seven-degree-of-freedom robotic arm configuration for use in an embodiment of the present invention
FIG. 3 is a schematic diagram of a Gaussian mixture model learning process and an adaptive sampler according to the present invention
FIG. 4 is a schematic diagram of a graph retrieval process employed by the present invention
FIG. 5 is a schematic diagram of the path pruning process of the present invention
FIG. 6 is a diagram illustrating an empirical graph updating process according to the present invention
Detailed Description
The invention is described in detail below with reference to specific embodiments.
The invention provides a planning method for repeated operation and movement of a mechanical arm, which enables the mechanical arm to learn experiences from past planning and to be used for guiding the rapid solution of subsequent planning problems, and can adapt to the tiny dynamic change of the environment.
In order to make the aforementioned objects, features and advantages of the present invention more comprehensible, the present invention is described in further detail with reference to the accompanying drawings and specific examples:
according to the method, a YuMi mechanical arm is taken as an embodiment mechanical arm model, as shown in FIG. 2, D-H parameters of the mechanical arm are obtained according to a D-H description method, and a D-H model of the mechanical arm is constructed; the D-H parameters obtained are shown in Table 1:
TABLE 1 YuMi mechanical arm left arm D-H parameter table
In table d i Indicating the distance, theta, of the arm links i Representing the angle between the two links, a i Indicating the offset distance, alpha, between the two links i Indicating the torsion angle of the two links, /) i Denotes the lower limit of the joint angle, u i Representing the upper limit of the joint angle. And a kinematic model of the mechanical arm can be constructed through the D-H parameters.
Initializing parameters of the motion planner, starting the motion planner for the first time, and setting the experience graph and the road strength library to be empty;
when the mechanical arm executes repeated operation tasks, for each task instance, a vision sensor is adopted to obtain the initial coordinate position and the target coordinate position of an object to be operated in a working space and calculate the corresponding pose of the end effector of the mechanical arm, and the initial pose and the target pose of the end effector of the mechanical arm in the working space are mapped into the initial joint state and the target joint state of a space to be searched (joint space) according to the D-H parameter model;
taking the initial joint state, the target joint state position, the empirical graph and the Gaussian mixture model as input parameters of a motion planner, and solving a collision-free path from the initial joint state to the target joint state of the mechanical arm; the flow chart of the planner is shown in fig. 1:
step 1, according to the starting joint state and the target joint of the mechanical arm in the joint spaceState building and initialization of the Start exploration Tree T a And target exploration tree T b And sets a start tree T a Is the current spanning tree;
step 2, if the path library is not empty, adopting a Gaussian mixture model to successfully plan a Gaussian mixture probability density distribution model of the obtained path points before learning from the path library, and combining a uniform sampler and a sampler based on the Gaussian mixture model to construct an adaptive sampler, as shown in FIG. 3;
step 3, sampling in a space to be searched according to the self-adaptive sampler, judging whether the current expansion tree exploration identifier is true, if so, expanding the expansion tree according to the random sampling point, and if not, switching the current expansion tree; the exploration process of the sampler is as shown in fig. 3, firstly, a random number rand from 0 to 1 is generated through a random number generator, the random number is compared with a set threshold value xi =0.5, if the random number is less than or equal to the set threshold value or a Gaussian mixture model is empty, a random sampling strategy is adopted, if the random number is greater than the set threshold value, sampling is carried out by adopting a sampling rate of the Gaussian mixture model, and finally, a generated sampling point is returned; the tree expansion process resembles the RRT algorithm;
and 4, judging whether the expansion node obtained by the expansion tree is empty, if so, judging whether the returned expansion node can be directly connected with another exploration tree, and if the connection is successful, obtaining a collision-free path connecting the starting joint state of the mechanical arm to the target joint state by adopting a path reverse retrieval method.
Step 5, if the current expansion tree and another exploration tree can not be directly connected, continuously judging whether the current expansion tree can be connected with the experience graph, if so, stopping the exploration of the current expansion tree, and setting a false identifier of the current expansion tree;
step 6, if the exploration tree T is started a And target exploration tree T b If the search identifiers are false, the starting search tree and the target search tree are connected to the empirical graph, and a conventional graph search algorithm A is adopted to search the local shortest path from the empirical graph; reverse searching method adopting pathDetermining a path with the minimum weight from a starting point to the empirical graph and from a target point to the empirical graph, and forming a complete path of the mechanical arm from the starting position to the target position in the joint space together with the local shortest path in the graph; the graph retrieval process is as shown in FIG. 4
Step 7, if the tree graph search algorithm fails to search, the initial exploration tree T is searched a And target exploration tree T b Resetting the exploration identifier to true, and recovering the exploration states of the two exploration trees to continuously explore in the space to be explored;
step 8, in the time allowed range, if a collision-free path from the initial position to the target position of the mechanical arm in the joint space is not found after the steps 3 to 8 are completed, exchanging the initial exploration tree T a And target exploration tree T b If the current expansion tree is T a Then switch the current tree to T b If the current expansion tree is T b Then switch the current spanning tree to T a (ii) a Repeating the step 3 to the step 8 until a collision-free progressive optimal path is found within a time allowed range;
step 9, after the path from the initial position to the target position of the mechanical arm in the joint space is found, pruning the road strength by adopting a path pruning algorithm, and removing redundant path points in the path; the pruning process is shown in FIG. 6, with q in the path 0 Can cross q 1 Directly with q 2 Then, it indicates q 1 Removing the path redundant points as path redundant points;
step 10, after the steps 1 to 9 are completed, adding the pruned path into the path library, and according to the exploration result of the current task tree and a new experience graph, namely, merging the new exploration of the planner on the search space into the original experience graph; finally, returning to the finally obtained collision-free progressive optimal path of the mechanical arm from the initial position to the target position in the joint space;
the key frame for simulating the repeated operation of the mechanical arm in the simulation environment of the embodiment is shown in fig. 6.
According to the specific embodiment provided by the invention, the invention discloses the following technical effects: the invention provides a mechanical arm repeated operation motion planning algorithm, which introduces a Gaussian mixture model and constructs a self-adaptive sampler, thereby improving the sampling efficiency of sampling points; in addition, the exploration of the search space of the current task is saved in an empirical graph, and the subsequent task and the local effective experience are retrieved from the empirical graph, so that the solution of the planning problem is accelerated, the path planning efficiency of the mechanical arm is obviously improved, and the generation efficiency of repeated operation tasks can be improved.
The invention has been explained by applying specific embodiments, and the above description of the embodiments is only for the purpose of helping understanding the method and the core idea of the invention; also, it will be appreciated that those skilled in the art, upon attaining an understanding of the present disclosure, may readily produce alterations to, variations of, and equivalents to such variations, modifications, alterations to, combinations of, and simplifications which may be made without departing from the spirit or scope of the present disclosure.
Claims (8)
1. A planning method for repeated operation and movement of a mechanical arm is characterized by comprising the following steps:
determining the D-H parameters of the mechanical arm by adopting a D-H description method according to the physical characteristics of the mechanical arm, and constructing a D-H model of the mechanical arm;
the adopted motion planner is an experience-based bidirectional rapid exploration random search tree motion planner; when the motion planner is started for the first time, initializing an experience graph and a road strength library to be empty; the path library is a path set obtained by planning; the experience graph saves the exploration of the mechanical arm to-be-searched space in the previous planning task; the space to be searched is a joint space of the mechanical arm;
detecting the initial coordinate position and the target coordinate position of an object to be operated in a working space by adopting a visual sensor, and determining the corresponding pose of the end effector of the mechanical arm in the working space; according to the D-H model of the mechanical arm, aiming at each task instance, mapping the initial pose and the target pose of the end effector of the mechanical arm in a working space into the initial joint state and the target joint state of the mechanical arm in a joint space by adopting an inverse kinematics algorithm;
taking the initial joint state of the mechanical arm in the joint space as an initial point of the space to be searched, and taking the target state of the mechanical arm in the joint space as a target point of the space to be searched;
taking the path library, the empirical graph, the starting point and the target point of the mechanical arm in the space to be searched as the input of the experience-based bidirectional rapid exploration random search tree motion planner, and solving a feasible collision-free path from the starting point to the target point of the mechanical arm in the space to be searched, wherein the method comprises the following specific steps:
step 1, respectively constructing an initial exploration tree T at an initial point and a target point of the space to be searched a And target exploration tree T b (ii) a Respectively taking the starting point and the target point of the space to be searched as root nodes to initialize T a And T b Initialization of T a And T b Is empty, initializes T a And T b Is true and initializes the starting exploration tree T a Is the current spanning tree; the search marker indicates whether the search tree can continue to search in the search space, and if false, the search of the tree in the search space is stopped;
step 2, learning a path library by adopting a Gaussian mixture model, finding out an expected area possibly existing in the optimal gradual path, constructing a Gaussian mixture model sampler, and constructing an adaptive sampler by combining the Gaussian mixture model sampler and a uniform sampler;
step 3, determining random sampling points q in a search space by the self-adaptive sampler rand (ii) a Judging whether the current expanded tree exploration identifier is in true position, if so, carrying out q rand Determining an expansion node q of a current expansion tree for a search direction new (ii) a If not, exchanging the initial exploration tree and the target exploration tree; if the current expanded tree is the initial exploration tree T a If so, switching the target exploration tree T b For the current expansion tree, if the current expansion tree is the target exploration tree T b Then switch the initial exploration tree T a Is the current spanning tree;
step 4, if the current expansion tree is successfully expanded and can successfully return to the expansion node q new Determine another oneWhether the exploration tree can be connected with the returned extension node q new Direct collision-free connection, if yes, the initial exploration tree T a And target exploration tree T b The mechanical arm can be connected, the whole exploration process of the planner is stopped, and a collision-free feasible path from the starting point of the mechanical arm to the target point in the joint space is determined by adopting a path reverse retrieval method;
step 5, if the above mentioned initial search tree T a And target exploration tree T b If the current expansion tree can not be directly connected with the experience graph, continuing to judge whether the current expansion tree can be connected with the experience graph, if so, stopping the current expansion tree exploration, and setting a false identifier of the current expansion tree exploration;
step 6, if the exploration tree T is started a And target exploration tree T b If the search identifiers are false, the initial search tree and the target search tree are connected to the empirical graph, and a graph search algorithm is adopted to search the local shortest path from the empirical graph; determining a path with the minimum weight from a starting point to the empirical graph and from a target point to the empirical graph by adopting a path reverse retrieval method, and forming a complete path of the mechanical arm from the starting point to the target point in the joint space together with the local shortest path in the graph;
step 7, if the search algorithm of the tree graph fails to search, the initial exploration tree T is used a And target exploration tree T b Resetting the exploration identifier to true, and recovering the exploration states of the two exploration trees to continuously explore in the space to be explored;
step 8, in the time allowed range, after the steps 3 to 7 are completed, if a collision-free path from the starting point to the target point of the mechanical arm in the space to be searched is not found, exchanging the starting exploration tree T a And target exploration tree T b If the current spanning tree is T a Then switch the current spanning tree to T b If the current expansion tree is T b Then switch the current spanning tree to T a (ii) a Repeating the step 3 to the step 7 until a collision-free progressive optimal path is found within a time allowed range;
step 9, after the mechanical arm is found out from a path from a starting point to a target point in the space to be searched, pruning the road strength by adopting a path pruning algorithm, and eliminating redundant path points in the path;
step 10, after the steps 1 to 9 are completed, adding the pruned path into the path library, and updating the experience graph according to the search result of the current task search tree; and finally returning the finally obtained collision-free progressive optimal path of the mechanical arm from the starting point to the target point in the space to be searched.
2. The method for planning the repetitive operation movement of the mechanical arm according to claim 1, wherein the step 2 adopts a Gaussian mixture model from a path library
Learning in the process of the process so as to obtain a discrete probability distribution model of the path points; the path library is a set of path points obtained by previous successful planning
The gaussian mixture model is expressed by equation (1):
wherein
π k Represents the weight of the k-th Gaussian component, wherein
θ＝θ k I =1 \ 8230and K is a parameter to be solved for the kth Gaussian component; phi (q | theta) k ) The density function representing each gaussian component is represented by equation (2):
in the formula (2) < theta > k ＝(μ k ,Σ k )，μ k Sum-sigma k Respectively representing the mean value and covariance matrix of the kth Gaussian component, wherein d is the dimension of a mechanical arm configuration space, namely an exploration space; then adopting EM algorithm to iteratively solve the parameter mu of the Gaussian component k 、Σ k And pi k As shown in equation (3):
the obtained gaussian component parameters are expressed by formula (4):
3. the method for planning the repetitive operation motion of the mechanical arm according to claim 1, wherein the step 3 of combining the stochastic homogeneous sampler and the adaptive sampler based on the gaussian mixture model comprises the steps of:
setting a random sampling threshold xi, xi =0.5;
generating a random number rand of 0-1 by adopting a random number generator;
and comparing the random number rand with a threshold value xi, if the random number rand is less than or equal to the threshold value xi or the Gaussian mixture model is empty, determining a random sampling point in a space to be searched of the mechanical arm by adopting a random uniform sampling strategy, and if the random number rand is greater than the threshold value xi, determining the random sampling point in the space to be searched by adopting a sampling strategy based on the Gaussian mixture model.
4. The method for planning the repetitive operation movement of a mechanical arm according to claim 1, wherein in the step 3 tree expanding process, q is the number of the trees new In the field of q new Reselecting a parent node for the node located at q new Nodes in the domain perform a pruning operation,to obtain a progressive optimal path; the field is from q new Nearest m nodes, where k is chosen to satisfy m>e (1 + 1/d), d is the dimension of the exploration space, and e is the natural logarithm.
5. The method according to claim 1, wherein in the reverse path search process in step 4, the path length of each node and the connected nodes is calculated as a path weight, and a path with the minimum total path weight is searched from a starting point to a target point of the robot in the space to be searched to obtain the final path.
6. The method for planning the repetitive operation and movement of the mechanical arm according to claim 1, wherein in the graph searching process of the step 6, firstly, a path is searched from the graph by using a conventional graph searching algorithm a without considering the collision detection condition of the path, then the path is checked by using a lazy collision detection strategy, if the whole path has no collision with the obstacle, the obtained path is returned, and if some edges in the path have collision with the obstacle, the path is regarded as invalid, and the collision edges are removed from the empirical graph; repeating the A search and the lazy collision detection until a collision-free path is obtained; if no collision-free path can be obtained finally, the graph search fails.
7. The method for planning the repetitive operation motion of the mechanical arm according to claim 1, wherein in the step 9 of path pruning, a greedy connection strategy is adopted to eliminate redundant points in a path to obtain a shorter path; the greedy linking strategy is represented as: suppose q is 1 ,q 2 ,q 3 Three successive path points, q, being a section of collision-free path 1 ,q 3 Q can be skipped 2 Direct collision-free connection indicates q 2 And eliminating the redundant point of the path segment.
8. The method for planning the repetitive operation and movement of the mechanical arm according to claim 1, wherein in the step 10, the path obtained by the current task is added to the path library during the updating process of the path library and the empirical chart; and according to the current tree exploration result, searching adjacent nodes for each node in the tree for connection, and fusing the edge set obtained by connection into the original empirical graph without considering collision detection.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211350644.4A CN115723125B (en) | 2022-10-31 | 2022-10-31 | Planning method for repeated operation movement of mechanical arm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211350644.4A CN115723125B (en) | 2022-10-31 | 2022-10-31 | Planning method for repeated operation movement of mechanical arm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115723125A true CN115723125A (en) | 2023-03-03 |
| CN115723125B CN115723125B (en) | 2024-06-21 |
Family
ID=85294187
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211350644.4A Active CN115723125B (en) | 2022-10-31 | 2022-10-31 | Planning method for repeated operation movement of mechanical arm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115723125B (en) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2002073130A (en) * | 2000-06-13 | 2002-03-12 | Yaskawa Electric Corp | Planning method for gross motion path of robot and its controller |
| CN106695802A (en) * | 2017-03-19 | 2017-05-24 | 北京工业大学 | Improved RRT<*> obstacle avoidance motion planning method based on multi-degree-of-freedom mechanical arm |
| CN110509279A (en) * | 2019-09-06 | 2019-11-29 | 北京工业大学 | A kind of trajectory path planning method and system of apery mechanical arm |
| US20200230815A1 (en) * | 2019-01-22 | 2020-07-23 | Mitsubishi Electric Research Laboratories, Inc. | System and Method for Automatic Error Recovery in Robotic Assembly |
| CN113103236A (en) * | 2021-04-22 | 2021-07-13 | 山东大学 | Rapid and gradual optimal mechanical arm obstacle avoidance path planning method |
| CN113485373A (en) * | 2021-08-12 | 2021-10-08 | 苏州大学 | Robot real-time motion planning method based on Gaussian mixture model |
-
2022
- 2022-10-31 CN CN202211350644.4A patent/CN115723125B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2002073130A (en) * | 2000-06-13 | 2002-03-12 | Yaskawa Electric Corp | Planning method for gross motion path of robot and its controller |
| CN106695802A (en) * | 2017-03-19 | 2017-05-24 | 北京工业大学 | Improved RRT<*> obstacle avoidance motion planning method based on multi-degree-of-freedom mechanical arm |
| US20200230815A1 (en) * | 2019-01-22 | 2020-07-23 | Mitsubishi Electric Research Laboratories, Inc. | System and Method for Automatic Error Recovery in Robotic Assembly |
| CN110509279A (en) * | 2019-09-06 | 2019-11-29 | 北京工业大学 | A kind of trajectory path planning method and system of apery mechanical arm |
| CN113103236A (en) * | 2021-04-22 | 2021-07-13 | 山东大学 | Rapid and gradual optimal mechanical arm obstacle avoidance path planning method |
| CN113485373A (en) * | 2021-08-12 | 2021-10-08 | 苏州大学 | Robot real-time motion planning method based on Gaussian mixture model |
Non-Patent Citations (1)
| Title |
|---|
| 于建均等: "基于高斯混合模型的轨迹模仿学习表征参数优化", 北京工业大学学报, vol. 43, no. 05, 31 May 2017 (2017-05-31), pages 719 - 728 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115723125B (en) | 2024-06-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Moll et al. | Randomized physics-based motion planning for grasping in cluttered and uncertain environments | |
| CN111546347B (en) | Mechanical arm path planning method suitable for dynamic environment | |
| CN110509279B (en) | Motion path planning method and system of humanoid mechanical arm | |
| CN109960880B (en) | Industrial robot obstacle avoidance path planning method based on machine learning | |
| Bohlin et al. | Path planning using lazy PRM | |
| Yershova et al. | Improving motion-planning algorithms by efficient nearest-neighbor searching | |
| CN103471591B (en) | The multiple-moving target data interconnection method of logic-based method, global arest neighbors and bogey heading information | |
| CN112223291B (en) | Mechanical arm obstacle avoidance method and device based on three-dimensional task space constraint | |
| CN109163722B (en) | Humanoid robot path planning method and device | |
| CN110986953B (en) | Path planning method, robot and computer readable storage medium | |
| CN115723129B (en) | Continuous operation motion planning method for mechanical arm | |
| CN109940614B (en) | Mechanical arm multi-scene rapid motion planning method integrating memory mechanism | |
| Przybylski et al. | D* Extra Lite: A dynamic A* with search–tree cutting and frontier–gap repairing | |
| CN115958590A (en) | RRT-based mechanical arm deep frame obstacle avoidance motion planning method and device | |
| CN111216132A (en) | Six-degree-of-freedom mechanical arm path planning method based on improved RRT algorithm | |
| CN116276955A (en) | Drilling and anchoring robot drill boom track planning method, system and electronic equipment | |
| US11673271B2 (en) | Trajectory generation apparatus, multi-link system, and trajectory generation method | |
| CN113203420B (en) | Industrial robot dynamic path planning method based on variable density search space | |
| CN115723125A (en) | Mechanical arm repeated operation motion planning method | |
| CN111230875B (en) | Double-arm robot humanoid operation planning method based on deep learning | |
| CN115533912A (en) | Mechanical arm motion planning method based on rapid expansion random tree improvement | |
| CN114310904B (en) | Novel bidirectional RRT method suitable for space path planning of mechanical arm joint | |
| Tian et al. | Sampling-based planning for retrieving near-cylindrical objects in cluttered scenes using hierarchical graphs | |
| Panov et al. | Automatic formation of the structure of abstract machines in hierarchical reinforcement learning with state clustering | |
| CN114281087B (en) | Path planning method based on life planning A and speed obstacle 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 |