Inverse Kinematics Solution and Self-Collision Detection Method Based on a Multi-Degree-of-Freedom Manipulator

Abstract

The present invention discloses an inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom robotic arm. The present invention relates to the technical field of robotic arm control, and solves the problems existing in the existing robotic arm modeling, collision detection, and inverse solution technologies. Through the modeling and inverse solution methods, the present invention can effectively calculate the joint angles of the robotic arm, and ensure the legality and safety of the solution through collision detection and angle correction; this method is applicable to robotic arms with 7 degrees of freedom, can handle complex kinematics and collision detection problems, and has high practicability and reliability; in practical applications, it can accurately control the movement of the robotic arm, effectively avoid self-collision situations, improve the working efficiency and safety of the robotic arm, and has broad application prospects and practical value.

Description

Technical Field

[0001] The present invention relates to the technical field of robotic arm control, and specifically to an inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom robotic arm. Background Art

[0002] In the fields of modern industrial automation and intelligent robots, the robotic arm, as a core execution component, its performance directly affects production efficiency, product quality, and system stability; with the development of the manufacturing industry towards high precision and high flexibility, the requirements for the motion control accuracy and safety of robotic arms are increasing day by day. However, there are many bottlenecks in the existing technologies for robotic arm modeling and inverse kinematics solution;

[0003] In terms of establishing a coordinate system for the robotic arm, the coordinate systems established by traditional methods often lack generality and accuracy; some robotic arm coordinate systems are simply based on the geometric characteristics of the mechanical structure, without fully considering the joint motion characteristics and dynamic factors, which makes it difficult to accurately describe the pose changes of each part of the robotic arm in complex motion scenarios; when it comes to multi-joint collaborative motion, due to the defects in the definition of the coordinate system, it is difficult to accurately establish the kinematic relationship between joints, resulting in deviations in motion control and affecting the accuracy of the robotic arm in performing tasks.

[0004] Collision detection technology also faces challenges; currently, most collision detection algorithms have high computational complexity and poor real-time performance; traditional collision detection methods based on grid division or voxelization require a large amount of computational resources and time for model construction and collision judgment when dealing with robotic arms with complex shapes, and cannot meet the requirements of real-time collision detection during the high-speed movement of the robotic arm; some simple collision detection strategies only consider collisions between adjacent components, ignoring the self-collisions that non-adjacent components may occur in special motion postures, which poses a large safety hazard when the robotic arm performs complex tasks, easily leading to damage to the robotic arm, production interruption, and safety accidents. The deficiencies of the existing robotic arm modeling, collision detection, and inverse kinematics solution technologies seriously restrict the application of robotic arms in complex industrial environments and high-precision tasks. Summary of the Invention

[0005] In view of the deficiencies of the existing technology, the present invention provides an inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom robotic arm, which solves the problems existing in the existing robotic arm modeling, collision detection, and inverse kinematics solution technologies.

[0006] To achieve the above objectives, the present invention is realized through the following technical solutions: An inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom robotic arm, including the following steps:

[0007] Step 1. Construct the robotic arm model: For each joint in the multi-degree-of-freedom robotic arm, establish a coordinate system. The z-axis direction of each coordinate system is defined as the rotation axis direction of the corresponding joint. Define the child system through a single rotation transformation relative to the parent system, and determine the three-axis directions of the child system. Based on the coordinate system model of the robotic arm, the three connecting rods from the base to the wrist are respectively enclosed by cylinders with radii r0, r1, r2 and heights l0, l1, l2 to complete the construction of the robotic arm model;

[0008] Step 2. Implement the inverse solution process: The input of the inverse solver is the end pose and the constraint of a certain dimension of the elbow, and the output is the joint angle. The overall process is as follows: First, calculate the key point coordinates based on the end position and elbow information, and use the collision detection algorithm to verify the legality of the solution; then, calculate the candidate solutions of the angles based on the key points and the end pose, and use the correction algorithm to correct them to the legal interval; there are often multiple solutions for the candidate value ji. Solve them sequentially from the parent system to the child system. If there is no solution for a certain joint ji, then backtrack to j(i - 1) and explore other solutions of j(i - 1), where i = 2, ……, 7. The specific sub-steps of implementing the inverse solution process are as follows:

[0009] S21. Define mathematical quantities: Define the translation matrix and the rotation matrices for rotation along the X / Y / Z axes. Let the coordinate system before the rotation of the nth joint be Sn, and the coordinate system after the rotation be Sn'. Then, confirm the transformation matrix from Sn' to Sn + 1' in sequence; Let the pose of the end (wrist) in the base system be

[0010] , where the attitude angles are in the order of "yaw, pitch, roll"; According to the meaning of the translation matrix (x e , y e , z e , α, β, γ)

[0011] and the rotation matrices for rotation along the X / Y / Z axes and the meaning of the end pose, the transformation matrix T70 from S0 to S7 can be obtained;

[0012] S22. Calculate the key points: The position of the shoulder remains unchanged during the movement. Therefore, in the base system S0, there is a constant coordinate P1_0 = (0, l0, 0); P3_0 = (x3_0, y3_0, z3_0) = (xe, ye, ze); In two different cases, solve the coordinates of P2_0 = (x2_0, y2_0, z2_0) respectively; If it is solved that y2_0 = l0 - l1, then the connecting rod P0P1 and the connecting rod P1P2 collide, and the solution is discarded;

[0013] S23. Perform angle calculation: If there is a solution for the key points, solve the angle based on the key points; when in the Sn system, the direction change of the Zn+2 axis is only determined by joint jn+1; according to this rule, a general solution can be proposed: In the Sn system, record the Zn+2 axis directions before and after the rotation of joint jn+1, and use a function to solve for jn+1 (0 ≤ n ≤ 5).

[0014] S24. Correct the calculated angles of the associated points: The above inverse solution has calculated several candidate solutions, and the range of these solutions is all within [-pi, pi]. Next, the calculation results need to be corrected to the legal interval according to the angle limit and movement amplitude requirements. If the correction is impossible, discard this set of solutions.

[0015] S25. Detect self-collision behavior: The collision of adjacent links can be avoided by angle limits, and the collision of non-adjacent links needs to be detected separately; when modeling the collision bodies, the links are regarded as cylinders. If the cylinders have an intersection, it is regarded as self-collision; the relationship between two straight lines in three-dimensional space is divided into four cases: parallel, coincident, intersecting, and skew.

[0016] The present invention provides an inverse kinematics calculation and self-collision detection method based on a multi-degree-of-freedom robotic arm. Compared with the prior art, it has the following beneficial effects:

[0017] Through the modeling and inverse calculation method, the present invention can effectively calculate the joint angles of the robotic arm, and ensure the legality and safety of the solutions through collision detection and angle correction; this method is applicable to robotic arms with 7 degrees of freedom, can handle complex kinematics and collision detection problems, and has high practicality and reliability;

[0018] In practical applications, accurately control the movement of the robotic arm, effectively avoid the occurrence of self-collision, improve the working efficiency and safety of the robotic arm, and have broad application prospects and practical value. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 It is a schematic structural diagram of the target robot of the present invention;

[0020] Figure 2 It is a first schematic diagram of key point calculation of the present invention;

[0021] Figure 3 It is a second schematic diagram of key point calculation of the present invention;

[0022] Figure 4 It is a flow chart of the correction algorithm of the present invention;

[0023] Figure 5 It is a schematic diagram of the relationship between the central axes of two cylinders in three dimensions of the present invention;

[0024] Figure 6 It is a schematic diagram of the parallel or coincident central axes of two cylinders of the present invention;

[0025] Figure 7 Schematic diagram for judging whether a cylinder collides in the x-axis direction of the present invention;

[0026] Figure 8 Schematic diagram for the non-coplanar or intersecting axes of the two cylinders of the present invention;

[0027] Figure 9 Schematic diagram for finding the projected width of the collision interval of the present invention;

[0028] Figure 10 Schematic diagram of the projection of the collision interval of the present invention on the xOy plane. Specific embodiments

[0029] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0030] The first embodiment

[0031] Please refer to Figure 1 , the present application provides an inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom robotic arm, including the following steps:

[0032] Step 1. Construct a robotic arm model:

[0033] S11. Construct a coordinate system: For each joint existing in the multi-degree-of-freedom robotic arm, establish a coordinate system. The z-axis direction of each coordinate system is defined as the rotation axis direction of the corresponding joint. The child system is defined through a single rotation transformation relative to the parent system to determine the three-axis directions of the child system. The origin of the coordinate system is translated on the rotation axis:

[0034] Translate the coordinate systems of j1, j3, and j5 to P0, P1, and P2 respectively, and finally merge them into four key points P0, P1, P2, and P3 (corresponding to the base, shoulder, elbow, and wrist respectively). Combining Figure 1 , its shoulder corresponds to the position of the j2 node, the elbow corresponds to the position of the j4 node, and the wrist corresponds to the position of the j7 node;

