CN112356033A

CN112356033A - Mechanical arm path planning method integrating low-difference sequence and RRT algorithm

Info

Publication number: CN112356033A
Application number: CN202011242965.3A
Authority: CN
Inventors: 代伟; 李创业; 缪燕子; 马小平
Original assignee: China University of Mining and Technology CUMT
Current assignee: China University of Mining and Technology CUMT
Priority date: 2020-11-09
Filing date: 2020-11-09
Publication date: 2021-02-12
Anticipated expiration: 2040-11-09
Also published as: CN112356033B

Abstract

The invention discloses a mechanical arm path planning method integrating a low-difference sequence and an RRT algorithm. According to the invention, a low-difference sequence is used for replacing a pseudo-random sequence in an RRT algorithm, so that uniformly-different sampling points are generated, and the repetition of the sampling points is avoided; meanwhile, a target deflection strategy is introduced for further accelerating the searching speed, so that the algorithm has certain target in the searching process, and the searching of invalid areas is reduced; then, a sampling pool is established in the sampling process, and the sampling point is optimized to improve the quality of the sampling point; the method is applied to the mechanical arm joint space for path planning, so that inverse kinematics solution is avoided, and the calculated amount is reduced.

Description

Mechanical arm path planning method integrating low-difference sequence and RRT algorithm

Technical Field

The invention relates to the field of robot path planning, in particular to a mechanical arm path planning method integrating a low-difference sequence and an RRT algorithm.

Background

The land reclamation of the abandoned mining area is an important way for relieving agricultural land and improving the environment of the mining area, but the severe environment of the mining area may have potential dangers of heavy metals, unknown collapse, environmental pollution and the like, and the adoption of a mobile robot for unmanned soil sampling is the primary link of mining area restoration. The mechanical arm is used as an actuating mechanism of the mobile robot to undertake the important task of soil sampling, and due to the fact that unknown obstacles exist in complex mining area environments, path planning needs to be carried out on the mechanical arm to avoid the obstacles to reach a target point. With the development of robotics, scholars at home and abroad have proposed a plurality of path planning algorithms, such as an artificial potential field method, an a-x algorithm, an RRT algorithm, and the like, wherein the artificial potential field method and the a-x algorithm are easy to fall into a local minimum value in path planning although the path planning can meet the requirements of optimality and real-time performance, and the a-x algorithm has the problems of large memory overhead, long running time, large accumulated turning angle, and the like. The RRT algorithm is a path planning method based on random sampling, does not need to preprocess a state space, has the characteristics of high search speed, convenience in processing constraint and the like, can effectively solve the problem of path planning in a complex environment, and is widely applied to the field of robot path planning in recent years. However, the standard RRT also has some inherent drawbacks: (1) the global random sampling causes the algorithm to consume a large cost wastefully, so that the convergence speed becomes slow; (2) the randomness of the algorithm may result in a generated path that is not smooth and difficult to directly execute by the robot.

Disclosure of Invention

The invention aims to provide a mechanical arm path planning method integrating a low-difference sequence and an RRT algorithm, which is used for improving the path planning efficiency of a mechanical arm and optimizing a planned path.

In order to achieve the above object, the method for planning the path of the mechanical arm by fusing the low-difference sequence and the RRT algorithm provided by the invention comprises the following steps:

the method comprises the following steps: initializing a working environment

Establishing a kinematic model of the mechanical arm and determining a motion space and an Obstacle of the mechanical arm; acquiring a path starting point (starting point) x in a mechanical arm state space_startAnd end point of path (target point) x_goalInitializing a random tree T_nodeAnd determining RRT algorithm parameters, which mainly comprises the following steps:

(1) and (3) the target offset probability p is used for enabling the sampling point to be equal to the target point with a certain probability p, so that the blindness of the algorithm is reduced.

(2) Capacity of sampling cell n perThe sub-generated n sampling points are placed into a sampling pool to select the optimal sampling point in the sampling pool to generate a new node x_new。

(3) And step, determining a new node according to the nearest node, the position of the sampling point and the step by the algorithm.

(4) And an allowable error, and when the Euclidean distance between the new node and the target node is less than the allowable error, considering that the path search is completed.

Step two: joint angle space sampling

Setting a target bias probability p and generating a random number rand between interval ranges (0, 1); comparing the random number with the bias probability p, if rand is less than or equal to p, directly making the sampling point x_sampleEqual to target point (end of path) x_goalConversely, n sampling point pairs with uniform difference and a target point (path end point) x are generated by using the low-difference Sobol sequence_goalThe Euclidean distance of the sampling pool is far and near, and the sampling pool is arranged from small to large.

