Geometric inverse solution calculation method for six-degree-of-freedom mechanical arm

Abstract

The invention relates to the field of robot control and kinematics, in particular to a six-degree-of-freedom mechanical arm geometric inverse solution calculation method which comprises the following steps that S1, a mechanical arm structure is analyzed, and a coordinate system is established; s2, constructing a D-H parameter table according to the established coordinate system; s3, the tail end position of the mechanical arm is determined; s4, solving the expression 0P6 of the coordinate origin 6 in the coordinate system 0, and solving an angle theta1 according to the coordinate projection of the coordinate origin 6 in the coordinate system 0; s5, a coordinate system 7 represents # imgabs0 # in the coordinate system 1, an axis z7 represents # imgabs1 # in the coordinate system 1, a position 1P4 of a coordinate origin 4 is solved, and a triangle is constructed to solve an angle theta3 and an angle theta2; s6, constructing a coordinate system 8, solving a representation 8P6 of a coordinate origin 6 in the coordinate system 8, and solving an angle theta 4 according to projection; s7, solving a representation 4P7 of the origin 7 of the coordinates in the coordinate system 4, and solving an angle theta 5 according to the projection; and S8, solving the expression # imgabs2 of the coordinate system 7 in the coordinate system 5, and solving an angle theta 6 according to the projection of x7 in the coordinate system 5. The method overcomes the problems of high calculation complexity, poor real-time performance and poor adaptability of a traditional method.

Description

Technical Field

[0001] The present invention relates to the fields of robot control and kinematics, and in particular to a geometric inverse solution calculation method for a six-degree-of-freedom robotic arm. Background Art

[0002] With the advancement of science and technology, industrial development, and rising labor costs, the development of robotics has garnered significant attention worldwide and has become a key area of national policy support. Today, various types of robots are widely used in various fields. As machines that assist humans in completing various tasks, robots have diverse application scenarios, encompassing household, medical, educational, and public spaces. Robots assisting humans in work require the ability to recognize scenarios, clearly perceive, and dynamically adapt to the service tasks they face in order to complete specific task planning. Robots also require bottom-level motion capabilities, as tasks such as catering and household assistance require mobility. Robots also require hand motion capabilities, as most robot tasks require the use of hands. For example, surgical robots in the medical field and service robots in the catering field both require hands for operation. Robot hand motion primarily involves both forward and inverse solutions for the robot arm. The inverse solution for the robot arm is crucial for determining the joint angles required to position and pose the robot's end effector. It is essential for the robot arm to perform its tasks. Numerous inverse solution algorithms have been proposed. While traditional numerical iterative methods are highly versatile, they suffer from slow convergence, poor real-time performance, and a tendency to fall into local optima. Analytical methods rely on specific structures and are difficult to adapt to various configurations. Therefore, a method that can quickly and accurately achieve the inverse geometric solution of a six-degree-of-freedom manipulator is proposed. This method is especially suitable for manipulators with non-standard structures and is of great significance for the execution of tasks by manipulators. Summary of the Invention

[0003] Based on the problems mentioned above, this paper provides a geometric inverse solution calculation method for a six-degree-of-freedom robotic arm. It overcomes the problems of high computational complexity, poor real-time performance, and poor adaptability of traditional methods, achieves efficient and accurate solution of the end-point pose, and analyzes the Drobot CR5 robotic arm.

[0004] The present invention proposes an inverse solution algorithm for a six-degree-of-freedom manipulator that combines geometric decomposition and trigonometric function analysis, comprising the following steps:

[0005] The above technical objectives of the present invention are achieved through the following technical solutions: A six-degree-of-freedom manipulator geometry inverse solution calculation method, comprising the following steps:

[0006] S1. Analyze the structure of the robotic arm and establish a coordinate system;

[0007] S2. Construct a DH parameter table based on the established coordinate system;

[0008] S3, determine the end position of the robotic arm;

[0009] S4. Solve the representation of the coordinate origin 6 in the coordinate system 0 0 P6, solve the angle θ1 based on the coordinate projection of the coordinate origin 6 in the coordinate system 0;

