Inverse kinematics analytical solution system for solving shoulder and elbow angles of industrial mechanical arm

Abstract

The invention provides an industrial mechanical arm shoulder-elbow angle inverse kinematics analytic solution solving system which comprises a processing unit, a sensor unit and a driving unit, the sensor unit is used for determining the position of the tail end of a robot, and the processing unit is used for establishing a mechanical arm joint coordinate system and driving the robot to move according to a preset track when the robot runs according to the preset track. Performing uniform interpolation on track points of a fourth axis, a fifth axis and a sixth axis of the robot to obtain joint angles corresponding to any track point position of a fourth joint, a fifth joint and a sixth joint on the track; and an inverse kinematics equation is established according to the coordinates of the tail end of the robot and the joint angles corresponding to the fourth joint, the fifth joint and the sixth joint, and the angles corresponding to the first joint, the second joint and the third joint are calculated. Therefore, the angles corresponding to the first joint, the second joint and the third joint of the mechanical arm can be solved very quickly, the robot is prevented from colliding with other objects in the working space, and the movement continuity and stability of the mechanical arm are guaranteed.

Description

Background Art

[0002] At present, the inverse kinematics problem of articulated manipulators is to find the angle values ​​of all manipulator joint rotations given the position and posture of the manipulator end effector coordinate system relative to the base coordinate system, as well as the geometric parameters of all manipulator joints. This is the inverse process of forward kinematics. According to the structural geometric parameters of the manipulator, forward kinematics can obtain the homogeneous coordinate transformation matrix T between the front and rear adjacent joint coordinate systems, that is, the pose matrix between the two joints; if the rotation angle θ of each joint is known, the pose matrix of the manipulator end effector coordinate system can be obtained by successively right-multiplying the homogeneous transformation matrix T of each joint in sequence, and the result obtained is unique.

[0003] However, the solution of inverse kinematics is relatively complex and may have no solution or multiple solutions. For example, there is no solution when the end effector coordinate system is at a singular point, and the periodicity of inverse trigonometric functions leads to multiple solutions in theory for the analytical solution of the robot arm joint rotation angle. Summary of the invention

[0004] In view of the defects in the prior art, the object of the present invention is to provide an analytical solution system for the inverse kinematics of shoulder-elbow angles of an industrial robot arm.

[0005] In the first aspect, an embodiment of the present application provides an analytical solution system for solving shoulder and elbow angle inverse kinematics of an industrial robot arm, which is applied to a robot with a 6-DOF robot arm. The system includes: a processing unit, a sensor unit, and a drive unit. The sensor unit is used to determine the position of the robot end. The processing unit includes: a spatial coordinate system establishment unit, a trajectory point interpolation unit, a joint angle determination unit, and a joint angle calculation unit, wherein:

[0006] The spatial coordinate system establishment unit is used to establish the robot arm joint coordinate system and configure the shoulder-elbow-wrist structure according to the six degrees of freedom of the robot, and define the 0th to 6th joints from the base to the end respectively;

[0007] The driving unit is used to control the robot to run according to a preset trajectory;

[0008] The trajectory point interpolation unit is used to uniformly interpolate the trajectory points of the 4th axis, the 5th axis and the 6th axis of the robot;

[0009] The joint angle determination unit is used to determine the joint angles of the fourth joint, the fifth joint, and the sixth joint corresponding to any trajectory point position on the trajectory after completing the trajectory point interpolation;

[0010] The joint angle calculation unit is used to establish an inverse kinematics equation based on the robot terminal coordinates and the joint angles corresponding to the 4th joint, the 5th joint and the 6th joint, and calculate the angles corresponding to the 1st joint, the 2nd joint and the 3rd joint.

[0011] Optionally, the spatial coordinate system establishing unit is specifically used to:

[0012] A rectangular coordinate system is established with the position corresponding to the robot base as the origin; among them, the robot end coordinate is the target coordinate position that the robot arm execution end needs to reach.

[0013] Optionally, it further includes: a preset trajectory generating unit, wherein the preset trajectory generating unit is used to generate a preset trajectory according to known robot end coordinates.

[0014] Optionally, the joint angle determination unit is specifically used to:

[0015] Obtaining the coordinate value of each trajectory point in the interpolated trajectory;

[0016] Based on the robot arm joint coordinate system, the joint angles corresponding to the 4th joint, the 5th joint, and the 6th joint are determined according to the coordinate values ​​of the trajectory points.

[0017] Optionally, the joint angle calculation unit is specifically used to:

[0018] Construct the forward kinematics equations of the robot's end coordinates;

[0019] Based on the process of solving inverse kinematics from the forward kinematics equation, the joint angles corresponding to the first joint, the second joint, and the third joint are reversely calculated.

[0020] Optionally, the forward motion equation of the robot end coordinates is as follows:

[0021] t 0T =t 01 +rotz(q1)*(t 12 +roty(q2)*(t 23 +roty(q3)*(t 34 +rotx(q4)*roty(q5)*roty(q6)*t 6T )))

