Inverse kinematics calculation and self-collision detection method based on multi-degree-of-freedom mechanical arm

Abstract

The invention discloses an inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom mechanical arm, relates to the technical field of mechanical arm control, solves the problem that existing mechanical arm modeling, collision detection and inverse solution technologies have defects, and can effectively calculate a joint angle of the mechanical arm through modeling and inverse solution methods. The legality and safety of the solution are ensured through collision detection and angle correction; the method is suitable for the mechanical arm with seven degrees of freedom, complex kinematics and collision detection problems can be solved, and high practicability and reliability are achieved; the motion of the mechanical arm is accurately controlled in practical application, meanwhile, the self-collision situation is effectively avoided, the working efficiency and safety of the mechanical arm are improved, and the mechanical arm has wide application prospects and practical value.

Description

Technical Field

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

[0002] In the field of modern industrial automation and intelligent robots, the performance of the robotic arm as a core actuator is directly related to production efficiency, product quality and system stability. As the manufacturing industry develops towards high precision and high flexibility, the demand for motion control accuracy and safety of the robotic arm is increasing day by day. However, the existing technology has many bottlenecks in the modeling and inverse solution of the robotic arm.

[0003] In terms of robot arm system construction, the coordinate system established by traditional methods often lacks universality and accuracy; some robot arm 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 position changes of various parts of the robot arm in complex motion scenarios; when it comes to multi-joint coordinated motion, due to the defects in the coordinate system definition, the kinematic relationship between the joints is difficult to accurately establish, resulting in deviations in motion control and affecting the accuracy of the robot 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 lot of computing resources and time for model construction and collision judgment when dealing with complex-shaped robotic arms, and cannot meet the requirements of real-time collision detection during high-speed movement of robotic arms. Some simple collision detection strategies only consider collisions between adjacent components, and ignore the possible self-collision of non-adjacent components under special motion postures. This poses a major safety hazard when the robotic arm performs complex tasks, and can easily lead to damage to the robotic arm, production interruptions and safety accidents. The deficiencies in existing robotic arm modeling, collision detection and inverse solution technologies have seriously restricted the application of robotic arms in complex industrial environments and high-precision tasks. Summary of the invention

[0005] In view of the deficiencies in the prior art, 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 deficiencies in the existing robotic arm modeling, collision detection and inverse solution technologies.

[0006] To achieve the above objectives, the present invention is implemented by the following technical scheme: an inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom manipulator, comprising the following steps:

[0007] Step 1: Construct a robot model: For each joint in the multi-DOF robot, 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 by a rotation transformation relative to the parent system, and the three-axis direction of the child system is determined. Based on the coordinate system model of the robot, the three links from the base to the wrist are respectively enclosed by cylinders with radii of r0, r1, and r2 and heights of l0, l1, and l2 to complete the construction of the robot model.

[0008] Step 2: Implement the inverse solution process: The input of the inverse solver is the end position and the constraint of a certain dimension of the elbow, and the output is the joint angle. The overall process is: first, the key point coordinates are obtained based on the end position and elbow information, and the collision detection algorithm is used to verify the legality of the solution; then, the candidate solution of the angle is obtained based on the key points and the end posture, and the correction algorithm is used to correct it to the legal range; the candidate value ji often has multiple solutions, and the solutions are solved from the parent system to the child system in sequence. If a joint ji has no solution, 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:

[0009] S21. Define mathematical quantities: Define the translation matrix and the rotation matrix along the X / Y / Z axis. 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 turn; let the position 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 translation matrix (x e ,y e ,z e ,α,β,γ)

[0011] The meaning of the rotation matrix along the X / Y / Z axis and the end position can be obtained by the transformation matrix T70 from S0 to S7;

[0012] S22. Calculate the key points: the position of the shoulder remains unchanged during the movement, so the constant coordinates 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 the solution is y2_0 = l0-l1, the connecting rod P0P1 collides with the connecting rod P1P2, and the solution is discarded;

[0013] S23, calculate the angle: if the key point has a solution, solve the angle according to the key point; in the Sn 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 system, record the Zn+2 axis before and after the rotation of the joint jn+1, and use the function to solve jn+1 (0≤n≤5);

[0014] S24, correcting the angle of the solved associated point: the above inverse solution has solved several candidate solutions, and the range of these solutions is [-pi, pi]. Next, the solution results should be corrected to a legal range according to the angle limit and amplitude requirements. If the correction is not possible, the group of solutions will be discarded;

[0015] S25. Detect self-collision behavior: The collision of adjacent connecting rods can be avoided by angle limiting, and the collision of non-adjacent connecting rods needs to be detected separately; when the collision body is modeled, the connecting rod is regarded as a cylinder. If the cylinders have intersections, it is regarded as a self-collision; the relationship between two straight lines in three-dimensional space is divided into four situations: parallel, coincident, intersecting and non-coplanar.

[0016] The present invention provides an inverse kinematics solution 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] The present invention can effectively calculate the joint angles of the manipulator through modeling and inverse solution methods, and ensure the legitimacy and safety of the solution through collision detection and angle correction; the method is applicable to a manipulator with 7 degrees of freedom, can handle complex kinematics and collision detection problems, and has high practicality and reliability;

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

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

[0020] Figure 2 A diagram showing the key point calculation of the present invention;

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

[0022] Figure 4 The flowchart of the algorithm for the present invention is modified;

[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 This is a schematic diagram of the present invention in which the central axes of the two cylinders are parallel or coincident;

[0025] Figure 7 This is a schematic diagram of the present invention for judging whether cylinders collide in the x-axis direction;

[0026] Figure 8 This is a schematic diagram of the central axes of two cylinders of the present invention being skewed or intersecting;

[0027] Fig. 9 This is a schematic diagram of the projection width of the collision interval obtained in the present invention;

[0028] Fig.10 It is a schematic diagram of the projection of the collision zone of the present invention on the xOy plane. DETAILED DESCRIPTION

[0029] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0030] First embodiment

[0031] See also Figure 1 The present application provides an inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom manipulator, comprising the following steps:

[0032] Step 1: Build the robotic arm model:

[0033] S11. Construct a coordinate system: For each joint in the multi-DOF manipulator, 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 by a rotation transformation relative to the parent system, and the three-axis direction of the child system is determined. The origin of the coordinate system is translated on the rotation axis:

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

[0035] S12. Robotic arm modeling: Based on the coordinate system model of the robotic arm, the three links from the base to the wrist are respectively enclosed by cylinders with radii of r0, r1, and r2 and heights of l0, l1, and l2 to complete the construction of the mechanical wave model; if there is an intersection between non-adjacent cylinders, it is considered a self-collision;

[0036] Step 2: Implement the inverse solution process. The input of the inverse solver is the end position and the constraint of a certain dimension of the elbow (optional), and the output is the joint angle. The overall process is: first, the key point coordinates are obtained based on the end position and elbow information, and the collision detection algorithm is used to verify the legality of the solution; then, the candidate solution of the angle is obtained based on the key points and the end posture, and the correction algorithm is used to correct it to the legal range; the candidate value ji often has multiple solutions, and the solution is solved from the parent system to the child system (from j1 to j7) in sequence. If a joint ji has no solution, it is backtracked to j(i-1) and other solutions of j(i-1) are explored, where i=2, ..., 7. The entire exploration process is a deep traversal of the tree, using calculation and verification; if any set of solutions is verified by the correction and collision algorithms at the same time, the solution value is returned and no other solutions are explored. The detailed process of the inverse solution includes:

[0037] S21. Define mathematical quantities: define the translation matrix and the rotation matrix along the X / Y / Z axis, where 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] Assume that the coordinate system before the rotation of the nth joint is Sn, and the coordinate system after the rotation is Sn', then the transformation matrix from Sn' to Sn+1' is:

[0042]

[0043] Assume that the position of the end (wrist) in the base system is (x e ,y e ,z e ,α,β,γ), where the attitude angles are in the order of "yaw, pitch, roll"; according to the translation matrix and the rotation matrix along the X / Y / Z axis and the meaning of the terminal posture, the transformation matrix T from S0 to S7 can be obtained 70 Formula (2-3); where c and s are the abbreviations of cos and sin respectively:

[0044]

[0045] In order to facilitate subsequent calculations, the data volume is defined as follows:

[0046] The suffix _n indicates a mathematical quantity in the Sn series;

[0047] The vector from point Pi to point Pj under the Sn coordinate system is represented as dij_n = (xij_n, yij_n, zij_n);