Step three: generating a new node

3.1 finding a random Tree T_nodeMiddle distance sampling point x_sampleOne nearest point is taken as a nearest node x_nearest；

3.2 at the nearest node x_nearestTo sample point x_sampleStep-by-step progression in the direction of the connection to generate a new node x_new；

3.3 carrying out collision detection, if the collision is detected, reselecting the sampling point to generate a new node, and if the collision is not detected, connecting the new node x_newStore to random Tree T_nodeAnd nearest node x_nearestTreated as a new node x_newA parent node of (a);

3.4 according to the updated random Tree T_nodeRepeating the second step to the third step until the latest node x is obtained_newSatisfy | x_new-x_goalDetermining that the target point is found, wherein the error is an allowable error;

3.5 in random Tree T_nodeAnd reversely searching to find the planned path according to the parent-child relationship of each node.

Step four: path optimization processing

And fitting the path obtained in the third step into six smooth curves by using a fifth-order polynomial fitting method based on a least square method.

As an improvement, in the second step, the Sobol sequence is a more uniformly distributed sequence than the pseudo-random sequence and is a group of { v ] based on "direct number_iThe sequence of (c) },

m_i＜2ⁱand is positive odd; v. of_i，m_iDepends on a simple polynomial f (z) of order p with coefficients a, the polynomial coefficient being only 0 or 1, of the form:

f(z)＝z^p+a₁z^p-1+…+a_p-1z+a_p

when i < p, m needs to be specified_iAnd when i > p, m_iThe recurrence formula is:

wherein

Meaning that the rounding is done down,

representing binary bitwise XOR, again for v_iThe recursive formula of (1) is:

thus, the ith number of the Sobol sequence can be obtained:

… b therein₃b₂b₁Is a binary representation of i。

The sampling point is composed of each joint angle of the mechanical arm, and each joint angle needs to be generated by a Sobol sequence. By varying the coefficients of the polynomial f (z), different Sobol sequences can be obtained. The obtained Sobol sequences are uniformly distributed in (0,1) space, and one joint angle in sampling points can be expressed as min + (max-min) × x_iWhere min and max represent the upper and lower limits of the joint angle.

As an improvement, the step 3.3 specifically comprises: judgment of x_newIf the corresponding mechanical arm posture collides with the obstacle, if the current sampling point is equal to the path end point, the step is switched to for re-sampling, if the current sampling point is not equal to the path end point, the sampling point is abandoned, the next sampling point in the sampling pool is selected in sequence, and the step is switched to the step three to generate a new node x_newAnd sampling again unless the new nodes generated by the sampling points in the sampling pool do not meet the requirements. New node x without collision_newJoining random Tree T_nodeIn and define x_nearestIs x_newThe parent node of (2).

Has the advantages that: according to the invention, a low-difference sequence is used for replacing a pseudo-random sequence in an RRT algorithm, so that uniformly-different sampling points are generated, and the repetition of the sampling points is avoided; meanwhile, a target deflection strategy is introduced for further accelerating the searching speed, so that the algorithm has certain target in the searching process, and the searching of invalid areas is reduced; then, a sampling pool is established in the sampling process, and the sampling point is optimized to improve the quality of the sampling point; the method is applied to the mechanical arm joint space for path planning, so that inverse kinematics solution is avoided, and the calculated amount is reduced.

Drawings

Fig. 1 is a search expansion schematic diagram of the mechanical arm path planning method fusing a low-variance sequence and an RRT algorithm provided by the present invention.

FIG. 2 shows an AUBO-i5 robotic arm used in an embodiment of the invention.

Fig. 3 is a schematic diagram of a simulation environment of a robot arm according to the present invention.

Detailed Description

The present invention will be described in detail below with reference to the accompanying drawings.

As shown in fig. 1, a method for planning a mechanical arm path by fusing a low variance sequence and an RRT algorithm includes the following steps:

1. for the AUBO-i5 mechanical arm shown in FIG. 2, the pose is described by a D-H method, a link coordinate system of the mechanical arm is established, D-H parameters of the mechanical arm are determined, and then a D-H model of the mechanical arm is obtained through calculation. The connecting rod coordinate system is used for describing the displacement relation among the connecting rods and between the rod pieces and the joints, and a homogeneous transformation matrix of the tail end coordinate system relative to the base coordinate system is obtained. The homogeneous transformation matrix is a function of each joint variable and is used for solving a positive kinematic solution from a joint space of the mechanical arm to a Cartesian space. The kinematics positive solution is the mapping from the joint space of the mechanical arm to the Cartesian space, so that the terminal pose of the mechanical arm under the given attitude and joint angle of the mechanical arm can be solved, and the path planning is realized. As shown in fig. 3, the link coordinate system is established by the following rule:

(1)z_iis the positive direction of the motion axis of the joint i.

(2)x_iAxis perpendicular to z_i-1Axial and pointing z_i-1z_iThe direction of the axis.

(3)y_iThe axes are established according to the right hand rule.

A corresponding table of D-H parameters was established as shown in table 1. Where the parameters α and d represent the arm link length parameters. In the table: alpha is alpha_i-1Is from Z_i-1To Z_iAlong x_i-1The distance of (d); d_iIs from x_i-1To x_iAlong Z_iThe distance of (d); a is_i-1Is from Z_i-1To Z_iAround x_i-1The angle of rotation; theta_iIs from x_i-1To x_iAround Z_iThe angle of rotation.

TABLE 1D-H PARAMETERS

2. The path planning of the mechanical arm is carried out in the joint space of the mechanical arm, and each joint of AUBO-i5The angular ranges are all [ -pi, pi [ -pi [ ]]Initial joint angle (starting point of path) x_startAnd target joint angle (end of path) x_goalAre respectively [0,0,0,0,0,0]，[-135,-90,55,60,100,40]The target offset probability p is 0.3, the step length step is 6, the capacity of the sampling pool is 10 sampling points, and the allowable error is 6; the error is defined as the Euclidean distance between the current joint angle and the target joint angle. The barrier and the mechanical arm connecting rod are wrapped by a regular geometric enveloping body so as to simplify collision judgment conditions.

3. A random number rand is generated in the (0,1) space.

4. If the random number rand is less than or equal to 0.3, sampling is not carried out, and the sampling point x is directly ordered_sampleEqual to the target joint angle x_goal。

5. If the random number rand is greater than 0.3, 10 sampling points x are generated by using the low-difference Sobol sequence_sampleAnd placing the sample into a sampling pool from small to large according to the Euclidean distance from the target point. The AUBO-i5 robot used in this example has 6 degrees of freedom, and therefore, it is necessary to generate 6 series of Sobol sequences. Sobol sequences are a set based on the "direct number" { v_iThe sequence of (c) },

f(z)＝z^p+a₁z^p-1+…+a_p-1z+a_p

when i < p, m needs to be specified_iA value of (d); when i > p, m_iThe recurrence formula is:

wherein

Meaning that the rounding is done down,

representing binary bitwise XOR, again for v_iThe recursive formula of (1) is:

thus, the ith number of the Sobol sequence can be obtained:

… b therein₃b₂b₁Is a binary representation of i.

By changing the coefficients of the polynomial f (z), different Sobol sequences can be obtained. The obtained Sobol sequences are uniformly distributed in (0,1) space, and one joint angle in sampling points can be expressed as min + (max-min) × x_iWherein min-pi and max-pi represent the upper and lower limits of the joint angle.

6. Selecting distance sample points x in a random tree_sampleThe nearest point is taken as a nearest node x_nearest。

7. At the nearest node x with a certain step size of step 6_nearestAnd sample point x_sampleExpanding new node in connecting line direction

8. Judgment of x_newWhether the corresponding mechanical arm posture collides with an obstacle or not, and if the sampling point x is in the collision condition_sampleIs equal to target point x_goalThen sample point x is discarded_sampleGo to step 3, if sample point x_sampleIf the sampling point is obtained from the sampling pool, the sampling point is abandoned and the next sampling point is selected in sequence to step 6 to generate a new node x_newAnd sampling is carried out again unless the sampling points in the sampling pool are exhausted. New node x without collision_newJoining random Tree T_nodeAnd defines the nearest node x_nearestAs a new node x_newThe parent node of (2).

9. According to the updated random tree T_nodeAnd repeating the steps 3-8 until the latest node x is obtained_newSatisfy | x_new-x_goalI ≦ error, considered to find the target point, error refers to the allowed error.

10. In a random tree T_nodeAnd reversely searching to find the planned path according to the parent-child relationship of each node.

11. Because the random of the algorithm causes that the planned path is not smooth and can not be directly applied to the mechanical arm, firstly, the planned joint angle track is fitted with a smooth curve by using a quintic polynomial fitting method based on a least square method, wherein the first-time fitting result is as follows:

y₁＝-0.003x⁴-0.02x³+0.5719x²-7.3594x+10.2835 (1)

y₂＝-0.004x⁴-0.0256x³+0.6914x²-7.5403x+12.4140 (2)

y₃＝-0.001x⁴-0.0022x³+0.0326x²+0.7793x-3.8404 (3)

y₄＝-0.0004x³+0.0401x²+1.1348x-4.8081 (4)

y₅＝-0.0001x⁴+0.0078x³-0.1672x²+3.1304x-7.3039 (5)

y₆＝0.0002x⁴-0.0118x³+0.3291x²-2.4139x+8.1399 (6)

from a given initial joint angle (initial point) x_startTo a target joint angle (target point) x_goalAccording to the fitted fifth degree polynomial y₁…y₆And uniformly sampling to obtain joint angle sampling points which enable the mechanical arm to run smoothly. And sending a control instruction to the joint angle sampling point through a serial port according to a communication protocol of the mechanical arm controller.

The foregoing is only a preferred embodiment of this invention and it should be noted that modifications can be made by those skilled in the art without departing from the principle of the invention and these modifications should also be considered as the protection scope of the invention.

Claims

1. A mechanical arm path planning method fusing a low-difference sequence and an RRT algorithm is characterized by comprising the following steps:

the method comprises the following steps: initializing a working environment

Establishing a kinematic model of the mechanical arm and determining a motion space and an Obstacle of the mechanical arm; acquiring a starting point x in a mechanical arm state space_startAnd target point x_goal(ii) a Initializing a random tree T_node(ii) a And determining RRT algorithm parameters: target offset probability p, sampling pool capacity n, step length step and allowable error;

step two: joint angle space sampling

2.1 setting a target bias probability p and generating a random number rand between the interval ranges (o, 1);

2.2 if rand is less than or equal to p, no sampling is carried out, and the sampling point x is enabled_sampleIs equal to target point x_goal；

23 if rand > p Using Sobol sequence to generate n sample points x_samplePressing and targeting point x_goalThe distance of the Euclidean distance is arranged from small to large and enters a sampling pool;

step three: generating a new node

3.3 Collision detection, if collision is detected, reselecting the sampling point x_sampleGenerating a new node x_newIf no collision is detected, the new node x is determined_newStore to random Tree T_nodeAnd nearest node x_nearestTreated as a new node x_newA parent node of (a);

3.4 according to the updated random Tree T_nodeRepeating the second step to the third step untilThe latest node x obtained_newSatisfy | x_new-x_goalIf < error, the target point is found;

3.5 in random Tree T_nodeAccording to the parent-child relationship of each node, reverse searching is carried out to find a planned path;

step four: path optimization processing

And fitting the path obtained in the third step into a smooth curve by using a quintic polynomial fitting method based on a least square method, sampling points on the smooth curve and sending a control instruction through a serial port according to a communication protocol of the mechanical arm controller.

2. The method for planning a mechanical arm path by fusing a low variance sequence and an RRT algorithm according to claim 1, wherein: in the second step, the Sobol sequence is a more uniformly distributed sequence than the pseudo-random sequence and is a group based on { v } v_iThe sequence of (c) },