[0022] Where: t 0T represents the displacement of the end flange relative to the 0th joint, t 01 represents the displacement of the first joint relative to the 0th joint, rotz(q1) represents the rotation angle q1 around the Z axis, and t 12 represents the displacement of the second joint relative to the first joint, roty(q2) represents the rotation angle q2 around the Y axis, and t 23 represents the displacement of the third joint relative to the second joint, roty(q3) represents the rotation angle q3 around the Y axis, and t 34represents the displacement of the fourth joint relative to the third joint, rotx(q4) represents the rotation of q4 angle around the X axis, roty(q5) represents the rotation of q5 angle around the Y axis, roty(q6) represents the rotation of q6 angle around the Y axis, t 6T represents the displacement of the end flange relative to the sixth joint; where:

[0023]

[0024]

[0025] When the joint angles q4, q5, and q6 corresponding to the 4th, 5th, and 6th joints are known,

[0026] t 4T =rotx(q4)*roty(q5)*roty(q6)*t 6T

[0027] Where: t 4T represents the displacement of the end flange relative to the fourth joint, and t 4T Substitute t 0T The calculation formula is as follows:

[0028] t 0T =t 01 +rotz(q1)*(t 12 +roty(q2)*(t 23 +roty(q3)*(t 34 +t 4T )))

[0029] The process of solving inverse kinematics: reversely calculate the joint angles q1, q2, q3 corresponding to the first joint, the second joint, and the third joint;

[0030] Let t 1T =t 0T -t 01 ;

[0031] Where: t 1T represents the displacement of the end flange relative to the first joint, and t 1T The coordinates are marked as [t 1x , t 1y , t 1z ];

[0032] Let T 1T =t 12 +roty(q2)*(t 23 +roty(q3)*t 3T );

[0033] Where: t 3Trepresents the displacement of the end flange relative to the third joint. 1T The coordinates are marked as [T 1x , T 1y , T 1z ];

[0034] Since the component of a coordinate remains unchanged when it rotates around the corresponding axis, assuming t 12 ,t 23 ,t 3T The variables about the Y axis are t 12y ,t 23y ,t 3Ty , then T 1T The coordinates of have the following relationship:

[0035] T 1z =t 1z ;

[0036] T 1y =t 12y +t 23y +t 3Ty ;

[0037] Among them, T 1x =+ / -sqrt(t 1y 2 +t 1x 2 -T 1y 2 );

[0038] When T 1T When known, q1 is calculated according to the Paden-Kahan subproblem algorithm;

[0039] When q1 is known, there exists ||T 1T -t 12 ||=||t 23 +roty(q3)*t 3T ||, q3 is calculated according to the Paden-Kahan subproblem algorithm;

[0040] When q3 is known, according to T 1T -t 12 =roty(q2)*(t 23 +roty(q3)*t 3T ), and the Paden-Kahan subproblem algorithm is used to calculate q2.

[0041] Optionally, there are at most two sets of solutions for q1, at most two sets of solutions for q3, and only one set of solutions for q2, so there are at most four different sets of q1, q2, and q3 angles.

[0042] Optionally, it also includes: a motion control unit, which is used to control the robot arm to pass through the singularity point according to the angles corresponding to the first joint, the second joint, and the third joint, and the joint angles corresponding to the fourth joint, the fifth joint, and the sixth joint.

[0043] In a second aspect, an embodiment of the present application provides a robot with a collaborative robot arm, comprising: a base, a drive system, a first joint assembly, a second joint assembly, a third joint assembly, a fourth joint assembly, a fifth joint assembly, a sixth joint assembly, and an industrial robot arm inverse kinematics analytical solution system for solving shoulder and elbow angles as described in any one of the first aspects, wherein:

[0044] The drive system controls the movement of the robot arm according to the solved angles corresponding to the first joint, the second joint, and the third joint, as well as the known robot end coordinates and the angles corresponding to the fourth joint, the fifth joint, and the sixth joint.

[0045] Compared with the prior art, the present invention has the following beneficial effects:

[0046] The industrial robot arm shoulder elbow angle inverse kinematics analytical solution system provided by the present application is provided with a processing unit, a sensor unit, and a driving unit. The sensor unit is used to determine the position of the robot end. The processing unit includes: a space coordinate system establishment unit, a trajectory point interpolation unit, a joint angle determination unit, and a joint angle calculation unit. The robot arm joint coordinate system is established by the space coordinate system establishment unit, and the shoulder-elbow-wrist structure is configured according to the 6 degrees of freedom of the robot, and the 1st to 6th joints are defined from the base to the end respectively; the robot is controlled to run according to the preset trajectory by the driving unit; the trajectory points of the 4th axis, the 5th axis, and the 6th axis of the robot are uniformly interpolated by the trajectory point interpolation unit; after completing the trajectory point interpolation, the joint angle determination unit determines the joint angle corresponding to any trajectory point position of the 4th joint, the 5th joint, and the 6th joint on the trajectory respectively; the joint angle calculation unit establishes the inverse kinematics equation according to the robot end coordinates and the joint angles corresponding to the 4th joint, the 5th joint, and the 6th joint, and calculates the angles corresponding to the 1st joint, the 2nd joint, and the 3rd joint. In this way, the angles corresponding to the first, second and third joints of the robot arm can be solved very quickly, avoiding collisions between the robot and other objects in the working space and ensuring the continuity and stability of the robot arm's movement. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following will briefly introduce the drawings required for use in the embodiments or the prior art descriptions. Obviously, the drawings in the following descriptions are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without creative work. By reading the detailed description of the non-limiting embodiments with reference to the following drawings, other features, purposes and advantages of the present invention will become more obvious:

[0048] Figure 1 A schematic diagram of the structure of an articulated six-degree-of-freedom robot provided in an embodiment of the present application;

[0049] Figure 2 A schematic diagram of the structure of an analytical solution system for solving shoulder-elbow angle inverse kinematics of an industrial robot arm provided in an embodiment of the present application;

[0050] Figure 3 A schematic diagram of the structure of another analytical solution system for solving the shoulder-elbow angle inverse kinematics of an industrial robot arm provided in an embodiment of the present application;

[0051] Figure 4 A schematic diagram of the structure of another analytical solution system for solving shoulder-elbow angle inverse kinematics of an industrial robot arm provided in an embodiment of the present application;

[0052] Figure 5 A schematic diagram of the structure of an analytical solution device for solving the inverse kinematics of shoulder-elbow angles of an industrial robot arm provided in one embodiment of the present application;

[0053] Figure 6 It is a schematic diagram of the structure of a computer-readable storage medium in an embodiment of the present invention.

[0054] In the figure: 1- the first joint, 2- the second joint, 3- the third joint, 4- the fourth joint, 5- the fifth joint, 6- the sixth joint, 7- the end of the robot. DETAILED DESCRIPTION

[0055] In order to make the purpose, technical solution and advantages of the embodiments of the present application clearer, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0056] It should be noted that when a component is said to be "fixed to" another component, it can be directly on the other component or there can be a central component. When a component is said to be "connected to" another component, it can be directly connected to the other component or there can be a central component at the same time.

[0057] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art to which this application belongs. The terms used herein in the specification of this application are only for the purpose of describing specific embodiments and are not intended to limit this application. The term "and / or" used herein includes any and all combinations of one or more of the related listed items.

[0058] The terms "first", "second", "third", "fourth", etc. (if any) in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present invention described herein, for example, can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0059] The following specific embodiments are used to describe in detail the technical solution of the present invention and how the technical solution of the present application solves the above technical problems. The following specific embodiments can be combined with each other, and the same or similar concepts or processes may not be repeated in some embodiments.

[0060] In conjunction with the accompanying drawings, some embodiments of the present application are described in detail below. In the absence of conflict, the following embodiments and features in the embodiments can be combined with each other.

[0061] Figure 1 A schematic diagram of the structure of an articulated six-degree-of-freedom robot provided in an embodiment of the present application is shown in FIG. Figure 1 As shown, first, the robot arm joint coordinate system is established, and the shoulder-elbow-wrist structure is configured according to the six degrees of freedom of the robot, and the 0th to 6th joints are defined from the base to the end, the base is the 0th joint, and from bottom to top they are the 1st joint 1, the 2nd joint 2, the 3rd joint 3, the 4th joint 4, the 5th joint 5, the 6th joint 6, and the robot end 7; secondly, a rectangular coordinate system is established with the position corresponding to the robot base as the origin; among them, the robot end coordinates are the target coordinate positions that the robot arm execution end needs to reach.

[0062] Exemplarily, the distances between the joints of the robot arm are known, and the coordinates of the robot end are also known. The robot end coordinates are used to plan the trajectory of the robot arm, generate a preset trajectory, and then uniformly interpolate the trajectory points of the 4th, 5th, and 6th axes of the robot to obtain the joint angles corresponding to the positions of any trajectory points of the 4th, 5th, and 6th joints on the trajectory. Furthermore, according to the robot end coordinates and the joint angles corresponding to the 4th, 5th, and 6th joints, an inverse kinematics equation is established to calculate the angles corresponding to the 1st, 2nd, and 3rd joints of the robot arm. In this way, the angles corresponding to the 1st, 2nd, and 3rd joints of the robot arm can be solved very quickly, avoiding collisions between the robot and other objects in the working space, and ensuring the continuity and stability of the robot arm movement.

[0063] Figure 2 A schematic diagram of the structure of an analytical solution system for solving the shoulder-elbow angle inverse kinematics of an industrial robot arm provided in an embodiment of the present application is shown in FIG. Figure 2 As shown, the industrial robot arm shoulder-elbow angle inverse kinematics analytical solution system 200 in this embodiment is applied to a robot with six degrees of freedom. The system includes: a processing unit 210, a sensor unit 220, and a drive unit 230. The sensor unit 220 is used to determine the position of the robot end. The processing unit 210 includes: a spatial coordinate system establishment unit 211, a trajectory point interpolation unit 212, a joint angle determination unit 213, and a joint angle calculation unit 214. Among them, the spatial coordinate system establishment unit 211 is used to establish the robot arm joint coordinate system, and configure the shoulder-elbow-wrist structure according to the six degrees of freedom of the robot, from the base to the end. The 0th to 6th joints are defined separately; a driving unit 230 is used to control the robot to run according to a preset trajectory; a trajectory point interpolation unit 212 is used to uniformly interpolate the trajectory points of the 4th axis, the 5th axis and the 6th axis of the robot; a joint angle determination unit 213 is used to determine the joint angles corresponding to the positions of any trajectory points of the 4th joint, the 5th joint and the 6th joint on the trajectory after completing the trajectory point interpolation; a joint angle calculation unit 214 is used to establish an inverse kinematics equation according to the robot terminal coordinates and the joint angles corresponding to the 4th joint, the 5th joint and the 6th joint, and calculate the angles corresponding to the 1st joint, the 2nd joint and the 3rd joint.

