Inverse kinematics optimization method of SRS configuration seven-DOF manipulator based on arm angle parameters

Abstract

The present invention discloses an inverse kinematics optimization method of an SRS configuration seven-degree-of-freedom manipulator based on arm angle parameters, including: defining redundant parameters arm angle, reference plane and arm plane according to the SRS configuration seven-degree-of-freedom manipulator structure; solving the expressions of elbow joint angle θ4, shoulder joint angle θ1, θ2, θ3 and wrist joint angle θ5, θ6, θ7 of the SRS configuration seven-degree-of-freedom manipulator based on the reference plane and the arm plane; combining the limit of each joint angle of the SRS configuration seven-degree-of-freedom manipulator, solving the range of all feasible arm angles ψ of the manipulator under a given terminal posture. The beneficial effects of the present invention are: fast solution speed, obvious geometric meaning, suitable for configuration control tasks including obstacle avoidance, and to a certain extent, also improving the operability of the robot.

Description

Technical Field

[0001] The invention relates to the field of robot arm control, and in particular to an inverse kinematics optimization method for a seven-degree-of-freedom robot arm of SRS configuration based on arm angle parameters. Background Art

[0002] As the complexity of industrial automation tasks increases, traditional six-DOF industrial robots can no longer meet the needs of many operating scenarios. Therefore, in recent years, the use of seven-DOF robotic arms has become more and more widespread and gradually appeared in the public eye. Compared with six-DOF robotic arms, the main feature of seven-DOF robotic arms is "self-motion", that is, while keeping the end position unchanged, the joint configuration can still be switched, so it is more flexible and can achieve singularity avoidance, joint limit avoidance, and torque optimization.

[0003] Kinematics is the basis of manipulator control. For redundant manipulators, numerical solutions or analytical solutions can be used, but numerical solutions are generally slow and not conducive to real-time motion control of manipulators. The analytical solution using fixed joints does not fully utilize the redundancy of the seven-degree-of-freedom manipulator, which violates its original design intention. For SRS (spherical-rotating-spherical) manipulators with special geometric configurations, the inverse kinematics solution based on the arm angle can be solved. The geometric meaning is obvious, suitable for configuration control tasks, and fully utilizes the redundancy of the seven-degree-of-freedom manipulator. In order to improve the maneuverability of the manipulator, in the continuous trajectory planning of the manipulator, each joint should be as far away from the joint limit as possible. Therefore, it is necessary to select the optimal feasible arm angle to optimize the inverse kinematics to avoid the joint limit. Summary of the invention

[0004] The main technical problem solved by the present invention is to solve the problem of selecting the optimal joint configuration of an SRS-configured seven-degree-of-freedom manipulator with the optimization goal of avoiding joint limits. In order to solve the above technical problems, the technical solution of the present invention provides an inverse kinematics optimization method for an SRS-configured seven-degree-of-freedom manipulator based on arm angle parameters. First, by defining redundant parameters of the arm angle, reference plane and arm plane, the parameter expressions of the elbow joint, shoulder joint and wrist joint about the arm angle are given, and combined with the limits of each joint of the manipulator, all feasible arm angle ranges of the manipulator under a given terminal posture are solved, and the selection of the effective arm angle when the algorithm is singular is considered. For Cartesian space trajectory planning, the concept of virtual force is introduced, and on this basis, the objective function of joint limit optimization is defined. The unique optimal feasible arm angle is solved by the arm angle of the previous path point and the terminal posture of the path point, which is used to obtain the unique joint configuration under a given terminal posture.

[0005] The present invention provides an inverse kinematics optimization method for an SRS configuration seven-degree-of-freedom manipulator based on arm angle parameters, comprising the following steps:

[0006] S101: Define redundant parameters of arm angle, reference plane and arm plane according to the SRS configuration seven-DOF manipulator structure;

[0007] S102: Based on the reference plane and the arm plane, solve the elbow joint angle θ of the SRS configuration seven-DOF manipulator 4 、Shoulder joint angle θ 1 ,θ 2 ,θ 3 and wrist angle θ 5 ,θ 6 ,θ 7 Expression of

[0008] S103: Combined with the limits of the joint angles of the SRS-configured seven-DOF manipulator, the range of all feasible arm angles ψ of the manipulator under a given end position is solved.

[0009] Further, in step S101, the reference plane is composed of the highest point of the elbow E, shoulder S and wrist W of the robotic arm; the arm plane changes with the movement of the robotic arm, and the current arm plane is composed of the current elbow E, shoulder S and wrist W of the robotic arm; wherein, the shoulder S is the intersection of the first three joint rotation axes of the robotic arm; and the wrist W is the intersection of the last three joint rotation axes of the robotic arm.

[0010] Furthermore, the elbow joint angle θ 4 The expression is as follows:

[0011]

[0012] Where acos() represents the arccosine function; d SE Indicates the distance from the shoulder S to the elbow E; d EW represents the distance from elbow E to wrist W; p SW =[-d WT *ax+px-d WT *ay+py-d WT *az+pz-d BS ] T , terminal pose d WT Represents the distance between the wrist W and the origin T of the last joint angle coordinate system; d BS Indicates the distance between the shoulder S and the origin B of the robot base coordinate system.

[0013] Furthermore, the shoulder joint angle θ 1 ,θ 2 ,θ 3 The expression is as follows:

[0014] cosθ 2 =-A s (3,2)sinψ-Bs (3,2)cosψ-C s (3,2)

[0015]

[0016]

[0017] In the formula, is the rotation matrix of the first three joints when the arm angle ψ is 0 and the elbow E is at the highest point; uSW is the unit vector corresponding to the vector formed by the shoulder S and the wrist W; [uSW×] is the antisymmetric matrix of uSW; A s (i,j),B s (i,j),C s (i,j) represent the matrix A respectively s , B s , C s The element in the i-th row and j-th column of .

[0018] Furthermore, the wrist joint angle θ 5 ,θ 6 ,θ 7 The expression is as follows:

[0019] cosθ 6 =A w (3,3)sinψ+B w (3,3)cosψ+C w (3,3)

[0020]

[0021]

[0022] Where:

[0023]

[0024]

[0025]

[0026]

[0027]

[0028]

[0029]

[0030] where cosθ i c i, sinθ i For i , * indicates ignored items.

[0031] Furthermore, in step S103, the range of all feasible arm angles ψ of the robot arm under a given end position is solved as follows:

[0032] Joint angle θ 4 and θ 7 The corresponding feasible arm angle ψ interval is [-pi,pi];

[0033] Joint angle θ 2 and θ 6 The feasible arm angle ψ is solved as follows: Solution The effective range of ψ is as follows:

[0034] When Z min >1 or Z max <-1, there is no effective arm angle at this time, that is

[0035] When Z min <-1 and -1<=Z max <=1, at this time

[0036] When Z min <-1 and Z max >1, then ψ∈[-pi,pi];

[0037] When -1<=Z min <1 and -1<=Z max <=1, at this time

[0038]

[0039] When -1<=Z min <=1 and Z max >1, at this time

[0040] in a, b, c are the coefficients of joint angles 2 and 6: cosθ i =a sinψ+b cosψ+c.

[0041] Furthermore, the joint angle θ 1 ,θ 3 ,θ 5 The feasible arm angle ψ is solved as follows:

[0042]

[0043] when a t2 +b t 2 -c t 2 When <0, the upper and lower ranges of the arm angle are solved according to the upper and lower limits of the joint angle, except for the following two cases:

[0044] If c t >0, the effective range of arm angle ψ is

[0045] If c t <0, the effective range of arm angle is

[0046] when a t 2 +b t 2 -c t 2 When >0, the arm angles corresponding to the upper and lower limits of the joint angle θ are as follows:

[0047]

[0048]

[0049] If θ min >θ u orθ max <θ l , at this time there is no effective arm angle,

[0050] If θ l ≤θ min ≤θ u andθ l ≤θ max ≤θ u , the feasible arm angle ψ range is [-pi,pi];

[0051] If θ min <θ l andθ l ≤θ max ≤θ u , further classified, if θ l >atan2(c n -b n ,c d -b d ) At this time, the feasible arm angle ψ range is If θ l ≤atan2(c n -b n ,c d -b d ), the feasible arm angle ψ range is

[0052] If θ l ≤θ min ≤θ u andθ max >θ u , further classified, if θ u >atan2(c n -b n ,c d -b d ), the feasible arm angle ψ range is If θ u ≤atan2(c n -b n ,c d -b d ), the feasible arm angle ψ range is

[0053] If θ min <θ l andθ max >θ u , according to θ l and θ u Further classification by position, if θ l ≤atan2(c n -b n ,c d -b d )andθ u ≤atan2(c n -b n ,c d -b d ), the feasible arm angle ψ range is If θ l ≤atan2(c n -b n ,c d -b d )andθ u >atan2(c n -b n ,c d -b d ), if the trend of θ with respect to ψ is first maximum and then minimum, then the range of feasible arm angle ψ is:

[0054] Otherwise, the feasible arm angle ψ range is:

[0055]

[0056] If θ l >atan2(c n-b n ,c d -b d ), then the feasible arm angle ψ range is

[0057] when The range of feasible arm angle ψ is solved by singular method.

[0058] After the range of the feasible arm angle ψ is solved in step S103, the optimal feasible solution for the robot arm path planning is solved by combining the range of the feasible arm angle ψ through the virtual force weight method.

[0059] Compared with the prior art, the beneficial effects of the present invention include: 1. The analytical inverse kinematics solution method of the arm angle parameters is adopted, which has a fast solution speed and obvious geometric meaning, and is suitable for configuration control tasks including obstacle avoidance. 2. For any given end posture, all feasible arm angles are solved when the joint limits are known, and a method for processing the effective arm angle when the algorithm singularity occurs is given. 3. By defining an objective function with virtual force as weight to optimize the joint limit, the optimal arm angle that makes each joint as far away from the joint limit as possible can be obtained, which also improves the operability of the robot to a certain extent. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] Figure 1 It is a flow chart of an inverse kinematics optimization method of an SRS configuration seven-degree-of-freedom manipulator based on arm angle parameters provided by the present invention;

[0061] Figure 2 It is a schematic diagram describing the self-motion;

[0062] Figure 3 is a reference plane schematic diagram;

[0063] Figure 4 is a t 2 +b t 2 -c t 2 Schematic diagram when <0;

[0064] Figure 5 is a t 2 +b t 2 -c t 2 Schematic diagram of the special case of solving the arm angle range when <0.

[0065] Figure 6 is a t 2 +b t 2 -ct 2 >0 solution diagram;

[0066] Figure 7 It is c t >0 and lim left >lim right Schematic diagram of the relationship between joint angle and arm angle;

[0067] Figure 8 is θ - Schematic diagram of arm angle solution ≥0;

[0068] Fig. 9 is θ - Schematic diagram for solving arm angle ≤0;

[0069] Fig.10 It is c t <0andlim left <lim right Schematic diagram of arm angle solution;

[0070] Fig.11 It is c t <0 and θ - Schematic diagram of the arm angle solution ≥0;

[0071] Fig.12 It is c t <0 and θ - Schematic diagram of the arm angle solution of ≤0;

[0072] Fig.13 It is a schematic diagram of the simulation of the end trajectory of the robotic arm;

[0073] Fig.14 It is a schematic diagram of arm angle change;

[0074] Fig.15 It is a schematic diagram of joint angle changes. DETAILED DESCRIPTION

[0075] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0076] The present invention provides an inverse kinematics optimization method for an SRS-configured seven-DOF manipulator based on arm angle parameters. Figure 1 , Figure 1 is a flow chart of the method of the present invention.

[0077] The method comprises the following steps:

[0078] S101: Define redundant parameters of arm angle, reference plane and arm plane according to the SRS configuration seven-DOF manipulator structure;

[0079] As an embodiment, the robot arm involved in the present invention is a seven-degree-of-freedom SRS configuration robot arm, and the robot arm DH parameters are shown in the following Table 1:

[0080] Table 1 SRS configuration seven-DOF manipulator model

[0081]

[0082] The seven-degree-of-freedom manipulator is a redundant manipulator, that is, the manipulator's degrees of freedom exceed the degrees of freedom required for the operating space, so it has the "self-motion" characteristic, that is, the joint configuration can still be switched when the end position remains unchanged. The special feature of the SRS configuration seven-degree-of-freedom manipulator is that its self-motion is reflected in the elbow being able to make a circular motion around the line connecting the shoulder and wrist when the end position remains unchanged. The self-motion description is as follows Figure 2 shown. Figure 2 It is a schematic diagram describing the self-motion.

[0083] Figure 2 In the figure, B is the base, i.e. the origin of the coordinate system 0; S is the shoulder, i.e. the intersection of the first three joint rotation axes; E is the elbow, i.e. the origin of the coordinate system 4; W is the wrist, i.e. the intersection of the last three joint rotation axes; T is the origin of the coordinate system 7. The plane formed by the highest point of the elbow, S, and W is taken as the reference plane, and the plane formed by the current elbow E, S, and W is the current arm plane.

[0084] The arm angle ψ is thus defined as the right-hand screw rule around the reference plane about the vector p SW The angle required to turn to the current arm plane. The valid value range of the arm angle is [-pi,pi].

[0085] Given the end pose and the arm angle ψ, the rotation matrix corresponding to the end position is

[0086] S102: Based on the reference plane and the arm plane, solve the elbow joint angle θ of the SRS configuration seven-DOF manipulator 4 、Shoulder joint angle θ 1 ,θ 2 ,θ 3 and wrist angle θ 5 ,θ 6 ,θ 7 Expression of

[0087] As an example, solving the elbow joint angle θ 4 The process is as follows:

[0088] Translate the end position d in the opposite direction of z7 WTThe wrist position can be reached, so

[0089]

[0090] p W =[-d WT *ax+px -d WT *ay+py -d WT *az+pz] T (2)

[0091] p SW =[-d WT *ax+px -d WT *ay+py -d WT *az+pz-d BS ] T (3)

[0092] So by Figure 2 The geometric relationship is known

[0093]

[0094] The elbow joint angle can be obtained

[0095]

[0096] Where acos() represents the arccosine function; d SE Indicates the distance from the shoulder S to the elbow E; d EW Indicates the distance from elbow E to wrist W; d WT Represents the distance between the wrist W and the origin T of the last joint angle coordinate system; d BS Indicates the distance between the shoulder S and the origin B of the robot base coordinate system;

[0097] As an embodiment, the shoulder joint angle θ 1 ,θ 2 ,θ 3 The specific process of solving the expression is as follows:

[0098] Given Rodriguez as follows

[0099] R(k,θ)=I+sinθ*[k×]+(1-cosθ)[k×] 2 ,k=[k 1 k 2 k 3 ] T (6)

[0100] Where I is the unit matrix, [k×] is the antisymmetric matrix of k, and this formula represents the rotation matrix obtained by rotating θ around any vector k in three-dimensional space. The reference plane diagram is given as follows Figure 3 As shown, Figure 3 The reference plane diagram is shown in Figure 1. According to the construction rules of the DH coordinate system:

[0101] p SW +y3*d SE =z4*d EW

[0102] For the convenience of calculation, let cosθ i c i , sinθ i For i , from the kinematic formula we know

[0103]

[0104] so

[0105]