f(z)＝z^p+a₁z^p-1+…+a_p-1z+a_p

wherein

Representing binary bitwise XOR for v_iThe recursive formula of (c) is:

wherein

Indicating a rounding down.

Number i of the Sobol sequence was obtained:

… b therein₃b₂b₁Is a binary representation of i;

the sampling point x_sampleThe robot arm comprises joint angles of the robot arm, wherein each joint angle is generated by a Sobol sequence; by changing the coefficients of the polynomial f (z), different Sobol sequences are obtained; the obtained Sobol sequences are uniformly distributed in a (0,1) space; sample point x_sampleOne joint angle in (b) is denoted as min + (max-min) × x_iWhere min and max represent the upper and lower limits of the joint angle.

3. The method for planning a mechanical arm path by fusing a low variance sequence and an RRT algorithm according to claim 1, wherein: in the third step, the generated new node x is judged_newWhether the corresponding pose of the mechanical arm collides with the barrier or not; in case of collision with an obstacle, if the current sampling point x_sampleThe result is obtained in the step 2.2, and the step is switched to for resampling; if the current sampling point x_sampleIs obtained by step 23, discarding the sampling point and selecting the next sampling point x in the sampling pool in sequence_sampleAnd go to step three to generate a new node x_newUnless sampling point x in the sampling cell_sampleGenerated new node x_nearestThe sampling is carried out again when the requirements are not met.