[0035] S12. Robotic arm modeling: On the basis of the coordinate system model of the robotic arm, the three connecting rods from the base to the wrist are respectively enclosed by cylinders with radii r0, r1, and r2 and heights l0, l1, and l2 to complete the construction of the robotic arm model; if there is an intersection between non-adjacent cylinders, it is regarded as self-collision;

[0036] Step 2: Implement the inverse kinematics process. The input of the inverse kinematics solver is the end - effector pose and the constraint of a certain dimension of the elbow (optional), and the output is the joint angles. The overall process is as follows: First, calculate the key - point coordinates based on the end - effector position and elbow information, and use the collision - detection algorithm to verify the legality of the solution. Then, calculate the candidate solutions of the angles based on the key - points and the end - effector pose, and use the correction algorithm to correct them to the legal interval. There are often multiple solutions for the candidate value ji. Solve them sequentially from the parent system to the child system (from j1 to j7). If a certain joint ji has no solution, backtrack to j(i - 1) and explore other solutions of j(i - 1), where i = 2, …, 7. The entire exploration process is a depth - first traversal of the tree, and verification is performed while calculating. If any set of solutions passes the verification of both the correction and collision algorithms, return the calculated value and stop exploring other solutions. The detailed process of the inverse kinematics calculation includes:

[0037] S21. Define mathematical quantities: Define the translation matrix and the rotation matrices for rotation along the X / Y / Z axes. The translation matrix is:

[0038] The rotation matrix after rotation along the X - axis is:

[0039] The rotation matrix after rotation along the Y - axis is:

[0040] The rotation matrix after rotation along the Z - axis is:

[0041] Let the coordinate system before the rotation of the nth joint be Sn, and the coordinate system after the rotation be Sn'. Then the transformation matrix from Sn' to Sn + 1' is successively:

[0042]

[0043] Let the pose of the end - effector (wrist) in the base coordinate system be (x e , y e , z e , α, β, γ), where the attitude angles are in the order of "yaw, pitch, roll". According to the translation matrix, the rotation matrices for rotation along the X / Y / Z axes, and the meaning of the end - effector pose, the transformation matrix T from S0 to S7 can be obtained 70 Equation (2 - 3); where c and s are abbreviations for cos and sin respectively:

[0044]

[0045] For the convenience of subsequent calculations, define the data quantities:

[0046] The suffix _n represents the mathematical quantity in the Sn coordinate system;

[0047] dij_n=(xij_n,yij_n,zij_n) represents the vector from point Pi to point Pj in the Sn frame;

[0048] Functions (2-1) and (2-2) are point transformation and vector transformation operations respectively, and function (2-3) is the rotation angle of vector v2 relative to vector v1 on [-pi, pi].

[0049]

[0050] S22. Calculate the key points. The shoulder remains in the same position throughout the motion. In base system S0, the constant coordinates are P1_0 = (0, l0, 0); P3_0 = (x3_0, y3_0, z3_0) = (xe, ye, ze). In the following two cases, solve for the coordinates of P2_0 = (x2_0, y2_0, z2_0). If the solution is y2_0 = l0 - l1, the connecting rods P0P1 and P1P2 collide, and the solution is discarded:

[0051] The first case is the elbow constraint: when the value of a dimension of the elbow (X, Y, or Z) is m, the two distance equations can be combined to find P2_0;

[0052]

[0053] The second case is elbow adaptation (that is, unconstrained): when the elbow position is unconstrained, it adapts to the position with the minimum amplitude.

[0054] Given the lengths of l1 and l2, and l13 being the distance between points P1_0 and P3_0, the solution set for P2_0 in space has the following three cases:

[0055] When l13>l1+l2 or l13<=|l1-l2|, the solution set of P2_0 is empty (taking the equal sign will result in collision);

[0056] When l13=l1+l2, the solution set of P2_0 is a point;

[0057] When |l1-l2| <l13<l1+l2时,P2_0的解集为一个圈;

[0058] Let e13_0 be the unit vector from P1_0 to P3_0, then for case 2), we can easily get

[0059] For case 3), let the angle between l1 and l13 be θ ( Figure 2 ), introducing the cosine theorem, it is easy to get the center and radius of the circle:

[0060]

[0061] Build a coordinate system Sc for the circle plane, as follows Figure 3 ; Set e13_0 as the z-axis direction of Sc. Calculate the orthogonal x-axis on the plane corresponding to the minimum component of e13_0, and then obtain the y-axis by taking the cross product of the z-axis and the x-axis, that is:

[0062]

[0063] Then the transformation matrix from Sc to S0 can be determined as

[0064]

[0065] Use function (2-3) to transform the previous position of the elbow to the Sc coordinate system

[0066] oriP 2_c =(x 2_c ,y 2_c ,z 2_c ) = multTP(T 0c -1 ,oriP 2_0 )

[0067] An initial angle phi0 for preferential exploration on the circle; if oriP2_c is on the Zc axis, then phi0 = 0, otherwise phi0 = atan2(y2_c, x2_c); assume the candidate elbow coordinate P2_c = (r * cos(phi), r * sin(phi), 0) in the Sc coordinate system, where phi ∈ [phi0 - pi, phi0 + pi]; then phi starts from phi0 and explores towards both sides with a certain step size, and the candidate angles are phi1 and phi2. Finally, use function (2-3) to transform the candidate point to the coordinate P in the S0 coordinate system 2_0 = multTP(T 0c ,P 2_c )

[0068] S23. Perform angle calculation: If the key points have solutions (at this time, the value range of y2_0 is (l0 - l1, l0 + l1], and the value range of x3_2 is [-l1 - l2, l2 - l1)), then solve the angles according to the key points;

[0069] Note that when in the Sn coordinate system, the direction change of the Zn+2 axis is only determined by the joint jn+1; based on this rule, a general solution can be proposed: In the Sn coordinate system, record the Zn+2 axis directions before and after the rotation of the joint jn+1, and use function (2-3) to solve jn+1 (0 ≤ n ≤ 5):

[0070] S231. Solve j1 and j2:

[0071] First, discuss the singular point y2_0 = l0 + l1 (i.e., P2_0 is on the Y0 axis); at this time, j2 = 0 and j1 has infinitely many solutions. To minimize the movement amplitude, let j1 traverse from the old value along a certain step size towards ±pi.

[0072] Secondly, discuss the general case l0 - l1 < y2_0 < l0 + l1; in the S0 system, the original axial direction of Z2_0, oriZ2_0 = (1, 0, 0), and the current axial direction Z2_0 = ±(d01_0 × d12_0). Then j1 = calcuVecAng(oriZ2_0, Z2_0, -Z2_0[2]) has two solutions. Using Equation (2-2) and function (2-2) to transform to the S1 system, the original axial direction of Z3_1, oriZ3_1 = (0, 0, 1), and the current axial direction is the same as d12_1. Calculate the vector d12_1 = multTV(T10, d12_0), then j2 = calcuVecAng(oriZ3_1, d12_1, -d12_1[1]);

[0073] S232. Solve for j3 and j4: After obtaining the values of j1 and j2, P3_2 = multTP(T21, multTP(T10, P3_0)) can be calculated. The other mathematical quantities can also be transformed to the S2 system in a similar way. First, discuss the singular point x3_2 = -l1 - l2; at this time, j4 = 0 and j3 has infinitely many solutions. To minimize the movement amplitude, let j3 traverse from the old value along a certain step size towards ±pi.

[0074] Secondly, discuss the general case -l1 - l2 < x3_2 < l2 - l1. Similar to the S0 system, in the S2 system, the original axial direction of Z4_2, oriZ4_2 = (0, -1, 0), and the current axial direction Z4_2 = ±(d12_2 × d23_2). Then j3 = calcuVecAng(oriZ4_2, Z4_2, Z4_2[2]) has two solutions. Using Equation (2-2) and function (2-2) to transform to the S3 system, the original axial direction of Z5_3, oriZ5_3 = (0, 0, 1), and the current axial direction is the same as d23_3. Calculate the vector d23_3, then j4 = calcuVecAng(oriZ5_3, d23_3, -d23_3[0]);

[0075] S233. Solve for j5 and j6: From Equation (2-3), it is easy to obtain the Z7 axis in the S0 system

[0076] Z 7_0 = (sβ * cγ * cα + sγ * sα, sβ * sα * cγ - sγ * cα, cβ * cγ)

[0077] After obtaining the values of j3 and j4, transform Z7_0 to Z7_4 using the same method. If Z7_4 is coaxial with d23_4, there is a singular point. Specifically, when Z7_4[1] = ±1, j6 = -pi / 2 * Z7_4[1]. To minimize the movement amplitude, let j5 traverse from the old value along a certain step size towards ±pi.

