CN113459086A

CN113459086A - Super-redundant mechanical arm path planning method

Info

Publication number: CN113459086A
Application number: CN202110593223.3A
Authority: CN
Inventors: 贾龙飞; 黄玉平; 傅捷; 郑继贵; 郭亚星; 陈婷
Original assignee: Beijing Research Institute of Precise Mechatronic Controls
Current assignee: Beijing Research Institute of Precise Mechatronic Controls
Priority date: 2021-05-28
Filing date: 2021-05-28
Publication date: 2021-10-01
Anticipated expiration: 2041-05-28
Also published as: CN113459086B

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_maxMaximum deflection angle psi of the arm rod during the movement of the ith arm rod end point along one of the straight paths_maxArm length L_iPath step length l_wThe 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

Super-redundant mechanical arm path planning method

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 under the condition of the joint slip angle limit so that the mechanical arm passes through a narrow space. 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_wIn 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_maxThe 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_iPath step length l_wThe 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_wTaking the initial point xinit of the path planning as a root node of the random tree T;

step 42: randomly selecting a node x_randSelecting a distance x_randNearest node x_nearAccording to node x_nearAnd target point x_goalA distance d between_goalDetermining Ψ_maxTaking the value of (A); then according to the relation and x in step 3_rand、x_nearT analysis x_newTo calculate x_newThe position of (a);

step 43: if x_newIf the condition requirement is met, i.e. the unreachable area determined in the step 2 is not entered, x is added_newNearby nodes deposit to x_neighborAnd storing ancestors of these nearby nodesIs put to x_parentIn this way, a group x is obtained_newArranging a Xin set;

step 44: find x in Xin_newNew father node x with minimum cost and meeting deflection angle requirement_parDelete x_newWith the original parent node x_nearConnecting line of (2), connecting x_newAnd x_parAnd pruning is carried out in T;

step 45: if x_neighborOne node x in_fromIs greater than the current x_newThe 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_newNode as x_fromA parent node of (a);

step 46: repeatedly searching for x_newAnd pruning x_newAnd (4) connection relation with nearby nodes until reaching the target point.

Preferably, the step 42 is implemented by:

s1, if x_nearIs a starting point x_initThen x is randomly selected between-60 DEG and 60 DEG_newIs performed, step S2, otherwise, at-min (Ψ)_max) To min (Ψ)_max) Randomly select x between_newStep S2 is performed; min (Ψ)_max) Is node x_nearAnd target point x_goalPsi determined at different distances between them_maxMinimum value of (d);

s2, judging whether the following inequality Abs (ang) is satisfied_rand-ang_near)≤Ψ_maxIf yes, then x is maintained_newDirection; if not, let Ψ_iIs a boundary value in the selectable range;

wherein x is_randAnd x_nearThe included angle of a line segment formed by connecting two points relative to the horizontal line is ang_randReading x in T_nearThe line segment connected with the father node of the node_near。

Preferably, Ψ is determined in step 42 by_maxThe value of (A) is as follows:

d_goal<l, then let Ψ_maxTaking a value according to the penultimateCombining the arm rods with the maximum included angle between two adjacent linear paths calculated by the relation in the step 3;

d_goalif greater than or equal to L, let Ψ_maxThe 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, Ψ_iFor boundary values in the selectable range, Ψ in the two-dimensional case_i＝ang_near+Sign(ang_rand-ang_near)·Ψ_max，x_newIs 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_newIs 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_iThe 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_iMaximum 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_iMaximum 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_endwhen 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_midwhen 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_parIn 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_newIs 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_newNode as x_fromThe 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_newTo x_newIs x and_newto x_fromIs 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_newSelecting 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_xyzThe body coordinate system of the base { O }₀And a body coordinate system of eight arms { O }₁}-{O₈}. 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_iYaw angle alpha of the ith arm_iAngle of pitch theta with i-th arm_iAnd (4) correlating. The driving rope passes through a coordinate system (O) relative to the arm lever body_iThe 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. All points on the surface of each arm are at a maximum distance of 75 from the axis of the arm, considering that the arm moves along a linear path, onlyTwo end points of the arm rod move along the path, and errors such as control errors and positioning errors exist in the motion process of the mechanical arm, so that 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_maxAll 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_iMaximum included angle psi of joint_maxPath step length l_wMaximum angle psi between two adjacent straight lines in the path_maxThe 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 line path, the moving process and the (i) th jointThe change in declination angle is shown in fig. 5.