[0106] Let the arm angle be 0, that is, when the elbow is at the highest point, the rotation matrices of the first three joints are So we can get

[0107]

[0108] Substituting equation (10) into equation (7), we can get

[0109]

[0110] The first three joint rotation matrices are obtained by self-motion at any arm angle, that is, the first three joint rotation matrices change due to the circular motion of the robot elbow, so we can get

[0111]

[0112] Substitute the above formula into the Rodriguez formula, so:

[0113]

[0114]

[0115] in:

[0116]

[0117] From direct kinematics we know that

[0118]

[0119] Where * means ignoring this item. Combining equation (16) and equation (13), and comparing the matrix elements, we can get

[0120]

[0121] In formula (17), is the rotation matrix of the first three joints when the arm angle ψ is 0 and the elbow E is at the highest point; uSW is the unit vector corresponding to the vector formed by the shoulder S and the wrist W; [uSW×] is the antisymmetric matrix of uSW; A s (i,j),B s (i,j),C s (i,j) represent the matrix A respectively s , B s , C s The element in the i-th row and j-th column of .

[0122] As an embodiment, the wrist joint angle θ 5 ,θ 6 ,θ 7 The specific process of solving the expression is as follows:

[0123] Since rotation matrices are all orthogonal matrices,

[0124]

[0125] therefore

[0126]

[0127] in

[0128]

[0129] The rotation matrices of the last three joints can be obtained from the forward kinematics formula:

[0130]

[0131] The * means ignoring this item. Similarly, by comparing the matrix elements, we can see that:

[0132]

[0133] S103: Combined with the limits of the joint angles of the SRS-configured seven-DOF manipulator, the range of all feasible arm angles ψ of the manipulator under a given end position is solved.

[0134] As an embodiment, the present invention solves the effective arm angle interval as follows.

[0135] Any mechanical system has mechanical inaccessibility. When designing the joints of a robotic arm, it is impossible to make the range of motion of each joint reach ±360°. On the one hand, it is limited by the mechanical structure, and on the other hand, it is to prevent the robotic arm from self-collision. Therefore, each joint has a joint limit constraint. For each given end position, the SRS configuration seven-degree-of-freedom robotic arm may not be able to complete a complete circular motion when it moves by itself, because the joint angle corresponding to its arm angle may exceed the joint limit. Therefore, the corresponding effective arm angle range must be solved according to each joint limit, so that an effective joint configuration can be selected to achieve a given end position. Because θ 4 The size is independent of the arm angle and θ 7 The range of motion is ±360°, so θ 4 and θ 7 The corresponding effective arm angle interval is [-pi,pi].

[0136] From the inverse kinematics solution, we know that the relationship between the joint angle and the arm angle can be divided into two types: cos type and tan type. Therefore, the effective arm angle ranges of these two cases are discussed below.

[0137] (1) cos joint angle

[0138] The cos joint angle mainly includes joints 2 and 6, and its format is as follows

[0139]

[0140] remember Let θ i The value range is Z i The value range is [Z min ,Z max ], considering that the range of motion of joints 2 and 6 is [-120°, 120°]. Therefore, the problem of solving the effective arm angle of the cosine joint angle is transformed into known Solution The effective range of ψ is , so the following cases are discussed separately:

[0141] When Z min >1 or Z max <-1, there is no effective arm angle at this time, that is

[0142] When Z min <-1 and -1<=Z max <=1, at this time

[0143] When Z min <-1 and Z max>1, then ψ∈[-pi,pi];

[0144] When -1<=Z min <1 and -1<=Z max <=1, at this time

[0145]

[0146] When -1<=Z min <=1 and Z max >1, at this time

[0147] in

[0148] 2) Tan joint angle

[0149] Because the range of motion of joint 7 is ±360°, only joints 1, 3, and 5 are considered here. The general format is as follows:

[0150]

[0151] By taking the derivative of both sides of the above equation with respect to ψ, we can obtain:

[0152]

[0153] in There are two cases where θ is monotonic or periodic with respect to ψ. Let the derivative be 0 and solve for the corresponding arm angle.

[0154]

[0155] when a t 2 +b t 2 -c t 2 <0, c t >0, equation (25) is always greater than 0, so θ is positively correlated with ψ, c t When <0, equation (25) is always less than 0, so θ is negatively correlated with ψ. In summary, θ is monotonic with respect to ψ, that is, θ and ψ correspond one to one.

[0156] When solving ψ based on θ, two solutions may appear. Therefore, it is necessary to determine the correctness of the signs of sinθ and cosθ based on ψ and θ and then remove invalid solutions. However, due to the particularity of the tangent function, the function of θ with respect to ψ may have jump discontinuities. In the arm angle range [-pi, pi], if θ reaches ±pi, a jump of 2*pi will occur.

[0157] Therefore a t 2 +b t2 -c t 2 <0 can be divided into the following situations: Please refer to Figure 4 , Figure 4 is a t 2 +b t 2 -c t 2 Schematic diagram when <0.

[0158] Through analysis, we can know that there are two cases that need special discussion. The upper and lower limits of the joint angle are shown by the blue dotted line. The effective arm angle range of other cases can be solved according to the monotonic processing method.