[0078] For the remaining cases, transform each mathematical quantity to the S4 system; the original axial direction of Z6_4, oriZ6_4 = (0, 0, 1); since d23_4 and Y6_4 are always in the same direction, and X6_4 and Z7_4 are always in the opposite direction, and there is a right - hand screw relationship among X6_4, Y6_4, and Z6_4, so the current axial direction Z6_4 has a unique solution Z6_4 = (-Z7_4) × d23_4, then j5 = calcuVecAng(oriZ6_4, Z6_4, Z6_4[0]); transform to the S5 system, the original axial direction of Z7_5, oriZ7_5 = (-1, 0, 0), and the current axial direction is obtained by transforming Z7_4, then j6 = calcuVecAng(oriZ7_5, Z7_5, -Z7_5[2]);

[0079] S234. Solve for j7: In the S6 system, j7 can be calculated from the change of X7_6; the original axial direction of X7_6, oriX7_6 = (0, 0, 1), using a method similar to that for finding Z7_4, calculate the current axial direction X7_6; then j7 = calcuVecAng(oriX7_6, X7_6, X7_6[1]);

[0080] S24. Correct the calculated associated point angles: The above inverse solution has calculated several candidate solutions, and the range of these solutions is all in [-pi, pi]. Next, according to the angle limit and movement amplitude requirements, correct the calculation results to the legal interval. If it cannot be corrected, discard this set of solutions; assume the limit of angle j is (a, b), the previous action value is prej, and the required movement amplitude does not exceed g, then there is a correction algorithm for each angle as follows Figure 4 ; When the output of any angle is false, terminate the loop and discard the solution. When the output of all angles is true, retain the corrected solution;

[0081] S25. Detect self - collision behavior: The collision of adjacent links can be avoided by angle limits, and the collision of non - adjacent links needs to be detected separately; when modeling the collision bodies, the links are regarded as cylinders. If the cylinders have an intersection, it is regarded as self - collision; the relationship between two straight lines in three - dimensional space can be roughly divided into four cases: parallel, coincident, intersecting, and skew. The following is a classification discussion;

[0082] Suppose that under a certain system, the coordinates of the two end points of the axis of cylinder 1 are P1 and P2 respectively, the radius is r1, and the height is l12; the coordinates of the two end points of the axis of cylinder 2 are P3 and P4 respectively, the radius is r2, and the height is l34; first, find the direction vectors d12 = P2 - P1, d34 = P4 - P3, d13 = P3 - P1. Let e12 and e34 be their corresponding unit vectors, then the normal vector n of the plane formed by the two axes is n = e12 × e34( Figure 5 ); if the area of the parallelogram S = ||n|| = 0, then the axes are parallel or coincident, otherwise, the axes are skew or intersecting;

[0083] When the axes are parallel or coincident, establish a coordinate system so that the positive direction of the x-axis is the same as that of d12, and the y-axis is coplanar and perpendicular to the two axes( Figure 6 ):

[0084] In the y-axis direction, the distance h between the two axes is h = ||d13 × e34||; if h ≥ r1 + r2, then the cylinders do not collide, otherwise, proceed to the next judgment;

[0085] In the x-axis direction, project P3 and P4 onto the x-axis respectively to obtain the projection points P3' and P4'; the offset b13' from P1 to P3' is b13' = d13 · e12; let k = e34 · e12( Figure 7 ), when k = 1, there is no collision when b13' ≤ -l34 or b13' ≥ l12; when k = -1, there is no collision when b13' ≤ 0 or b13' ≥ l12 + l34; then the two cases can be combined as: when b13' ≤ -(1 + k) / 2 * l34 or b13' ≥ l12 + (1 - k) / 2 * l34, the cylinders do not collide;

