CN115958590A - RRT-based mechanical arm deep frame obstacle avoidance motion planning method and device - Google Patents
RRT-based mechanical arm deep frame obstacle avoidance motion planning method and device Download PDFInfo
- Publication number
- CN115958590A CN115958590A CN202111600481.6A CN202111600481A CN115958590A CN 115958590 A CN115958590 A CN 115958590A CN 202111600481 A CN202111600481 A CN 202111600481A CN 115958590 A CN115958590 A CN 115958590A
- Authority
- CN
- China
- Prior art keywords
- point
- mechanical arm
- rrt
- obstacle
- node
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 67
- 230000033001 locomotion Effects 0.000 title claims abstract description 39
- 230000008569 process Effects 0.000 claims abstract description 25
- 238000005070 sampling Methods 0.000 claims description 20
- 239000011159 matrix material Substances 0.000 claims description 15
- 238000004364 calculation method Methods 0.000 claims description 14
- 230000004888 barrier function Effects 0.000 claims description 13
- 238000005457 optimization Methods 0.000 claims description 7
- 230000008859 change Effects 0.000 claims description 6
- 238000013459 approach Methods 0.000 claims description 5
- 238000004422 calculation algorithm Methods 0.000 description 20
- 230000003044 adaptive effect Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 1
- 230000006872 improvement Effects 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
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Manipulator (AREA)
Abstract
The invention discloses a method and a device for planning mechanical arm deep frame obstacle avoidance motion based on RRT, wherein the method comprises the following steps: initializing a starting point, a target point and a search step length of the mechanical arm, identifying the position of an obstacle in the environment and constructing a corresponding bounding box; the RRT search tree is expanded in a probability random point-taking mode, mechanical arm obstacle-avoiding planning is carried out, in the expanding process, if an obstacle with the size smaller than the planning step length is encountered, self-adaptive constraint variable step length is carried out, if the current node is in a deep frame obstacle, the strategy of probability random point-taking is changed, and the strategy is adjusted to be an autonomous escape strategy, so that the current node escapes from the deep frame obstacle; optimizing the path point sequence obtained by the expansion in the step two; according to the optimized path point sequence, the mobile mechanical arm moves from the initial point to the target point without collision; the invention has the advantages that; the method is suitable for planning the mechanical arm deep frame obstacle avoidance motion.
Description
Technical Field
The invention relates to the technical field of mechanical arm obstacle avoidance path planning, in particular to a method and a device for planning mechanical arm deep frame obstacle avoidance motion based on RRT.
Background
In a mainstream mechanical arm obstacle avoidance path planning technology, an RRT algorithm based on random sampling is generally adopted, and the method is mainly suitable for searching in a high-dimensional space such as a mechanical arm because the method is high in speed and searching capability and does not need to accurately model an obstacle, and can ensure that the mechanical arm avoids environmental obstacles in the autonomous planning process and improve the safety of the mechanical arm. However, the method is too strong in randomness, the probability is complete but a better track cannot be solved, and meanwhile, when a deep-frame obstacle such as a box exists in the environment and the starting point or the ending point is in the deep-frame obstacle, the RRT algorithm is very easy to trap in a dead loop in the deep frame and cannot escape, although the probability escapes, the escaping time is possibly long, and finally planning fails. In addition, although the step size of the conventional RRT algorithm is fixed in the joint space, the step size that maps it into the working space through positive kinematics is not fixed, and if an obstacle with a size smaller than the planned step size, such as a shell class, is encountered, the conventional fixed-step RRT method cannot ensure that a collision-free path is generated.
Chinese patent application No. 202011484365.8 discloses an industrial robot path obstacle avoidance planning algorithm for improving RRT, which aims at the problems that the traditional RRT algorithm lacks guidance and has low convergence speed in the motion planning of a mechanical arm, and the like, and provides an expansion point selection strategy and a self-adaptive step size strategy on the basis of the traditional RRT algorithm, and when the algorithm falls into a local minimum value, a regression avoidance mechanism is adopted to quickly separate from the minimum value. And then optimizing the path generated by the improved algorithm by combining the Dijkstra algorithm to obtain an optimized path. And finally, converting the obtained effective path at the tail end of the mechanical arm into an optimal pose path of the mechanical arm through a mechanical arm planning module. The method can effectively guide the growth direction of the RRT tree, avoid trapping in a minimum value, improve the convergence speed of the algorithm and improve the motion planning efficiency of the mechanical arm in simulation. However, the method does not solve the problem that when a deep-frame obstacle exists in the environment and the starting point or the end point is in the deep-frame obstacle, the RRT algorithm is very easy to trap into a dead loop in the deep frame and cannot escape, and the method does not solve the problem that when obstacles with the size smaller than the planning step length, such as thin shells, are encountered, a collision-free path cannot be generated, and a better track of the deep-frame obstacle avoidance motion of the mechanical arm cannot be solved, so that the method is not suitable for the deep-frame obstacle avoidance motion of the mechanical arm.
Disclosure of Invention
The invention aims to solve the technical problem that the prior art lacks a track planning method suitable for the deep-frame obstacle avoidance movement of a mechanical arm, when a deep-frame obstacle exists in the environment and a starting point or an end point is in the deep-frame obstacle, an RRT algorithm is very easy to sink into a dead cycle in the deep frame and cannot escape, and when the deep-frame obstacle exists in the environment and meets the obstacles with the size smaller than the planning step length, such as thin shells, the collision-free path cannot be generated, and a better track of the deep-frame obstacle avoidance movement of the mechanical arm cannot be solved.
The invention solves the technical problems through the following technical means: a mechanical arm deep frame obstacle avoidance motion planning method based on RRT comprises the following steps:
the method comprises the following steps: initializing a starting point, a target point and a search step length of the mechanical arm, identifying the position of an obstacle in the environment and constructing a corresponding bounding box;
step two: the RRT search tree is expanded in a probability random point-taking mode, mechanical arm obstacle-avoiding planning is carried out, in the expanding process, if an obstacle with the size smaller than the planning step length is encountered, self-adaptive constraint variable step length is carried out, if the current node is in a deep frame obstacle, the strategy of probability random point-taking is changed, and the strategy is adjusted to be an autonomous escape strategy, so that the current node escapes from the deep frame obstacle;
step three: optimizing the path point sequence obtained by the expansion in the second step;
step four: and according to the optimized path point sequence, the mobile mechanical arm moves from the starting point to the target point without collision.
When a deep-frame obstacle exists in the environment and the starting point or the ending point is in the deep-frame obstacle, the strategy of randomly taking points by probability is changed and adjusted to be an autonomous escape strategy, so that the current node escapes from the deep-frame obstacle, the RRT algorithm is prevented from entering a dead loop in the deep frame and being incapable of escaping, when an obstacle with the size smaller than the planned step length, such as a thin shell type obstacle, is encountered, the step length is adaptively constrained and changed, a collision-free path is ensured to be generated, the sequence of path points obtained by expansion is optimized, and the optimal path of the deep-frame obstacle avoidance motion of the mechanical arm can be solved, so that the method is suitable for path planning of the deep-frame obstacle avoidance motion of the mechanical arm.
Further, the first step comprises: in a configuration space, the initial pose and the target pose of obstacle avoidance pickup are respectively obtained through forward kinematics calculation of a robot, the initial point qs and the target point qd of the mechanical arm are obtained through inverse kinematics preferential calculation, the position of an obstacle is searched through a sensor, a corresponding bounding box is constructed, and the search step length is initialized.
Further, the second step includes:
step 21: initializing the initial node of the RRT search tree, expanding the RRT search tree during searching, sampling to a target point within a joint space range by a probability alpha, randomly sampling within the whole six-dimensional space by 1-alpha to obtain a random point sample,
step 22: finding a tree node closest to a random point sample in the existing RRT tree nodes, and extending a step length segment from the tree node closest node to the random point sample to obtain a current extended joint point new _ point _ temp;
step 23: judging whether the current expansion joint point new _ point _ temp generates collision or not, and if so, repeating the steps from step 21 to step 23; and if the collision does not occur, recording the new _ point _ temp into the RRT search tree until the new _ point _ temp is equal to the target point qd, and exiting the iteration.
Further, the acquiring process of the current extended joint point new _ point _ temp in step 22 is as follows:
finding out the closest tree node to sample in the existing RRT tree nodes, starting from the closest tree node to random point sample, and using formula
And expanding one step towards the random point to obtain the new _ point _ temp of the current extended joint point.
Further, if an obstacle with a size smaller than the step length is encountered during the process of expanding the RRT search tree in step 22, the adaptive constraint variable step size includes:
establishing an obstacle repulsive force field through a formula R = rb + R and adding the obstacle repulsive force field to the mechanical arm model, wherein rb represents the radius of the original mechanical arm connecting rod model, R represents the radius of the repulsive force field, and R represents the radius of the updated mechanical arm connecting rod model;
giving a maximum step value Pmax allowed in a working space;
by the formula
Calculating a Jacobian matrix of a tool coordinate system of the mechanical arm under the current node qnode, wherein q is 1 To q 6 Respectively represents each joint variable in six-dimensional joint space, and>
and &>
Respectively represents the partial derivatives of the three components of the tail end position of the mechanical arm to the ith joint variable, and is/is selected>
And &>
Respectively representing the partial derivatives of the three components of the tail end attitude of the mechanical arm to the ith joint variable.
By the formula
Calculating Jacobian matrix J of tool coordinate system tool Norm operator A of, wherein (J) tool ) ij Represents the Jacobian matrix J tool Row ith and column jth.
Using the formula P max Meter of = max (A · | θ |)Calculating a step size in configuration space | a θ |,
when detecting that a certain connecting rod of the mechanical arm approaches or enters the obstacle repulsive force field, updating the step length segment in the RRT search tree expansion process through the formula segment length = | delta theta |.
Further, in step 21, when the current node qnode is in a deep-frame obstacle, changing a strategy of taking a random point according to a probability α, and adjusting to an autonomous escape strategy, where the autonomous escape strategy includes:
by the formula
Make the current node q node Gradually adjusting the expansion direction along with the change of i and generating a new tree node q node ' the escape strategy is stopped until a new tree node escapes from the deep-box obstacle, where i is the given number of escapes and n is the given angle to be changed for each escape.
Further, the third step includes:
numbering all nodes in a path formed from a starting point qs to a target point qd in sequence, making the index of the starting point qs be 1, making the index of the target point qd be n, directly connecting the starting point qs to the target point qd, if a connecting line of two nodes collides, connecting the node with the index of 2 with the target point qd, if the collision continues, advancing the node with the index of 3 to connect with the target point qd and detecting whether the collision occurs, and so on, if the node with the index of i does not collide with the target point qd, making qd = qi at this time, updating the target point qd, repeating the step until the starting point qs can be connected with the current updated target point qd and the collision does not occur, ending the loop iteration, and returning the optimized smooth path point sequence.
The invention also provides a device for planning the deep-frame obstacle avoidance movement of the mechanical arm based on the RRT, which comprises:
the initialization module is used for initializing a starting point, a target point and a search step length of the mechanical arm, identifying the position of an obstacle in the environment and constructing a corresponding bounding box;
the path planning module is used for expanding the RRT search tree in a probability random point taking mode and planning mechanical arm obstacle avoidance, wherein in the expanding process, if an obstacle with the size smaller than the planning step length is encountered, the step length is adaptively restrained and changed, and if the current node is in the deep frame obstacle, the strategy of probability random point taking is changed and adjusted to an autonomous escape strategy, so that the current node escapes the deep frame obstacle;
the optimization module is used for optimizing the path point sequence obtained by the path planning module expansion;
and the motion execution module is used for moving the mechanical arm from the starting point to the target point in a collision-free manner according to the optimized path point sequence.
Further, the initialization module is further configured to: the method comprises the steps of respectively obtaining an initial pose and a target pose of obstacle avoidance picking in a configuration space through positive kinematics calculation of a robot, obtaining an initial point qs and a target point qd of a mechanical arm through inverse kinematics preferential calculation, searching the position of an obstacle through a sensor, constructing a corresponding bounding box, and initializing a search step length.
Further, the path planning module further includes:
the sampling unit is used for initializing the initial node of the RRT search tree, expanding the RRT search tree during searching, sampling to a target point within a joint space range by a probability alpha, randomly sampling within the whole six-dimensional space by 1-alpha to obtain a random point sample,
the expansion unit is used for finding a tree node closest to the random point sample in the existing RRT tree nodes, and expanding a step length segment from the tree node closest node to the random point sample to obtain a new _ point _ temp of the current extended joint point;
the iteration unit is used for judging whether the current expansion joint point new _ point _ temp collides or not, and if so, the sampling unit is repeatedly executed to the iteration unit; if collision does not occur, recording the new _ point _ temp in the RRT search tree until the new _ point _ temp is equal to the target point qd, and exiting iteration.
Furthermore, the process of acquiring the current extension node new _ point _ temp in the extension unit is as follows:
finding out the closest tree node to sample in the existing RRT tree nodes, starting from the closest tree node to random point sample, and using formula
And expanding one step towards the random point to obtain the new _ point _ temp of the current extended joint point.
Furthermore, if an obstacle with a size smaller than the step length is encountered during the expansion of the RRT search tree in the expansion unit, the adaptive constraint variable step size includes:
establishing a barrier repulsive force field through a formula R = rb + R and adding the barrier repulsive force field to the mechanical arm model, wherein rb represents the radius of the original mechanical arm connecting rod model, R represents the radius of the repulsive force field, and R represents the radius of the updated mechanical arm connecting rod model;
giving a maximum step value Pmax allowed in a working space;
by the formula
Calculating a Jacobian matrix of a tool coordinate system of the mechanical arm under the current node qnode, wherein q is 1 To q 6 Respectively represents each joint variable in the six-dimensional joint space, and>
and &>
Respectively represents the partial derivative of the three components of the tail end position of the mechanical arm to the ith joint variable and is combined with the partial derivative of the mechanical arm>
And &>
Respectively representing the partial derivatives of the three components of the tail end posture of the mechanical arm to the ith joint variable;
by the formula
Calculating Jacobian matrix J of tool coordinate system tool Norm operator A of, wherein (J) tool ) ij Represents the Jacobian matrix J tool Row i, column j;
using the formula P max = max (a | Δ θ |) computing the configuration the step size in space is # a theta #,
when detecting that a certain connecting rod of the mechanical arm approaches or enters the obstacle repulsive force field, updating the step length segment in the RRT search tree expansion process through the formula segment length = | delta theta |.
Further, when the current node qnode is in a deep frame obstacle, the strategy of taking a random point according to the probability α is changed and adjusted to be an autonomous escape strategy, where the autonomous escape strategy includes:
by the formula
Make the current node q node Gradually adjusting the expansion direction along with the change of i and generating a new tree node q node ' the escape strategy is stopped until a new tree node escapes from the deep-box obstacle, where i is the given number of escapes and n is the given angle to be changed for each escape.
Further, the optimization module is further configured to:
numbering all nodes in a path formed from a starting point qs to a target point qd in sequence, making the index of the starting point qs be 1, making the index of the target point qd be n, directly connecting the starting point qs to the target point qd, if a connecting line of two nodes collides, connecting the node with the index of 2 with the target point qd, if the collision continues, advancing the node with the index of 3 to connect with the target point qd and detecting whether the collision occurs, and so on, if the node with the index of i does not collide with the target point qd, making qd = qi at this time, updating the target point qd, repeating the step until the starting point qs can be connected with the current updated target point qd and the collision does not occur, ending the loop iteration, and returning the optimized smooth path point sequence.
The invention has the advantages that: when a deep frame obstacle exists in the environment and the starting point or the end point is in the deep frame obstacle, the strategy of randomly taking points by probability is changed and adjusted to be an autonomous escape strategy, so that the current node escapes from the deep frame obstacle, the phenomenon that the RRT algorithm is trapped into a dead loop in the deep frame and cannot escape is avoided, when the current node encounters an obstacle with the size smaller than the planned step length, such as a thin shell type obstacle, the step length is adaptively restricted and changed, a collision-free path is generated, the expanded path point sequence is optimized, and a more optimal path of the deep frame obstacle avoidance movement of the mechanical arm can be solved, so that the method is suitable for path planning of the deep frame obstacle avoidance movement of the mechanical arm.
Drawings
Fig. 1 is an algorithm flowchart of a method for planning a deep-frame obstacle avoidance motion of a mechanical arm based on RRT according to embodiment 1 of the present invention;
fig. 2 is a schematic diagram of an execution process of an RRT algorithm in a C space and a working space in the method for planning the deep-frame obstacle avoidance motion of the mechanical arm based on the RRT disclosed in embodiment 1 of the present invention;
fig. 3 is a schematic diagram of path connection when an RRT tree node to be expanded is in a deep-frame obstacle in the method for planning a robot arm deep-frame obstacle avoidance motion based on an RRT according to embodiment 1 of the present invention;
fig. 4 is a schematic diagram illustrating a comparison between an original path and an optimized path in a method for planning a deep-frame obstacle avoidance motion of a mechanical arm based on an RRT according to embodiment 1 of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the embodiments of the present invention, and it is obvious that the described embodiments are some embodiments of the present invention, but not all embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Example 1
As shown in fig. 1, a method for planning a deep frame obstacle avoidance motion of a mechanical arm based on RRT includes:
step1: initializing a starting point, a target point and a search step length of the mechanical arm, identifying the position of an obstacle in the environment and constructing a corresponding bounding box; the method comprises the following specific steps: the method comprises the steps of searching paths in a configuration space (C space), giving two groups of joint angles which cannot collide with a self-body and an environmental barrier in a teaching mode, respectively obtaining the initial pose and the target pose of obstacle avoidance pickup through forward kinematics calculation of a robot, obtaining the initial point qs and the target point qd of a mechanical arm through inverse kinematics solution preferential calculation, searching the position of the barrier through a sensor, constructing a corresponding bounding box, and initializing the search step length. In this embodiment, the configuration space has six dimensions, the starting position is ps = [461.1361, -23.1651, -88.5467,3.049,0.3795,1.5133], the end position is pd = [ -34.8133,460.4032, -88.5467, -2.7534,0.0387, -0.2085], the starting point is qs = [0,0.0628,0.3770, -0.1257, -0.7540,0] is obtained by inverse kinematics solution optimization calculation, the target point is qd = [1.6965,0.0628,0.3770, -0.1257, -0.7540,0], the initial position is in the first box barrier, and the target position is in the second box barrier; and initializing a search step length =0.05.
Step2: the RRT search tree is expanded in a probability random point-taking mode, mechanical arm obstacle-avoiding planning is carried out, in the expanding process, if an obstacle with the size smaller than the planning step length is encountered, self-adaptive constraint variable step length is carried out, if the current node is in a deep frame obstacle, the strategy of probability random point-taking is changed, and the strategy is adjusted to be an autonomous escape strategy, so that the current node escapes from the deep frame obstacle; it should be noted that the initialization process of Step1 and the process of expanding the RRT search tree in Step2 by means of probability random point selection are not main improvement points of the present invention, and both belong to the prior art, and refer to chinese patent application No. 202011132708.4 specifically, a mechanical arm obstacle avoidance method and apparatus based on three-dimensional task space constraint is disclosed, and are not described herein again. The specific process of Step2 is as follows:
step 21: initializing the initial node of the RRT search tree, expanding the RRT search tree during searching, sampling a target point within a joint space range by a probability alpha, randomly sampling in the whole six-dimensional space by 1-alpha to obtain a random point sample,
step 22: finding a tree node closest to a random point sample in the existing RRT tree nodes, extending a step length segment from the tree node closest node to the random point sample to obtain a current extended joint point new _ point _ temp, wherein the extension formula is as formula (1);
step 23: judging whether the new _ point _ temp of the current expansion joint point is collided, and if so, repeating the steps 21 to 23; and if the collision does not occur, recording the new _ point _ temp into the RRT search tree until the new _ point _ temp is equal to the target point qd, and exiting the iteration.
Generally, the fixed-step RRT grows in the configuration space in a fixed-step segmentLength, and the collision detection algorithm can generally only be performed at nodes in the workspace, as shown in fig. 2. The RRT search tree expands outwards in the C space by a fixed step length, but the step length in the working space obtained after the RRT search tree is subjected to positive kinematic calculation is not fixed, and the method is intuitively represented that no collision occurs between the closed nodes and the new-point in the configuration space, so that the algorithm cannot determine the collision and directly records the new-point in the RRT tree. But actually the fixed step length in the configuration space is mapped to the step length P after the working space closestNode P new_point And the size of the obstacle is larger than that of the obstacle, so that the robot can collide in the process of actually moving from the closed node point to the new _ point. Therefore, if an obstacle with a size smaller than the step length is encountered during the process of expanding the RRT search tree in step 22, the adaptive constraint variable step size includes:
establishing a barrier repulsive force field through a formula R = rb + R and adding the barrier repulsive force field to the mechanical arm model, wherein rb represents the radius of the original mechanical arm connecting rod model, R represents the radius of the repulsive force field, and R represents the radius of the updated mechanical arm connecting rod model;
giving a maximum step value Pmax allowed in a working space;
by the formula
Calculating a Jacobian matrix of a tool coordinate system of the mechanical arm under the current node qnode, wherein q is 1 To q 6 Respectively represents each joint variable in the six-dimensional joint space, and>
and &>
Respectively represents the partial derivatives of the three components of the tail end position of the mechanical arm to the ith joint variable, and is/is selected>
And &>
Respectively representing the partial derivatives of the three components of the tail end attitude of the mechanical arm to the ith joint variable;
by the formula
Calculating Jacobian matrix J of tool coordinate system tool Norm operator A of (a), wherein (J) tool ) ij Represents the Jacobian matrix J tool Row i, column j;
using the formula P max = max (A · | Δ θ |) a step size in space | a θ |,
when detecting that a connecting rod of the mechanical arm approaches or enters a repulsive field of the obstacle, updating the step length segment in the RRT search tree expansion process through the formula segment length = | delta theta |.
In addition, assuming that the RRT tree node qnode to be expanded is still in a deep-frame obstacle such as a box, as shown in fig. 3, the connection with the end point qd is always in collision. Therefore, in step 21, when the current node qnode is in a deep-frame obstacle, the strategy of taking a random point according to the probability α is changed and adjusted to an autonomous escape strategy, where the autonomous escape strategy includes:
by the formula
So that the current node q node Gradually adjusting the expansion direction along with the change of i and generating a new tree node q node ', the escape strategy is stopped until a new tree node escapes from the deep-box obstacle, where i is the given number of escapes and n is the given angle to be changed for each escape.
Step3: optimizing the path point sequence obtained by Step2 expansion; the method specifically comprises the following steps: all nodes in a path formed from a starting point qs to a target point qd are sequentially numbered, the index of the starting point qs is 1, the index of the target point qd is n, the starting point qs is directly connected to the target point qd, if the connecting line of the two nodes collides, the node with the index of 2 is connected with the target point qd, if the collision continues, the node with the index of 3 is connected with the target point qd and whether the collision occurs is detected, and so on, if the node with the index of i does not collide with the target point qd, the node with the index of i is enabled to = qi at this time, the target point qd is updated, the steps are repeated until the starting point qs can be connected with the current updated target point qd and the collision does not occur, the circular iteration is finished, and the optimized smooth path point sequence is returned.
Step4: according to the optimized path point sequence, the mobile mechanical arm moves from the starting point to the target point without collision, and particularly, refer to fig. 4. Simulation shows that under the environment of deep-frame obstacles such as boxes, the method provided by the invention can quickly and effectively realize collision-free path planning of the six-degree-of-freedom mechanical arm, and can obtain a more excellent and smooth motion planning path after an optimization algorithm is used for the path.
Through the technical scheme, when a deep-frame obstacle exists in the environment and the starting point or the end point is in the deep-frame obstacle, the strategy of randomly taking points of the probability is changed and adjusted to be an autonomous escape strategy, so that the current node escapes from the deep-frame obstacle, the phenomenon that the RRT algorithm is trapped into a dead loop in the deep frame and cannot escape is avoided, when the obstacle with the size smaller than the planned step size, such as thin-shell obstacles, is encountered, the step size is adaptively constrained and changed, a collision-free path is ensured to be generated, the sequence of the path points obtained through expansion is optimized, and a better path of the deep-frame obstacle avoidance motion of the mechanical arm can be solved, so that the method is suitable for path planning of the deep-frame obstacle avoidance motion of the mechanical arm.
Example 2
Based on embodiment 1 of the present invention, embodiment 2 of the present invention further provides a device for planning a deep-frame obstacle avoidance motion of a mechanical arm based on an RRT, where the device includes:
the initialization module is used for initializing a starting point, a target point and a searching step length of the mechanical arm, identifying the position of an obstacle in an environment and constructing a corresponding bounding box;
the path planning module is used for expanding the RRT search tree in a probability random point taking mode and planning mechanical arm obstacle avoidance, wherein in the expanding process, if an obstacle with the size smaller than the planning step length is encountered, the step length is adaptively restrained and changed, and if the current node is in the deep frame obstacle, the strategy of probability random point taking is changed and adjusted to an autonomous escape strategy, so that the current node escapes the deep frame obstacle;
the optimization module is used for optimizing the path point sequence obtained by the path planning module expansion;
and the motion execution module is used for moving the mechanical arm from the starting point to the target point in a collision-free manner according to the optimized path point sequence.
Specifically, the initialization module is further configured to: the method comprises the steps of respectively obtaining an initial pose and a target pose of obstacle avoidance picking in a configuration space through positive kinematics calculation of a robot, obtaining an initial point qs and a target point qd of a mechanical arm through inverse kinematics preferential calculation, searching the position of an obstacle through a sensor, constructing a corresponding bounding box, and initializing a search step length.
Specifically, the path planning module further includes:
the sampling unit is used for initializing the initial node of the RRT search tree, expanding the RRT search tree during searching, sampling to a target point within a joint space range by a probability alpha, randomly sampling within the whole six-dimensional space by 1-alpha to obtain a random point sample,
the extension unit is used for finding a tree node closest to the random point sample in the existing RRT tree nodes, and extending a step length segment from the tree node closest node to the random point sample to obtain a current extension joint point new _ point _ temp;
the iteration unit is used for judging whether the current expansion joint point new _ point _ temp collides or not, and if so, the sampling unit is repeatedly executed to the iteration unit; if collision does not occur, recording the new _ point _ temp in the RRT search tree until the new _ point _ temp is equal to the target point qd, and exiting iteration.
More specifically, the process of acquiring the current extension node new _ point _ temp in the extension unit is as follows:
finding out a tree node closest to sample in the existing RRT tree nodes, starting from the tree node closest to the sample of the random point, and carrying out formula processing
And expanding one step towards the random point to obtain the new _ point _ temp of the current extended joint point.
More specifically, if an obstacle with a size smaller than the segmentLength of the step size is encountered during the expansion of the RRT search tree in the expansion unit, the adaptive constraint variable step size includes:
establishing a barrier repulsive force field through a formula R = rb + R and adding the barrier repulsive force field to the mechanical arm model, wherein rb represents the radius of the original mechanical arm connecting rod model, R represents the radius of the repulsive force field, and R represents the radius of the updated mechanical arm connecting rod model;
giving a maximum step value Pmax allowed in a working space;
by the formula
Calculating a Jacobian matrix of a tool coordinate system of the mechanical arm under the current node qnode, wherein q is 1 To q 6 Respectively represents each joint variable in the six-dimensional joint space, and>
and &>
Respectively represents the partial derivatives of the three components of the tail end position of the mechanical arm to the ith joint variable, and is/is selected>
And &>
Respectively representing the partial derivatives of the three components of the tail end attitude of the mechanical arm to the ith joint variable;
by the formula
Calculating Jacobian matrix J of tool coordinate system tool Norm operator A of, wherein (J) tool ) ij Represents the Jacobian matrix J tool Row i, column j;
using the formula P max = max (A · | Δ θ |) a step size in space | a θ |,
when detecting that a certain connecting rod of the mechanical arm approaches or enters the obstacle repulsive force field, updating the step length segment in the RRT search tree expansion process through the formula segment length = | delta theta |.
More specifically, when the current node qnode is in a deep frame obstacle, the strategy of taking a random point according to the probability α is changed and adjusted to be an autonomous escape strategy in the sampling unit, where the autonomous escape strategy includes:
by the formula
Make the current node q node Gradually adjusting the expansion direction along with the change of i and generating a new tree node q node ' stopping the escape strategy until a new tree node escapes from the deep-box obstacle, where i is a given number of escapes and n is a given change needed for each escapeThe angle of (c).
Specifically, the optimization module is further configured to:
all nodes in a path formed from a starting point qs to a target point qd are sequentially numbered, the index of the starting point qs is 1, the index of the target point qd is n, the starting point qs is directly connected to the target point qd, if the connecting line of the two nodes collides, the node with the index of 2 is connected with the target point qd, if the collision continues, the node with the index of 3 is connected with the target point qd and whether the collision occurs is detected, and so on, if the node with the index of i does not collide with the target point qd, the node with the index of i is enabled to = qi at this time, the target point qd is updated, the steps are repeated until the starting point qs can be connected with the current updated target point qd and the collision does not occur, the circular iteration is finished, and the optimized smooth path point sequence is returned.
The above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, it should be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.
Claims (10)
1. A mechanical arm deep frame obstacle avoidance motion planning method based on RRT is characterized by comprising the following steps:
the method comprises the following steps: initializing a starting point, a target point and a search step length of the mechanical arm, identifying the position of an obstacle in the environment and constructing a corresponding bounding box;
step two: the RRT search tree is expanded in a probability random point-taking mode, mechanical arm obstacle-avoiding planning is carried out, in the expanding process, if an obstacle with the size smaller than the planning step length is encountered, self-adaptive constraint variable step length is carried out, if the current node is in a deep frame obstacle, the strategy of probability random point-taking is changed, and the strategy is adjusted to be an autonomous escape strategy, so that the current node escapes from the deep frame obstacle;
step three: optimizing the path point sequence obtained by the expansion in the second step;
step four: and according to the optimized path point sequence, the mobile mechanical arm moves from the starting point to the target point without collision.
2. The RRT-based mechanical arm deep frame obstacle avoidance motion planning method according to claim 1, wherein the first step comprises: in a configuration space, the initial pose and the target pose of obstacle avoidance pickup are respectively obtained through forward kinematics calculation of a robot, the initial point qs and the target point qd of the mechanical arm are obtained through inverse kinematics preferential calculation, the position of an obstacle is searched through a sensor, a corresponding bounding box is constructed, and the search step length is initialized.
3. The RRT-based mechanical arm deep frame obstacle avoidance motion planning method according to claim 1, wherein the second step comprises:
step 21: initializing the initial node of the RRT search tree, expanding the RRT search tree during searching, sampling to a target point within a joint space range by a probability alpha, randomly sampling within the whole six-dimensional space by 1-alpha to obtain a random point sample,
step 22: finding a tree node closest to a random point sample in the existing RRT tree nodes, and extending a step length segment from the tree node closest node to the random point sample to obtain a current extended joint point new _ point _ temp;
step 23: judging whether the new _ point _ temp of the current expansion joint point is collided, and if so, repeating the steps 21 to 23; if collision does not occur, recording the new _ point _ temp in the RRT search tree until the new _ point _ temp is equal to the target point qd, and exiting iteration.
4. The RRT-based mechanical arm deep frame obstacle avoidance motion planning method according to claim 3, wherein the acquisition process of the current extended joint point new _ point _ temp in the step 22 is as follows:
finding out the closest tree node to sample from the existing RRT tree nodesStarting from a tree node closest to the random point sample, and performing formula
And expanding one step towards the random point to obtain the new _ point _ temp of the current extended joint point.
5. The RRT-based mechanical arm deep frame obstacle avoidance motion planning method according to claim 3, wherein in the step 22, if an obstacle with a size smaller than a step length is encountered in the process of expanding the RRT search tree, the step size is adaptively constrained and changed, and the step of adaptively constraining and changing the step size is as follows:
establishing a barrier repulsive force field through a formula R = rb + R and adding the barrier repulsive force field to the mechanical arm model, wherein rb represents the radius of the original mechanical arm connecting rod model, R represents the radius of the repulsive force field, and R represents the radius of the updated mechanical arm connecting rod model;
giving a maximum step value Pmax allowed in a working space;
by the formula
Calculating a Jacobian matrix of a tool coordinate system of the mechanical arm under the current node qnode, wherein q is 1 To q 6 Respectively represents each joint variable in six-dimensional joint space, and>
and &>
Respectively represents the partial derivatives of the three components of the tail end position of the mechanical arm to the ith joint variable, and is/is selected>
And &>
Respectively representing machinesPartial derivatives of the posture three components of the tail end of the arm to the ith joint variable;
by the formula
Calculating Jacobian matrix J of tool coordinate system tool Norm operator A of, wherein (J) tool ) ij Express Jacobian matrix J tool Row i, column j;
using the formula P max = max (A · | Δ θ |) a step size in space | a θ |,
when detecting that a connecting rod of the mechanical arm approaches or enters a repulsive field of the obstacle, updating the step length segment in the RRT search tree expansion process through the formula segment length = | delta theta |.
6. The RRT-based mechanical arm deep frame obstacle avoidance motion planning method according to claim 3, wherein in the step 21, when the current node qnode is in a deep frame obstacle, a strategy of taking a random point according to a probability alpha is changed and adjusted to be an autonomous escape strategy, and the autonomous escape strategy comprises the following steps:
by the formula
Make the current node q node Gradually adjusting the expansion direction along with the change of i and generating a new tree node q node ', the escape strategy is stopped until a new tree node escapes from the deep-box obstacle, where i is the given number of escapes and n is the given angle to be changed for each escape.
7. The RRT-based mechanical arm deep frame obstacle avoidance motion planning method according to claim 1, wherein the third step comprises:
numbering all nodes in a path formed from a starting point qs to a target point qd in sequence, making the index of the starting point qs be 1, making the index of the target point qd be n, directly connecting the starting point qs to the target point qd, if a connecting line of two nodes collides, connecting the node with the index of 2 with the target point qd, if the collision continues, advancing the node with the index of 3 to connect with the target point qd and detecting whether the collision occurs, and so on, if the node with the index of i does not collide with the target point qd, making qd = qi at this time, updating the target point qd, repeating the step until the starting point qs can be connected with the current updated target point qd and the collision does not occur, ending the loop iteration, and returning the optimized smooth path point sequence.
8. The utility model provides a dark frame of arm keeps away barrier motion planning device based on RRT which characterized in that, the device includes:
the initialization module is used for initializing a starting point, a target point and a search step length of the mechanical arm, identifying the position of an obstacle in the environment and constructing a corresponding bounding box;
the path planning module is used for expanding the RRT search tree in a probability random point taking mode and planning mechanical arm obstacle avoidance, wherein in the expanding process, if an obstacle with the size smaller than the planning step length is encountered, the step length is adaptively restricted and changed, and if the current node is in the deep-frame obstacle, the strategy of the probability random point taking is changed and adjusted to be an autonomous escape strategy, so that the current node escapes the deep-frame obstacle;
the optimization module is used for optimizing the path point sequence obtained by the path planning module expansion;
and the motion execution module is used for moving the mechanical arm from the starting point to the target point in a collision-free manner according to the optimized path point sequence.
9. The RRT-based robot arm deep frame obstacle avoidance motion planning device of claim 8, wherein the initialization module is further configured to: the method comprises the steps of respectively obtaining an initial pose and a target pose of obstacle avoidance picking in a configuration space through positive kinematics calculation of a robot, obtaining an initial point qs and a target point qd of a mechanical arm through inverse kinematics preferential calculation, searching the position of an obstacle through a sensor, constructing a corresponding bounding box, and initializing a search step length.
10. The RRT-based robot arm deep frame obstacle avoidance motion planning device of claim 8, wherein the path planning module further comprises:
a sampling unit for initializing the initial node of the RRT search tree, expanding the RRT search tree during searching, sampling to a target point within a joint space range by a probability alpha, randomly sampling in the whole six-dimensional space by 1-alpha to obtain a random point sample,
the expansion unit is used for finding a tree node closest to the random point sample in the existing RRT tree nodes, and expanding a step length segment from the tree node closest node to the random point sample to obtain a new _ point _ temp of the current extended joint point;
the iteration unit is used for judging whether the current expansion joint point new _ point _ temp generates collision or not, and if the collision occurs, the sampling unit is repeatedly executed to the iteration unit; and if the collision does not occur, recording the new _ point _ temp into the RRT search tree until the new _ point _ temp is equal to the target point qd, and exiting the iteration.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111600481.6A CN115958590A (en) | 2021-12-24 | 2021-12-24 | RRT-based mechanical arm deep frame obstacle avoidance motion planning method and device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111600481.6A CN115958590A (en) | 2021-12-24 | 2021-12-24 | RRT-based mechanical arm deep frame obstacle avoidance motion planning method and device |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115958590A true CN115958590A (en) | 2023-04-14 |
Family
ID=87353538
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111600481.6A Pending CN115958590A (en) | 2021-12-24 | 2021-12-24 | RRT-based mechanical arm deep frame obstacle avoidance motion planning method and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115958590A (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116439018A (en) * | 2023-05-05 | 2023-07-18 | 仲恺农业工程学院 | Seven-degree-of-freedom fruit picking robot and picking method thereof |
| CN117148848A (en) * | 2023-10-27 | 2023-12-01 | 上海伯镭智能科技有限公司 | Intelligent obstacle avoidance method and system for unmanned vehicle |
| CN117260735A (en) * | 2023-11-01 | 2023-12-22 | 广东工业大学 | Path planning method for robot deep frame grabbing |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9981383B1 (en) * | 2016-08-02 | 2018-05-29 | X Development Llc | Real-time trajectory generation for actuators of a robot to reduce chance of collision with obstacle(s) |
| CN110497403A (en) * | 2019-08-05 | 2019-11-26 | 上海大学 | A kind of manipulator motion planning method improving two-way RRT algorithm |
| CN111844007A (en) * | 2020-06-02 | 2020-10-30 | 江苏理工学院 | Pollination robot mechanical arm obstacle avoidance path planning method and device |
| CN112223291A (en) * | 2020-10-21 | 2021-01-15 | 哈工大机器人（合肥）国际创新研究院 | Mechanical arm obstacle avoidance method and device based on three-dimensional task space constraint |
| US20210252707A1 (en) * | 2020-02-19 | 2021-08-19 | Fanuc Corporation | Collision avoidance motion planning method for industrial robot |
| CN113664829A (en) * | 2021-08-17 | 2021-11-19 | 西北工业大学 | Space manipulator obstacle avoidance path planning system and method, computer equipment and storage medium |
-
2021
- 2021-12-24 CN CN202111600481.6A patent/CN115958590A/en active Pending
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9981383B1 (en) * | 2016-08-02 | 2018-05-29 | X Development Llc | Real-time trajectory generation for actuators of a robot to reduce chance of collision with obstacle(s) |
| CN110497403A (en) * | 2019-08-05 | 2019-11-26 | 上海大学 | A kind of manipulator motion planning method improving two-way RRT algorithm |
| US20210252707A1 (en) * | 2020-02-19 | 2021-08-19 | Fanuc Corporation | Collision avoidance motion planning method for industrial robot |
| CN111844007A (en) * | 2020-06-02 | 2020-10-30 | 江苏理工学院 | Pollination robot mechanical arm obstacle avoidance path planning method and device |
| CN112223291A (en) * | 2020-10-21 | 2021-01-15 | 哈工大机器人（合肥）国际创新研究院 | Mechanical arm obstacle avoidance method and device based on three-dimensional task space constraint |
| CN113664829A (en) * | 2021-08-17 | 2021-11-19 | 西北工业大学 | Space manipulator obstacle avoidance path planning system and method, computer equipment and storage medium |
Non-Patent Citations (1)
| Title |
|---|
| "《机械工程学报》2015年度1~24期总目次", 机械工程学报, no. 24, 20 December 2015 (2015-12-20) * |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116439018A (en) * | 2023-05-05 | 2023-07-18 | 仲恺农业工程学院 | Seven-degree-of-freedom fruit picking robot and picking method thereof |
| CN116439018B (en) * | 2023-05-05 | 2024-01-02 | 仲恺农业工程学院 | Seven-degree-of-freedom fruit picking robot and picking method thereof |
| CN117148848A (en) * | 2023-10-27 | 2023-12-01 | 上海伯镭智能科技有限公司 | Intelligent obstacle avoidance method and system for unmanned vehicle |
| CN117148848B (en) * | 2023-10-27 | 2024-01-26 | 上海伯镭智能科技有限公司 | Intelligent obstacle avoidance method and system for unmanned vehicle |
| CN117260735A (en) * | 2023-11-01 | 2023-12-22 | 广东工业大学 | Path planning method for robot deep frame grabbing |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115958590A (en) | RRT-based mechanical arm deep frame obstacle avoidance motion planning method and device | |
| CN112223291B (en) | Mechanical arm obstacle avoidance method and device based on three-dimensional task space constraint | |
| CN112677153B (en) | Improved RRT algorithm and industrial robot path obstacle avoidance planning method | |
| CN110228069B (en) | Online obstacle avoidance motion planning method for mechanical arm | |
| CN110509279B (en) | Motion path planning method and system of humanoid mechanical arm | |
| US8825209B2 (en) | Method and apparatus to plan motion path of robot | |
| US9411335B2 (en) | Method and apparatus to plan motion path of robot | |
| CN106569496B (en) | Planning method of motion path | |
| CN109960880A (en) | A kind of industrial robot obstacle-avoiding route planning method based on machine learning | |
| WO2023024317A1 (en) | Robot obstacle avoidance method and apparatus, and robot | |
| CN112809665B (en) | Mechanical arm motion planning method based on improved RRT algorithm | |
| CN110986953A (en) | Path planning method, robot and computer readable storage medium | |
| CN113359718A (en) | Method and equipment for fusing global path planning and local path planning of mobile robot | |
| CN111251306A (en) | Mechanical arm path planning method with chassis error | |
| Liu et al. | Improved RRT path planning algorithm for humanoid robotic arm | |
| CN115723129B (en) | Continuous operation motion planning method for mechanical arm | |
| CN116117822A (en) | RRT mechanical arm track planning method based on non-obstacle space probability potential field sampling | |
| CN109807933B (en) | Capability map point cloud updating method, device, equipment and storage medium | |
| Liang et al. | PR-RRT*: Motion Planning of 6-DOF Robotic Arm Based on Improved RRT Algorithm | |
| Wang et al. | Path planning of manipulator based on improved RRT algorithm | |
| CN115946117B (en) | Three-dimensional space path planning method | |
| Liu et al. | A planning method for safe interaction between human arms and robot manipulators | |
| CN116852367A (en) | Six-axis mechanical arm obstacle avoidance path planning method based on improved RRTstar | |
| CN115533912A (en) | Mechanical arm motion planning method based on rapid expansion random tree improvement | |
| Li et al. | Efficient Loop Closure Detection Method for Lidar SLAM in Challenging Environment |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |