A six-degree-of-freedom robot arm geometric inverse solution calculation method

Abstract

The present application relates to the field of robot control and kinematics, in particular to a kind of six degrees of freedom mechanical arm geometric inverse solution calculation method, comprising the following steps: S1, the structure of mechanical arm is analyzed, establishes coordinate system;S2, according to the coordinate system built D-H parameter table is constructed;S3, the position of mechanical arm end is determined;S4, the representation of coordinate origin 6 in coordinate system 0 is solved 0 P6, according to the coordinate projection of coordinate origin 6 in coordinate system 0, angle θ1 is solved;S5, the representation of coordinate system 7 in coordinate system 1 is solved, the representation of axis z7 in coordinate system 1 is solved, the position of coordinate origin 4 is solved 1 P4, construct triangle to solve angle θ3 and angle θ2;S6, construct coordinate system 8, solve the representation of coordinate origin 6 in coordinate system 8 8 P6, according to projection, angle θ4 is solved;S7, the representation of coordinate origin 7 in coordinate system 4 is solved 4 P7, according to projection, angle θ5 is solved;S8, the representation of coordinate system 7 in coordinate system 5 is solved, according to the projection of x7 in coordinate system 5, angle θ6 is solved, the present application overcomes the problems of traditional method, and the problems of high complexity, poor real-time performance and poor adaptability are solved.

Description

Technical Field

[0001] This invention relates to the field of robot control and kinematics, specifically to a geometric inverse kinematics calculation method for a six-degree-of-freedom robotic arm. Background Technology

[0002] With the advancement of science and technology, industrial development, and rising labor costs, the development of robotics technology has gained high attention from various countries and has become a key area supported by national policies. 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, covering homes, healthcare, education, public spaces, and more. Robots assisting humans in work need the ability to recognize scenarios and clearly understand and dynamically adapt to the service tasks they face in order to complete specific task planning. Robots need bottom mobility, requiring movement capabilities for tasks such as catering and home assistance. Robots also need hand mobility, as most robot tasks require the use of hands, such as surgical robots in the medical field and service robots in the catering field. Robot hand movement mainly involves the forward and inverse problems of the robotic arm. The inverse problem of the robotic arm is a key issue in determining the joint angles when the robot's end effector is positioned and oriented, and is an essential technology for the robotic arm to perform tasks. Currently, many inverse problem algorithms for robotic arms have been proposed. While traditional numerical iterative methods are highly versatile, they suffer from slow convergence speed, poor real-time performance, and a tendency to get trapped in local optima. Analytical methods rely on specific structures and are difficult to adapt to various configurations. Therefore, this paper proposes a method that can quickly and accurately realize the geometric inverse solution of a six-degree-of-freedom robotic arm, which is especially suitable for robotic arms with non-standard structures and is of great significance for the robotic arm to perform tasks. Summary of the Invention

[0003] Based on the problems mentioned above, this paper presents a geometric inverse kinematics calculation method for a six-degree-of-freedom robotic arm, which overcomes the problems of high computational complexity, poor real-time performance, and poor adaptability of traditional methods, and achieves efficient and accurate solution of end-effector pose. The method is analyzed specifically for the Drobot CR5 robotic arm.

[0004] This invention proposes an inverse kinematics algorithm for a six-degree-of-freedom robotic arm that combines geometric decomposition and trigonometric function analysis, comprising the following steps:

[0005] The above-mentioned technical objective of this invention is achieved through the following technical solution: a geometric inverse kinematics calculation method for a six-degree-of-freedom robotic arm, comprising the following steps:

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

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

[0008] S3. Determine the position of the robotic arm's end effector;

[0009] S4. Solve for the representation of the origin 6 in coordinate system O. 0 P6, solve for angle θ1 based on the coordinate projection of the origin 6 onto 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 the origin 4. 1 P4, construct a triangle to find angles θ3 and θ2;

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

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

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

[0014] Preferably, step S1 employs a Dobot CR5 robotic arm with six degrees of freedom, as follows: Figure 1 Establish the coordinate system as shown.

[0015] Preferably, step S2 specifically involves 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 between the adjacent coordinate systems of the robotic arm in the coordinate system established in step S1 is:

[0017]

[0018] The pose of the robotic arm's end effector is defined as follows:

[0019]

[0020] Preferably, step S4, which involves solving for angle θ1, specifically involves:

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

[0022]

