Robot shaft configuration and joint angle conversion method

Abstract

The invention provides a robot shaft configuration and joint angle conversion method. The robot posture is recorded by using a joint angle or a position in a Cartesian space, a quaternion and a shaft configuration parameter; the joint limit of the mechanical arm is determined, and the posture of the mechanical arm relative to the singular point is described through shaft configuration parameters; solving the shaft configuration parameters according to a preset solving method; and the input joint angle is converted into the shaft configuration parameters according to a preset conversion method. The robot shaft configuration and joint angle conversion method has the beneficial effects that quadrant number solving methods (cf1, cf4 and cf6) of the shaft 1, the shaft 4 and the shaft 6 and a calculation method of the overall configuration number (cfx) of the mechanical arm are established by analyzing the limiting range of each joint of the robot in detail. And a homogeneous transformation matrix and a standard D-H parameter model are utilized to accurately solve the joint position and posture of the robot.

Description

Technical Field

[0001] The invention belongs to the field of robot arms, and in particular relates to a robot axis configuration and joint angle conversion method. Background Art

[0002] In the field of robotics, the posture control and joint angle conversion of the robot arm are the key to achieving precise robot movement. There are two main traditional ways to record the posture of a robot: one is to record the posture in the joint space using the robot's six joint angles; the other is to record the posture in the Cartesian space using the position, posture (usually expressed as quaternions) and axis configuration parameters of the robot's end.

[0003] However, there are three singular points within the working range of the 6-DOF serial robot. When some joints of the robot (such as axis 1, axis 4, and axis 6) need to rotate more than 180°, multiple possible robot configurations will appear. This brings complexity to the forward and inverse solutions of the robot kinematics, path planning, and the implementation of the control algorithm.

[0004] In the existing technology, there is a lack of a unified and efficient algorithm to realize the conversion between joint angles and axis configuration parameters, resulting in that in practical applications, the robot may not be able to select the best motion path, affecting work efficiency and accuracy. Summary of the invention

[0005] In view of this, the present invention aims to provide a robot axis configuration and joint angle conversion method to at least solve one of the problems in the background technology.

[0006] To achieve the above object, the technical solution of the present invention is achieved as follows:

[0007] A robot axis configuration and joint angle conversion method, characterized by comprising:

[0008] Record robot poses using joint angles or positions in Cartesian space, quaternions, and axis configuration parameters;

[0009] Determine the joint limits of the robot and use axis configuration parameters to describe the posture of the robot relative to the singularity point;

[0010] Solve the axis configuration parameters according to the preset solution method;

[0011] Convert the input joint angle into axis configuration parameters according to the preset conversion method.

[0012] Further, solving the axis configuration parameters according to a preset solution method includes:

[0013] Solution of axis configuration parameter cf1: limit the quadrant division of axis configuration parameter cf1 to between -2 and 1, and the input angle range to between -175° and 175°;

[0014] Solution of axis configuration parameter cf1: limit the quadrant division of axis configuration parameter cf4 to between -2 and 1, and the input angle range to between -155° and 155°;

[0015] Solution of axis configuration parameter cf1: limit the quadrant division of axis configuration parameter cf6 to between -5 and 4, and the input angle range to between -395° and 395°.

[0016] Further, solving the axis configuration parameters according to a preset solution method includes:

[0017] Determine the relative position according to the position of the J2 joint to solve the axis configuration parameter cf2, and determine the position relative to Axis1 through the horizontal coordinate of joint 2, where:

[0018] If the horizontal axis is greater than 0, cf2=1;

[0019] If the horizontal axis is less than 0, cf2=-1.

[0020] Furthermore, the solution method for the axis configuration parameter cf2 is:

[0021] The homogeneous transformation matrix of the robot J2 joint coordinate system {O1} relative to the base coordinate system {O0} is obtained as:

[0022]

[0023] That is: P J2 =[a1c1 a1s1 d1];

[0024] Then the horizontal coordinate of the center point of the J2 joint in the base coordinate system is x J2 =a1c1, then:

[0025] When x J2 When <0, cf2=-1;

