CN113103236B - Rapid and gradual optimal mechanical arm obstacle avoidance path planning method - Google Patents
Rapid and gradual optimal mechanical arm obstacle avoidance path planning method Download PDFInfo
- Publication number
- CN113103236B CN113103236B CN202110437046.XA CN202110437046A CN113103236B CN 113103236 B CN113103236 B CN 113103236B CN 202110437046 A CN202110437046 A CN 202110437046A CN 113103236 B CN113103236 B CN 113103236B
- Authority
- CN
- China
- Prior art keywords
- path
- new
- node
- mask
- mechanical arm
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
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
- B25J9/1666—Avoiding collision or forbidden zones
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Feedback Control In General (AREA)
Abstract
A rapid and gradual optimal mechanical arm obstacle avoidance path planning method comprises the steps of obtaining environment information of a working space obstacle, establishing a mechanical arm collision model and a kinematics model, and determining an initial state and a target state of a joint space form; rapidly obtaining an initial path through a strategy of double-tree opposite growth; then establishing a heuristic hyper-ellipsoid sampling subspace, and carrying out sampling, double-tree growth and path optimization in the subspace; the path shrinkage algorithm further optimizes the existing path and also accelerates the compression of the hyper-ellipsoid space; the strategy of optimizing the shortest path by multiple sampling growth and once updating also ensures that the search tree can fully explore the space and optimize the path, and avoids excessive and useless path contraction. The method disclosed by the invention integrates various heuristic ideas, balances the rapidity and the optimality of the RRT path planning algorithm, and can rapidly, gradually and optimally obtain a collision-free path.
Description
Technical Field
The invention relates to the field of mechanical arm motion planning, in particular to a method for quickly searching a shortest feasible collision-free path for a high-dimensional mechanical arm in a static barrier environment.
Background
The planning of the mechanical arm path is a basic problem in the research field of robots, namely, a feasible collision-free path theta(s) (0 ≦ s ≦ 1) is obtained by giving a starting state and a target state, wherein theta (0) represents the starting state, theta (1) represents the target state, and collision-free means that no collision occurs between the mechanical arms and the surrounding environment for any state on the path.
The traditional path planning algorithms of the mobile robot such as an artificial potential field method, an A-star method and the like all need to display and express obstacles in a configuration space, however, the dimension in the path planning of the mechanical arm is high, the calculation amount of the algorithms is obviously increased, and the success rate and the real-time performance of the algorithms are seriously reduced. To address this problem, researchers have proposed sampling-based planning methods such as PRM (probabilistic road marking), RRT (fast-spanning random tree). The algorithm samples in a state space, and judges the feasibility of sampling points through collision detection, thereby avoiding the display expression of environmental constraints and showing relatively high-efficiency path planning capability in a high-order configuration space.
The RRT (rapid-expanding random tree) algorithm randomly samples in a state space, feasible sampling points which are not collided are added into a random exploration tree according to a certain expansion mode, the whole state space can be fully explored along with the sampling, the random tree can grow to a target state or the vicinity of the target state, and a feasible collision-free path from an initial state (a root node of the tree) to the target state is formed, so that the RRT algorithm is an algorithm with complete probability.
The planning effect of the RRT algorithm can be improved by adding various inspiration ideas. The RRT-connect respectively generates a random tree from an initial state and a target state, and a feasible path is formed when the two trees meet, so that the planning speed is greatly improved, but the path randomness is high and is not optimal; RRT continuously adjusts the parent-child relationship between the newly generated node and the nodes nearby the newly generated node, gradually reduces the path cost, realizes the gradual optimization of the path, and has long planning time; the method has the advantages that the informed-RRT algorithm explores and samples in a heuristic hyper-ellipsoid subspace, the progressive optimization speed of RRT is increased, the initial path obtaining is still slow, and the hyper-ellipsoid space shrinkage is slow; it can be seen that the algorithm based on the fast random search tree still has the problems that the rapidity and the optimality are difficult to balance, and the optimization speed is slow.
Disclosure of Invention
Aiming at the problem that the rapidity and optimality of mechanical arm path planning are difficult to balance, the invention provides a rapid gradual optimal mechanical arm obstacle avoidance path planning method, which is used for carrying out variation on an RRT algorithm and finding a mechanical arm collision-free motion path in rapid gradual optimization.
The invention discloses a rapid and gradual optimal mechanical arm obstacle avoidance path planning method which specifically comprises the following steps:
step 1: acquiring environment information of a working space obstacle, establishing a mechanical arm collision model and a kinematic model, and determining an initial state and a target state of a joint space form;
establishing a mechanical arm space kinematic model containing a collision model, acquiring position and size information of obstacles in all mechanical arm working spaces in the environment, and providing collision information for collision detection in an algorithm;
step 2: acquiring an initial path;
determining a starting state s _ init and a target state s _ goal, and in the form of a joint space (q)1,q2,...,qn) In representation, n represents the degree of freedom of the mechanical arm; if the joint space form is already existed, the next step is entered; if the joint angle is expressed in a Cartesian space form, a group of collision-free joint angles are obtained through inverse kinematics of the serial mechanical arm and collision detection and are set as state points; if the initial state or the target state of the collision-free joint space cannot be solved, the initial state or the target state needs to be reset until the collision-free state is solved; generating an initial path:
generating initial random trees Ta and Tb by taking an initial state s _ init and a target state s _ goal as root nodes respectively, wherein Ta is (Va, Ea), Va represents a set formed by nodes in the trees, Ea represents a connection relation between the nodes in Va, Ea represents a form of an element in (v _ parent, v), and v _ parent represents a parent node of a node v; tb ═ (Vb, Eb), meaning similar to the Ta tree;
firstly, expanding a random tree Ta, uniformly sampling in a state space of a mechanical arm to obtain a sampling point s _ rand, finding a node s _ nearest to the s _ rand in a node set Va of the Ta, wherein the node s _ nearest is argmin (| s _ rand-V |), V belongs to V, argmin (. |) represents a value corresponding to an independent variable when a function takes a minimum value, and | is | |. represents a Euclidean distance; generating a new node s _ new from s _ nearest to s _ rand, wherein constant step represents the maximum step size of node growth; when | | | s _ rand-s _ nearest | | ventilation>At the time of step, the temperature of the film is controlled,
when | | | s _ rand-s _ nearest | | | is less than or equal to step, s _ new ═ s _ rand.
Judging whether a local path between s _ new and s _ nearest is collided, wherein delta represents the detection step length of collision detection, and collecting:
wherein
Representing a maximum integer not greater than (·), if one state in any C satisfies no collision of the mechanical arm and the surrounding environment, then no collision exists in a local path between s _ new and s _ new, adding a new node s _ new to a node set Va of Ta, and adding edges (s _ new ) before a parent node s _ new and a new child node s _ new to an edge set Ea of Ta; if the local path is collided, the fourth step of the step 2 is carried out;
expansion random tree Tb; taking a new node s _ new of Ta as a sampling point s _ rand of Tb, growing Tb according to the methods of the first step and the second step, if the growth is successful and | | s _ rand-s _ nearest | | | is less than or equal to step, namely s _ rand of Tb is equal to s _ new of Ta, forming an intersection point between the two trees at s _ new, adding s _ new into a double-tree intersection point set Xsolns, and entering the step 3;
exchanging two random search trees Ta and Tb, and entering the first step of the step 2;
and step 3: updating the long axis cbest of the hyperellipsoid sampling space to obtain an optimal path;
and calculating the major axis this _ cbest of the hyperellipsoid sampling space at this time to obtain the current optimal path this _ optimal _ path.
One intersection point in the intersection point set Xsolns corresponds to a path from a starting point to a target point, a shortest distance path is obtained from all paths corresponding to the Xsolns, path contraction is carried out, then a plurality of paths are randomly selected from the rest paths for path contraction, the shortest distance in all contracted paths is calculated and used as the shortest distance this _ cbest calculated at this time, and the contracted path corresponding to the shortest distance is used as the optimal path this _ optimal _ path calculated at this time.
The PATH method for obtaining a PATH from s _ init to s _ goal from an intersection soln is as follows: setting a random search tree actually taking s _ init as a root node during the calculation as initTree, and a random search tree actually taking s _ coarse as a root node as a coarse tree, wherein the random search tree can be determined by judging whether the root node of Ta and Tb is equal to s _ init or s _ coarse; firstly, determining a path first _ path from an intersection soln to an s _ init part; according to the data structure characteristics of the tree, the child node only has a unique father node; firstly adding soln into first _ path, then finding parent node parent _ soln in the edge set initE of initTree, adding first _ path, making soln equal to parent _ soln, continuously searching the parent node of soln and adding first _ path, and continuously carrying out until soln is equal to s _ init; determining the second _ path of the path from soln to s _ good part is the same as the method; and finally:
PATH ═ flip (first _ PATH) + second _ PATH; where flip () denotes the order of the inverted sequence;
the method for calculating the length of a PATH is as follows: let the number of elements in PATH be n, PATH [ i]Representing i elements, each element representing a path point in the path, a point-to-point local path planner being adopted between each path point, and the distance between the path points being an Euclidean distance; the length is as follows:
the method for path contraction is as follows: let the number of elements in PATH be n, PATH [ i ] represents i elements, the contraction PATH is marked as P, i is 1, j is 3, PATH [ i ] is added to P,
carrying out collision detection on edges (PATH [ i ], PATH [ j ]) according to the method in the step 2, if collision occurs, adding P into PATH [ j-1], enabling i to be j-1 and j to be j +1, and if no collision occurs, enabling j to be j + 1;
and when j is not larger than n, circularly executing the step I.
Finally, adding PATH [ n ] into P, wherein P is the PATH contracted from the original PATH PATH.
The process of randomly selecting a plurality of paths in the remaining paths is as follows: assuming that the total number of elements in the rendezvous point set is n, corresponding to n paths, selecting the rest n-1 paths not including the shortest path in a uniformly distributed mode
Strip pathA path shrink algorithm is performed.
And 4, step 4: if the shortest distance this _ cbest calculated this time is shorter than the existing shortest distance cbest, making cbest equal to this _ cbest, existing shortest contraction path optimal _ path equal to this _ optimal _ path, updating Mask sets Mask _ Ta and Mask _ Tb, and updating an intersection point set Xsolns;
if the shortest distance this _ cbest calculated this time is shorter than the existing shortest distance cbest, let cbest be this _ cbest, have shortest contraction path optimal _ path be this _ optimal _ path, update Mask sets Mask _ Ta and Mask _ Tb, update intersection point set Xsolns, if this _ cbest is greater than or equal to cbest, then directly enter step 5.
Note that: the cbest initial value indicating the shortest distance is infinite (Inf), and the optimal _ path initial value indicating the shortest path is null (None).
Wherein, the process of updating the mask set is as follows: the mask set is an index of all nodes of the random search tree in the hyperellipsoid sampling space, and when the long axis cbest of the hyperellipsoid sampling space is reduced, the mask set needs to be updated. Taking an example of updating a Mask set Mask _ Ta of Ta, traversing a node set Va of Ta, if a certain node v satisfies: and if the Mask set of Tb is less than cbest, the index number of v is added into Mask _ Ta, and the Mask set of Tb is updated in the same way.
The process of updating the rendezvous point set comprises the following steps: traversing the elements of the existing intersection point set Xsolns, if the index of the element in the Ta tree node set Va is not in Mask _ Ta, removing the element, and otherwise, keeping the element.
And 5: obtaining uniform sampling points s _ helix in a super-ellipsoid space;
the focus of the hyper-ellipsoid space is s _ init and s _ coarse, the major axis is cbest, the distance cmin between the two focuses is | | | s _ coarse-s _ init | |, and the center of the hyper-ellipsoid space
Short axis of
The sampling points s _ ball uniformly distributed in the n-dimensional unit hyper-sphere are subjected to deformation transformation C and rotation transformationThe L transform, and the s _ center translation transform may result in evenly distributed sampling points s _ epipse in the hyper-ellipsoid space:
s_ellipse＝C·L·s_ball+s_center。，，
wherein s _ ball represents uniform sampling in an n-dimensional unit hyper-sphere, and n represents the degree of freedom of the mechanical arm; uniform sampling within n-dimensional units of hypersphere using the method of rejection sampling: generating n random floating point numbers which are uniformly distributed in an open interval (-1,1) to form a sampling point s _ ball ═ (a1, a2, …, an), if | | s _ ball | <1, reserving the sampling point, otherwise, repeatedly generating the sampling point until | | | s _ ball | <1 is met;
l represents the transformation from a unit hypersphere to a hypersphere:
C denotes a rotational transformation matrix from the hyper-ellipsoid to the world coordinate system: c ═ U · diag {1, ·,1, det (U) det (V) } · V ═ V ·TWherein diag { } denotes a diagonal matrix whose diagonal elements number n degrees of freedom of the mechanical arm; det (.) denotes determinant;
and
is to satisfy U sigma VTUnitary matrix of M [ identical to ] can be obtained by singular value decomposition of matrix M
Wherein
Representing the transpose of the first column of the identity matrix I.
Note that C and s _ center need only be calculated once at the start of the program.
Step 6: growing a double tree, if the node is successfully grown, optimizing the path of the node in the mask set, adding the mask set, and if a new path is formed, adding the intersection point into the intersection point set;
calculating a node s _ new of s _ neighbor extending towards the direction of s _ neighbor according to the same algorithm as the first step in the step 2 for the Ta tree, performing collision detection on the sides (s _ neighbor, s _ new) according to the algorithm in the step 2, and entering the step 7 if collision occurs;
and if the side (s _ nearest, s _ new) has no collision, selecting a nearby node Near which has no collision and is not more than the distance r from the s _ new as { s belongs to Va [ Mask _ Ta ] | | s-s _ new | < r & noCollision (s, s _ new) }. Va [ Mask _ Ta ] represents a node set of Ta obtained by the Mask set index in the super-ellipsoid space; noCollision (s, s _ new) indicates that the edge (s, s _ new) needs no collision, and the collision detection algorithm is consistent with the algorithm II in the step 2; the distance r is defined as:
then, the optimal parent node s _ min of the optimal s _ new is selected in the set { s _ nearest & } as:
s _ min ═ argmin (cost(s) + | | s-s _ new) s ∈ { s _ nearest £ Near }, where cost(s) denotes the distance from s to the root node path. And adding a new node s _ new into Va, adding edges (s _ min, s _ new) into Ea, and adding the index of s _ new into Mask _ Ta. The shortest path length of S _ new is S _ new ═ cost (S _ min) + | | S _ min-S _ new |.
And then traversing a set { S _ nearest [. sup.N.sub \ S _ min }, wherein "\\" represents difference set operation, if an element S satisfies S _ new + | S-S _ new | < cost (S), deleting (S _ parent, S) in the edge set Ea, and adding (S _ new, S), namely changing the parent node of S into S _ new.
And expansion of the random tree Tb. And (3) taking a new node s _ new of Ta as a sampling point s _ ellise of Tb, operating Tb according to the algorithm of the first step and the second step, if the growth is successful and | | | s _ ellise-s _ nearest | | | is less than or equal to step, namely the s _ ellise of Tb is equal to s _ new of Ta, forming an intersection point in s _ new by the two trees, and adding s _ new into the double-tree intersection point set Xsolns.
And 7: exchanging two random search trees Ta and Tb, exchanging Mask sets Mask _ Ta and Mask _ Tb, and repeating the steps 5-7 for M times, wherein M is a self-defined positive integer.
And 8: and repeating the steps 3-7 for N times, wherein N is a self-defined positive integer, or the optimization process can be finished after the optimization process is performed for a certain limited time. The shortest contraction path optimal _ path is the obtained optimal path, and cbest is the path distance of the optimal _ path;
and step 9: the time parameterization optimal _ path is obtained under the constraint conditions of the speed, the acceleration and the like of the mechanical arm, a track containing time information is sent to a controller of the mechanical arm, and the mechanical arm can run from an initial state to a target state without collision according to the path.
The invention has the advantages that:
the dual-tree expansion strategy accelerates the acquisition of an initial path and also accelerates the growth optimization process of nodes in a heuristic hyper-ellipsoid space; the heuristic hyper-ellipsoid space limits the searching and optimization of the path to a smaller space, thereby avoiding the blind exploration of the whole space; the path shrinkage algorithm further optimizes the existing path and also accelerates the compression of the hyper-ellipsoid space; the shortest path and a plurality of randomly selected paths are shrunk, so that the shortest path is guaranteed to be found, and the influence of excessive path shrinkage on the overall time efficiency of the algorithm is avoided; the strategy of optimizing the shortest path by multiple sampling growth and once updating also ensures that the search tree can fully explore the space and optimize the path, and avoids excessive and useless path contraction.
Drawings
Fig. 1 is an overall flowchart of the mechanical arm obstacle avoidance path planning method for fast progressive optimization according to the present invention.
FIG. 2 is a schematic diagram of a two-dimensional path shrinking algorithm.
Fig. 3 is a two-dimensional elliptical sampling space diagram.
Detailed Description
The principles and features of this invention are described below in conjunction with the following drawings, which are set forth by way of illustration only and are not intended to limit the scope of the invention.
Firstly, a simulation platform of a seven-degree-of-freedom mechanical arm is built by using an open source robot operating system ROS; and obtaining the obstacle information of the working space by scanning with a depth camera, and obtaining the size and position information of all obstacles by simplifying the environment information by adopting an AABB bounding box.
Determining a starting state s _ init and a target state s _ goal, and in the form of a joint space (q)1,q2,...,qn) In this example, n represents the degree of freedom of the robot arm, and in this example, n is 7. If the joint angle is expressed in a Cartesian space form, a group of collision-free joint angles are obtained through inverse kinematics of a serial mechanical arm and collision detection and are set as state points; if the starting or target state of the joint space without collision cannot be determined, the starting or target state needs to be reset until the collision-free state is determined.
Then, an improved RRT algorithm of the invention is adopted to plan a collision-free path for the mechanical arm, the overall flow chart is shown in figure 1, and the detailed steps are as follows:
step 1: an initial path is generated. Generating initial random trees Ta and Tb by taking an initial state s _ init and a target state s _ goal as root nodes respectively, wherein Ta is (Va, Ea), Va represents a set formed by nodes in the trees, Ea represents a connection relation between the nodes in Va, Ea represents a form of an element in (v _ parent, v), and v _ parent represents a parent node of a node v; tb ═ (Vb, Eb), meaning similar to Ta trees.
(1) The method comprises the steps of expanding a random tree Ta, uniformly sampling in a state space of a mechanical arm to obtain a sampling point s _ rand, finding a node s _ nearest to the s _ rand in a node set Va of the Ta, wherein the node s _ nearest is argmin (| s _ rand-V |) V ∈ V, the argmin (. |) represents a value corresponding to an independent variable when a function is the minimum value, and | |. represents an Euclidean distance. Generating a new node s _ new from s _ neighbor to s _ rand, constant step tableShowing the maximum step length of node growth, when | | s _ rand-s _ nearest | | survival>At the time of step, the temperature of the film is controlled,
when | | s _ r is less than or equal to a, s _ new ═ s _ rand. In this example, step is 0.3.
(2) It is determined whether the local path between s _ new and s _ nearest is a collision. δ represents a detection step of collision detection, where δ is 0.1, and δ is not preferably too small, otherwise the time for collision detection is greatly increased. For the set:
wherein
Representing the largest integer not greater than (·), if one element in any set C satisfies no collision of the robot arm itself and no collision of the robot arm with the surrounding environment, then there is no collision of the local path between s _ new and s _ new, adding the new node s _ new to the node set V of Ta, and adding the edges (s _ new ) before the parent node s _ new and the new child node s _ new to the edge set Ea of Ta. If the local path has a collision, step (4) of this step is entered.
(3) The random tree Tb is expanded. And (3) taking a new node s _ new of Ta as a sampling point s _ rand of Tb, growing Tb according to the methods (1) to (2), if the growth is successful and | | s _ rand-s _ nearest | | | is less than or equal to step, namely the s _ rand of Tb is equal to s _ new of Ta, forming an intersection point between the two trees at s _ new, adding s _ new into the double-tree intersection point set Xsolns, and entering the step 4.
(4) Two random search trees Ta and Tb are exchanged, and the step (1) is entered.
Step 2: and updating the current calculation to obtain the major axis this _ cbest of the hyperellipsoid sampling space, and obtaining the current optimal path.
One intersection point in the intersection point set Xsolns corresponds to a path from a starting point to a target point, a shortest distance path is obtained from all paths corresponding to the Xsolns, path contraction is carried out, then a plurality of paths are randomly selected from the rest paths for path contraction, the shortest distance in all contracted paths is calculated and used as the shortest distance this _ cbest calculated at this time, and the contracted path corresponding to the shortest distance is used as the optimal path this _ optimal _ path calculated at this time.
The PATH method for obtaining a PATH from s _ init to s _ goal from an intersection soln is as follows: assuming that the random search tree actually using s _ init as the root node in this calculation is initTree, and the random search tree actually using s _ coarse as the root node is coarse, it can be determined by determining whether the root node of Ta and Tb is equal to s _ init or s _ coarse. First a path first _ path is determined from the intersection soln to the s _ init part. According to the data structure characteristics of the tree, a child node has only a unique parent node. Firstly adding soln into first _ path, then finding parent node parent _ soln in the edge set initE of initTree, adding first _ path, making soln equal to parent _ soln, continuously searching parent node of soln and adding first _ path, and continuously carrying out till soln is equal to s _ init. Determining the second _ path of the path from soln to s _ good portion is the same as the above method. And finally:
PATH is flip (first _ PATH) + second _ PATH. Where flip () indicates the order of the inverted sequence.
The method for calculating the length of a PATH is as follows: let the number of elements in PATH be n, PATH [ i]And representing i elements, wherein each element represents a path point in the path, a point-to-point local path planner is adopted between every two path points, and the distance between every two path points adopts an Euclidean distance. The length is as follows:
the method for path contraction is as follows: let the number of elements in PATH be n, PATH [ i ] represents i elements, the contraction PATH is marked as P, i is 1, j is 3, PATH [ i ] is added to P,
(1) performing collision detection on the edges (PATH [ i ], PATH [ j ]) according to the method described in step 1 (2), if collision occurs, adding PATH [ j-1] into P, making i equal to j-1 and j equal to j +1, if no collision occurs, making j equal to j +1
(2) When j is not more than n, executing (1) in a loop
Finally, adding PATH [ n ] into P, wherein P is the PATH contracted from the original PATH PATH. A schematic diagram of a systolic path in a two-dimensional environment is shown in fig. 2.
The details of randomly selecting several paths among the remaining paths are: assuming that the total number of elements in the rendezvous point set is n, corresponding to n paths, selecting the rest n-1 paths not including the shortest path in a uniformly distributed mode
The strip path performs a path shrink algorithm.
And step 3: if the shortest distance this _ cbest calculated this time is shorter than the existing shortest distance cbest, let cbest be this _ cbest, have shortest contraction path optimal _ path be this _ optimal _ path, update Mask sets Mask _ Ta and Mask _ Tb, update intersection point set Xsolns, if this _ cbest is greater than or equal to cbest, then directly enter step 4.
Note that: the cbest initial value indicating the shortest distance is infinite (Inf), and the optimal _ path initial value indicating the shortest path is null (None).
The step of updating the mask set is: the mask set is an index of all nodes of the random search tree in the hyperellipsoid sampling space, and when the long axis cbest of the hyperellipsoid sampling space is reduced, the mask set needs to be updated. Taking an example of updating a Mask set Mask _ Ta of Ta, traversing a node set Va of Ta, if a certain node v satisfies: and if the Mask set Mask _ Tb is updated, the index number of v is added into the Mask _ Ta, and the method for updating the Mask set Mask _ Tb is the same.
The step of updating the intersection point set is as follows: traversing the elements of the existing intersection point set Xsolns, if the index of the element in the Ta tree node set Va is not in Mask _ Ta, removing the element, and otherwise, keeping the element.
And 4, step 4: and uniformly sampling in a super-ellipsoid space to obtain s _ epipse.
The focus of the hyper-ellipsoid space is s _ init and s _ coarse, the major axis is cbest, the distance cmin between the two focuses is | | | s _ coarse-s _ init | |, and the center of the hyper-ellipsoid space
Short axis of
A two-dimensional elliptical sampling space diagram is shown in fig. 3. The sampling points s _ ball uniformly distributed in the n-dimensional unit hyper-sphere are subjected to deformation transformation C, rotation transformation L and translation transformation s _ center to obtain the sampling points s _ capsule uniformly distributed in the hyper-sphere space:
s_ellipse＝C·L·s_ball+s_center
where s _ ball represents uniform sampling within an n-dimensional unit hypersphere and n represents the degree of freedom of the mechanical arm, which is 7 in this example. Uniform sampling within n-dimensional units of hypersphere using the method of rejection sampling: generating n random floating point numbers uniformly distributed in an open interval (-1,1) to form a sampling point s _ ball ═ (a1, a2, …, an), if | | s _ ball | <1, retaining the sampling point, otherwise, repeatedly generating the sampling point until | | s _ ball | <1 is satisfied.
L represents the transformation from a unit hypersphere to a hypersphere:
C denotes a rotational transformation matrix from the hyper-ellipsoid to the world coordinate system: c ═ U · diag {1, ·,1, det (U) det (V) } · V ═ V ·TThe number of diagonal elements of the diag { } diagonal matrix is the determinant for which the degree of freedom n, det (·) of the mechanical arm represents (·),
and
is to satisfy U sigma VTUnitary matrix of M [ identical to ] can be obtained by singular value decomposition of matrix M
Wherein
Representing the transpose of the first column of the identity matrix I.
Note that C and s _ center need only be calculated once at the start of the program.
And 5: and (4) growing the double trees, if the nodes are successfully grown, optimizing the paths of the nodes in the mask set, adding the mask set, and if a new path is formed, adding the intersection points into the intersection point set.
(1) And (3) calculating a node s _ new of the Ta tree, which is expanded from s _ nearest to s _ overlap direction, according to the same algorithm as the algorithm in the step (1) in the step 1, performing collision detection on the side (s _ nearest, s _ new) according to the algorithm in the step (2) in the step 1, and entering the step 6 if the collision occurs.
(2) And if the edges (s _ nearest, s _ new) have no collision, selecting a nearby node new which is not more than the distance r from s _ new and has no collision as { s is in the range of Va [ Mask _ Ta ] | | | s-s _ new | < r & noCollision (s, s _ new) }, wherein Va [ Mask _ Ta ] represents a node set of Ta in the super-ellipsoid space obtained by the Mask set index, noCollision (s, s _ new) represents that the edges (s, s _ new) need no collision, and the collision detection algorithm is consistent with the algorithm (2) in the step 1. The distance r is defined as:
Then, the optimal parent node s _ min of the optimal s _ new is selected in the set { s _ nearest & } as:
s _ min ═ argmin (cost(s) + | | s-s _ new), s ∈ { s _ nearest £ Near }, cost(s) denotes the distance from s to the root node path. And adding a new node s _ new into Va, adding edges (s _ min, s _ new) into Ea, and adding the index of s _ new into Mask _ Ta. At this time, the shortest path length of S _ new is S _ new ═ cost (S _ min) + | | S _ min-S _ new |.
Then, the set { S _ nearest [. sup.sub. } Near \ S _ min } is traversed, and "\\" represents difference set operation, if the element S satisfies S _ new + | S-S _ new | < cost (S), the element S is deleted (S _ parent, S) in the edge set Ea, and (S _ new, S) is added, namely, the parent node of S is changed into S _ new.
(3) The random tree Tb is expanded. And (3) taking a new node s _ new of Ta as a sampling point s _ ellise of Tb, operating Tb according to the algorithms in the steps (1) to (2), if the growth is successful and | | s _ ellise-s _ nearest | | | is less than or equal to step, namely the s _ ellise of Tb is equal to s _ new of Ta, forming an intersection point in s _ new by the two trees, and adding s _ new into the double-tree intersection point set Xsolns.
Step 6: and exchanging two random search trees Ta and Tb, exchanging Mask sets Mask _ Ta and Mask _ Tb, and repeating the step 4-6M times, wherein M is 20.
And 7: and (5) running the steps 3-6 for N times, wherein N is 100, or running for a certain limited time can also finish the optimization process.
Finally, optimal _ path is the obtained optimal path, and cbest is the obtained shortest path distance. According to a time optimal method, a track containing time information is obtained through time parameterization under the constraint conditions of the speed, the acceleration and the like of the mechanical arm, the track is sent to a controller of the mechanical arm, and the mechanical arm can run from an initial state to a target state without collision according to the path.
Claims (8)
1. A rapid and gradual optimal mechanical arm obstacle avoidance path planning method is characterized by comprising the following steps:
(1) acquiring environment information of a working space obstacle, establishing a mechanical arm collision model and a kinematic model, and determining an initial state and a target state of a joint space form;
(2) acquiring an initial path;
(3) updating the long axis of the super-ellipsoid sampling space to obtain an optimal path;
(4) if the shortest distance this _ cbest calculated this time is shorter than the existing shortest distance cbest, making cbest equal to this _ cbest, existing shortest contraction path optimal _ path equal to this _ optimal _ path, updating Mask sets Mask _ Ta and Mask _ Tb, and updating junction point set Xsolns;
(5) uniformly sampling in a super-ellipsoid space to obtain s _ epipse;
(6) growing a double tree, if the node is successfully grown, optimizing the path of the node in the mask set, adding the mask set, and if a new path is formed, adding the intersection point into the intersection point set;
(7) exchanging two random search trees Ta and Tb, exchanging Mask sets Mask _ Ta and Mask _ Tb, and repeating the steps (5) - (7) for M times;
(8) the optimization process can be finished by running the steps (3) to (7) for N times or running for a certain limited time, and the optimal _ path is the obtained optimal path;
(9) the time parameterization optimal _ path is obtained under the constraint conditions of the speed and the acceleration of the mechanical arm, a track containing time information is sent to a controller of the mechanical arm, and the mechanical arm can run from an initial state to a target state without collision according to the path.
2. The method for planning the obstacle avoidance path of the mechanical arm for the fast and gradual optimization as claimed in claim 1, wherein: the method for acquiring the initial path in the step (2) comprises the following steps:
generating initial random trees Ta and Tb by taking an initial state s _ init and a target state s _ goal as root nodes respectively, wherein Ta is (Va, Ea), Va represents a set formed by nodes in the trees, Ea represents a connection relation between the nodes in Va, Ea represents a form of an element in (v _ parent, v), and v _ parent represents a parent node of a node v; tb ═ (Vb, Eb), meaning similar to the Ta tree; firstly, expanding a random tree Ta, uniformly sampling in a state space of a mechanical arm to obtain a sampling point s _ rand, finding a node s _ nearest to the s _ rand in a node set Va of the Ta, wherein the node s _ nearest is argmin (| s _ rand-V |) V ∈ V, the argmin (. |) represents a value corresponding to an independent variable when a function takes a minimum value, and | | is | | | represents an Euclidean distance; generating a new node s _ new from s _ nearest to s _ rand, wherein constant step represents the maximum step size of node growth, when | | | s _ rand-s _ nearest | |>At the time of step, the temperature of the film is controlled,
when | | | s _ rand-s _ nearest | | | is less than or equal to step, s _ new ═ s _ rand;
then, judging whether a local path between s _ new and s _ nearest is collided, wherein delta represents a detection step size of collision detection, and the set is as follows:
wherein
Representing a maximum integer not greater than (·), if one state in any C satisfies no collision of the mechanical arm and the surrounding environment, then no collision exists in a local path between s _ new and s _ new, adding a new node s _ new to a node set V of Ta, and adding edges (s _ new ) before a parent node s _ new and a new child node s _ new to an edge set Ea of Ta; if the local path is collided, exchanging Ta and Tb, and resampling for growth;
then, a new node s _ new of Ta is used as a sampling point s _ rand of Tb, the random tree Tb is expanded in the same way as the growing Ta, if the growth is successful and | | s _ rand-s _ nearest | | | is less than or equal to step, namely the s _ rand of Tb is equal to s _ new of Ta, the two trees form an intersection point in s _ new, and s _ new is added into a double-tree intersection point set Xsolns, so that an initial path is obtained; if the initial path is not formed, Ta and Tb are exchanged, and the sample growth is continued.
3. The method for planning the obstacle avoidance path of the mechanical arm for the fast and gradual optimization as claimed in claim 1, wherein: the method for updating the long axis cbest of the hyperellipsoid sampling space in the step (3) comprises the following steps:
an intersection point in an intersection point set Xsolns corresponds to a path from a starting point to a target point, a shortest distance path is obtained from all paths corresponding to the Xsolns, path contraction is carried out, then a plurality of paths are randomly selected from the rest paths for path contraction, the shortest distance in all contracted paths is calculated and is used as the shortest distance this _ cbest calculated at this time, and the contracted path corresponding to the shortest distance is used as the optimal path this _ optimal _ path calculated at this time;
the PATH method for obtaining a PATH from s _ init to s _ goal from an intersection soln is as follows: setting a random search tree actually taking s _ init as a root node during the calculation as initTree, and a random search tree taking s _ coarse as a root node as a coarse tree, and determining whether the root node of Ta and Tb is equal to s _ init or s _ coarse; firstly, determining a path first _ path from an intersection soln to an s _ init part, according to the data structure characteristics of a tree, enabling child nodes to only have a unique father node, firstly adding soln into the first _ path, then finding the father node parent _ soln in an edge set initE of the initTree, adding the first _ path, enabling soln to be equal to the parent _ soln, continuously finding the father node of the soln and adding the first _ path, and continuously carrying out the steps until the soln is equal to the s _ init; determining the second _ path of the path from soln to s _ good part is the same as the method; and finally:
PATH ═ flip (first _ PATH) + second _ PATH, flip (·) denotes the order of the reverse sequence;
the method for calculating the length of a PATH is as follows: let the number of elements in PATH be n, PATH [ i]Representing the ith element, each element representing a path point in the path, adopting a point-to-point local path planner between each path point, adopting Euclidean distance between the path points, and the length being:
the process of randomly selecting a plurality of paths in the remaining paths is as follows: assuming that the total number of elements in the rendezvous point set is n, corresponding to n paths, selecting the rest n-1 paths not including the shortest path in a uniformly distributed mode
The strip path performs a path shrink algorithm.
4. The method for planning the obstacle avoidance path of the mechanical arm for the fast progressive optimization as claimed in claim 3, wherein the path shrinkage method comprises:
let the number of elements in PATH be n, PATH [ i ] represents the ith element, the contraction PATH is denoted as P, let i be 1, j be 3, add PATH [ i ] to P,
carrying out collision detection on edges (PATH [ i ], PATH [ j ]) according to the method in the step (2), if collision occurs, adding PATH [ j-1] into P, and enabling i to be j-1 and j to be j +1, if no collision occurs, enabling j to be j + 1;
when j is not more than n, circularly executing the step I,
finally, adding PATH [ n ] into P, P is the PATH contracted from the original PATH PATH.
5. The method for planning the obstacle avoidance path of the mechanical arm, which is fast and gradually optimized, as claimed in claim 1, is characterized in that: the process of updating the mask set in the step (4) is as follows: the mask set is all node indexes of the random search tree in the hyperellipsoid sampling space, and when the long axis cbest of the hyperellipsoid sampling space is reduced, the mask set needs to be updated; when a Mask set Mask _ Ta of Ta is updated, traversing a node set Va of Ta, and if a certain node v meets the following conditions: if | | v-s _ init | + | | | v-s _ good | | < cbest, adding the index number of v into Mask _ Ta; the Mask set Mask _ Tb method for updating Tb is the same.
6. The method for planning the obstacle avoidance path of the mechanical arm, which is fast and gradually optimized, as claimed in claim 1, is characterized in that: the process of updating the intersection point set in the step (4) is as follows: traversing the elements of the existing intersection point set Xsolns, if the index of the element in the Ta tree node set Va is not in Mask _ Ta, removing the element, and otherwise, keeping the element.
7. The method for planning the obstacle avoidance path of the mechanical arm, which is fast and gradually optimized, as claimed in claim 1, is characterized in that: the method for obtaining the s _ epipse by uniformly sampling in the super-ellipsoid space in the step (5) comprises the following steps:
the focus of the hyper-ellipsoid space is s _ init and s _ coarse, the major axis is cbest, the distance cmin between the two focuses is | | | s _ coarse-s _ init | |, and the center of the hyper-ellipsoid space
Short axis of
The sampling points s _ ball uniformly distributed in the n-dimensional unit hyper-sphere are subjected to deformation transformation C, rotation transformation L and translation transformation s _ center to obtain the sampling points s _ capsule uniformly distributed in the hyper-sphere space:
s_ellipse＝C·L·s_ball+s_center，
wherein s _ ball represents uniform sampling in a n-dimensional unit hyper-sphere, n represents the degree of freedom of the mechanical arm, and the method of rejection sampling is adopted to uniformly sample in the n-dimensional unit hyper-sphere: generating n random floating point numbers which are uniformly distributed in an open interval (-1,1) to form a sampling point s _ ball ═ (a1, a2, …, an), if | | s _ ball | <1, reserving the sampling point, otherwise, repeatedly generating the sampling point until | | | s _ ball | <1 is met;
l represents the transformation from a unit hypersphere to a hypersphere:
c denotes a rotational transformation matrix from the hyper-ellipsoid to the world coordinate system: c ═ U · diag {1, ·,1, det (U) det (V) } · V ═ V ·TWherein, the number of diagonal elements of the diag { } diagonal matrix is the degree of freedom n of the mechanical arm; det (·) denotes a determinant of (·);
and
is to satisfy U sigma VTUnitary matrix of M [ identical to ] obtained by singular value decomposition of M
Wherein
A transpose representing a first column of the identity matrix I;
c and s _ center need only be calculated once at the beginning.
8. The method for planning the obstacle avoidance path of the mechanical arm, which is fast and gradually optimized, as claimed in claim 1, is characterized in that: the process of growing the double trees and optimizing the path in the step (6) is as follows:
firstly, operating a Ta tree, calculating a node s _ new of s _ nearest extending to s _ kernel direction, carrying out collision detection on edges (s _ nearest, s _ new), and if the edges (s _ nearest, s _ new) have no collision, selecting a nearby node set which is not more than a distance r from the s _ new and has no collision:
near [ { s ∈ Va [ Mask _ Ta ] | | | s-s _ new | | < r & noCollision (s, s _ Near) }, where Va [ Mask _ Ta ] represents a node set of Ta in the hyper-ellipsoid space obtained by a Mask set index, noCollision (s, s _ new) represents that an edge (s, s _ new) needs no collision, and a distance r is defined as:
then, the optimal parent node s _ min of the optimal s _ new is selected in the set { s _ nearest & } as:
s _ min ═ argmin (cost(s) +| | s-s _ new |), s ∈ { s _ nearest £ Near }, cost(s) denotes the distance from s to the Ta root node path; adding a new node S _ new into Va, adding edges (S _ min, S _ new) into Ea, and adding an index of S _ new into Mask _ Ta, wherein the shortest path length of S _ new is S _ new ═ cost (S _ min) + | S _ min-S _ new |;
then, traversing a set { S _ nearest [. sup.s \/S _ min }, wherein "\" represents difference set operation, if an element S satisfies S _ new + | S-S _ new | < cost (S), deleting (S _ parent, S) in an edge set Ea, adding (S _ new, S), namely changing a parent node of S into S _ new;
and then taking the newly-generated node s _ new of Ta as the sampling point s _ ellise of Tb, operating Tb according to the same method as the method for growing and optimizing the Ta tree, if the growth is successful and | | s _ ellise-s _ nearest | | | | is less than or equal to step, namely the s _ ellise of Tb is equal to s _ new of Ta, forming an intersection point in s _ new by the two trees, and adding s _ new into the double-tree intersection point set Xsolns.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110437046.XA CN113103236B (en) | 2021-04-22 | 2021-04-22 | Rapid and gradual optimal mechanical arm obstacle avoidance path planning method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110437046.XA CN113103236B (en) | 2021-04-22 | 2021-04-22 | Rapid and gradual optimal mechanical arm obstacle avoidance path planning method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113103236A CN113103236A (en) | 2021-07-13 |
| CN113103236B true CN113103236B (en) | 2022-06-10 |
Family
ID=76719900
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110437046.XA Active CN113103236B (en) | 2021-04-22 | 2021-04-22 | Rapid and gradual optimal mechanical arm obstacle avoidance path planning method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113103236B (en) |
Families Citing this family (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113485367B (en) * | 2021-08-06 | 2023-11-21 | 浙江工业大学 | Path planning method for stage multifunctional mobile robot |
| CN113448336B (en) * | 2021-08-30 | 2022-01-14 | 天津施格机器人科技有限公司 | 3D obstacle avoidance path planning method |
| CN113873610B (en) * | 2021-09-08 | 2023-07-14 | 广州杰赛科技股份有限公司 | Path planning method, device, equipment and medium for wireless information transmission |
| CN113799141B (en) * | 2021-10-14 | 2022-09-27 | 福州大学 | Six-degree-of-freedom mechanical arm obstacle avoidance path planning method |
| CN113885535B (en) * | 2021-11-25 | 2023-09-12 | 长春工业大学 | Impact constraint robot obstacle avoidance and time optimal track planning method |
| CN114700937B (en) * | 2022-01-13 | 2024-02-13 | 深圳市越疆科技有限公司 | Mechanical arm, motion path planning method thereof, control system, medium and robot |
| CN115122317A (en) * | 2022-03-02 | 2022-09-30 | 山东大学 | Redundant manipulator path planning method and system based on optimal target configuration screening |
| CN114633258B (en) * | 2022-04-24 | 2023-06-20 | 中国铁建重工集团股份有限公司 | Planning method and related device for movement track of mechanical arm in tunnel environment |
| CN116058176A (en) * | 2022-11-29 | 2023-05-05 | 西北农林科技大学 | Fruit and vegetable picking mechanical arm control system based on double-phase combined positioning |
| CN117762149B (en) * | 2024-02-22 | 2024-05-17 | 本溪钢铁（集团）信息自动化有限责任公司 | Slag dragging robot control method, device, equipment and medium |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110744543A (en) * | 2019-10-25 | 2020-02-04 | 华南理工大学 | Improved PRM obstacle avoidance motion planning method based on UR3 mechanical arm |
| CN110919661A (en) * | 2019-12-26 | 2020-03-27 | 中国科学院沈阳自动化研究所 | Motion planning method for mechanical arm in glove box closed space |
Family Cites Families (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101691939B1 (en) * | 2009-08-10 | 2017-01-02 | 삼성전자주식회사 | Method and apparatus of path planing for a robot |
| KR101667029B1 (en) * | 2009-08-10 | 2016-10-17 | 삼성전자 주식회사 | Method and apparatus of path planing for a robot |
| US20200159233A1 (en) * | 2018-11-16 | 2020-05-21 | Great Wall Motor Company Limited | Memory-Based Optimal Motion Planning With Dynamic Model For Automated Vehicle |
| CN110228069B (en) * | 2019-07-17 | 2022-04-01 | 东北大学 | Online obstacle avoidance motion planning method for mechanical arm |
| CN110497403A (en) * | 2019-08-05 | 2019-11-26 | 上海大学 | A kind of manipulator motion planning method improving two-way RRT algorithm |
| CN111678523B (en) * | 2020-06-30 | 2022-05-17 | 中南大学 | Rapid BI _ RRT obstacle avoidance trajectory planning method based on STAR algorithm optimization |
| CN112338916A (en) * | 2020-10-29 | 2021-02-09 | 深圳市大象机器人科技有限公司 | Mechanical arm obstacle avoidance path planning method and system based on fast expansion random tree |
| CN112286202B (en) * | 2020-11-11 | 2021-07-27 | 福州大学 | Mobile robot path planning method for non-uniform sampling FMT |
-
2021
- 2021-04-22 CN CN202110437046.XA patent/CN113103236B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110744543A (en) * | 2019-10-25 | 2020-02-04 | 华南理工大学 | Improved PRM obstacle avoidance motion planning method based on UR3 mechanical arm |
| CN110919661A (en) * | 2019-12-26 | 2020-03-27 | 中国科学院沈阳自动化研究所 | Motion planning method for mechanical arm in glove box closed space |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113103236A (en) | 2021-07-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113103236B (en) | Rapid and gradual optimal mechanical arm obstacle avoidance path planning method | |
| CN113219998B (en) | Improved bidirectional-RRT-based vehicle path planning method | |
| CN109947136B (en) | Collaborative active sensing method for unmanned aerial vehicle group rapid target search | |
| CN110515094B (en) | Robot point cloud map path planning method and system based on improved RRT | |
| CN113885535B (en) | Impact constraint robot obstacle avoidance and time optimal track planning method | |
| CN112809665B (en) | Mechanical arm motion planning method based on improved RRT algorithm | |
| CN112987799B (en) | Unmanned aerial vehicle path planning method based on improved RRT algorithm | |
| CN110181508B (en) | Three-dimensional route planning method and system for underwater robot | |
| CN112338916A (en) | Mechanical arm obstacle avoidance path planning method and system based on fast expansion random tree | |
| CN112129296A (en) | Robot trajectory planning method and system | |
| CN112965485A (en) | Robot full-coverage path planning method based on secondary region division | |
| CN113848889A (en) | Path planning method based on combination of whale optimization algorithm and artificial potential field method | |
| CN111664851B (en) | Robot state planning method and device based on sequence optimization and storage medium | |
| CN111474925B (en) | Path planning method for irregular-shape mobile robot | |
| CN115122317A (en) | Redundant manipulator path planning method and system based on optimal target configuration screening | |
| CN116117822A (en) | RRT mechanical arm track planning method based on non-obstacle space probability potential field sampling | |
| CN114815801A (en) | Adaptive environment path planning method based on strategy-value network and MCTS | |
| CN110597067B (en) | Cluster control method and system for multiple mobile robots | |
| CN117773911A (en) | Obstacle avoidance method for industrial robot in complex environment | |
| CN116477505A (en) | Tower crane real-time path planning system and method based on deep learning | |
| CN115167388A (en) | RRT multi-robot formation path planning algorithm based on target guidance | |
| CN115533912A (en) | Mechanical arm motion planning method based on rapid expansion random tree improvement | |
| CN115056222A (en) | Mechanical arm path planning method based on improved RRT algorithm | |
| CN115008460A (en) | RRT mechanical arm obstacle avoidance planning method based on target offset and obstacle factors | |
| Li et al. | End-to-end autonomous exploration for mobile robots in unknown environments through deep reinforcement learning |
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 |