[0023] The diagram for solving angle θ1 is shown below. Figure 2 , Figure 2 The projection of the origin 6 onto the xy plane of coordinate system 0 is P2. The point of tangency between P2 and the circle R = 0.141 is P3. The distance between P2 and P3 is L1.

[0024]

[0025] Preferably, step S5, which involves solving for θ2 and θ3, specifically involves:

[0026] Step S4 has yielded two solutions for θ1. The following solution process will use θ1 to replace the two solutions for θ1. Representation of coordinate system 1 to coordinate system 0, using This represents the representation of coordinate system 7 in coordinate system 1, using... 1 P6 represents the coordinate origin 6 in coordinate system 1:

[0027]

[0028] Let the z-axis of coordinate system 7 be represented in coordinate system 1 as follows: To be perpendicular to The normal to the plane, the position of the origin 4 is perpendicular to The line of intersection between the plane and the plane with y = -0.141 and Let the intersection of the circles be... Let be the normal to the plane with y = -0.141m. To be perpendicular to 1 Let the vector of the intersection of the plane Z7 and the plane y = -0.141m be given. The vector length is

[0029]

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

[0031]

[0032] But when 1 When Z7 is perpendicular to the plane y = -0.141, P4 has infinitely many solutions. Therefore, 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 R 46 Uniform sampling is performed on a circle with radius , and two samples that meet the conditions are randomly selected. 1 Position P4;

[0033] The above steps yield two results. 1 Position P4, with 1 P4 represents 1 Two positions of P4 1 P 41 , 1 P 42 ;

[0034] like Figure 4 As shown, there are two solutions for each position of P4, and θ2 and θ3 have the following relationship:

[0035]

[0036] make 1 P 4xz for 1 The projection vector of P4 onto the xz plane in coordinate system 1:

[0037]

[0038] make for 1 P2 and 1 The distance between P3 is given 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 With θ 31 Correspondingly, let θ 22 With θ 32 Corresponding to:

[0044]

[0045] Since the positive direction of coordinate axis 3 is opposite to the projection direction used to calculate θ3, it is necessary to invert θ3:

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

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

[0048] Preferably, step S6, which involves solving for angle θ4, specifically involves:

[0049] The above steps have yielded θ1, θ2, and θ3. Step S4 yields two solutions for θ1, which are represented here as θ1. Step S4 also yields two solutions for P4, and θ2 and θ3 each yield two solutions for each P4, which are represented here as θ2 and θ3. Although steps S4 and S5 each have multiple solutions, resulting in a total of four solutions obtained in the first two steps, we will still use θ1, θ2, and θ3 as generalized representations for the solution. This is because the solution process for the angles below based on the eight solutions above is the same. The specific calculations can be performed according to the formulas derived below. Therefore, the following analysis only focuses on the generalized form.

[0050] make Let coordinate system 2 be represented in coordinate system 1. For the representation of coordinate system 3 in coordinate system 2, then the representation of coordinate system 1 in coordinate system 2 is... Representation of coordinate system 2 in coordinate system 3

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

[0052]

[0053]

[0054] according to Figure 5 The schematic diagram for solving θ4 can be obtained as follows:

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

[0056] Preferably, step S7, which involves solving for angle θ5, specifically involves:

[0057] If θ4 is known, then it can be calculated.

[0058]

[0059] according to Figure 6 The schematic diagram for solving θ5 can be obtained as follows:

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

[0061] Preferably, step S8, which involves solving for angle θ6, specifically involves:

[0062] If θ5 is known, then it can be calculated.

[0063]

[0064] according to Figure 7 The schematic diagram for solving θ6 shows:

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

[0066] In summary, this invention has the following advantages: it overcomes the problems of high computational complexity, poor real-time performance, and poor adaptability of traditional methods, and achieves efficient and accurate solutions for end-effector pose. It is particularly suitable for trajectory planning and joint angle solving in industrial robots, service robots, and automated equipment. Especially in the inverse kinematics solution of robots with specific configurations (such as spherical wrists and planar structures), it combines high efficiency, determinism, and stability. It directly calculates joint angles through geometric relationships (such as trigonometric decomposition), which is fast and naturally excludes non-physical solutions. It avoids the divergence risk of iterative methods and the multiple solution selection of analytical methods, and maintains numerical stability near singular points. It is the simplest and optimal solution for real-time control. Attached Figure Description

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

[0068] Figure 2 A schematic diagram for solving θ1;

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

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

[0071] Figure 5 A schematic diagram for solving θ4;

[0072] Figure 6 A schematic diagram for solving θ5;