[0159] Please refer to Figure 5 , Figure 5 is a t 2 +b t 2 -c t 2 Schematic diagram of the special case of solving the arm angle range when <0.

[0160] against Figure 5 For the special case of , we give the corresponding arm angle range as follows:

[0161] When c t >0, the effective range of arm angle is

[0162] When c t <0, the effective range of arm angle is

[0163] In a t 2 +b t 2 -c t 2 >0 (a t 2 +b t 2 -c t 2 When θ is equal to 0, it is a singular case, which will be explained later). At this time, there are two stationary points, one of which makes θ reach a maximum value and the other makes θ reach a minimum value. We define the maximum value of θ as θ max , and its corresponding arm angle is ψ max ; Define the minimum value of θ as θ min , and its corresponding arm angle is ψ min .therefore By taking the derivative of both sides of equation (25) with respect to ψ, we can analyze that ψ min,maxThe corresponding global maximum and global minimum values ​​of θ are the same when ψ=±pi. Therefore, a t 2 +b t 2 -c t 2 When >0, it can be divided into the following two cases: Please refer to Figure 6 , Figure 6 is a t 2 +b t 2 -c t 2 >0 solution diagram;

[0164] First, the arm angles corresponding to the upper and lower limits of θ are given as follows

[0165] Let’s start the classification discussion:

[0166] (I)θ min >θ u orθ max <θ l , no effective arm angle;

[0167] (II)θ l ≤θ min ≤θ u andθ l ≤θ max ≤θ u , the feasible arm angle range is [-pi,pi];

[0168] (III)θ min <θ l andθ l ≤θ max ≤θ u , here we need to discuss classification, that is

[0169] [1]θ l >atan2(c n -b n ,c d -b d ), the effective range of arm angle is

[0170] [2]θ l ≤atan2(c n -b n ,c d -b d ), the effective range of arm angle is

[0171] (IV)θ l ≤θ min ≤θ u andθ max >θ u , here we need to discuss classification, that is

[0172] [1]θ u >atan2(c n -b n ,c d -b d ), the effective range of arm angle is

[0173] [2]θ u ≤atan2(c n -b n ,c d -b d ), the effective range of arm angle is

[0174] (V)θ min <θ l andθ max >θ u

[0175] According to θ l and θ u The different positions can be divided into the following situations:

[0176] [1]θ l ≤atan2(c n -b n ,c d -b d )andθ u ≤atan2(c n -b n ,c d -b d ), the effective arm angle range is:

[0177]

[0178] [2]θ l ≤atan2(c n -b n ,c d -b d )andθ u >atan2(c n -b n ,c d -b d ), if the trend of θ changing with respect to ψ is first maximum and then minimum, then the effective range of the arm angle is:

[0179]

[0180] Otherwise the effective arm angle range is:

[0181]

[0182] [3]θ l >atan2(c n -b n ,c d -b d ), the effective arm angle range is:

[0183]

[0184] In step S103, when solving the range of all feasible arm angles ψ of the robot arm under a given end posture, a singular processing method is also used for solving.

[0185] As an embodiment, the algorithm singularity problem existing in cos-type joint angles and tan-type joint angles is discussed below.

[0186] (1) The cosine joint angle algorithm is singular

[0187] If a=b=0, then cosθ i =c, the joint angle θ is a constant, so the corresponding effective arm angle range is [-pi,pi].

[0188] (2) The tan joint angle algorithm is singular

[0189] f d When (ψ) = 0, the algorithm will be singular. t 2 +b t 2 -c t 2 = 0, there is only one stationary point Now we can prove that f n (ψ) = f d (ψ)=0, the size of θ cannot be determined, and the algorithm becomes singular. It can be proved that except for the singular point ψ 0 Beyond The sign does not change, that is, θ is either positively or negatively correlated with ψ. Substituting ψ 0 +δ to equation (24) to obtain

[0190]

[0191] Find the left and right limits of the singular point

[0192]

[0193] It is easy to know that the difference between the two corresponding θ is pi. Before the following discussion, we need to know the distribution of the arm angle interval corresponding to the joint limit. Let The lower limit corresponds to the interval [-pi,ψ 0 ] arm angle, The lower limit corresponds to the interval [ψ 0 ,pi] arm angle; let The upper limit corresponds to the interval [-pi,ψ 0 ] arm angle, The upper limit corresponds to the interval [ψ 0 ,pi], and let θ-=atan2(c n -b n ,c d -b d ). Let’s start the classification discussion.

[0194] (a)c t =0

[0195] At this time a t =b t =0, so That is, θ does not change with ψ, so the effective arm angle range is [-pi,pi].

[0196] (b)c t >0

[0197] θ is positively correlated with ψ, that is, the function is monotonically increasing except for the singular points.

[0198] (I)lim left >lim right , please refer to Figure 7 As shown, Figure 7 It is c t >0 and lim left >lim right Schematic diagram of the relationship between joint angle and arm angle;

[0199] [1]θ u <lim right orθ l >lim left , no effective arm angle;

[0200] [2]θ l <lim right andlim right ≤θ u <θ - , the effective arm angle range is

[0201] [3]θl <lim right andθ - ≤θ u ≤lim left , the effective arm angle range is

[0202] [4]θ l <lim right andθ u >lim left , the effective arm angle range is [-pi,pi];