[0026] When x J2 >0, cf2=1.

[0027] Further, solving the axis configuration parameters according to a preset solution method includes:

[0028] Determined based on the position of the wrist center relative to the lower arm, the direction of the rotation axis is determined using the rotation vector between J35 and J32 and the right-hand rule is applied to solve for the axis configuration parameter cf3, where:

[0029] When the direction of the rotation axis is consistent with the y-axis direction of the base coordinate system, cf3 = 1;

[0030] When the direction is opposite, cf3=-1.

[0031] Furthermore, the solution method for the axis configuration parameter cf3 is:

[0032] The front and rear positions of the wrist center point relative to the lower arm are determined by calculating the direction of the common perpendicular line between the vector axis J35 and the vector axis J32 relative to the base coordinate system, specifically:

[0033] When the right-hand rule is used to rotate from vector J35 to vector J32, and the direction of the common perpendicular line of the rotation axis is consistent with the y-axis direction of the base coordinate system, it is recorded as cf3 = 1;

[0034] When the direction of the common perpendicular line is opposite to the y-axis direction of the base coordinate system, it is recorded as cf3=-1.

[0035] Furthermore, the solution method for the axis configuration parameter cf3 is:

[0036] Based on the STD_DH method, the homogeneous transformation matrix between the coordinate systems of adjacent links is:

[0037]

[0038]

[0039] The homogeneous transformation matrix of the robot J3 joint coordinate system {O2} relative to the base coordinate system {O0} is obtained as:

[0040]

[0041] That is: P J3 =[c1(a1+a2s2)s1(a1+a2s2)a2c2+d1];

[0042] The homogeneous transformation matrix of the robot J5 joint coordinate system {O4} relative to the base coordinate system {O0} is:

[0043]

[0044] That is, P J5 =[c1(a1+a2s2+d4c 23 )s1(a1+a2s2+d4c 23 )a2c2+d1-d4s 23 ],but:

[0045]

[0046] When n yWhen <0, cf3=-1;

[0047] When n y >0, cf3=1.

[0048] Further, solving the axis configuration parameters according to a preset solution method includes:

[0049] The axis configuration parameter cf5 is determined by the angle of the J5 joint, where:

[0050] If the angle of J5 is less than 0, cf5 = -1;

[0051] If the angle of J5 is greater than or equal to 0, cf5=1.

[0052] Furthermore, cfx is solved according to the obtained axis configuration parameters, wherein the cfx parameters represent the overall configuration relative to the singular point.

[0053] Furthermore, the method of converting the input joint angle into the axis configuration parameter according to the preset conversion method includes:

[0054] Enter the joint angles of the robot arm;

[0055] Solve the quadrant numbers cf1, cf4, and cf6 of each axis according to the joint angle range;

[0056] Use the homogeneous transformation matrix to calculate the position of joint 2 and solve cf2;

[0057] Determine cf3 through the positional relationship between joints 3 and 5;

[0058] Determine cf5 based on the angle of J5;

[0059] Finally, cfx is solved by the combination of cf2, cf3, and cf5;

[0060] Outputs the complete axis configuration parameters Confdata.

[0061] Compared with the prior art, the robot axis configuration and joint angle conversion method of the present invention has the following beneficial effects:

[0062] The robot axis configuration and joint angle conversion method described in the present invention, by analyzing the limit range of each joint of the robot in detail, establishes the quadrant numbering solution method (cf1, cf4, cf6) of axis 1, axis 4, and axis 6, as well as the calculation method of the overall configuration number (cfx) of the robot arm. The robot joint position and posture are accurately solved by using the homogeneous transformation matrix and the standard DH parameter model. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] The accompanying drawings constituting a part of the present invention are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the accompanying drawings:

[0064] Figure 1 Schematic diagrams for explaining the relative positions of the front and rear sides of the embodiment of the present invention (where (a) is a schematic diagram for explaining the position of the J1 axis center relative to Axis1, and (b) is a schematic diagram for explaining the position of the Wrist center relative to the Lower arm);

[0065] Figure 2 This is a schematic diagram of the robot posture when cfx has different values ​​according to an embodiment of the present invention. DETAILED DESCRIPTION

[0066] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other.

[0067] The present invention will be described in detail below with reference to the accompanying drawings and in conjunction with embodiments.

[0068] This solution discloses a method for robot axis configuration and joint angle conversion. The specific implementation process is as follows:

[0069] First, any posture of the robot can be recorded in a variety of forms, including: in joint space, using the robot's six joint angles to record a certain posture of the robot; or in Cartesian space, using the position of the robot's end + quaternion (posture) + axis configuration parameters to record a certain posture of the robot;

[0070] The description of the robot axis configuration parameter confdata in Robotstudio is as follows:

[0071] For a serial 6-DOF robot, there are three singular points within the robot's working range. The axis configuration is used to describe the robot's placement relative to the three singular points. The 6-DOF robot will detect whether axis 1, axis 4, and axis 6 will move more than 180°. cf1, cf4, cf6, and cfx are used to represent axis 1, axis 4, and axis 6, as well as the possible different robot configurations corresponding to cf1, cf4, and cf6.

[0072] For the robot, it is a 6-DOF robot, and the limits of the 6 joint angles are shown in Table 1 below:

[0073] Table 1 Soft limit of each joint of the robot

[0074] axis Range of motion (soft limit) 1 axis ±175° 2-axis ﹢100°～-55° 3-axis ﹢50°～-200° 4-axis ±155° 5-axis ±93° 6-axis ±395°

[0075] Therefore, the values ​​of the axis configuration parameters cf1, cf4, cf6 and cfx are defined as follows:

[0076] (1) Solution of cf1

[0077] Table 2cf1: Quadrant numbers for axis 1

[0078]

[0079] (2) Solution of cf4

[0080] Table 3cf4: Quadrant numbers for axis 4

[0081]

[0082] (3) Solution of cf6

[0083] Table 4cf6: Quadrant numbers for axis 6

[0084]

[0085] Description of cfx: cfx is numbered from 0 to 7 to represent one of eight possible robot arm configurations. Table 5 below describes various configurations of placing the robot arm relative to the three singular points, such as Figure 1 and Figure 2 As shown, taking the posture of the robot at zero position as the reference, the left side of Axis1 is the back, and the right side of Axis1 is the front; the left side of the Lower arm is the back, and the right side of the Lower arm is the front:

[0086] Table 5cfx: Various configuration numbers of the robot relative to the three singular points

[0087] cfx Axis 2 center relative to Axis 1 Wrist center relative to Lower arm Axis 5 joint angle 0 In the front, cf2 = 1 In the front, cf3 = 1 Positive, cf5=1 1 In the front, cf2 = 1 In the front, cf3 = 1 Negative, cf5 = -1 2 In the front, cf2 = 1 At the back, cf3 = -1 Positive, cf5=1 3 In the front, cf2 = 1 At the back, cf3 = -1 Negative, cf5 = -1 4 At the back, cf2 = -1 In front, cf3 = -1 Positive, cf5=1 5 At the back, cf2 = -1 In front, cf3 = -1 Negative, cf5 = -1 6 At the back, cf2 = -1 At the back, cf3=1 Positive, cf5=1 7 At the back, cf2 = -1 At the back, cf3=1 Negative, cf5 = -1

[0088] (4) Solution of cf2

[0089] First, the front and rear positions of the center point of J2 relative to Axis1 can be expressed by the position of the x-axis of the J2 point in the base coordinate system {0}:

[0090] When the horizontal coordinate of the center point of J2 is greater than 0, it means it is in the front, and it is recorded as cf2=1;

[0091] When the horizontal coordinate of the center point of J2 is less than 0, it means it is at the back, and it is recorded as cf2=-1.

[0092] The calculation method is as follows:

[0093] Based on the STD_DH method, the homogeneous transformation matrix of the robot J2 joint coordinate system {O1} relative to the base coordinate system {O0} can be obtained as:

[0094]

[0095] That is: P J2 =[a1c1 a1s1 d1].

[0096] Then the horizontal coordinate of the center point of the J2 joint in the base coordinate system is x J2 =a1c1, then:

[0097] When x J2 When <0, cf2=-1;

[0098] When x J2 >0, cf2=1.

[0099] (5) Solution of cf3

[0100] Secondly, the front and rear position of the wrist center point relative to the lower arm can be determined by calculating the direction of the common perpendicular line between the vector axis J35 and the vector axis J32 relative to the base coordinate system.

[0101] ●When the right-hand rule is used, the rotation from vector J35 to vector J32 is in the same direction as the base.

[0102] When the y-axis of the coordinate system is in the same direction, cf3 = 1;

[0103] ●When the direction of the common perpendicular line is opposite to the y-axis direction of the base coordinate system, it is recorded as cf3=-1.

[0104] The calculation method is as follows:

[0105] Based on the STD_DH method (ZX rule), the homogeneous transformation matrix between the coordinate systems of adjacent links is:

[0106]

[0107] Based on the STD_DH method, the homogeneous transformation matrix of the robot J3 joint coordinate system {O2} relative to the base coordinate system {O0} can be obtained as:

[0108]

[0109] That is: P J3 =[c1(a1+a2s2)s1(a1+a2s2)a2c2+d1]

[0110] The homogeneous transformation matrix of the robot J5 joint coordinate system {O4} relative to the base coordinate system {O0} is:

[0111]

[0112] That is: P J5=[c1(a1+a2s2+d4c 23 )s1(a1+a2s2+d4c 23 )a2c2+d1-d4s 23 ]but

[0113]

[0114] When n y When <0, cf3=-1;

[0115] ●When n y >0, cf3=1.

[0116] (6) Solution of cf5

[0117] at last,

[0118] When the J5 joint angle is less than 0, it is recorded as cf5 = -1;

[0119] ●When the J5 joint angle is greater than 0, it is recorded as cf5=1.

[0120] (7) Solving cfx

[0121] According to Table 5 and the obtained cf2, cf3, and cf5, cfx can be solved.

[0122] Algorithm verification results

[0123] Table 6 Algorithm verification

[0124] Input joint angleJointPos Robot posture Robot axis configuration parameters Confdata [0,0,0,0,30,0] [0,0,0,0] [0,0,0,155,-30,0] [0,1,0,1] [0,90,-180,0,30,0] [0,0,0,2] [0,90,-180,0,-30,0] [0,0,0,3] [-175,-90,-40,-155,30,0] [-2,-2,0,4] [-175,-90,-40,0,-60,0] [-2,0,0,5] [-175,-30,-150,0,30,0] [-2,0,0,6] [-175,-30,-150,0,-30,0] [-2,0,0,7]

[0125] Those of ordinary skill in the art will appreciate that the units and method steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of the two. In order to clearly illustrate the interchangeability of hardware and software, the composition and steps of each example have been generally described in terms of function in the above description. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0126] In the several embodiments provided in the present application, it should be understood that the disclosed methods and systems can be implemented in other ways. For example, the division of the units described above is only a logical function division, and there may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. The above-mentioned units may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the embodiments of the present invention.

[0127] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein by equivalents. These modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present invention, and they should all be included in the scope of the claims and specification of the present invention.

[0128] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A robot axis configuration and joint angle conversion method, characterized in that: include: Record robot poses using joint angles or positions in Cartesian space, quaternions, and axis configuration parameters; Determine the joint limits of the robot and use axis configuration parameters to describe the posture of the robot relative to the singularity point; Solve the axis configuration parameters according to the preset solution method; Convert the input joint angle into axis configuration parameters according to the preset conversion method.

2. The robot axis configuration and joint angle conversion method according to claim 1, characterized in that: Solving the axis configuration parameters according to a preset solution method includes: Solution of axis configuration parameter cf1: limit the quadrant division of axis configuration parameter cf1 to between -2 and 1, and the input angle range to between -175° and 175°; Solution of axis configuration parameter cf1: limit the quadrant division of axis configuration parameter cf4 to between -2 and 1, and the input angle range to between -155° and 155°; Solution of axis configuration parameter cf1: limit the quadrant division of axis configuration parameter cf6 to between -5 and 4, and the input angle range to between -395° and 395°.

