CN113459086B - Super-redundancy mechanical arm path planning method - Google Patents
Super-redundancy mechanical arm path planning method Download PDFInfo
- Publication number
- CN113459086B CN113459086B CN202110593223.3A CN202110593223A CN113459086B CN 113459086 B CN113459086 B CN 113459086B CN 202110593223 A CN202110593223 A CN 202110593223A CN 113459086 B CN113459086 B CN 113459086B
- Authority
- CN
- China
- Prior art keywords
- path
- new
- arm
- max
- 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.)
- 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
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1643—Programme controls characterised by the control loop redundant control
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
The invention relates to a super-redundant mechanical arm path planning method, which comprises the following steps of 1: establishing a kinematic model for the super-redundant mechanical arm, and determining a conversion relation among the rope length, the deflection angle and the coordinates; step 2: establishing an obstacle model, and determining a non-reachable area corresponding to an obstacle; and step 3: theoretically analyzing the maximum included angle psi between two adjacent straight-line paths through sine theorem and two extreme points on the basis of the step 1 max Maximum deflection angle psi of the arm rod during the movement of the ith arm rod end point along one of the straight paths max Arm length L i Path step length l w The relationship between; and 4, step 4: and (3) according to the unreachable area determined in the step (2) and the constraint relation in the step (3), providing an improved RRT algorithm for path planning. The feasibility and the optimization of the method are superior to those of the traditional algorithm.
Description
Technical Field
The invention relates to the technical field of mechanical arm path planning, in particular to a super-redundancy mechanical arm path planning method based on an improved RRT algorithm.
Background
Path planning techniques are widely used in many fields. However, with the development of science and technology and the improvement of demand, the traditional path planning algorithm is difficult to satisfy the planning conditions in a complex environment. The fast-expanding Random Tree (RRT) algorithm based on node sampling has the advantages of being capable of being used for path planning of a complete system, adding constraint conditions for autonomous path finding of an incomplete system and the like, and opens up a solution for path planning under a high-dimensional complex environment.
The RRT algorithm makes great progress in the field of path planning, but the algorithm still has the defects of large calculated amount, unstable path, low node utilization rate, difficulty in obtaining a path meeting the conditions and the like. And few algorithms analyze how to plan a path so that the mechanical arm passes through a narrow space under the condition of the joint slip angle limit. Currently, paths meeting joint angle limits can be planned by means of some software, programs, tool boxes (e.g., RVC, ROS), and the like, but in these plans, it is necessary to check whether the constraints are met in real time during the planning process. Therefore, a method is needed to be provided, which can directly plan a path that meets the joint angle limitation, and can ensure that the robot arm can not only pass through a narrow space to reach a target point when moving along the planned path, but also ensure that the joint deflection angle of the robot arm is always within the limited range.
Disclosure of Invention
The technical problem solved by the invention is as follows: a path planning method for a super-redundant mechanical arm is provided. The method can be combined with path planning algorithms such as a Probability Route Map (PRM) algorithm, an A-x algorithm, an artificial potential energy (APF) algorithm and the like, and can directly plan a path which meets the joint angle limitation, thereby greatly improving the feasibility and the optimization.
The technical scheme of the invention is as follows: a super-redundant mechanical arm path planning method comprises the following steps:
step 1: establishing a mechanical arm kinematics model aiming at the super-redundant mechanical arm, and determining a conversion relation among the length of a driving rope, the deflection angle of an arm lever and the coordinates of the tail end point of the arm lever;
step 2: establishing an obstacle model aiming at an obstacle in the environment, and obtaining the position, shape and size information of the obstacle from the obstacle model so as to determine a non-reachable area corresponding to the obstacle;
and step 3: assume that adjacent arms are the same length and equal to the path step length l w In a linear relation, theoretically analyzing the maximum included angle psi between two adjacent straight-line paths according to the mechanical arm kinematics model determined in the step 1 max The maximum value psi max of the deflection angle change of the arm lever and the length L of the arm lever during the movement of the ith arm lever initial point along one section of linear path i Path step length l w The relationship between;
and 4, step 4: and planning a path according to the unreachable area corresponding to the obstacle determined in the step 2 and the relation constraint in the step 3.
Preferably, in step 2, the shortest distance between the path and the obstacle is set to be the farthest distance between all points on the surface of the arm and the axis of the arm, and the farthest distance between a point on the axis of the arm and the straight path, and a range where the distance from the obstacle is equal to or less than the shortest distance is regarded as the unreachable area.
Preferably, the MDA + RRT algorithm is adopted to perform path planning in step 4, which is specifically implemented by the following method:
step 41: defining a step δ i.e. l w Taking the initial point xinit of the path planning as a root node of the random tree T;
step 42: randomly selecting a node x rand Selecting a distance x rand Nearest node x near According to node x near And target point x goal A distance d between goal Determining Ψ max Taking the value of (A); then according to the relation and x in step 3 rand 、x near T analysis x new To calculate x new The position of (a);
step 43: if x new If the condition requirement is met, i.e. the unreachable area determined in the step 2 is not entered, x is added new Nearby nodes deposit to x neighbor And storing the ancestors of the nearby nodes to x parent In this way, a group x is obtained new Arranging a Xin set;
step 44: find x in Xin new New father node x with minimum cost and meeting deflection angle requirement par Delete x new With the original parent node x near Connecting line of (2), connecting x new And x par And pruning is carried out in T;
step 45: if x neighbor One node x in from Is greater than the current x new The sum of the weight of (2) and the distance to the node, and if the deflection angle requirement is met, then the current x is used new Node as x from A parent node of (a);
step 46: repeatedly searching for x new And pruning x new And (4) connection relation with nearby nodes until reaching the target point.
Preferably, the step 42 is implemented by:
s1, if x near Is a starting point x init Then x is randomly selected between-60 DEG and 60 DEG new Is performed, step S2, otherwise, at-min (Ψ) max ) To min (Ψ) max ) Randomly select x between new Step S2 is performed; min (Ψ) max ) Is node x near And target point x goal Psi determined at different distances between them max Minimum value of (d);
s2, judging whether the following inequality Abs (ang) is satisfied rand -ang near )≤Ψ max If yes, then x is maintained new Direction; if not, let Ψ i Is a boundary value in the selectable range;
wherein x is rand And x near The included angle of a line segment formed by connecting two points relative to the horizontal line is ang rand Reading x in T near The line segment connected with the father node of the node near 。
Preferably, Ψ is determined in step 42 by max The value of (A) is as follows:
d goal <l, then let Ψ max The value is the maximum included angle between two adjacent straight paths calculated according to the penultimate arm lever and the relation in the step 3;
d goal if greater than or equal to L, let Ψ max The value is the maximum included angle between two adjacent linear paths calculated according to the combination of the longest arm lever and the relation in the step 3;
l is the sum of the lengths of the last two arms.
Preferably, Ψ i For boundary values in the selectable range, psi in the two-dimensional case i ＝ang near +Sign(ang rand -ang near )·Ψ max ，x new Is a point on the boundary; in the three-dimensional situation, the included angle between two adjacent straight-line paths is ensured to be psi max, and the selected space of xnew is a space with the radius delta.sin (psi) max ) Circle of (a), x new Is a point on the circle.
Preferably, the relationship in step 3 is determined by:
let l w ＝k·L i ＝k·L i-1 ，k∈(0,1]The starting point of the ith-1 th arm rod is moved along the w-th straight line path, and the deflection angle psi of the ith arm rod is generated in the moving process i The maximum value of the change appears at two positions; the first point is the terminal point of the ith-1 arm rod, namely the initial point of the ith arm rod, and moves to the initial point or the terminal point of a straight line path, wherein psi i Maximum value of variation is denoted psi i_end (ii) a Second position is when the initial point of the ith arm rod moves to the middle point of a straight line path i Maximum value of variation is denoted psi i_mid ；
After the two maximum values are obtained, the maximum value max (psi) of the arm deflection angle change is obtained i_end ,ψ i_mid )。
Preferably, when k is 1, the relationship in step 3 is determined as follows:
simplifying two adjacent arm rods into a straight line along the axis, forming two triangles by the two adjacent arm rods and three continuous straight lines on the path, and determining the relationship according to the sine theorem and the angle transformation of the triangles as follows:
9. the method for planning the path of the super-redundant mechanical arm according to claim 7, wherein:
N end When the ith arm rod end point moves to the end point of a section of straight line path, a plurality of path turning points are arranged between the two end points of the arm rod;
N mid when the ith arm rod end point moves to the middle point of a straight path, a plurality of path turning points are arranged between the two end points of the arm rod.
Preferably, in step 44, a new parent node x is found par In addition to satisfying the conditions of cost reduction and no collision, the method also needs to satisfy the path from the parent node of xin to xin and the path from xin to x new Is not more than psi max (ii) a Xin is the node in the Xin set.
Preferably, in the step 45, if the current x is used, the current x is used new Node as x from The parent node of (2) not only needs to satisfy the conditions of cost reduction and no collision, but also needs to satisfy the condition of from x new To x new Is x and new to x from Is not more than psi max 。
Preferably, the super-redundant mechanical arm has 2N +1 degrees of freedom, and N is the number of mechanical arms and is not less than 3.
Preferably, the method is applied to automatic driving of unmanned automobiles, autonomous planning of remote control planes and autonomous collision-free movement of robots.
Compared with the prior art, the invention has the beneficial effects that:
(1) the method can ensure that the joint deflection angle of the mechanical arm is always in a certain range in two-dimensional and three-dimensional (2D and 3D) environments. (2) Compared with the RRT algorithm, the MDA + RRT algorithm does not increase the calculation amount, and the feasibility and the optimization of the planned path are better than those of the corresponding RRT algorithm. (3) The method can be combined with any RRT algorithm or other path planning algorithms, can plan paths meeting angle constraint conditions for various moving objects, and has universality.
Drawings
FIG. 1 is a flow chart of the MDA + RRT algorithm;
FIG. 2 is a diagram of a super redundant robotic arm;
FIG. 3 is a schematic diagram of unreachable regions in planning a path;
FIG. 4 is a schematic and equivalent view of a robotic arm in a straight path;
FIG. 5 is a schematic diagram of the movement of the robot arm and a diagram of the change of joint angles;
FIG. 6 x new Selecting a schematic diagram of a range;
FIG. 7 is a tree distribution diagram of six algorithms in a first obstacle;
FIG. 8 is a tree distribution diagram of six algorithms in a second obstacle;
FIG. 9 is a tree distribution diagram of the six algorithms in a third obstacle;
FIG. 10 is a graph of performance parameters for a path planned by the six algorithms for three obstacles;
FIG. 11 is a diagram of the motion process of the super redundant robotic arm.
Detailed Description
The invention is further illustrated by the following examples.
The method can be used for path planning of the super-redundant mechanical arm, and can also be applied to path planning of other moving objects (such as automatic driving of an unmanned automobile, autonomous planning of a remote control plane, autonomous collision-free action of a robot and the like) so as to plan a path in a specified deflection angle range. The algorithm flow chart is as shown in fig. 1, firstly, a mechanical arm kinematics model and a barrier model are established, then, the relation between all parameters is theoretically analyzed, and finally, improvement is carried out according to the corresponding RRT algorithm to obtain the corresponding MDA + RRT algorithm.
In the present embodiment, the ranges of the three planning maps are 2000 × 1665, 3000 × 2500, 1700 × 2000 × 1400, respectively. The number N of the robot arms is 8.
As shown in FIG. 1, the method comprises the following steps:
step 1: and (3) establishing a mechanical arm kinematics model, namely a D-H coordinate system established aiming at the corresponding super-redundant mechanical arm, and deducing a conversion relation among parameters in the mechanical arm model.
The D-H coordinate system is established as shown in FIG. 2, which includes a fixed and unchangeable world coordinate system { O } relative to the ground xyz The body coordinate system of the base { O } 0 And eight armsBody coordinate system of (O) 1 }-{O 8 }. The coordinate systems can be converted through a conversion matrix between the coordinate systems, the conversion matrix between the body coordinate systems of two adjacent arm rods and the length L of the ith arm rod i Yaw angle alpha of the ith arm i Angle of pitch theta with i-th arm i And (4) correlating. The driving rope passes through a coordinate system (O) relative to the arm lever body i The hole is fixed to drive the mechanical arm, so that the deflection angle of the arm rod can be obtained according to the length of the driving rope, the terminal point coordinate of the arm rod can be obtained, and the deflection angle of the arm rod can be obtained according to the terminal point coordinate of the arm rod, and the length of the driving rope can be obtained. If the repeated motion path of the mechanical arm is known, the position of the tail end point of the arm rod can be obtained, and therefore the deflection angle psi between two adjacent arm rods can be obtained i 。
Step 2: establishing an obstacle model: a corresponding model is established for the obstacle in the environment, and the information such as the position, the shape, the size and the like of the obstacle can be obtained from the model.
Taking the first obstacle as an example, the analysis should ensure that the distance between the path and the obstacle is kept above in order to ensure that the robot arm does not touch the obstacle. The farthest distance between all points on the surface of each arm rod and the axis of the arm rod is 75, and considering that when the arm rod moves along a straight path, only two end points of the arm rod move along the path, and errors such as control errors and positioning errors also exist in the motion process of the mechanical arm, a certain distance exists between the point on the axis of the arm rod and the path (the maximum distance is considered to be delta) max ) So that the shortest distance between the planned path and the obstacle is 75+ delta max . Will be at a distance of 75+ delta or less from the obstacle max All are treated as unreachable regions, and the regions within the black line frame shown in fig. 3 are all unreachable regions. Therefore, whether the space point can reach or not can be judged according to the closest distance between the space point and the obstacle, and the obstacle model is changed into the unreachable area model corresponding to the obstacle in the subsequent path planning.
And step 3: theoretical analysis arm lever length L i Maximum included angle psi of joint max Road, roadRadial step length l w Maximum angle psi between two adjacent straight lines in the path max The relationship between these four parameters, thereby converting the problem of joint declination limitation to the problem of path declination limitation.
When l is analyzed w ＝L i ＝L i-1 (w is 1,2,3 …), the respective parameters should satisfy the relationship. The two arm axes shown in fig. 4 form two triangles with three straight lines along the path: Δ ABC and Δ CDE. Max (psi) can be obtained according to sine theorem and angle conversion of triangle i ) The mathematical model of (a) is as follows:
further, it is possible to obtain:
second analysis of w ＝k·L i ＝k·L i-1 (where k.epsilon. (0, 1)]And w is 1,2,3 …), the respective parameters should satisfy the relationship. The starting point of the i-1 th arm rod is moved along the w-th straight path, and the moving process and the deflection angle change at the i-th joint are shown in fig. 5.
ψ i There are two locations where maxima of (a) occur. When the first point is the terminal point of the ith-1 arm rod, i.e. the initial point of the ith arm rod, moves to the initial point or the terminal point of a straight line path (a is 0 or b is 0 in fig. 3), psi at the point is derived i_end As follows:
second position is when the ith arm rod initial point moves to the middle point of a straight line path (a ═ b in fig. 3), and psi at the point i_mid As follows:
after the two maximum values are obtained, the maximum deflection angle psi of the joint can be obtained max ＝max(ψ i_end ,ψ i_mid )。
N in the formula end When the end points of the arm rod move to the end points of a straight path, there are several path turning points between the two end points of the arm rod, for example, N in the first diagram of FIG. 5 end 1. Wherein N is mid When the end points of the arm rod move to the middle point of a straight path, there are several path turning points between the two end points of the arm rod, for example, N in the second diagram of FIG. 5 mid 2. Let l w ＝300、Ψ max ＝22.40°、L i ＝L i-1 486, substituting the formula to obtain psi i_end ＝39.99°、ψ i_mid 35.77 °, that is, when the mechanical arm with the arm rod length 486 moves along the path with the step size of 300 and the maximum included angle between two adjacent straight lines of 22.40 °, the joint deflection angle psi i does not exceed the limit range of 40 ° all the time.
According to the formula, the current l can be determined w ＝k·L i ＝k·L i-1 、ψ max At 40 °, Ψ max How much value should be taken is shown in table 1.
TABLE 1 psi at different step sizes max Value of (k is 0.1, 0.2 … 1)
It can be seen from the table that the smaller k is, the smaller Ψ max The smaller, i.e. | w The smaller Ψ max The smaller, and w to Ψ max There is a non-linear relationship between them.
The following describes how to plan a motion route meeting the structural requirements of the mechanical arm by taking the MDA-QRRT algorithm after the Q-RRT algorithm is improved as an example.
And 4, step 4: definition and step size delta (i.e. path compensation l) w ) Starting point x of path planning init As a randomThe root node of the tree T. MDA-QRTT algorithm starts with a path x init As root node of the random tree T, node x in the tree i Storing by a set V, wherein the V is provided with n columns in an n-dimensional space, each row corresponds to the position data of one node, the connection between the nodes is stored by an edge set E, and the first column in the E represents a node x i Is the second node, the 2 nd column is node x i The 3 rd column is the node x i To its parent node and then to the parent node up to x init The superimposed euclidean distance of (c). Always guarantee all data in V and E are at X free In (1).
And 5: selecting an x by a random function rand Selecting a distance x rand Nearest node x near Then in the Angle-MDA function according to the conclusion in step 3, according to x rand 、x near And T analysis x new The selection range of (1). When determining good psi i I.e. x new After the direction of (c), x can be calculated according to the following formula new The position of (a).
x new ＝x near +δ·Ψ i
Wherein the Angle-MDA function is according to x rand 、x near And T analysis x new To determine the next discrete point x new The specific location of (a).
In this function, x is first determined rand And x near The angle ang of the line segment formed by the two points relative to the horizontal line rand And reading x in T near The angle ang of the line segment connected with the father node of the line segment relative to the horizontal line near . The algorithm aims to ensure that the joint deflection angle is always in a limited range, and the length of the arm lever of the mechanical arm is not constant, so that the joint deflection angle is required to be firstly determined according to a node x near And target point x goal A distance d therebetween goal To analyze Ψ max The value of (a). Because the lengths of the arms of the mechanical arm selected by the invention are unequal, L is put into the previous path planning i When the length of 486, namely the longest arm rod, is used, the distance between a discrete point on a path and a target object is less than L 7 +L 8 When in use, handle L i The length of the penultimate arm is counted as 206. According to the conclusion in step three, if d goal <L 7 +L 8 Let us order Ψ max If d is 40 deg., if d goal ≥L 7 +L 8 Let us order Ψ max ＝22.40°。
Maximum deviation angle psi between two adjacent straight lines in a well-defined path max Then, analysis x new Whether the selectable range is exceeded. If x near Is a starting point x init Then x is randomly selected between-60 DEG and 60 DEG new In the direction of (a). Otherwise, randomly selecting x between-22.40 degrees and 22.40 degrees new The direction of (a);
if the angle between two adjacent straight lines Abs (ang) rand -ang near )≤Ψ max Then x is maintained new Direction of (2) i ＝ang rand . If the angle between two adjacent straight lines Abs (ang) is rand -ang near )>Ψ max Let us order Ψ i Becoming a boundary value in the selectable range, fig. 6 lists the case where ang exceeds the selectable range. Let Ψ in two-dimensional case i ＝ang near +Sign(ang rand -ang near )·Ψ max ，x new The selected point is unique and is a point on the boundary; in three-dimensional condition, ensuring that the included angle between two adjacent straight lines is psi max ，x new The selected space is a space with the radius delta.sin (psi) max ) Circle of (a), x new Is a point on the circle.
Step 6: if x new Composite conditional requirements, i.e. x new And x near With no obstacle in between, then x new Nearby nodes deposit to X neighbor And storing the ancestors of the nearby nodes in X parent In this way, a group x is obtained new Finished X in And (4) collecting.
And 7: using ChooseParent-MDA function at X in Is found in new New father node x with minimum cost and meeting deflection angle requirement par Delete x new With the original parent node x near To connect toIs connected to x new And x par And pruning is performed in T.
ChooseParent-MDA function content is as follows, at initialization minimum parent node x min Minimum path σ min Minimum cost c min Then, if x new Is at X in Node x in (1) in Cost is reduced as a new parent node, and x in And x new The path σ formed satisfies collision-free and satisfies from x in To x in Path and x of in To x new Has an angle of not more than psi max Then node x is added in Is set to x new A new parent node. Wherein X in Not only include x new Nearby nodes also include ancestors of those nodes, thereby increasing the optimization speed of the algorithm.
And step 8: if X neighbor One node x in from Is greater than the current x new The sum of the weight of (2) and the distance to the node, and if the deflection angle requirement is met, then the current x is used new Node as x from The specific process of the parent node of (2) is as follows.
Not only pruning and x are needed in the MDA-QRTT algorithm new The related path also searches for X neighbor The more optimal parent of all nodes in the cluster. Analyzing X in the function neighbor All nodes x in nei And x new And x new Is formed by parent nodes (i.e. analysis x) nei And x from New path formed), whether the cost is reduced and the conditions for no collision and angle constraints are met (i.e., satisfied from x) new To x new Is x and new to x from Is not more than psi max . ). If so, x is added nei Is updated to x from And pruning the path.
And step 9: repeatedly searching for x new And pruning x new And modifying the data in the random tree T until reaching the target point according to the connection relation with the nearby nodes.
Simulation result 1: for three obstacles, each algorithm is run 1 time to generate 500 root nodes, and the tree distribution diagrams shown in fig. 7, 8 and 9 are obtained respectively. In the operation process of generating 500 root nodes, the running time of the MDA-RRT algorithm, the MDA-QRRT algorithm and the running time of the RRT algorithm and the Q-RRT algorithm are closer, which shows that the calculation amount of the algorithm is not increased by the improved algorithm, only the selection range of the next node is limited, and the constraint is utilized to ensure that the planned path can meet the requirement of joint deflection angle limitation. From the simulation results, after 500 root nodes are generated, a path meeting the requirement of joint deflection angle limitation can be planned by adopting three MDA + RRT algorithms, namely simple path planning in obs1, complex path planning in obs2 and three-dimensional path planning in obs 3. Due to the randomness of the RRT algorithm, it is important to quickly obtain a high-quality initial solution in the path planning related to the RRT algorithm.
Simulation result 2: and modifying the termination condition from 500 node samples to plan a path which can reach the end point, adopting the six algorithms to run for 50 times in each obstacle, and analyzing the time and the quality required for planning the initial path by adopting the six algorithms. The simulation result is shown in fig. 10, and it can be seen from the figure that the feasibility and the optimization of the three MDA + RRT algorithms are better than those of the corresponding RRT algorithms. Of these six algorithms, the MDA-QRRT algorithm is the best feasible and optimal.
As shown in fig. 11, there are shown static diagrams corresponding to 5 positions of the robot arm during the movement and the final static diagram when the robot arm grabs the target object. The red line segment in the figure is a path with the length of 3085mm marked by adopting MDA-QRTT algorithm, and the figure also comprises an obstacle, a super-redundant mechanical arm, the size of an offset angle psi i between the ith-1 arm rod and the ith arm rod corresponding to the current position and some comments.
It can be seen from the figure that the mechanical arm does not touch the obstacle all the time in the moving process, can grab the target object, and can know that the joint drift angle is less than 40 degrees all the time according to the previous simulation result, thereby verifying the feasibility of the path planned by adopting the improved algorithm in the invention.
Aiming at the problem of joint deflection angle limitation in a super-redundancy mechanical arm structure, the invention provides an MDA + RRT algorithm for path planning on the basis of an RRT algorithm. In the invention, the relation among all parameters is theoretically analyzed, the problem of joint deflection angle limitation is converted into the problem of path deflection angle limitation, on the basis of the theoretical analysis, a plurality of RRT algorithms are improved, the MDA + RRT algorithm after improvement is found to not increase the calculated amount compared with the RRT algorithm before improvement, only the selection range of nodes is limited, and the constraint is utilized to ensure that the planned path can meet the requirement of joint deflection angle limitation.
The invention can also carry out variable step RRT algorithm, bidirectional RRT algorithm or other intelligent RRT algorithm on the basis of the algorithm, thereby planning the path meeting the constraint. In addition to RRT: PRM algorithm, a-algorithm, APF algorithm.
The MDA + RRT algorithm is used for path planning of the super-redundant mechanical arm, and the MDA + RRT algorithm can be applied to path planning of other moving objects (such as automatic driving of an unmanned automobile, autonomous planning of a remote control plane, cruise missile avoidance radar search, autonomous non-collision action of a robot and the like), so that a path within a designated deflection angle range is planned.
The invention has not been described in detail in part in the common general knowledge in the art.
Claims (12)
1. A super-redundant mechanical arm path planning method is characterized by comprising the following steps:
step 1: establishing a mechanical arm kinematics model aiming at the super-redundant mechanical arm, and determining a conversion relation among the length of a driving rope, the deflection angle of an arm lever and the coordinates of the tail end point of the arm lever;
step 2: establishing an obstacle model aiming at an obstacle in the environment, and obtaining the position, shape and size information of the obstacle from the obstacle model so as to determine a non-reachable area corresponding to the obstacle;
and step 3: assume that adjacent arms are the same length and equal to the path step length l w In a linear relation, theoretically analyzing the maximum included angle psi between two adjacent straight-line paths according to the mechanical arm kinematics model determined in the step 1 max Maximum deflection angle psi of the arm during movement of the ith arm initial point along one of the straight paths max Arm length L i Path step length l w The relationship between;
and 4, step 4: planning a path according to the unreachable area corresponding to the obstacle determined in the step 2 and the relation constraint in the step 3;
and 4, planning the path by adopting an MDA + RRT algorithm, and specifically realizing the method by adopting the following steps:
Step 41: defining a step δ i.e. l w Starting point x of path planning init As the root node of the random tree T;
step 42: randomly selecting a node x rand Selecting a distance x rand Nearest node x near According to node x near And target point x goal A distance d between goal Determining Ψ max Taking the value of (A); then according to the relation and x in step 3 rand 、x near T analysis x new To calculate x new The position of (a);
step 43: if x new If the condition requirement is met, i.e. the unreachable area determined in the step 2 is not entered, x is added new Nearby nodes deposit to x neighbor And storing the ancestors of the nearby nodes to x parent In this way, a group x is obtained new Arranging a Xin set;
step 44: find x in Xin new New father node x with minimum cost and meeting deflection angle requirement par Delete x new With the original parent node x near Connecting line of (2), connecting x new And x par And pruning is carried out in T;
step 45: if x neighbor One node x in from Is greater than the current x new The sum of the weight of (2) and the distance to the node, and if the deflection angle requirement is met, then the current x is used new Node as x from A parent node of (a);
step 46: repeatedly searching for x new And pruning x new Connections to nearby nodesUntil the target point is reached.
2. The method according to claim 1, wherein in step 2, the shortest distance between the path and the obstacle is set as the farthest distance between all points on the surface of the arm and the axis of the arm, and the farthest distance between the point on the axis of the arm and the straight path, and a range where the distance from the obstacle is less than or equal to the shortest distance is considered as the unreachable area.
3. The method according to claim 2, wherein the step 42 is implemented by:
s1, if x near Is a starting point x init Then x is randomly selected between-60 DEG and 60 DEG new Is performed, step S2, otherwise, at-min (Ψ) max ) To min (Ψ) max ) Randomly select x between new Step S2 is performed; min (Ψ) max ) Is node x near And target point x goal Psi determined at different distances between them max Minimum value of (d);
s2, judging whether the following inequality Abs (ang) is satisfied rand -ang near )≤Ψ max If yes, then x is maintained new Direction; if not, let Ψ i Is a boundary value in the selectable range;
wherein x is rand And x near The included angle of a line segment formed by connecting two points relative to the horizontal line is ang rand Reading x in T near The line segment connected with the father node of the node near 。
4. The method of claim 2, wherein the determining Ψ in step 42 is performed by max The value of (A) is as follows:
d goal <l, then let Ψ max Taking values of two calculated according to the relation of the penultimate arm and the relation in the step 3The maximum included angle between adjacent straight paths;
d goal if greater than or equal to L, let Ψ max The value is the maximum included angle between two adjacent linear paths calculated according to the combination of the longest arm lever and the relation in the step 3;
L is the sum of the lengths of the last two arms.
5. The method as claimed in claim 3, wherein when Ψ is a system for planning the path of the robot arm i Psi in two dimensions for the selection of boundary values in the range i ＝ang near +Sign(ang rand -ang near )·Ψ max ，x new Is a point on the boundary; under the three-dimensional condition, the included angle between two adjacent linear paths is ensured to be
The selected space is a space with the radius delta.sin (psi) max ) Circle of (a), x new Is a point on the circle.
6. The method for planning the path of the super-redundant mechanical arm according to claim 1 or 4, wherein the relationship in the step 3 is determined by:
let l w ＝k·L i ＝k·L i-1 ，k∈(0，1]The starting point of the ith-1 th arm rod is moved along the w-th straight line path, and the deflection angle psi of the ith arm rod is generated in the moving process i The maximum value of the change appears at two positions; the first point is the terminal point of the ith-1 arm rod, namely the initial point of the ith arm rod, and moves to the initial point or the terminal point of a straight line path, wherein psi i Maximum value of variation is denoted psi i_end (ii) a Second position is when the initial point of the ith arm rod moves to the middle point of a straight line path i Maximum value of variation is denoted psi i_mid ；
After the two maximum values are obtained, the maximum value max (psi) of the arm deflection angle change is obtained i_end ,ψ i_mid )。
7. The method for planning the path of the super-redundant mechanical arm according to claim 6, wherein: when k is 1, the relationship in step 3 is determined as follows:
Simplifying two adjacent arm rods into a straight line along the axis, forming two triangles by the two adjacent arm rods and three continuous straight lines on the path, and determining the relationship according to the sine theorem and the angle transformation of the triangles as follows:
8. the method for planning the path of the super-redundant mechanical arm according to claim 6, wherein:
N end when the ith arm rod end point moves to the end point of a section of straight line path, a plurality of path turning points are arranged between the two end points of the arm rod;
N mid when the ith arm rod end point moves to the middle point of a straight path, a plurality of path turning points are arranged between the two end points of the arm rod.
9. The method as claimed in claim 1, wherein in step 44, a new parent node x is found par In addition to satisfying the conditions of cost reduction and no collision, the method also needs to satisfy the path from the parent node of xin to xin and the path from xin to x new Is not more than psi max (ii) a Xin is the node in the Xin set.
10.The method as claimed in claim 1, wherein the step 45 is executed if the current x is used as the x new Node as x from The parent node of (2) not only needs to satisfy the conditions of cost reduction and no collision, but also needs to satisfy the condition of from x new To x new Is x and new to x from Is not more than psi max 。
11. The method as claimed in claim 1, wherein the super-redundant robot arm has 2N +1 degrees of freedom, N being the number of robot arms and not less than 3.
12. The method for planning the path of the ultra-redundant mechanical arm according to claim 1, wherein the method is applied to automatic driving of an unmanned automobile, automatic planning of a remote control plane and automatic collision-free movement of a robot.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110593223.3A CN113459086B (en) | 2021-05-28 | 2021-05-28 | Super-redundancy mechanical arm path planning method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110593223.3A CN113459086B (en) | 2021-05-28 | 2021-05-28 | Super-redundancy mechanical arm path planning method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113459086A CN113459086A (en) | 2021-10-01 |
| CN113459086B true CN113459086B (en) | 2022-07-29 |
Family
ID=77871623
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110593223.3A Active CN113459086B (en) | 2021-05-28 | 2021-05-28 | Super-redundancy mechanical arm path planning method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113459086B (en) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4967126A (en) * | 1990-01-30 | 1990-10-30 | Ford Aerospace Corporation | Method of controlling a seven degree of freedom manipulator arm |
| JP2000020117A (en) * | 1998-06-29 | 2000-01-21 | Yaskawa Electric Corp | Method and device for planning operation route of robot |
| DE102015211865B3 (en) * | 2015-06-25 | 2016-05-12 | Kuka Roboter Gmbh | Method for redundancy-optimized planning of operation of a mobile robot |
| CN106695802A (en) * | 2017-03-19 | 2017-05-24 | 北京工业大学 | Improved RRT<*> obstacle avoidance motion planning method based on multi-degree-of-freedom mechanical arm |
| CN106737689A (en) * | 2017-01-19 | 2017-05-31 | 哈尔滨工业大学深圳研究生院 | Super redundant mechanical arm based on mode function mixes Converse solved method and system |
| CN108908331A (en) * | 2018-07-13 | 2018-11-30 | 哈尔滨工业大学（深圳） | The barrier-avoiding method and system, computer storage medium of super redundancy flexible robot |
| CN109227549A (en) * | 2018-11-08 | 2019-01-18 | 汕头大学 | A kind of smooth avoidance motion planning method of robot based on tangent line recursion |
| CN111070238A (en) * | 2020-01-14 | 2020-04-28 | 北京精密机电控制设备研究所 | Combined weak torsion continuous mechanical arm connecting joint |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP6576255B2 (en) * | 2016-01-25 | 2019-09-18 | キヤノン株式会社 | Robot trajectory generation method, robot trajectory generation apparatus, and manufacturing method |
| US11667035B2 (en) * | 2019-07-01 | 2023-06-06 | Wisconsin Alumni Research Foundation | Path-modifying control system managing robot singularities |
-
2021
- 2021-05-28 CN CN202110593223.3A patent/CN113459086B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4967126A (en) * | 1990-01-30 | 1990-10-30 | Ford Aerospace Corporation | Method of controlling a seven degree of freedom manipulator arm |
| JP2000020117A (en) * | 1998-06-29 | 2000-01-21 | Yaskawa Electric Corp | Method and device for planning operation route of robot |
| DE102015211865B3 (en) * | 2015-06-25 | 2016-05-12 | Kuka Roboter Gmbh | Method for redundancy-optimized planning of operation of a mobile robot |
| CN106737689A (en) * | 2017-01-19 | 2017-05-31 | 哈尔滨工业大学深圳研究生院 | Super redundant mechanical arm based on mode function mixes Converse solved method and system |
| CN106695802A (en) * | 2017-03-19 | 2017-05-24 | 北京工业大学 | Improved RRT<*> obstacle avoidance motion planning method based on multi-degree-of-freedom mechanical arm |
| CN108908331A (en) * | 2018-07-13 | 2018-11-30 | 哈尔滨工业大学（深圳） | The barrier-avoiding method and system, computer storage medium of super redundancy flexible robot |
| CN109227549A (en) * | 2018-11-08 | 2019-01-18 | 汕头大学 | A kind of smooth avoidance motion planning method of robot based on tangent line recursion |
| CN111070238A (en) * | 2020-01-14 | 2020-04-28 | 北京精密机电控制设备研究所 | Combined weak torsion continuous mechanical arm connecting joint |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113459086A (en) | 2021-10-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Yuan et al. | A heuristic rapidly-exploring random trees method for manipulator motion planning | |
| CN110653805B (en) | Task constraint path planning method for seven-degree-of-freedom redundant manipulator in Cartesian space | |
| Cheng et al. | RRT-based trajectory design for autonomous automobiles and spacecraft | |
| KR101339480B1 (en) | Trajectory planning method for mobile robot using dual tree structure based on rrt | |
| CN111216125B (en) | Obstacle avoidance method and system of moving mechanical arm device facing narrow passage | |
| Hota et al. | Optimal geometrical path in 3D with curvature constraint | |
| CN113050640B (en) | Industrial robot path planning method and system based on generation of countermeasure network | |
| CN112356033B (en) | Mechanical arm path planning method integrating low-difference sequence and RRT algorithm | |
| CN112130582B (en) | Multi-agent formation forming method | |
| CN110580740B (en) | Multi-agent cooperative three-dimensional modeling method and device | |
| CN112762957A (en) | Multi-sensor fusion-based environment modeling and path planning method | |
| CN110146087B (en) | Ship path planning method based on dynamic planning idea | |
| CN114147708B (en) | Mechanical arm obstacle avoidance path planning method based on improved longhorn beetle whisker search algorithm | |
| CN111761582A (en) | Mobile mechanical arm obstacle avoidance planning method based on random sampling | |
| CN113625702B (en) | Unmanned vehicle simultaneous path tracking and obstacle avoidance method based on quadratic programming | |
| CN112497216B (en) | Industrial robot pose precision compensation method based on deep learning | |
| CN113467476A (en) | Non-collision detection rapid stochastic tree global path planning method considering corner constraint | |
| CN113589809A (en) | Obstacle-avoidable excavator working device operation track planning method and device | |
| Wang et al. | Research on AGV task path planning based on improved A* algorithm | |
| CN113459086B (en) | Super-redundancy mechanical arm path planning method | |
| CN117452965A (en) | Track planning method for variable-configuration unmanned aerial vehicle to pass through long and narrow channel | |
| CN112990549A (en) | Space non-cooperative target approaching fly-around observation trajectory optimization method | |
| Asqui et al. | Path planning based in algorithm rapidly-exploring random tree RRT | |
| CN116009558A (en) | Mobile robot path planning method combined with kinematic constraint | |
| Liu et al. | Simulation of manipulator path planning based on improved rrt* algorithm |
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 |