[0064] Exemplarily, the spatial coordinate system establishing unit 211 in this embodiment is specifically used to establish a rectangular coordinate system with the position corresponding to the robot base as the origin; wherein the robot end coordinate is the target coordinate position that the robot arm execution end needs to reach.

[0065] Exemplarily, the joint angle determination unit 213 in this embodiment is specifically used to: obtain the coordinate value of each trajectory point in the interpolated trajectory; based on the robot arm joint coordinate system, determine the joint angles corresponding to the 4th joint, the 5th joint, and the 6th joint according to the coordinate values ​​of the trajectory points.

[0066] Exemplarily, the joint angle calculation unit 214 in this embodiment is specifically used to: construct the forward kinematics equation of the robot's terminal coordinates; based on the process of solving inverse kinematics based on the forward kinematics equation, reversely calculate the joint angles corresponding to the first joint, the second joint, and the third joint.

[0067] In an optional embodiment, the forward motion equation of the robot end coordinates is as follows:

[0068] t 0T =t 01 +rotz(q1)*(t 12 +roty(q2)*(t 23 +roty(q3)*(t 34 +rotx(q4)*roty(q5)*roty(q6)*t 6T )))

[0069] Where: t 0T represents the displacement of the end flange relative to the 0th joint, t 01 represents the displacement of the first joint relative to the 0th joint, rotz(q1) represents the rotation angle q1 around the Z axis, and t 12 represents the displacement of the second joint relative to the first joint, roty(q2) represents the rotation angle q2 around the Y axis, and t 23 represents the displacement of the third joint relative to the second joint, roty(q3) represents the rotation angle q3 around the Y axis, and t 34 represents the displacement of the fourth joint relative to the third joint, rotx(q4) represents the rotation of q4 angle around the X axis, roty(q5) represents the rotation of q5 angle around the Y axis, roty(q6) represents the rotation of q6 angle around the Y axis, t 6T represents the displacement of the end flange relative to the sixth joint; where:

[0070]

[0071]

[0072] When the joint angles q4, q5, and q6 corresponding to the 4th, 5th, and 6th joints are known,

[0073] t 4T =rotx(q4)*roty(q5)*roty(q6)*t 6T

[0074] Where: t 4T represents the displacement of the end flange relative to the fourth joint, and t 4T Substitute t 0T The calculation formula is as follows:

[0075] t 0T =t 01 +rotz(q1)*(t 12 +roty(q2)*(t 23 +roty(q3)*(t 34 +t 4T )))

[0076] The process of solving inverse kinematics: reversely calculate the joint angles q1, q2, q3 corresponding to the first joint, the second joint, and the third joint;

[0077] Let t 1T =t 0T -t 01 ;

[0078] Where: t 1T represents the displacement of the end flange relative to the first joint, and t 1T The coordinates are marked as [t 1x , t 1y , t 1z ];

[0079] Let T 1T =t 12 +roty(q2)*(t 23 +roty(q3)*t 3T );

[0080] Where: t 3T represents the displacement of the end flange relative to the third joint. 1T The coordinates are marked as [T 1x , T 1y , T 1z ];

[0081] Since the component of a coordinate remains unchanged when it rotates around the corresponding axis, assuming t 12 ,t 23 ,t 3T The variables about the Y axis are t 12y ,t 23y ,t 3Ty , then T 1T The coordinates of have the following relationship:

[0082] T 1z =t 1z ;

[0083] T 1y =t 12y +t 23y +t 3Ty ;

[0084] Among them, T 1x =+ / -sqrt(t 1y 2 +t 1x 2 -T 1y 2 );

[0085] When T 1T When known, q1 is calculated according to the Paden-Kahan subproblem algorithm;

[0086] When q1 is known, there exists ||T 1T -t 12 ||=||t 23 +roty(q3)*t 3T ||, q3 is calculated according to the Paden-Kahan subproblem algorithm;

[0087] When q3 is known, according to T 1T -t 12 =roty(q2)*(t 23 +roty(q3)*t 3T ), and the Paden-Kahan subproblem algorithm is used to calculate q2.

[0088] In this embodiment, there are at most two solutions for q1, at most two solutions for q3, and only one solution for q2, so there are at most four different angles of q1, q2, and q3. This can greatly increase the calculation speed and quickly solve the angles corresponding to all joints of the robot arm.

[0089] Figure 3 A schematic diagram of the structure of another industrial robot arm inverse kinematics analytical solution system for solving shoulder-elbow angles provided in an embodiment of the present application is shown in FIG. Figure 3 As shown, the industrial robot arm shoulder and elbow angle inverse kinematics analytical solution system 200 in this embodiment is applied to a robot with 6 degrees of freedom. The system includes: a processing unit 210, a sensor unit 220, a drive unit 230, and a preset trajectory generation unit 240. The sensor unit 220 is used to determine the position of the robot end. The processing unit 210 includes: a spatial coordinate system establishment unit 211, a trajectory point interpolation unit 212, a joint angle determination unit 213, and a joint angle calculation unit 214. Among them, the preset trajectory generation unit 240 is used to generate a preset trajectory based on the known robot end coordinates.