ψ_iThere 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_endWhen 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_end1. Wherein N is_midWhen 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_mid2. Let l_w＝300、Ψ_max＝22.40°、L_i＝L_i-1486, 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、ψ_maxAt 40 °, Ψ_maxHow much value should be taken is shown in table 1.

Table 1 at different step sizesΨ_maxValue of (k is 0.1, 0.2 … 1)

It can be seen from the table that the smaller k is, the smaller Ψ_maxThe smaller, i.e. |_wThe smaller Ψ_maxThe smaller, and_wto Ψ_maxThere 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_initAs the root node of the random tree T. MDA-QRTT algorithm starts with a path x_initAs root node of the random tree T, node x in the tree_iStoring 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_iIs the second node, the 2 nd column is node x_iThe 3 rd column is the node x_iTo its parent node and then to the parent node up to x_initThe superimposed euclidean distance of (c). Always guarantee all data in V and E are at X_freeIn (1).

And 5: selecting an x by a random function_randSelecting a distance x_randNearest node x_nearThen in the Angle-MDA function according to the conclusion in step 3, according to x_rand、x_nearAnd T analysis x_newThe selection range of (1). When determining good psi_iI.e. x_newAfter the direction of (c), x can be calculated according to the following formula_newThe position of (a).

x_new＝x_near+δ·Ψ_i

Wherein the Angle-MDA function is according to x_rand、x_nearAnd T analysis x_newIs selected to ensureDefine the next discrete point x_newThe specific location of (a).

In this function, x is first determined_randAnd x_nearThe angle ang of the line segment formed by the two points relative to the horizontal line_randAnd reading x in T_nearThe angle ang of the line segment connected with the father node of the line segment relative to the horizontal line_near. The purpose of the algorithm is to ensure that the joint deflection angle is always within 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_nearAnd target point x_goalA distance d between_goalTo analyze Ψ_maxThe 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_iWhen the length of 486, i.e. the longest arm, is used, the distance between discrete point on the path and the target is less than L₇+L₈When in use, handle L_iThe length of the penultimate arm is counted as 206. According to the conclusion in step three, if d_goal<L₇+L₈Let us order Ψ_maxIf d is 40 deg., if d_goal≥L₇+L₈Let us order Ψ_max＝22.40°。

Maximum deviation angle psi between two adjacent straight lines in a well-defined path_maxThen, analysis x_newWhether the selectable range is exceeded. If x_nearIs a starting point x_initThen x is randomly selected between-60 DEG and 60 DEG_newIn the direction of (a). Otherwise, randomly selecting x between-22.40 degrees and 22.40 degrees_newThe direction of (a);

if the angle between two adjacent straight lines Abs (ang)_rand-ang_near)≤Ψ_maxThen x is maintained_newDirection of (2)_i＝ang_rand. If the angle between two adjacent straight lines Abs (ang)_rand-ang_near)>Ψ_maxLet us order Ψ_iBecoming 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_newThe selected point is unique and is a point on the boundary; in the three-dimensional caseThen, the included angle between two adjacent straight lines is ensured to be psi_max，x_newThe selected space is a space with the radius delta.sin (psi)_max) Circle of (a), x_newIs a point on the circle.

Step 6: if x_newComposite conditional requirements, i.e. x_newAnd x_nearWith no obstacle in between, then x_newNearby nodes deposit to X_neighborAnd storing the ancestors of the nearby nodes in X_parentIn this way, a group x is obtained_newFinished X_inAnd (4) collecting.