3. The robot axis configuration and joint angle conversion method according to claim 1, characterized in that: Solving the axis configuration parameters according to a preset solution method includes: Determine the relative position according to the position of the J2 joint to solve the axis configuration parameter cf2, and determine the position relative to Axis1 through the horizontal coordinate of joint 2, where: If the horizontal axis is greater than 0, cf2=1; If the horizontal axis is less than 0, cf2=-1.

4. The robot axis configuration and joint angle conversion method according to claim 3, characterized in that: The solution method for the axis configuration parameter cf2 is: The homogeneous transformation matrix of the robot J2 joint coordinate system {O1} relative to the base coordinate system {O0} is obtained as: That is: P J2 =[a1c1 a1s1 d1]; Then the horizontal coordinate of the center point of the J2 joint in the base coordinate system is x J2 =a1c1, then: When x J2 When <0, cf2=-1; When x J2 >0, cf2=1.

5. The robot axis configuration and joint angle conversion method according to claim 1, characterized in that: Solving the axis configuration parameters according to a preset solution method includes: Determined based on the position of the wrist center relative to the lower arm, the direction of the rotation axis is determined using the rotation vector between J35 and J32 and the right-hand rule is applied to solve for the axis configuration parameter cf3, where: When the direction of the rotation axis is consistent with the y-axis direction of the base coordinate system, cf3 = 1; When the direction is opposite, cf3=-1.

6. The robot axis configuration and joint angle conversion method according to claim 1, characterized in that: The solution method for the axis configuration parameter cf3 is: The front and rear positions of the wrist center point relative to the lower arm are determined by calculating the direction of the common perpendicular line between the vector axis J35 and the vector axis J32 relative to the base coordinate system, specifically: When the right-hand rule is used to rotate from vector J35 to vector J32, and the direction of the common perpendicular line of the rotation axis is consistent with the y-axis direction of the base coordinate system, it is recorded as cf3 = 1; When the direction of the common perpendicular line is opposite to the y-axis direction of the base coordinate system, it is recorded as cf3=-1.

7. The robot axis configuration and joint angle conversion method according to claim 6, characterized in that: The solution method for the axis configuration parameter cf3 is: Based on the STD_DH method, the homogeneous transformation matrix between the coordinate systems of adjacent links is: The homogeneous transformation matrix of the robot J3 joint coordinate system {O2} relative to the base coordinate system {O0} is obtained as: That is: P J3 =[c1(a1+a2s2)s1(a1+a2s2)a2c2+d1]; The homogeneous transformation matrix of the robot J5 joint coordinate system {O4} relative to the base coordinate system {O0} is: That is, P J5 =[c1(a1+a2s2+d4c 23 )s1(a1+a2s2+d4c 23 )a2c2+d1-d4s 23 ],but: When n y When <0, cf3=-1; When n y >0, cf3=1.

8. The robot axis configuration and joint angle conversion method according to claim 1, characterized in that: Solving the axis configuration parameters according to a preset solution method includes: The axis configuration parameter cf5 is determined by the angle of the J5 joint, where: If the angle of J5 is less than 0, cf5 = -1; If the angle of J5 is greater than or equal to 0, cf5=1.

9. The robot axis configuration and joint angle conversion method according to claim 1, characterized in that: Solve cfx based on the obtained axis configuration parameters, where the cfx parameters represent the overall configuration relative to the singular point.

10. The robot axis configuration and joint angle conversion method according to claim 1, characterized in that: The method of converting the input joint angle into the axis configuration parameter according to the preset conversion method includes: Enter the joint angles of the robot arm; Solve the quadrant numbers cf1, cf4, and cf6 of each axis according to the joint angle range; Use the homogeneous transformation matrix to calculate the position of joint 2 and solve cf2; Determine cf3 through the positional relationship between joints 3 and 5; Determine cf5 based on the angle of J5; Finally, cfx is solved by the combination of cf2, cf3, and cf5; Outputs the complete axis configuration parameters Confdata.

Images