[0010] S5, representation of coordinate system 7 in coordinate system 1 Find the representation of axis z7 in coordinate system 1 Find the position of coordinate origin 4 1 P4, construct a triangle to find angle θ3 and angle θ2;

[0011] S6. Construct coordinate system 8 and find the representation of coordinate origin 6 in coordinate system 8 8 P6, calculate angle θ4 based on projection

[0012] S7. Find the representation of coordinate origin 7 in coordinate system 4 4 P7, calculate the angle θ5 based on the projection;

[0013] S8. Find the representation of coordinate system 7 in coordinate system 5 Calculate angle θ6 based on the projection of x7 on coordinate system 5.

[0014] Preferably, the step S1 uses a Dobot CR5 robot arm with six degrees of freedom, as shown in the following example: Figure 1 Establish the coordinate system as shown.

[0015] Preferably, the step S2 specifically includes establishing a DH parameter table as shown in Table 1 based on the coordinate system established in step S1.

[0016] Preferably, the coordinate transformation matrix of the adjacent coordinate system of the manipulator in the coordinate system established in step S1 is:

[0017]

[0018] The pose of the end of the robotic arm is defined as

[0019]

[0020] Preferably, the step S4 of solving the angle θ1 is specifically as follows:

[0021] Let the coordinate origin 6 be expressed in coordinate system 7 as 7 P6, the coordinate origin 6 in the coordinate system 0 is represented as 0 P6

[0022]

[0023] The schematic diagram for solving angle θ1 is as follows Figure 2 , Figure 2 The projection of the mid-coordinate origin 6 on the xy plane of coordinate system 0 is P2, the tangent point of P2 and the circle with R=0.141 is P3, and the distance between P2 and P3 is L1:

[0024]

[0025] Preferably, the step S5 of solving θ2 and θ3 is specifically as follows:

[0026] Step S4 has obtained two sets of solutions for θ1. The following solution process uses θ1 to replace the two sets of solutions for θ1. Indicates the representation of coordinate system 1 to coordinate system 0, with Indicates the representation of coordinate system 7 under coordinate system 1, 1 P6 represents the coordinate origin 6 in coordinate system 1:

[0027]

[0028] Let the z-axis of coordinate system 7 be expressed in coordinate system 1 as is perpendicular to The normal of the plane, the position of the coordinate origin 4 is perpendicular to The intersection of the plane of y and the plane of y = -0.141 is The intersection of the circles, let Let y be the normal of the plane with y = -0.141m, is perpendicular to 1 The intersection vector of the plane of Z7 and the plane of y = -0.141m is The length of the vector is

[0029]

[0030] 1 The position relationship diagram of P4 is as follows Figure 3 , 1 The position of P4 is expressed as:

[0031]

[0032] But when 1 When Z7 is perpendicular to the plane y=-0.141, P4 has an infinite number of solutions, so it is necessary to sample the position of P4 to ensure 1 P2, 1 P3, 1 P4 can form a triangle. In this case, 1 P6 is the center of the circle R 46 Uniform sampling on a circle with a radius of 1 P4 position;

[0033] The above steps yield two 1 P4 position, 1 P4 stands for 1 Two positions of P4 1 P 41 、 1 P 42 ;

[0034] like Figure 4 It is shown that there are two sets of solutions for each position of P4, and the following relationship exists between θ2 and θ3:

[0035]

[0036] make 1 P 4xz for 1 Projection vector of P4 on the xz plane of coordinate system 1:

[0037]

[0038] make for 1 P2 and 1 The distance between P3, let for 1 P3 and 1 Distance between P4:

[0039]

[0040] θ 31 =180-∠ 1 P4 1 P3 1 P2*180 / π (1.17)

[0041] θ 32 =-θ 31 (1.18)

[0042] 1 θ 4xz =-atan2( 1 P x4 , 1 P z4 )*180 / π (1.19)

[0043] Let θ 21 and θ 31 Correspondingly, let θ 22 and θ 32 Correspondingly:

[0044]

[0045] Since the positive direction of coordinate axis 3 is opposite to the projection direction of θ3 above, θ3 needs to be negated:

[0046] θ 31 =-θ 31 (1.22)

[0047] θ 31 =-θ 31 (1.23)

[0048] Preferably, the step S6 of solving the angle θ4 is specifically as follows:

[0049] The above steps have solved θ1, θ2, and θ3. Step S4 has two solutions for θ1, which are generalized here as θ1. Step S4 has two solutions for P4, and θ2 and θ3 have two solutions for each P4, which are generalized here as θ2 and θ3. Although there are multiple solutions for both steps S4 and S5, resulting in a total of 4 solutions for the first two steps, the analysis here still uses θ1, θ2, and θ3 for generalization. This is because the solution process for the following angles is the same for the above 8 solutions. The specific calculations can be calculated according to the formulas introduced below. Therefore, the following analysis only focuses on the generalized form.

[0050] make is the representation of coordinate system 2 on coordinate system 1, let is the representation of coordinate system 3 on coordinate system 2, then the representation of coordinate system 1 on coordinate system 2 Representation of coordinate system 2 on coordinate system 3

[0051] Establish auxiliary coordinate system 8, the auxiliary coordinate system is as follows Figure 1 ,make The representation of coordinate system 8 in coordinate system 3 is make The representation of coordinate system 1 in coordinate system 8 is:

[0052]

[0053]

[0054] according to Figure 5 The schematic diagram of θ4 solution can be obtained:

[0055] θ4=-atan2( 8 P x6 , 8 P y6 )*180 / π(1.26)

[0056] Preferably, the step S7 of solving the angle θ5 is specifically as follows:

[0057] If θ4 is known, we can get

[0058]

[0059] according to Figure 6 The schematic diagram of θ5 solution can be obtained:

[0060] θ5=atan2( 4 P x7 , 4 P z7 )*180 / π(1.30)

[0061] Preferably, the step S8 of solving the angle θ6 is specifically as follows:

[0062] If θ5 is known, we can get

[0063]

[0064] according to Figure 7 The schematic diagram of θ6 solution can be obtained:

[0065] θ6=atan2( 5z X7, 5x X7)*180 / π(1.34)

[0066] In summary, the present invention has the following beneficial effects: it overcomes the problems of high computational complexity, poor real-time performance, and poor adaptability of traditional methods, and realizes efficient and accurate solution of end-point posture, and is particularly suitable for trajectory planning and joint angle solution in industrial robots, service robots and automation equipment, especially in the inverse kinematics solution of robots with specific configurations (such as spherical wrists and planar structures), and has high efficiency, determinism and stability; it directly calculates joint angles through geometric relationships (such as triangular decomposition), has fast calculation speed, naturally excludes non-physical solutions, avoids the divergence risk of iterative methods and multi-solution screening of analytical methods, and still maintains numerical stability near singular points, which is the simplest optimal solution to achieve real-time control. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] Figure 1 This is a schematic diagram of the robotic arm structure and coordinate system;

[0068] Figure 2 Schematic diagram for solving θ1;

[0069] Figure 3 This is a schematic diagram of the P4 position under normal circumstances;

[0070] Figure 4 Schematic diagram for solving θ2 and θ3;

[0071] Figure 5 Schematic diagram for solving θ4;

[0072] Figure 6 Schematic diagram for solving θ5;

[0073] Figure 7 Schematic diagram for solving θ6. DETAILED DESCRIPTION

[0074] In order to enable those skilled in the art to better understand the present invention, the embodiments of the present invention will be described below based on the Dobot CR5 robotic arm. The embodiments described below are only part of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work should fall within the scope of protection of the present invention. The purpose of the present invention is to propose a robotic arm inverse solution technology that quickly completes the robotic arm inverse solution operation through a geometric solution, thereby improving the efficiency of the robotic arm's grasping tasks.

[0075] The following technical solution implementation case uses the Dobot CR5 robotic arm with six degrees of freedom. Its structural diagram and coordinate system are established as follows: Figure 1 , where the blue line is the x-axis, the yellow line is the y-axis, and the green line is the z-axis. i Indicates the coordinate origin of coordinate system i. The DH parameter table based on this coordinate system establishment method is shown in Table 1 below.

[0076] Table 1D-H parameter table

[0077]

[0078]

[0079] Figure 1 The coordinate transformation matrix of the adjacent coordinate system of the manipulator under the established coordinate system is:

[0080]