And 7: using ChooseParent-MDA function at X_inIs found in_newNew father node x with minimum cost and meeting deflection angle requirement_parDelete x_newWith the original parent node x_nearConnecting line of (2), connecting x_newAnd x_parAnd pruning is performed in T.

ChooseParent-MDA function content is as follows, at initialization minimum parent node x_minMinimum path σ_minMinimum cost c_minThen, if x_newIs at X_inNode x in (1)_inCost is reduced as a new parent node, and x_inAnd x_newThe path σ formed satisfies collision-free and satisfies from x_inTo x_inIs x and_into x_newIs not more than psi_maxThen node x is added_inIs set to x_newA new parent node. Wherein X is_inNot only include x_newNearby nodes also include ancestors of those nodes, thereby increasing the optimization speed of the algorithm.

And 8: if X_neighborOne node x in_fromIs greater than the current x_newThe 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_newNode as x_fromThe specific process of the parent node of (2) is as follows.

Not only pruning and x are needed in the MDA-QRTT algorithm_newThe related path also searches for X_neighborThe more optimal parent of all nodes in the cluster. In the functionAnalysis of X_neighborAll nodes x in_neiAnd x_newAnd x_newIs formed by parent nodes (i.e. analysis x)_neiAnd x_fromNew path formed), whether the cost is reduced and the conditions for no collision and angle constraints are met (i.e., satisfied from x)_newTo x_newIs x and_newto x_fromIs not more than psi_max. ). If so, x is added_neiIs updated to x_fromAnd pruning the path.

And step 9: repeatedly searching for x_newAnd pruning x_newAnd 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 the 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-redundant 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

1. A super-redundant mechanical arm path planning method is characterized by comprising the following steps:

and step 3: assume that adjacent arms are the same length and equal to the path step length l_wIn 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_maxMaximum deflection angle psi of the arm during movement of the ith arm initial point along one of the straight paths_maxArm length L_iPath step length l_wThe relationship between;

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 for planning the path of the super-redundant manipulator according to claim 1, wherein the step 4 adopts an MDA + RRT algorithm to plan the path, and is implemented by:

step 43: if x_newIf the condition requirement is met, i.e. the unreachable area determined in the step 2 is not entered, x is added_newNearby nodes deposit to x_neighborAnd storing the ancestors of the nearby nodes to x_parentIn this way, a group x is obtained_newArranging a Xin set;

4. The method according to claim 3, wherein the step 42 is implemented by:

s1, if x_nearIs a starting point x_initThen x is randomly selected between-60 DEG and 60 DEG_newIs performed, step S2, otherwise, at-min (Ψ)_max) To min (Ψ)_max) Randomly select x between_newStep S2 is performed; min (Ψ)_max) Is node x_nearAnd target point x_goalDetermined at different distances betweenΨ_maxMinimum value of (d);

5. The method of claim 3, wherein the determining Ψ in step 42 is performed by_maxThe value of (A) is as follows:

d_goal<l, then let Ψ_maxThe 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;

l is the sum of the lengths of the last two arms.

6. The method of claim 4, wherein Ψ is a method for planning the path of the super-redundant robotic arm_iFor boundary values in the selectable range, Ψ in the two-dimensional case_i＝ang_near+Sign(ang_rand-ang_near)·Ψ_max，x_newIs 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_newIs a point on the circle.

7. The method for planning the path of the super-redundant mechanical arm according to claim 1, 3 or 5, wherein the relationship in the step 3 is determined by:

8. The method for planning the path of the super-redundant mechanical arm according to claim 7, wherein: when k is 1, the relationship in step 3 is determined as follows:

N_endfor the i-th arm end point movementWhen moving to the end point of a section of straight path, a plurality of path turning points are arranged between the two end points of the arm rod;

10. The method as claimed in claim 3, wherein in step 44, a new parent node x is searched_parIn 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_newIs not more than psi_max(ii) a Xin is the node in the Xin set.

11. The method as claimed in claim 3, wherein the step 45 is executed if the current x is used as the x_newNode as x_fromThe 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_newTo x_newIs x and_newto x_fromIs not more than psi_max。

12. 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.

13. 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.