[0090] In this embodiment, the preset trajectory can be planned and generated by the robot's own industrial computer and / or the remote communication host computer according to the position that the robot terminal needs to reach. It should be noted that the embodiment of the present application does not limit the generation method of the preset trajectory.

[0091] Figure 4 A schematic diagram of the structure of another industrial robot arm inverse kinematics analytical solution system for solving shoulder-elbow angles provided in an embodiment of the present application is shown in FIG. Figure 4 As shown, the industrial robot arm shoulder and elbow angle inverse kinematics analytical solution system 200 in this embodiment is applied to a robot with a 6-DOF robot arm. The system includes: a processing unit 210, a sensor unit 220, a drive unit 230, a preset trajectory generation unit 240, and a motion control unit 250. The sensor unit 220 is used to determine the position of the robot end. The processing unit 210 includes: a spatial coordinate system establishment unit 211, a trajectory point interpolation unit 212, a joint angle determination unit 213, and a joint angle calculation unit 214. Among them, the motion control unit 250 is used to control the robot arm to pass through the singularity point according to the angles corresponding to the first joint, the second joint, and the third joint, as well as the joint angles corresponding to the fourth joint, the fifth joint, and the sixth joint.

[0092] In this embodiment, since the angles of all joints can be quickly calculated, the robotic arm can be controlled to pass through the singularity point smoothly, ensuring the continuity and stability of the robotic arm's movement. It can also be applied in the field of robot collision detection to quickly determine whether a collision will occur in the robotic arm's movement trajectory.

[0093] The present application also provides a robot with a collaborative robot arm, characterized in that it includes: a base, a drive system, a first joint assembly, a second joint assembly, a third joint assembly, a fourth joint assembly, a fifth joint assembly, a sixth joint assembly, and Figure 2 to Figure 4 The industrial robot arm shown in the figure solves the inverse kinematics analytical solution system of the shoulder and elbow angles, wherein: the drive system controls the movement of the robot arm according to the angles corresponding to the first joint, the second joint, and the third joint obtained by the solution, as well as the known robot end coordinates, and the angles corresponding to the fourth joint, the fifth joint, and the sixth joint.

[0094] The embodiment of the present application also provides an analytical solution method for solving the inverse kinematics of the shoulder and elbow angle of an industrial robot arm. The method in this embodiment may include the following steps:

[0095] Step S1: Establish the robot arm joint coordinate system, and configure the shoulder-elbow-wrist structure according to the six degrees of freedom of the robot, and define the 0th to 6th joints from the base to the end.

[0096] The mechanical arm joint coordinates established in this embodiment can be found in Figure 1 As shown in the figure, a rectangular coordinate system is established with the position corresponding to the robot base as the origin; the robot end coordinate is the target coordinate position that the robot arm execution end needs to reach. Then, the six joints from the base to the end are defined respectively.

[0097] Step S2: When the robot runs according to the preset trajectory, the trajectory points of the 4th axis, 5th axis and 6th axis of the robot are uniformly interpolated to obtain the joint angles of the 4th joint, 5th joint and 6th joint corresponding to any trajectory point position on the trajectory.

[0098] In this embodiment, a preset trajectory can be generated based on the known robot end coordinates; based on the preset trajectory, the trajectory points of the 4th axis, 5th axis, and 6th axis of the robot are uniformly interpolated to obtain the interpolated trajectory; then, based on the coordinate value of each trajectory point in the interpolated trajectory, the joint angles corresponding to the 4th joint, 5th joint, and 6th joint are determined respectively.

[0099] Step S3: Establish an inverse kinematics equation based on the robot end coordinates and the joint angles corresponding to the 4th joint, the 5th joint, and the 6th joint, and calculate the angles corresponding to the 1st joint, the 2nd joint, and the 3rd joint.

[0100] In this embodiment, the forward kinematics equation of the robot is constructed, wherein the calculation formula of the robot end coordinates is as follows:

[0101] t 0T =t 01 +rotz(q1)*(t 12 +roty(q2)*(t 23 +roty(q3)*(t 34 +rotx(q4)*roty(q5)*roty(q6)*t 6T )))

[0102] Where: t 0T represents the displacement of the end flange relative to the 0th joint, t 01 represents the displacement of the first joint relative to the 0th joint, rotz(q1) represents the rotation angle q1 around the Z axis, and t 12 represents the displacement of the second joint relative to the first joint, roty(q2) represents the rotation angle q2 around the Y axis, and t 23 represents the displacement of the third joint relative to the second joint, roty(q3) represents the rotation angle q3 around the Y axis, and t 34 represents the displacement of the fourth joint relative to the third joint, rotx(q4) represents the rotation of q4 angle around the X axis, roty(q5) represents the rotation of q5 angle around the Y axis, roty(q6) represents the rotation of q6 angle around the Y axis, t6T represents the displacement of the end flange relative to the sixth joint; where:

[0103]

[0104]

[0105] When the joint angles q4, q5, and q6 corresponding to the 4th, 5th, and 6th joints are known,

[0106] t 4T =rotx(q4)*roty(q5)*roty(q6)*t 6T