[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. Since the position of the shoulder remains unchanged during movement, in the base coordinate system S0, there are constant coordinates P1_0 = (0, l0, 0); P3_0 = (x3_0, y3_0, z3_0) = (xe, ye, ze); In the following two cases, solve the coordinates of P2_0 = (x2_0, y2_0, z2_0) respectively. If y2_0 = l0 - l1 is solved, then the connecting rod P0P1 collides with the connecting rod P1P2, and the solution is discarded:

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

[0052]

[0053] The second case is elbow self-adaptation (that is, the unconstrained case): When the elbow position is unconstrained, it adapts to the position with the smallest movement amplitude.

[0054] Given the lengths of l1 and l2, let l13 be the distance between points P1_0 and P3_0. Then the solution set of 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 (when taking the equal sign, there must be a collision);

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

[0057] When |l1 - l2| < l13 < l1 + l2, the solution set of P2_0 is a circle;

[0058] Let e13_0 be the unit vector from P1_0 to P3_0. Then for case 2), it is easy to obtain

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

[0060]

[0061] Construct a system Sc for the circular plane, such as 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 multiply the x-axis by the z-axis to get the y-axis, that is:

[0062]

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

[0064]

[0065] Use function (2-3) to move the elbow to the next position of Sc

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

[0067] At an initial angle phi0 that is explored first on the circle; if oriP2_c is on the Zc axis, then phi0 = 0, otherwise phi0 = atan2(y2_c, x2_c); let the candidate elbow coordinates P2_c in the Sc system = (r*cos(phi), r*sin(phi), 0), phi∈[phi0-pi, phi0+pi]; then phi starts from phi0 and explores to both sides with a certain step length, and the candidate angles are phi1 and phi2. Finally, use function (2-3) to convert the candidate point into the coordinate P in the S0 system 2_0 =multTP(T 0c , P 2_c ).