[0073] Figure 7 A schematic diagram for solving θ6. Detailed Implementation

[0074] To enable those skilled in the art to better understand the present invention, embodiments of the present invention will be described below based on the Dobot CR5 robotic arm. The embodiments described below are merely some, not all, of the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without inventive effort should fall within the scope of protection of the present invention. The purpose of the present invention is to propose a robotic arm inverse kinematics technology that rapidly completes inverse kinematics calculations using geometric methods, thereby improving the efficiency of robotic arm grasping tasks.

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

[0076] Table 1 D-H Parameter Table

[0077]

[0078]

[0079] Figure 1 The coordinate transformation matrix for establishing the adjacent coordinate systems of the robotic arm in the given coordinate system is as follows:

[0080]

[0081] The inverse geometry method for robotic arms of this invention is applicable to descriptions of the end-effector position based on the base and where the end-effector pose is known. The example description assumes the end-effector pose is...

[0082]

[0083] Step 1: Solve for angle θ1

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

[0085]

[0086] The diagram for solving angle θ1 is shown below. Figure 2 , Figure 2 The projection of the origin 6 onto the xy plane of coordinate system O is P2. The point of tangency between P2 and the circle R = 0.141 is P3, and the distance between P2 and P3 is L1.

[0087]

[0088]

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

[0090] Step one has yielded two solutions for θ1. The following solution process will use θ1 to represent the two solutions for θ1. Representation of coordinate system 1 to coordinate system 0, using This represents coordinate system 7 in coordinate system 1. 1 P6 represents the coordinate origin 6 in coordinate system 1.

[0091]

[0092] Let the z-axis of coordinate system 7 be represented in coordinate system 1 as follows: To be perpendicular to The normal to the plane, the position of the origin 4 is perpendicular to The line of intersection between the plane and the plane with y = -0.141 and The intersection of the circles. Let Let be the normal to the plane with y = -0.141m. To be perpendicular to 1 Let the vector of the intersection of the plane Z7 and the plane y = -0.141m be given. The vector length is

[0093]

[0094] Under normal circumstances 1 The positional relationship diagram of P4 is as follows: Figure 3 , 1 The position of P4 can be represented as follows:

[0095]

[0096] But when 1 When Z7 is perpendicular to the plane y = -0.141, P4 has infinitely many solutions. Therefore, it is necessary to sample the position of P4 to ensure... 1 P2, 1 P3 1P4 can form a triangle. In this case, the method used in this paper is to... 1 P6 is the center R 46 Uniform sampling is performed on a circle with radius , and two samples that meet the conditions are randomly selected. 1 The steps above at position P4 yielded two results. 1 Position P4, as described below 1 P4 represents 1 Two positions of P4 1 P 41 , 1 P 42 .

[0097] as follows Figure 4 As shown, there are two solutions for each position of P4, and θ2 and θ3 have the following relationship.

[0098]

[0099] make 1 P 4xz for 1 The projection vector of P4 onto the xz plane of coordinate system 1

[0100]

[0101] make for 1 P2 and 1 The distance between P3 is given 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 With θ 31 Correspondingly, let θ 22 With θ32 Corresponding

[0107]

[0108] Since the positive direction of coordinate axis 3 is opposite to the projection direction used to calculate θ3, it is necessary to invert θ3:

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

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

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

[0112] The above two steps have yielded θ1, θ2, and θ3. Step one yields two solutions for θ1, which are represented here as θ1. Step two yields two solutions for P4, and θ2 and θ3 each yield two solutions for each P4, which are represented here as θ2 and θ3. Although both steps one and two have multiple solutions, resulting in a total of four solutions in the first two steps, we will still use θ1, θ2, and θ3 for generalization in this analysis. This is because the solution process for the angles below based on the eight solutions above is the same. The specific calculations can be performed using the formulas derived below. Therefore, the following analysis only focuses on the generalized form.

[0113] make Let coordinate system 2 be represented in coordinate system 1. For the representation of coordinate system 3 in coordinate system 2, then the representation of coordinate system 1 in coordinate system 2 is... Representation of coordinate system 2 in coordinate system 3

[0114] Establish an auxiliary coordinate system 8, as follows: Figure 1 ,make The representation of coordinate system 8 in coordinate system 3 is as follows: make 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, then it can be calculated.