[0107] Where: t 4T represents the displacement of the end flange relative to the fourth joint, and t 4T Substitute t 0T The calculation formula is as follows:

[0108] t 0T =t 01 +rotz(q1)*(t 12 +roty(q2)*(t 23 +roty(q3)*(t 34 +t 4T )))

[0109] The process of solving inverse kinematics: reversely calculate the joint angles q1, q2, q3 corresponding to the first joint, the second joint, and the third joint;

[0110] Let t 1T =t 0T -t 01 ;

[0111] Where: t 1T represents the displacement of the end flange relative to the first joint, and t 1T The coordinates are marked as [t 1x , t 1y , t 1z ];

[0112] Let T 1T =t 12 +roty(q2)*(t 23 +roty(q3)*t 3T );

[0113] Where: t 3T represents the displacement of the end flange relative to the third joint. 1T The coordinates are marked as [t 1x , T 1y , T1z ];

[0114] Since the component of a coordinate remains unchanged when it rotates around the corresponding axis, assuming t 12 ,t 23 ,t 3T The variables about the Y axis are t 12y ,t 23y ,t 3Ty ,but 1T The coordinates of have the following relationship:

[0115] T 1z =t 1z ;

[0116] T 1y =t 12y +t 23y +t 3Ty ;

[0117] Among them, T 1x =+ / -sqrt(t 1y 2 +t 1x 2 -T 1y 2 );

[0118] When T 1T When known, q1 is calculated according to the Paden-Kahan subproblem algorithm;

[0119] When q1 is known, there exists ||T 1T -t 12 ||=||t 23 +roty(q3)*t 3T ||, q3 is calculated according to the Paden-Kahan subproblem algorithm;

[0120] When q3 is known, according to T 1T -t 12 =roty(q2)*(t 23 +roty(q3)*t 3T ), and the Paden-Kahan subproblem algorithm is used to calculate q2.

[0121] Exemplarily, according to the method in the embodiment of the present application, there are at most two sets of solutions for q1, at most two sets of solutions for q3, and only one set of solutions for q2, so there are at most four different sets of q1, q2, and q3 angles.

[0122] In this embodiment, by establishing a mechanical arm joint coordinate system and configuring the shoulder-elbow-wrist structure according to the robot's six degrees of freedom, the 0th to 6th joints are defined from the base to the end; when the robot runs according to the preset trajectory, the trajectory points of the 4th, 5th and 6th axes of the robot are uniformly interpolated to obtain the joint angles corresponding to the positions of any trajectory points of the 4th, 5th and 6th joints on the trajectory; according to the robot's terminal coordinates and the joint angles corresponding to the 4th, 5th and 6th joints, the inverse kinematics equations are established to calculate the angles corresponding to the 1st, 2nd and 3rd joints. In this way, the angles corresponding to the 1st, 2nd and 3rd joints of the mechanical arm can be solved very quickly, avoiding collisions between the robot and other objects in the working space, and ensuring the continuity and stability of the mechanical arm movement.

[0123] Figure 5 The schematic diagram of the structure of the device for analytically solving the inverse kinematics of shoulder-elbow angles of an industrial robot arm provided in one embodiment of the present application, the device 500 for analytically solving the inverse kinematics of shoulder-elbow angles of an industrial robot arm in this embodiment may include: a processor 501 and a memory 502.

[0124] The memory 502 is used to store programs. The memory 502 may include volatile memory, such as random-access memory (RAM), such as static random-access memory (SRAM), double data rate synchronous dynamic random access memory (DDR SDRAM), etc. The memory may also include non-volatile memory, such as flash memory. The memory 502 is used to store computer programs (such as applications, functional modules, etc. that implement the above method), computer instructions, etc. The above computer programs, computer instructions, etc. may be partitioned and stored in one or more memories 502. And the above computer programs, computer instructions, data, etc. may be called by the processor 501.

[0125] The above-mentioned computer programs, computer instructions, etc. may be stored in one or more memories 502 in partitions. And the above-mentioned computer programs, computer instructions, data, etc. may be called by the processor 501.

[0126] The processor 501 is used to execute the computer program stored in the memory 502 to implement each step of the method involved in the above embodiment.

[0127] For details, please refer to the relevant description in the previous method embodiment.

[0128] The processor 501 and the memory 502 may be independent structures or integrated structures. When the processor 501 and the memory 502 are independent structures, the memory 502 and the processor 501 may be coupled and connected via a bus 503 .

[0129] The industrial robot arm shoulder-elbow angle inverse kinematics analytical solution device 500 of this embodiment can execute the technical solution in the above method. Its specific implementation process and technical principles can be found in the relevant description of the above method, which will not be repeated here.

[0130] It will be appreciated by those skilled in the art that various aspects of the present invention may be implemented as systems, methods or program products. Therefore, various aspects of the present invention may be specifically implemented in the following forms, namely: complete hardware implementation, complete software implementation (including firmware, microcode, etc.), or a combination of hardware and software implementations, which may be collectively referred to herein as "circuits", "modules" or "platforms".

[0131] In addition, an embodiment of the present application further provides a computer-readable storage medium, in which computer-executable instructions are stored. When at least one processor of a user device executes the computer-executable instructions, the user device executes the above-mentioned various possible methods.