[0081] The inverse solution method of the robot arm geometry of the present invention is applicable to the description of the end position of the robot arm based on the base and the end position of the robot arm is known. In the case description, it is assumed that the end position of the robot arm is

[0082]

[0083] Step 1: Solve for angle θ1

[0084] Let the coordinate origin 6 be expressed in coordinate system 7 as 7 P6, the coordinate origin 6 in the coordinate system 0 is represented as 0 P6

[0085]

[0086] The schematic diagram for solving angle θ1 is as follows Figure 2 , Figure 2 The projection of the coordinate origin 6 on the xy plane of the coordinate system 0 is P2, the tangent point of P2 and the circle R = 0.141 is P3, and the distance between P2 and P3 is L1

[0087]

[0088]

[0089] Step 2: Solve θ2 and θ3

[0090] Step 1 has obtained two sets of solutions for θ1. In the following solution process, θ1 is used to replace the two sets of solutions for θ1. Indicates the representation of coordinate system 1 to coordinate system 0, with Indicates the representation of coordinate system 7 under coordinate system 1. 1 P6 represents the representation of coordinate origin 6 in coordinate system 1.

[0091]

[0092] Let the z-axis of coordinate system 7 be expressed in coordinate system 1 as is perpendicular to The normal of the plane, the position of the coordinate origin 4 is perpendicular to The intersection of the plane of y and the plane of y = -0.141 is The intersection of the circle. Let y be the normal of the plane with y = -0.141m, is perpendicular to 1 The intersection vector of the plane of Z7 and the plane of y = -0.141m is The length of the vector is

[0093]

[0094] In general 1 The position relationship diagram of P4 is as follows Figure 3 , 1 The position of P4 can be expressed as

[0095]

[0096] But when 1 When Z7 is perpendicular to the plane y=-0.141, P4 has an infinite number of solutions, so it is necessary to sample the position of P4 to ensure 1 P2, 1 P3, 1P4 can form a triangle. For this case, the method adopted in this paper is to 1 P6 is the center of the circle R 46 Uniform sampling on a circle with a radius of 1 The above steps at P4 position yield two 1 P4 position, below 1 P4 stands for 1 Two positions of P4 1 P 41 、 1 P 42 .

[0097] as follows Figure 4 It is shown that there are two sets of solutions for each position of P4, and the relationship between θ2 and θ3 is as follows

[0098]

[0099] make 1 P 4xz for 1 Projection vector of P4 on the xz plane of coordinate system 1

[0100]

[0101] make for 1 P2 and 1 The distance between P3, let for 1 P3 and 1 The distance between P4

[0102]

[0103] θ 31 =180-∠ 1 P4 1 P3 1 P2*180 / π (1.17)

[0104] θ 32 =-θ 31 (1.18)

[0105] 1 θ 4xz =-atan2( 1 P x4 , 1 P z4 )*180 / π (1.19)

[0106] Let θ 21 and θ 31 Correspondingly, let θ 22 and θ32 Corresponding

[0107]

[0108] Since the positive direction of coordinate axis 3 is opposite to the projection direction of θ3 above, θ3 needs to be negated:

[0109] θ 31 =-θ 31 (1.22)

[0110] θ 31 =-θ 31 (1.23)

[0111] Step 3: Find θ4, θ5, θ6

[0112] The above two steps have already calculated θ1, θ2, and θ3. Step 1 yields two sets of solutions for θ1, which we generalize here using θ1. Step 2 yields two sets of solutions for P4. θ2 and θ3 each have two sets of solutions for each P4, which we generalize here using θ2 and θ3. Although both Step 2 and Step 1 have multiple solutions, resulting in a total of four solutions for the first two steps, we still generalize them using θ1, θ2, and θ3 for the analysis here. This is because the solution process for the following angles is the same for the eight solutions above. The specific calculations can be performed according to the formulas presented below, so the analysis below focuses solely on the generalized form.

[0113] make is the representation of coordinate system 2 on coordinate system 1, let is the representation of coordinate system 3 on coordinate system 2, then the representation of coordinate system 1 on coordinate system 2 Representation of coordinate system 2 on coordinate system 3