[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, then it can be calculated.

[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. The following section will verify the algorithm's accuracy by analyzing the end-effector posture. Here, we input the angles of six joints to obtain the forward solution, and then use the forward solution to solve for the inverse solution to verify the algorithm's correctness.

[0127] Table 2. Correct Input Angle

[0128]

[0129] For the above input, the correct solution yields the following end-effector pose:

[0130]

[0131]

[0132] Inputting the pose result from Equation 1.35 above into the inverse kinematics algorithm yields the following inverse kinematics result:

[0133]

[0134] Inputting the pose result from Equation 1.36 above into the inverse kinematics algorithm yields the following inverse kinematics result:

[0135]

[0136] In practice, this algorithm can quickly solve the inverse problem for any reasonable pose. Here, we will only use the two sets of solutions above for verification.

Claims

1. A method for calculating the geometric inverse kinematics of a six-degree-of-freedom robotic arm, characterized in that, The method comprises the following steps: S1, analyzing the mechanical arm structure and establishing a coordinate system; S2, constructing a D-H parameter table according to the established coordinate system; S3, determining the position of the mechanical arm end; S4, solving the coordinate origin 6 in the coordinate system 0 representation , according to the coordinate origin 6 in the coordinate system 0 coordinate projection solving angle ; S5, coordinate system 7 in the representation of coordinate system 1 , find the axis in the representation of coordinate system 1 , find the position of coordinate origin 4 , construct a triangle to find the angle with the angle ; S6, construct a coordinate system 8, find the representation of the coordinate origin 6 in the coordinate system 8 , find the angle according to the projection ; S7, find the coordinate origin 7 in the coordinate system 4 , according to the projection to find the angle ; S8, find the representation of coordinate system 7 in coordinate system 5 , according to find the angle in the projection of coordinate system 5 ; Solving the angle of the step S4 Specifically: Let the representation of the coordinate origin 6 in the coordinate system 7 be Let the representation of the coordinate origin 6 in the coordinate system 0 be angle Solve: The coordinate origin 6 is projected on the xy plane of the coordinate system 0 as , The distance between The tangent point of the m circle is , The distance between is : , are two sets of solutions of . The step S5 solves , Specifically: Step S4 has been calculated The following solution process uses two sets of solutions to obtain the solution. replace The two sets of solutions, Representation of coordinate system 1 to coordinate system 0, using This represents the representation of coordinate system 7 in coordinate system 1, using... Representation of the origin 6 in coordinate system 1: Let the z-axis of the coordinate system 7 be represented in the coordinate system 1 as , be the normal to the plane of , the position of the coordinate origin 4 be at the intersection of the line perpendicular to the plane of and the plane of m with the circle of , let be the normal to the plane of , let be the intersection line vector of the plane perpendicular to and the plane of , let be the vector with length : The position of the point of intersection of the lines is represented by: But when with the plane m is perpendicular There are no solutions, so need to sample the position of to ensure that , , Can form a triangle, for this case in the circle with center radius of uniform sampling, random access to two meet the conditions of position; The above steps find two positions, to , represent two positions; For each position of there are two sets of solutions, and there is the following relationship: ​ Let For The projection vector in the xz-plane of the coordinate system 1: Let be the distance between and Let be the distance between and ​ Let with corresponding, let with corresponding then: Since the positive direction of the coordinate axis 3 is opposite to the projection direction of the solution , the negative of the solution is taken: The step S6 solves the angle Specifically: The above steps have found , , , to generalized, the two sets of solutions of , to , generalized, the two sets of solutions of , for each two sets of solutions, respectively, The solution process is as follows: Let be the representation of coordinate system 2 in coordinate system 1, let be the representation of coordinate system 3 in coordinate system 2, then the representation of coordinate system 1 in coordinate system 2 is , the representation of coordinate system 2 in coordinate system 3 is ; An auxiliary coordinate system 8 is established, such that The representation of coordinate system 8 in coordinate system 3 is then , such that The representation of coordinate system 1 in coordinate system 8 is: The step S7 solves the angle Specifically: Given the known then the unknown can be found , : Solving the angle of the step S8 Specifically: known then it can be obtained , : 。 2. The method of claim 1, wherein, The step S1 adopts a Dobot CR5 mechanical arm with six degrees of freedom to establish a coordinate system.

3. The method of claim 2, wherein, The step S2 specifically comprises establishing a D-H parameter table based on the coordinate system established in the step S1.

4. The method of claim 3, wherein, The adjacent coordinate system coordinate transformation matrix of the mechanical arm under the coordinate system established in the step S1 is: The pose of the end of the robot arm is defined as : 。

Images