[0132] Among them, computer-readable media include computer storage media and communication media, wherein the communication media include any media that facilitates the transmission of computer programs from one place to another. The storage medium can be any available medium that can be accessed by a general or special-purpose computer. An exemplary storage medium is coupled to the processor so that the processor can read information from the storage medium and write information to the storage medium. Of course, the storage medium can also be an integral part of the processor. The processor and the storage medium can be located in an ASIC. In addition, the ASIC can be located in a user device. Of course, the processor and the storage medium can also be present in a communication device as discrete components.

[0133] The present application also provides a program product, which includes a computer program. The computer program is stored in a readable storage medium. At least one processor of the server can read the computer program from the readable storage medium. At least one processor executes the computer program so that the server implements any method of the above-mentioned embodiments of the present invention.

[0134] Those skilled in the art can understand that all or part of the steps of implementing the above method embodiments can be completed by hardware related to program instructions, and the aforementioned program can be stored in a computer-readable storage medium. When the program is executed, it executes the steps of the above method embodiments; and the aforementioned storage medium includes: read-only memory (ROM), random access memory (RAM), magnetic disk or optical disk, etc. Various media that can store program codes.

[0135] Figure 6 Schematic diagram of the structure of a computer-readable storage medium in an embodiment of the present invention. Figure 6 As shown, a program product 600 for implementing the above method according to an embodiment of the present invention is described, which can adopt a portable compact disk read-only memory (CD-ROM) and include program code, and can be run on a terminal device, such as a personal computer. However, the program product of the present invention is not limited thereto. In this document, a readable storage medium can be any tangible medium containing or storing a program, which can be used by or in combination with an instruction execution system, an apparatus or a device.

[0136] The program product may use any combination of one or more readable media. The readable medium may be a readable signal medium or a readable storage medium. The readable storage medium may be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device or device, or any combination of the above. More specific examples of readable storage media (a non-exhaustive list) include: an electrical connection with one or more wires, a portable disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above.

[0137] Computer readable storage media may include data signals propagated in baseband or as part of a carrier wave, wherein readable program codes are carried. Such propagated data signals may take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. The readable storage medium may also be any readable medium other than a readable storage medium, which may send, propagate, or transmit a program for use by or in conjunction with an instruction execution system, device, or device. The program codes contained on the readable storage medium may be transmitted using any suitable medium, including but not limited to wireless, wired, optical cable, RF, etc., or any suitable combination thereof.

[0138] Program code for performing the operations of the present invention may be written in any combination of one or more programming languages, including object-oriented programming languages ​​such as Java, C++, etc., and conventional procedural programming languages ​​such as "C" or similar programming languages. The program code may be executed entirely on the user computing device, partially on the user device, as a separate software package, partially on the user computing device and partially on a remote computing device, or entirely on a remote computing device or server. In the case of a remote computing device, the remote computing device may be connected to the user computing device through any type of network, including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computing device (e.g., through the Internet using an Internet service provider).

[0139] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments, and the same and similar parts between the embodiments can be referred to each other. The above description of the disclosed embodiments enables professionals and technicians in this field to implement or use the present invention. Various modifications to these embodiments will be obvious to professionals and technicians in this field, and the general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to these embodiments shown in this article, but will comply with the widest range consistent with the principles and novel features disclosed herein.

[0140] The above describes the specific embodiments of the present invention. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art may make various modifications or variations within the scope of the claims, which do not affect the essence of the present invention.

Claims

1. An analytical solution system for inverse kinematics of shoulder and elbow angles of an industrial robot arm, characterized in that: Applied to a robot with a 6-DOF manipulator, the system includes: a processing unit, a sensor unit, and a drive unit. The sensor unit is used to determine the position of the robot end. The processing unit includes: a spatial coordinate system establishment unit, a trajectory point interpolation unit, a joint angle determination unit, and a joint angle calculation unit, wherein: The spatial coordinate system establishment unit is used to establish the robot arm joint coordinate system and configure the shoulder-elbow-wrist structure according to the six degrees of freedom of the robot, and define the 0th to 6th joints from the base to the end respectively; The driving unit is used to control the robot to run according to a preset trajectory; The trajectory point interpolation unit is used to uniformly interpolate the trajectory points of the 4th axis, the 5th axis and the 6th axis of the robot; The joint angle determination unit is used to determine the joint angles of the fourth joint, the fifth joint, and the sixth joint corresponding to any trajectory point position on the trajectory after completing the trajectory point interpolation; The joint angle calculation unit is used to establish an inverse kinematics equation based on the robot terminal coordinates and the joint angles corresponding to the 4th joint, the 5th joint and the 6th joint, and calculate the angles corresponding to the 1st joint, the 2nd joint and the 3rd joint.

2. The industrial robot arm shoulder-elbow angle inverse kinematics analytical solution system according to claim 1, characterized in that: The spatial coordinate system establishing unit is specifically used for: A rectangular coordinate system is established with the position corresponding to the robot base as the origin; among them, the robot end coordinate is the target coordinate position that the robot arm execution end needs to reach.

3. The inverse kinematics analytical solution system for solving shoulder-elbow angle of an industrial robot arm according to claim 1 is characterized in that: Also includes: The preset trajectory generating unit is used to generate a preset trajectory according to the known robot terminal coordinates.