[0203] [5]θ l ≥lim right andθ u <θ - , the effective arm angle range is

[0204] [6]lim right ≤θ l <θ - andθ - ≤θ u ≤lim left , the effective arm angle range is

[0205] [7]lim right ≤θ l <θ - andθ u >lim left , the effective arm angle range is [8]θ-≤θ l andθ u ≤lim left , the effective arm angle range is

[0206] [9]θ-≤θ l ≤lim left andθ u >lim left , the effective arm angle range is

[0207] (II)lim left <lim right

[0208] [1]θ - ≥0, please refer to Figure 8 , Figure 8 It is a schematic diagram for solving θ-≥0.

[0209] A)lim left ≤θ u≤lim right , the effective arm angle range is

[0210] B)lim right ≤θ u ≤θ - andθ l ≤lim left , the effective arm angle range is

[0211] C)lim right ≤θ u ≤θ - andlim left ≤θ l ≤lim right , the effective arm angle range is

[0212] D)θ - <θ u andθ l <lim left , the effective arm angle range is

[0213] E)θ - <θ u and lim left ≤θ l , the effective arm angle range is (ψ 0 ,pi];

[0214] [2]θ - ≤0, please refer to Fig. 9 , Fig. 9 It is a schematic diagram for solving the arm angle when θ-≤0;

[0215] A)θ l <θ - and lim right ≤θ u ≤lim left , the effective arm angle range is (ψ 0 ,pi];

[0216] B)θ - ≤θ l <lim right and lim right ≤θ u ≤lim left , the effective arm angle range is

[0217] C)θ l <θ - andθ u >limleft , the effective arm angle range is

[0218] D)θ - ≤θ l <lim right andθ u >lim left , the effective arm angle range is

[0219] E)lim right ≤θ l ≤lim left andθ u >lim left , the effective arm angle range is

[0220] (c)c t <0, θ is negatively correlated with ψ, i.e., monotonically decreasing except for the singular points;

[0221] (I)lim left <lim right , please refer to Fig.10 , Fig.10 It is c t <0andlim left <lim right Schematic diagram of arm angle solution;

[0222] A)θ u <lim left orθ l >lim right No effective arm angle;

[0223] B)θ l <lim left and lim left ≤θ u <θ-, the effective arm angle range is

[0224] C)θ l <lim left andθ - ≤θ u ≤lim right , the effective arm angle range is

[0225] D)θ l <lim left andθ u >lim right , the effective arm angle range is [-pi,pi];

[0226] E)θl ≥lim left andθ u <θ - , the effective arm angle range is

[0227] F)lim left ≤θ l <θ - andθ - ≤θ u ≤lim right , the effective arm angle range is

[0228] G)lim left ≤θ l <θ-andθ u >lim right , the effective arm angle range is

[0229] H)θ - ≤θ l andθ u ≤lim right , the effective arm angle range is

[0230] I)θ - ≤θ l ≤lim right andθ u >lim right , the effective arm angle range is

[0231] (II)lim left >lim right

[0232] [1]θ - ≥0, please refer to Fig.11 , Fig.11 It is c t Schematic diagram of arm angle solution when θ<0 and θ≥0;

[0233] A)lim right ≤θ u ≤lim left andθ l <lim right , the effective arm angle range is

[0234] B)lim left <θ u ≤θ - andθ l <lim right, the effective arm angle range is

[0235] C)lim left <θ u ≤θ - and lim right ≤θ l ≤lim left , the effective arm angle range is

[0236] D)θ u >θ - andθ l <lim right , the effective arm angle range is

[0237] E)θ u >θ - andlim right ≤θ l ≤lim left , the effective arm angle range is [-pi,ψ 0 );

[0238] [2]θ - ≤0, please refer to Fig.12 , Fig.12 It is c t Schematic diagram of arm angle solution for θ<0 and θ-≤0;

[0239] A)θ l <θ - andlim right ≤θ u ≤lim left , the effective arm angle range is (ψ 0 ,pi];

[0240] B)θ - ≤θ l <lim right andlim right ≤θ u ≤lim left , the effective arm angle range is

[0241] C)θ l <θ - andθ u >lim left , the effective arm angle range is

[0242] D)θ-≤θ l <lim right andθ u>lim left , the effective arm angle range is

[0243] E)lim right ≤θ l ≤lim left andθ u >lim left , the effective arm angle range is

[0244] After the range of the feasible arm angle ψ is solved in step S103, the optimal feasible solution for the robot arm path planning is solved by combining the range of the feasible arm angle ψ through the virtual force weight method.

[0245] As an embodiment, when the robot is performing continuous trajectory planning, each joint must be far away from the joint limit to have a larger operating space. The SRS seven-degree-of-freedom redundant robot may still have countless sets of joint configurations under a given end posture, so it is necessary to select the optimal arm angle to make each joint as far away from the joint limit as possible. The robot joint limits discussed in this patent are shown in the following table:

[0246] Table 2 Robot arm joint limits

[0247]

[0248]

[0249] The range of motion of each joint of the robot arm is different, so it needs to be normalized. It is used to measure the distance between the joint angle and the joint limit. The concept of safe area is given. The safe area is 80% of the range of motion of each joint. The range of motion of joint 7 is ±360°, so there is no need to consider the safe area. Therefore, the safe areas of each joint except joint 7 are shown in the following table:

[0250] Table 3 Safety area of ​​each joint of the robot arm

[0251]

[0252] Introduce the concept of virtual force and give the expression of virtual force g(x)

[0253]

[0254] We hope that the virtual force changes slowly within 80% to 90% of the joint's movable range, and changes quickly between 90% and 100%. Combined with the concept of the safe area, let a = 0.8, b = 1. From the expression of the virtual force, it can be seen that when the joint is in the safe area, there is no virtual force. The closer the joint is to the joint limit, the greater the virtual force. In continuous trajectory planning, if there is a joint angle at a path point that is not in the safe area, the arm angle should be adjusted so that the corresponding joint of the next path point no longer continues to approach the joint limit, so that the joint angles of adjacent path points are as consistent as possible. Therefore, using the virtual force as the weight, the optimization objective function is given as follows:

[0255]

[0256] From the inverse kinematics solution process, we know that the relationship between the arm angle and the joint angle is very complicated. In order to simplify the calculation, we believe that within a small arm angle change Δψ, the relationship between the arm angle and the joint angle is linear. The linear relationship is as follows:

[0257]

[0258] The subscripts k and (k+1) represent path points k and (k+1) respectively. Substituting the simplified relationship between the arm angle and the joint angle into the optimization objective function, we can get the new optimization objective function

[0259] It can be concluded that f(ψ k+1 ) is ψ k+1 The quadratic function of the arm angle should be the extreme point of the quadratic function when the objective function takes the minimum value. When the extreme point of the quadratic function does not belong to the effective arm angle, it needs to be processed separately. If the effective arm angle closest to the extreme point is selected as the optimal arm angle, the joint angle corresponding to the arm angle may exceed the joint limit due to calculation errors, and the interval endpoint generally causes a joint to reach the joint limit. Based on the above considerations, the optimal arm angle is taken within the effective arm angle interval closest to the extreme point of the quadratic function, and the distance from the optimal arm angle to the endpoint of the effective arm angle interval closest to the extreme point is 30% of the length of the effective arm angle interval.

[0260] Please refer to Fig.13 , Fig.13 It is a simulation diagram of the end trajectory of the robot arm. The simulation verifies the effectiveness of the optimization algorithm. Three straight line trajectories are considered as the end trajectory of the robot arm, that is, moving from point (0.6, 0, 0.4) to point (0.6, -0.5, 0.4) and then to (-0.3, -0.5, 0.4) and finally arriving at (-0.3, -0.5, 0.6).

[0261] Given an initial pose The arm angle corresponding to the initial path point is -75°, and the joint configuration corresponding to the initial path point is [42.20° 61.95° -94.34° 78.47° 63.05° 99.18° -39.56°]. The posture of the robotic arm remains unchanged during the movement.

[0262] The simulation results are given below, where the dotted lines represent the unoptimized arm angles and joint angles, and the solid lines represent the arm angles and joint angles optimized using this optimization algorithm. Fig.14 and Fig.15 ; Fig.14 It is a schematic diagram of arm angle change; Fig.15 It is a schematic diagram of joint angle changes;

[0263] From the final simulation results, it is known that during the movement of the robot arm, only joints 6 and 7 enter the danger zone. If the arm angle is not adjusted, joint 6 will continue to approach the limit during the movement, and finally reach 116.79°. After adopting this optimization algorithm, joint 6 no longer approaches the limit, and finally only reaches 100°. For joint 7, the optimization algorithm can make the joint finally enter the safe zone. The optimization algorithm proposed in this patent realizes the avoidance of joint limits to a certain extent and improves the operability of the robot arm.

[0264] It should be understood that the size of the serial numbers of the steps in the above embodiments does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present application.

[0265] The beneficial effects of the present invention are: 1. The analytical inverse kinematics solution method of the arm angle parameters is adopted, which has a fast solution speed and obvious geometric meaning, and is suitable for configuration control tasks including obstacle avoidance. 2. For any given end posture, all feasible arm angles are solved when the joint limits are known, and a method for processing the effective arm angle when the algorithm singularity occurs is given. 3. By defining an objective function with virtual force as weight to optimize the joint limit, the optimal arm angle that makes each joint as far away from the joint limit as possible can be obtained, which also improves the operability of the robot to a certain extent.

[0266] The specific implementation of the present invention described above does not constitute a limitation on the protection scope of the present invention. Any other corresponding changes and modifications made based on the technical concept of the present invention should be included in the protection scope of the claims of the present invention.

Claims