[0068] S23, calculate the angle: if the key point has a solution (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 angle according to the key point;

[0069] Note that in the Sn 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 system, record the Zn+2 axis 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, 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 equations (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, and 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 equations (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), the Z7 axis in the S0 system can be easily obtained

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

[0077] After finding 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 singular point. Specifically, when Z7_4[1]=±1, j6=-pi / 2*Z7_4[1]. To minimize the amplitude, let j5 traverse from the old value along a certain step length to the relative ±pi.

[0078] For the rest of the cases, transform each mathematical quantity to the S4 system; the original axis of Z6_4 oriZ6_4 = (0,0,1); since d23_4 and Y6_4 are always in the same direction, X6_4 and Z7_4 are always in the opposite direction, and X6_4, Y6_4, and Z6_4 have a right-handed spiral relationship, the current axis 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 axis of Z7_5 oriZ7_5 = (-1,0,0), the current axis is obtained by transforming Z7_4, then j6 = calcuVecAng (oriZ7_5, Z7_5, -Z7_5[2]);

[0079] S234, solve j7: In the S6 system, j7 can be calculated from the change of X7_6; the original axis of X7_6 oriX7_6 ​​= (0,0,1), and the current axis X7_6 is calculated using a similar method to Z7_4; then j7 = calcuVecAng (oriX7_6, X7_6, X7_6[1]);

[0080] S24. Correct the angle of the related point being solved: The above inverse solution has solved several candidate solutions, and the range of these solutions is [-pi, pi]. Next, the solution results should be corrected to the legal range according to the angle limit and amplitude requirements. If correction is not possible, the group of solutions will be discarded; suppose the angle j is limited to (a, b), the previous action value is prej, and the amplitude is required not to exceed g, then there is a correction algorithm for each angle as follows Figure 4 ; When any angle outputs false, the loop is terminated and the solution is discarded. When all angles output true, the corrected solution is retained;

[0081] S25. Detect self-collision behavior: The collision of adjacent connecting rods can be avoided by angle limit, and the collision of non-adjacent connecting rods needs to be detected separately; when the collision body is modeled, the connecting rod is regarded as a cylinder. If the cylinders have intersections, it is regarded as a self-collision; the relationship between two straight lines in three-dimensional space can be roughly divided into four situations: parallel, coincident, intersecting and non-coplanar, which are discussed below;

[0082] Suppose in a certain system, the coordinates of the two endpoints of the axis of cylinder 1 are P1 and P2, the radius is r1, and the height is l12; the coordinates of the two endpoints of the axis of cylinder 2 are P3 and P4, the radius is r2, and the height is l34; first calculate the direction vectors d12 = P2-P1, d34 = P4-P3, d13 = P3-P1, let e12 and e34 be their corresponding unit vectors, then the plane normal vector n formed by the two axis is n = e12×e34 ( Figure 5 ); if the area of ​​the parallelogram S = ||n|| = 0, then the medial axes are parallel or coincident, otherwise, the medial axes are not in the same plane or intersect;

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

[0084] In the y-axis direction, the distance between the two axes is h = ||d13×e34||; if h≥r1+r2, the cylinders have no collision, otherwise proceed to the next step;

[0085] In the x-axis direction, project P3 and P4 onto the x-axis to obtain projection points P3' and P4'; the offset 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 into: when b13'≤-(1+k) / 2*l34 or b13'≥l12+(1-k) / 2*l34, the cylinder has no collision;

[0086] When the middle axes are not in the same plane or intersecting, establish the system Sp with P1 as the origin so that the positive direction of the z axis is Figure 2-5 The normal vector n is in the same direction, and the positive direction of the y axis is in the same direction as d12 ( Figure 8 ); the XYZ axes of the new system are: Z = n / ||n||, Y = e12, X = Y×Z, so the transformation matrix Tpk from the old system Sk to the new system Sp is:

[0087]

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

[0089] In the z-axis direction, if the mid-axis distance dist = |z3| is greater than or equal to r1+r2, the cylinder has no collision, otherwise proceed to the next step;

[0090] On the xOy plane, the parts of the cylinders that may collide are projected onto the xOy plane, approximately transformed into the problem of rectangle coincidence; points P1 and P2 are already on the xOy plane, and by setting the values of z3 and z4 to 0, the projected points P3' and P4' of P3 and P4 are obtained; assuming the collision layer thickness ( Fig. 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] Assume that the projection of the collision interval of cylinder i on the xOy plane is rectangle i, and the four endpoints of rectangle 2 are A, B, C, D ( Fig.10 ); if any of the four endpoints is inside rectangle 1, it is determined as a collision. Assume that the unit direction vector e34 from P3' to P4' is e34 = (x34, y34, 0), x34 ≠ 0, then the unit direction vector ω for the width extension of rectangle 2 is ω = (y34, -x34, 0), and the coordinates of points A, B, C, D can be obtained from P3' ± a2 * ω and P4' ± a2 * ω; if the coordinates (x, y, z) of any of points A, B, C, 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, and 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 and not 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 manipulator, characterized in that: The following steps are involved: Step 1: Construct a robot model: For each joint in the multi-DOF robot, 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 by a rotation transformation relative to the parent system, and the three-axis direction of the child system is determined. Based on the coordinate system model of the robot, the three links from the base to the wrist are respectively enclosed by cylinders with radii of r0, r1, and r2 and heights of l0, l1, and l2 to complete the construction of the robot model. Step 2: Implement the inverse solution process: The input of the inverse solver is the end position and the constraint of a certain dimension of the elbow, and the output is the joint angle. The overall process is: first calculate the coordinates of the key points based on the end position and elbow information, and use the collision detection algorithm to verify the legitimacy of the solution; Then, based on the key points and the end posture, the candidate solutions of the angle are obtained and corrected to the legal range using the correction algorithm; the candidate value ji often has multiple solutions, and the solutions are sought from the parent system to the child system. If a joint ji has no solution, backtrack to j(i-1) and explore other solutions of j(i-1), where i = 2, ..., 7.

2. The inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom manipulator according to claim 1, characterized in that: In the step 1, the origin of the coordinate system is translated on the rotation axis, and the coordinate systems of j1, j3, and j5 are translated to P0, P1, and P2 respectively, 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 manipulator according to claim 1, characterized in that: In step 2, the specific sub-steps of implementing the inverse solution process are: S21. Define mathematical quantities: Define the translation matrix and the rotation matrix along the X / Y / Z axis. 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 position of the end (wrist) in the base system be, where the attitude angles are in the order of "yaw, pitch, roll". According to the translation matrix (x e ,y e ,z e ,α,β,γ) The meaning of the rotation matrix along the X / Y / Z axis and the end position can be obtained by the transformation matrix T70 from S0 to S7; S22. Calculate the key points: the position of the shoulder remains unchanged during the movement, so the constant coordinates 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 the solution is y2_0 = l0-l1, the connecting rod P0P1 collides with the connecting rod P1P2, and the solution is discarded; S23, calculate the angle: if the key point has a solution, solve the angle according to the key point; in the Sn 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 system, record the Zn+2 axis before and after the rotation of the joint jn+1, and use the function to solve jn+1 (0≤n≤5); S24, correcting the angle of the solved associated point: the above inverse solution has solved several candidate solutions, and the range of these solutions is [-pi, pi]. Next, the solution results should be corrected to a legal range according to the angle limit and amplitude requirements. If the correction is not possible, the group of solutions will be discarded; S25. Detect self-collision behavior: The collision of adjacent connecting rods can be avoided by angle limiting, and the collision of non-adjacent connecting rods needs to be detected separately; when the collision body is modeled, the connecting rod is regarded as a cylinder. If the cylinders have intersections, it is regarded as a self-collision; the relationship between two straight lines in three-dimensional space is divided into four situations: parallel, coincident, intersecting and non-coplanar.

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

5. The inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom manipulator according to claim 4, characterized in that: The mathematical quantities defined in the step S21 further include: The suffix _n represents the mathematical quantity in the Sn system; dij_n = (xij_n, yij_n, zij_n) represents the vector from point Pi to point Pj in the Sn system; The functions (2-1) and (2-2) are respectively point transformation and vector transformation operations, and the function (2-3) is the rotation angle of vector v2 relative to vector v1 on [-pi, pi]; PointP = multTP(MatrixT0, PointP0) st(P,1) T =(P x ,P y ,P z ,1) T =T0(P 0x ,P 0y ,P 0z ,1) T Function (2-1) 6. The inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom manipulator according to claim 3, characterized in that: In the step S22, the first case is the elbow constraint: when the value of a certain dimension (X or Y or Z) of the elbow is m, two distance equations can be联立 to find P2_0; The second case is elbow self-adaptation (that is, the unconstrained case): when the elbow position is unconstrained, it adapts to the position with the smallest movement amplitude.

7. The inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom manipulator according to claim 5, characterized in that: In the step S23, the specific method for solving the angles of the key points is: S231. Solve j1 and j2: That is, P2_0 is on the Y0 axis. At this time, j2 = 0 and j1 has infinitely many solutions; to make the movement amplitude the smallest, let j1 traverse from the old value along a certain step size towards the relative ±pi; When l0 - l1 < y2_0 < l0 + l1; in the S0 system, the original axial direction oriZ2_0 of Z2_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; use equations (2-2) and the function (2-2) to transform to the S1 system, the original axial direction oriZ3_1 of Z3_1 = (0, 0, 1), and the current axial direction is the same as d12_1; find the vector d12_1 = multTV(T10, d12_0), then j2 = calcuVecAng(oriZ3_1, d12_1, -d12_1[1]); S232. Solve j3 and j4: After obtaining the values of j1 and j2, find P3_2 = multTP(T21, multTP(T10, P3_0)), and the other mathematical quantities can also be transformed to the S2 system by a similar method. First, discuss the singular point x3_2 = -l1 - l2; at this time, j4 = 0 and j3 has infinitely many solutions; to make the movement amplitude the smallest, let j3 traverse from the old value along a certain step size towards 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), 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 amplitude, 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 Z6_4, oriZ6_4 = (0, 0, 1). Since d23_4 is always in the same direction as Y6_4, and X6_4 is always in the opposite direction to Z7_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]). 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 calculating Z7_4, calculate the current axial direction X7_6, then j7 = calcuVecAng(oriX7_6, X7_6, X7_6[1]).

8. The inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom manipulator according to claim 4, characterized in that: 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 amplitude does not exceed g. When any angle output is false, terminate the loop and discard the solution. When all angle outputs are true, retain the corrected solution.

9. The inverse kinematics solution and self-collision detection method based on a multi-degree-of-freedom manipulator according to claim 4, characterized in that: 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 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 are obtained; 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 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 ω = (y34, -x34, 0) for the width extension of rectangle 2, 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 of the points A, B, C, and D satisfy "x ∈ (-a1, a1) ∧ y ∈ (0, l12)", then the cylinder collides.

Images