4. The industrial robot arm shoulder-elbow angle inverse kinematics analytical solution system according to claim 1, characterized in that: The joint angle determination unit is specifically used for: Obtaining the coordinate value of each trajectory point in the interpolated trajectory; Based on the robot arm joint coordinate system, the joint angles corresponding to the 4th joint, the 5th joint, and the 6th joint are determined according to the coordinate values ​​of the trajectory points.

5. The industrial robot arm shoulder-elbow angle inverse kinematics analytical solution system according to claim 1, characterized in that: The joint angle calculation unit is specifically used for: Construct the forward kinematics equations of the robot's end coordinates; Based on the process of solving inverse kinematics from the forward kinematics equation, the joint angles corresponding to the first joint, the second joint, and the third joint are reversely calculated.

6. The industrial robot arm shoulder-elbow angle inverse kinematics analytical solution system according to claim 5, characterized in that: The forward motion equation of the robot end coordinates is as follows: t 0T =t 01 +rotz(q1)*(t 12 +roty(q2)*(t 23 +roty(q3)*(t 34 +rotx(q4)*roty(q5) *company(q6)*t 6T ))) Where: t 0T represents the displacement of the end flange relative to the 0th joint, t 01 represents the displacement of the first joint relative to the 0th joint, rotz(q1) represents the rotation angle q1 around the Z axis, and t 12 represents the displacement of the second joint relative to the first joint, roty(q2) represents the rotation angle q2 around the Y axis, and t 23 represents the displacement of the third joint relative to the second joint, roty(q3) represents the rotation angle q3 around the Y axis, and t 34 represents the displacement of the fourth joint relative to the third joint, rotx(q4) represents the rotation of q4 angle around the X axis, roty(q5) represents the rotation of q5 angle around the Y axis, roty(q6) represents the rotation of q6 angle around the Y axis, t 6T represents the displacement of the end flange relative to the sixth joint; where:

7. The industrial robot arm shoulder-elbow angle inverse kinematics analytical solution system according to claim 6, characterized in that: Based on the process of solving the inverse kinematics equations, the joint angles corresponding to the first joint, the second joint, and the third joint are calculated in reverse, including: When the joint angles q4, q5, and q6 corresponding to the 4th, 5th, and 6th joints are known, t 4T =rotx(q4)*roty(q5)*roty(q6)*t 6T Where: t 4T represents the displacement of the end flange relative to the fourth joint, and t 4T Substitute t 0T The calculation formula is as follows: t 0T =t 01 +rotz(q1)*(t 12 +roty(q2)*(t 23 +roty(q3)*(t 34 +t 4T ))) The process of solving inverse kinematics: reversely calculate the joint angles q1, q2, q3 corresponding to the first joint, the second joint, and the third joint; Let t 1T =t 0T -t 01 ; Where: t 1T represents the displacement of the end flange relative to the first joint, and t 1T The coordinates are marked as [t 1x , t 1y , t 1z ]; Let T 1T = t 12 + roty(q2)*(t 23 + roty(q3)*t 3T ); Where: t 3T represents the displacement of the end flange relative to the third joint. 1T The coordinates are marked as [T 1x , T 1y , T 1z ]; Since the component of a coordinate remains unchanged when it rotates around the corresponding axis, assuming t 12 ,t 23 ,t 3T The variables about the Y axis are t 12y ,t 23y ,t 3Ty , then T 1T The coordinates of have the following relationship: T 1z =t 1z ; T 1y =t 12y +t 23y +t 3Ty ; Among them, T 1x =+ / -sqrt(t 1y 2 +t 1x 2 -T 1y 2 ); When T 1T When known, q1 is calculated according to the Paden-Kahan subproblem algorithm; When q1 is known, there exists ‖T 1T -t 12 ‖=‖t 23 +roty(q3)*t 3T ‖, q3 is calculated according to the Paden-Kahan subproblem algorithm; When q3 is known, according to T 1T -t 12 =roty(q2)*(t 23 +roty(q3)*t 3T ), and the Paden-Kahan subproblem algorithm is used to calculate q2.

8. The inverse kinematics analytical solution system for shoulder-elbow angle of an industrial robot arm according to claim 7, characterized in that: There are at most two sets of solutions for q1, at most two sets of solutions for q3, and only one set of solutions for q2, so there are at most four different angles of q1, q2, and q3.

9. The inverse kinematics analytical solution system for shoulder-elbow angle of an industrial robot arm according to any one of claims 1 to 8, characterized in that: Also includes: A motion control unit, the motion control unit is used to control the robot arm to pass through the singular point according to the angles corresponding to the first joint, the second joint, and the third joint, as well as the joint angles corresponding to the fourth joint, the fifth joint, and the sixth joint.

10. A robot with a collaborative robotic arm, characterized in that: include: A base, a drive system, a first joint assembly, a second joint assembly, a third joint assembly, a fourth joint assembly, a fifth joint assembly, a sixth joint assembly, and an analytical solution system for solving shoulder-elbow angle inverse kinematics of an industrial robot arm according to any one of claims 1 to 9, wherein: The drive system controls the movement of the robot arm according to the solved angles corresponding to the first joint, the second joint, and the third joint, as well as the known robot end coordinates and the angles corresponding to the fourth joint, the fifth joint, and the sixth joint.

Images