1. An inverse kinematics optimization method for an SRS configuration seven-DOF manipulator based on arm angle parameters, characterized in that: The following steps are involved: S101: Define redundant parameters of arm angle, reference plane and arm plane according to the SRS configuration seven-DOF manipulator structure; S102: Based on the reference plane and the arm plane, solve the expressions of the elbow joint angle θ4, shoulder joint angles θ1, θ2, θ3 and wrist joint angles θ5, θ6, θ7 of the SRS configuration seven-degree-of-freedom manipulator; S103: Combined with the limits of the joint angles of the SRS-configured seven-DOF manipulator, the range of all feasible arm angles ψ of the manipulator under a given terminal posture is solved; In step S103, the range of all feasible arm angles ψ of the robot arm under a given end position is solved as follows: The feasible arm angle ψ intervals corresponding to joint angles θ4 and θ7 are both [-pi, pi]; The feasible arm angle ψ of the joint angles θ2 and θ6 is solved as follows: Solution The effective range of ψ is as follows: When Z min >1 or Z max <-1, there is no effective arm angle at this time, that is When Z min <-1 and -1<=Z max <=1, at this time When Z min <-1 and Z max >1, then ψ∈[-pi,pi]; When -1<=Z min <1 and -1<=Z max <=1, at this time When -1<=Z min <=1 and Z max >1, at this time in a, b, c are the coefficients of joint angles 2 and 6: cosθ i =asinψ+bcosψ+c; The feasible arm angle ψ of the joint angles θ1, θ3, and θ5 is solved as follows: when a t 2 +b t 2 -c t 2 When <0, the upper and lower ranges of the arm angle are solved according to the upper and lower limits of the joint angle, except for the following two cases: If c t >0, the effective range of arm angle ψ is If c t <0, the effective range of arm angle is when a t 2 +b t 2 -c t 2 When >0, the arm angles corresponding to the upper and lower limits of the joint angle θ are as follows: If θ min >θ u orθ max <θ l , at this time there is no effective arm angle, If θ l ≤θ min ≤θ u andθ l ≤θ max ≤θ u , the feasible arm angle ψ range is [-pi,pi]; If θ min <θ l andθ l ≤θ max ≤θ u , further classified, if θ l >atan2(c n -b n ,c d -b d ) At this time, the feasible arm angle ψ range is If θ l ≤atan2(c n -b n ,c d -b d ), the feasible arm angle ψ range is If θ l ≤θ min ≤θ u andθ max >θ u , further classified, if θ u >atan2(c n -b n ,c d -b d ), the feasible arm angle ψ range is If θ u ≤atan2(c n -b n ,c d -b d ), the feasible arm angle ψ range is If θ min <θ l andθ max >θ u , according to θ l and θ u Further classification by position, if θ l ≤atan2(c n -b n ,c d -b d )andθ u ≤atan2(c n -b n ,c d -b d ), the feasible arm angle ψ range is If θ l ≤atan2(c n -b n ,c d -b d )andθ u >atan2(c n -b n ,c d -b d ), if the trend of θ with respect to ψ is first maximum and then minimum, then the range of feasible arm angle ψ is: Otherwise, the feasible arm angle ψ range is: If θ l >atan2(c n -b n ,c d -b d ), then the feasible arm angle ψ range is when a t 2 +b t 2 -c t 2 =0, a singular method is used to solve the range of feasible arm angle ψ.

2. The inverse kinematics optimization method of a seven-degree-of-freedom manipulator of SRS configuration based on arm angle parameters as claimed in claim 1, characterized in that: In step S101, the reference plane is composed of the highest point of the elbow E, shoulder S and wrist W of the robot arm; the arm plane changes with the movement of the robot arm, and the current arm plane is composed of the current elbow E, shoulder S and wrist W of the robot arm; wherein, the shoulder S is the intersection of the rotation axes of the first three joints of the robot arm; and the wrist W is the intersection of the rotation axes of the last three joints of the robot arm.

3. The inverse kinematics optimization method of a seven-DOF manipulator with SRS configuration based on arm angle parameters as claimed in claim 1, characterized in that: The expression of the elbow joint angle θ4 is as follows: Where acos() represents the arccosine function; d SE Indicates the distance from the shoulder S to the elbow E; d EW represents the distance from elbow E to wrist W; p SW =[-d WT *ax+px-d WT *ay+py-d WT *az+pz-d BS ] T , terminal pose d WT Represents the distance between the wrist W and the origin T of the last joint angle coordinate system; d BS Indicates the distance between the shoulder S and the origin B of the robot base coordinate system.

4. The inverse kinematics optimization method of a seven-DOF manipulator with SRS configuration based on arm angle parameters as claimed in claim 1, characterized in that: The expressions of the shoulder joint angles θ1, θ2, and θ3 are as follows: In the formula, is the rotation matrix of the first three joints when the arm angle ψ is 0 and the elbow E is at the highest point; uSW is the unit vector corresponding to the vector formed by the shoulder S and the wrist W; [uSW×] is the antisymmetric matrix of uSW; A s (i,j),B s (i,j),C s (i,j) represent the matrix A respectively s , B s , C s The element in the i-th row and j-th column of .

5. The inverse kinematics optimization method of an SRS configuration seven-DOF manipulator based on arm angle parameters as claimed in claim 4, characterized in that: The expressions of the wrist joint angles θ5, θ6, and θ7 are as follows: Where: where cosθ i c i , sinθ i For i , * indicates ignored items.

6. The inverse kinematics optimization method of a seven-DOF manipulator with SRS configuration based on arm angle parameters as claimed in claim 1, characterized in that: After the range of the feasible arm angle ψ is solved in step S103, the optimal feasible solution for the robot arm path planning is solved by combining the range of the feasible arm angle ψ through the virtual force weight method.

Images