[0086] When the axes are skew or intersecting, establish a coordinate system Sp with P1 as the origin, so that the positive direction of the z-axis is the same as Figure 2-5 the normal vector n in Figure 8 above, and the positive direction of the y-axis is the same as that of d12(

[0087]

[0088] The new coordinates in the new coordinate system can be obtained by multiplying the old coordinates on the left by Tpk. The following Pi = (xi, yi, zi) represents the point coordinates in the new coordinate system.

[0089] In the z-axis direction, if the distance between the axes dist = |z3| is greater than or equal to r1 + r2, then the cylinders do not collide, otherwise, proceed to the next judgment;

[0090] On the xOy plane, project the parts of the cylinders that may collide onto the xOy plane, and approximately transform it into a problem of rectangle coincidence; points P1 and P2 are already on the xOy plane. Set the values of z3 and z4 to 0, and then the projection points P3' and P4' of P3 and P4 can be obtained. Let the collision layer thickness ( Figure 9 the yellow part) m = r1 + r2 - dist(0 < m ≤ r1 + r2), then the projected half-width a can be obtained by the following formula:

[0091]

[0092] Let the projection of the collision interval of cylinder i on the xOy plane be rectangle i, and let the four endpoints of rectangle 2 be A, B, C, D ( Figure 10 ). If any of the four endpoints is inside rectangle 1, it is determined as a collision. Let the unit direction vector e34 from P3' to P4' be e34 = (x34, y34, 0), where x34 ≠ 0. Then the unit direction vector ω for the width extension of rectangle 2 is ω = (y34, -x34, 0). The coordinates of points A, B, C, and D can be obtained from P3' ± a2 * ω and P4' ± a2 * ω. If the coordinates (x, y, z) of any of the points A, B, C, and D satisfy "x ∈ (-a1, a1) ∧ y ∈ (0, l12)", then the cylinders collide.

[0093] Some of the data in the above formulas are numerically calculated after removing the dimensions. At the same time, the content not described in detail in this specification belongs to the prior art well-known to those skilled in the art.

[0094] The above embodiments are only used to illustrate the technical method of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those of ordinary skill in the art should understand that the technical method of the present invention can be modified or equivalently replaced without departing from the spirit and scope of the technical method of the present invention.

Claims

1. An inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom robotic arm, characterized in that, It includes the following steps: Step 1, construct a robotic arm model: For each joint in the multi-degree-of-freedom robotic arm, establish a coordinate system. The z-axis direction of each coordinate system is defined as the rotation axis direction of the corresponding joint. Define the child system through a single rotation transformation relative to the parent system, and determine the three-axis directions of the child system. On the basis of the coordinate system model of the robotic arm, the three connecting rods from the base to the wrist are respectively enclosed by cylinders with radii r0, r1, r2 and heights l0, l1, l2 to complete the construction of the robotic arm model; Step 2, perform the inverse solution process: The input of the inverse solver is the end pose and the constraint of a certain dimension of the elbow, and the output is the joint angle. The overall process is: first calculate the key point coordinates according to the end position and elbow information, and use the collision detection algorithm to verify the legality of the solution; Then calculate the candidate solutions of the angles according to the key points and the end pose, and correct them to the legal interval by the correction algorithm; there are often multiple solutions for the candidate value ji. Solve them sequentially from the parent system to the child system. If there is no solution for a certain joint ji, then backtrack to j(i-1) and explore other solutions of j(i-1), where i = 2,..., 7; The specific sub-steps are: S21. Define mathematical quantities: Define the translation matrix and the rotation matrices for rotation about the X / Y / Z axes. Let the coordinate system before the rotation of the nth joint be Sn, and the coordinate system after the rotation be Sn'. Then, sequentially confirm the transformation matrix from Sn' to Sn+1'. Let the pose of the end effector (wrist) in the base coordinate system be (x e , y e , z e , α, β, γ). Among them, the attitude angles are in the order of "yaw, pitch, roll"; according to the translation matrix, the rotation matrices rotating along the X / Y / Z axes and the meaning of the end pose, the transformation matrix T70 from S0 to S7 can be obtained; S22. Calculate the key points: The position of the shoulder remains unchanged during the movement, so there is a constant coordinate P1_0 = (0, l0, 0) in the base system S0; P3_0 = (x3_0, y3_0, z3_0) = (xe, ye, ze); in two different cases, solve the coordinates of P2_0 = (x2_0, y2_0, z2_0) respectively; if it is solved that y2_0 = l0 - l1, then the connecting rod P0P1 and the connecting rod P1P2 collide and the solution is discarded; S23. Calculate the angles: If there is a solution for the key points, then calculate the angles according to the key points; when in the Sn system, the direction change of the Zn+2 axis is only determined by the joint jn+1; according to this rule, a general solution method can be proposed: in the Sn system, record the Zn+2 axis directions before and after the rotation of the joint jn+1, and use the function to solve jn+1, 0 ≤ n ≤ 5; S24. Correct the angles of the solved associated points: The above inverse solution has calculated several candidate solutions, and the ranges of these solutions are all in [-pi, pi]. Next, the calculation results need to be corrected to the legal interval according to the angle limit and the dynamic range requirements. If it cannot be corrected, then discard this set of solutions; S25. Detect self-collision behavior: The collision of adjacent connecting rods can be avoided by the angle limit, and the collision of non-adjacent connecting rods needs to be detected separately; when modeling the collision body, the connecting rods are regarded as cylinders. If the cylinders have an intersection, it is regarded as self-collision; the relationship between two straight lines in three-dimensional space is divided into four situations: parallel, coincident, intersecting and skew; 2. The inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom robotic arm according to claim 1, wherein In the above Step 1, the origin of the coordinate system is translated on the rotation axis, and the coordinate systems of j1, j3, and j5 are respectively translated to P0, P1, and P2, and finally merged into four key points P0, P1, P2, and P3, corresponding to the base, shoulder, elbow, and wrist respectively; If there is an intersection between non-adjacent cylinders, it is considered a self-collision.

3. The inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom robotic arm according to claim 1, characterized in that In step S21: The translation matrix is as follows: The rotation matrix after rotation along the X-axis is: The rotation matrix after rotation along the Y-axis is as follows: The rotation matrix after rotation along the Z axis is: Then the transformation matrix from Sn' to Sn+1' is Transformation matrix T from S0 to S7 70 The formula is as follows:

4. The inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom robotic arm according to claim 1, wherein The mathematical quantities defined in step S21 also include: The suffix _n indicates a mathematical quantity in the Sn series; dij_n=(xij_n,yij_n,zij_n) represents the vector from point Pi to point Pj in the Sn frame; Functions (2-1) and (2-2) are point transformation and vector transformation operations respectively, and function (2-3) is the rotation angle of vector v2 relative to vector v1 on [-pi, pi]. PointP=multTP(MatrixT0, PointP0) s.t.(P,1) T =(P x ,P y ,P z ,1) T =T0(P 0x ,P 0y ,P 0z ,1) T Function (2-1) 5. The inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom robotic arm according to claim 3, characterized in that, In step S22, the first case is elbow constraint: when the value of a certain dimension of the elbow is m, two distance equations can be combined to obtain P2_0; The second case is elbow adaptation: when the elbow position is unconstrained, it adapts to the position with the minimum amplitude.

6. The inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom robotic arm according to claim 1, wherein In step S23, the specific method of solving the angle of the key point is: S231, solve j1, j2: That is, P2_0 is on the Y0 axis, at this time j2=0, j1 has infinite solutions; to minimize the amplitude, let j1 traverse from the old value along a certain step length to the relative ±pi; When l0-l1 <y2_0<l0+l1;在S0系下,Z2_0原始轴向oriZ2_0=(1,0,0),当前轴向Z2_0=±(d01_0×d12_0),则j1=calcuVecAng(oriZ2_0,Z2_0,-Z2_0[2])有两解;利用式(2-2)和函数(2-2)变换到S1系,Z3_1原始轴向oriZ3_1=(0,0,1),当前轴向与d12_1一致;求出向量d12_1=multTV(T10,d12_0),则j2=calcuVecAng(oriZ3_1,d12_1,-d12_1[1]); S232. Solve j3 and j4: After finding the values of j1 and j2, we can obtain P3_2 = multTP(T21, multTP(T10, P3_0)). The remaining mathematical quantities can also be transformed into the S2 system using a similar method. We will first discuss the singular point x3_2 = -l1-l2. At this time, j4 = 0, and j3 has infinite solutions. To minimize the amplitude, let j3 traverse from the old value along a certain step length to the relative ±pi. When -l1 - l2 < x3_2 < l2 - l1, similar to the case in the S0 system, in the S2 system, the original axial direction of Z4_2, oriZ4_2 = (0, -1, 0), and the current axial direction Z4_2 = ±(d12_2 × d23_2). Then, j3 = calcuVecAng(oriZ4_2, Z4_2, Z4_2[2]) has two solutions. Using Equation (2-2) and function (2-2) to transform to the S3 system, the original axial direction of Z5_3, oriZ5_3 = (0, 0, 1), and the current axial direction is the same as d23_3. Calculate the vector d23_ _3, then j4 = calcuVecAng(oriZ5_3, d23_3, -d23_3[0]); S233. Solve for j5 and j6: The Z7 axis in the S0 system can be easily obtained from Equation (2-3). Z 7_0 =(sβ * cγ * cα + sγ * sα, sβ * sα * cγ - sγ * cα, cβ * cγ) After obtaining the values of j3 and j4, use the same method to transform Z7_0 to Z7_4. If Z7_4 is coaxial with d23_4, there is a singularity. Specifically, when Z7_4[1] = ±1, j6 = -pi / 2 * Z7_4[1]; To minimize the movement range, let j5 traverse from the old value along a certain step size towards ±pi. For the remaining cases, transform each mathematical quantity to the S4 system; the original axial direction of Z_6_4, oriZ6_4 = (0, 0, 1); Since d23_4 and Y6_4 are always in the same direction, and X6_4 and Z7_4 are always in the opposite direction, then j5 = calcuVecAng(oriZ6_4, Z6_4, Z6_4[0]); Transform to the S5 system, the original axial direction of Z7_5, oriZ7_5 = (-1, 0, 0), and the current axial direction is obtained by transforming Z7_4, then j6 = calcuVecAng(oriZ7_5, Z7_5, -Z7_5[2]); S234. Solve for j7: In the S6 system, j7 can be calculated from the change of X7_6. The original axial direction of X7_6, oriX7_6 = (0, 0, 1). Using a similar method to find Z7_4, calculate the current axial direction X7_6, then j7 = calcuVecAng(oriX7_6, X7_6, X7_6[1]).

7. The inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom robotic arm according to claim 3, wherein In step S24, assume the limit of angle j is (a, b), the previous action value is prej, and it is required that the movement range does not exceed g; When any angle output is false, the loop is terminated and the solution is discarded. When all angle outputs are true, the corrected solution is retained.

8. The inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom robotic arm according to claim 3, wherein In step S25, when the central axes are parallel or coincident, establish a coordinate system such that the positive x-axis direction is the same as d12, and the y-axis is coplanar and perpendicular to the two axes: In the y-axis direction, the distance h between the two axes is h = ||d13 × e34||; If h ≥ r1 + r2, there is no collision between the cylinders, otherwise, proceed to the next judgment; In the x-axis direction, project P3 and P4 onto the x-axis respectively to obtain the projection points P3' and P4'; the offset b13' from P1 to P3' = d13·e12; let k = e34·e12, when k = 1, there is no collision when b13' ≤ -l34 or b13' ≥ l12; when k = -1, there is no collision when b13' ≤ 0 or b13' ≥ l12 + l34; then the two cases can be combined as: when b13' ≤ -(1 + k) / 2*l34 or b13' ≥ l12 + (1 - k) / 2*l34, the cylinder has no collision; When the central axes are skew or intersecting, establish a coordinate system Sp with P1 as the origin, making the positive direction of the z-axis the same as the normal vector n in Figure 2-5, and the positive direction of the y-axis the same as d12; the XYZ axes of the new coordinate system are respectively: Z = n / ||n||, Y = e12, X = Y×Z, so the transformation matrix Tpk from the old coordinate system Sk to the new coordinate system Sp is: The coordinates in the new coordinate system can be obtained by multiplying the coordinates in the old coordinate system by Tpk on the left. The following Pi = (xi, yi, zi) represents the point coordinates in the new coordinate system; In the z-axis direction, if the distance dist = |z3| of the central axis is greater than or equal to r1 + r2, the cylinder has no collision, otherwise proceed to the next judgment; On the xOy plane, project the possibly colliding part of the cylinder onto the xOy plane and approximately transform it into a problem of rectangle overlap; points P1 and P2 are already on the xOy plane. Set the values of z3 and z4 to 0, that is, obtain the projection points P3' and P4' of P3 and P4; let the collision layer thickness m = r1 + r2 - dist (0 < m ≤ r1 + r2), then the projection half-width a can be obtained by the following formula: Let the projection of the collision interval of cylinder i on the xOy plane be rectangle i, and let the four endpoints of rectangle 2 be A, B, C, and D respectively; if any one of the four endpoints is inside rectangle 1, it is determined as a collision; let the unit direction vector e34 = (x34, y34, 0) from P3' to P4', x34 ≠ 0, then the unit direction vector ω for the width extension of rectangle 2 = (y34, -x34, 0), then the coordinates of points A, B, C, and D can be obtained from P3' ± a2*ω and P4' ± a2*ω; if the coordinates (x, y, z) of any one of points A, B, C, and D satisfy "x ∈ (-a1, a1) ∧ y ∈ (0, l12)", the cylinder collides.

Images