[0114] Establish auxiliary coordinate system 8, the auxiliary coordinate system is as follows Figure 1 ,make The representation of coordinate system 8 in coordinate system 3 is make The representation of coordinate system 1 in coordinate system 8

[0115]

[0116] according to Figure 5 The schematic diagram for solving θ4 can be obtained

[0117] θ4=-atan2( 8 P x6 , 8 P y6 )*180 / π(1.26)

[0118] If θ4 is known, we can get

[0119]

[0120] according to Figure 6 The schematic diagram for solving θ5 can be obtained

[0121] θ5=atan2( 4 P x7 , 4 P z7 )*180 / π(1.30)

[0122] If θ5 is known, we can get

[0123]

[0124] according to Figure 7 The schematic diagram for solving θ6 can be obtained

[0125] θ6=atan2( 5z X7, 5x X7)*180 / π(1.34)

[0126] The above steps introduce the general solution algorithm for the Dobot CR5 robotic arm. Next, we will verify the accuracy of the algorithm by focusing on the end-point posture. Here, we input six joint angles, obtain the forward solution, and then use the forward solution to solve the inverse solution to verify the correctness of the algorithm.

[0127] Table 2 Correct solution input angle

[0128]

[0129] For the above input, the terminal pose obtained by the correct solution is as follows

[0130]

[0131]

[0132] Input the pose result of equation 1.35 above into the inverse solution algorithm to obtain the following inverse solution result:

[0133]

[0134] Input the pose result of Equation 1.36 above into the inverse solution algorithm to obtain the following inverse solution result:

[0135]

[0136] In practice, this algorithm can quickly obtain the inverse solution for any reasonable posture. Here, only the above two sets of solutions are used for verification.

Claims

1. A geometric inverse solution calculation method for a six-degree-of-freedom manipulator, characterized in that: The following steps are involved: S1. Analyze the structure of the robotic arm and establish a coordinate system; S2. Construct a DH parameter table based on the established coordinate system; S3, determine the end position of the robotic arm; S4. Solve the representation of the coordinate origin 6 in the coordinate system 0 0 P6, solve the angle θ1 based on the coordinate projection of the coordinate origin 6 in the coordinate system 0; S5, representation of coordinate system 7 in coordinate system 1 Find the representation of axis z7 in coordinate system 1 Find the position of coordinate origin 4 1 P4, construct a triangle to find angles θ3 and θ2; S6. Construct coordinate system 8 and find the representation of coordinate origin 6 in coordinate system 8 8 P6, calculate angle θ4 based on projection S7. Find the representation of coordinate origin 7 in coordinate system 4 4 P7, calculate the angle θ5 based on the projection; S8. Find the representation of coordinate system 7 in coordinate system 5 Calculate angle θ6 based on the projection of x7 on coordinate system 5.

2. A six-degree-of-freedom robotic arm geometry inverse solution calculation method according to claim 1, characterized in that: In step S1 , a Dobot CR5 robot arm with six degrees of freedom is used to establish a coordinate system as shown in FIG1 .

3. A six-degree-of-freedom manipulator geometry inverse solution calculation method according to claim 2, characterized in that: Specifically, step S2 is to establish a DH parameter table as shown in Table 1 based on the coordinate system established in step S1.

4. A six-degree-of-freedom robotic arm geometry inverse solution calculation method according to claim 3, characterized in that: The coordinate transformation matrix of the adjacent coordinate system of the manipulator under the coordinate system established in step S1 is: The pose of the end of the robotic arm is defined as 5. The method for calculating the geometric inverse solution of a six-degree-of-freedom manipulator according to claim 4, characterized in that: The step S4 for solving the angle θ1 is specifically as follows: Let the coordinate origin 6 be expressed in coordinate system 7 as 7 P6, the coordinate origin 6 in the coordinate system 0 is represented as 0 P6 The schematic diagram of solving the angle θ1 is shown in Figure 2. In Figure 2, the projection of the coordinate origin 6 on the xy plane of the coordinate system 0 is P2, the tangent point of P2 and the circle R = 0.141 is P3, and the distance between P2 and P3 is L1:

6. A six-degree-of-freedom robotic arm geometry inverse solution calculation method according to claim 5, characterized in that: The step S5 for solving θ2 and θ3 is specifically as follows: Step S4 has obtained two sets of solutions for θ1. The following solution process uses θ1 to replace the two sets of solutions for θ1. Indicates the representation of coordinate system 1 to coordinate system 0, with Indicates the representation of coordinate system 7 under coordinate system 1, 1 P6 represents the coordinate origin 6 in coordinate system 1: Let the z-axis of coordinate system 7 be expressed in coordinate system 1 as is perpendicular to The normal of the plane, the position of the coordinate origin 4 is perpendicular to The intersection of the plane of y and the plane of y = -0.141 is The intersection of the circles, let Let y be the normal of the plane with y = -0.141m, is perpendicular to 1 The intersection vector of the plane of Z7 and the plane of y = -0.141m is The length of the vector is 1 The position relationship diagram of P4 is shown in Figure 3. 1 The position of P4 is expressed as: But when 1 When Z7 is perpendicular to the plane y=-0.141, P4 has an infinite number of solutions, so it is necessary to sample the position of P4 to ensure 1 P2, 1 P3, 1 P4 can form a triangle. In this case, 1 P6 is the center of the circle R 46 Uniform sampling on a circle with a radius of 1 P4 position; The above steps yield two 1 P4 position, 1 P4 stands for 1 Two positions of P4 1 P 41 、 1 P 42 ; As shown in Figure 4, there are two sets of solutions for each position of P4, and the following relationship exists between θ2 and θ3: make 1 P 4xz for 1 Projection vector of P4 on the xz plane of coordinate system 1: make for 1 P2 and 1 The distance between P3, let for 1 P3 and 1 Distance between P4: i 31 =180-∠ 1 P4 1 P3 1 P2*180 / π(1.17) i 32 =-θ 31 (1.18) 1 i 4xz =-atan2( 1 P x4 , 1 P z4 )*180 / π(1.19) Let θ 21 and θ 31 Correspondingly, let θ 22 and θ 32 Correspondingly: Since the positive direction of coordinate axis 3 is opposite to the projection direction of θ3 above, θ3 needs to be negated: i 31 =-θ 31 (1.22) i 31 =-θ 31 (1.23) 7. A six-degree-of-freedom robotic arm geometry inverse solution calculation method according to claim 6, characterized in that: The step S6 for solving the angle θ4 is specifically as follows: The above steps have solved θ1, θ2, and θ3. Step S4 has two solutions for θ1, which are generalized here as θ1. Step S4 has two solutions for P4, and θ2 and θ3 have two solutions for each P4, which are generalized here as θ2 and θ3. Although there are multiple solutions for both steps S4 and S5, resulting in a total of 4 solutions for the first two steps, the analysis here still uses θ1, θ2, and θ3 for generalization. This is because the solution process for the following angles is the same for the above 8 solutions. The specific calculations can be calculated according to the formulas introduced below. Therefore, the following analysis only focuses on the generalized form. make is the representation of coordinate system 2 on coordinate system 1, let is the representation of coordinate system 3 on coordinate system 2, then the representation of coordinate system 1 on coordinate system 2 Representation of coordinate system 2 on coordinate system 3 Establish auxiliary coordinate system 8, the auxiliary coordinate system is shown in Figure 1, let The representation of coordinate system 8 in coordinate system 3 is make The representation of coordinate system 1 in coordinate system 8 is: According to the schematic diagram of θ4 solution in Figure 5, we can get: θ4=-atan2( 8 P x6 , 8 P y6 )*180 / π(1.26) 8. The method for calculating the geometric inverse solution of a six-degree-of-freedom manipulator according to claim 7, wherein: The step S7 for solving the angle θ5 is specifically as follows: If θ4 is known, we can get According to the schematic diagram of θ5 solution in Figure 6, we can get: θ5=atan2( 4 P x7 , 4 P z7 )*180 / π(1.30) 9. A six-degree-of-freedom robotic arm geometry inverse solution calculation method according to claim 8, characterized in that: The step S8 for solving the angle θ6 is specifically as follows: If θ5 is known, we can get According to the schematic diagram of θ6 solution in Figure 7, we can get: θ6=atan2( 5z X7, 5x X7)*180 / π(1.34).

Images