Numerical solution method and device for tip-type robot

Abstract

The invention provides a numerical solution method and device for a tip-type robot, and the method comprises the steps: building a mechanical arm joint coordinate system, configuring a shoulder-elbow-wrist structure according to the six degrees of freedom of the tip-type robot, and respectively defining the joints from the base to the tail end from the first joint to the sixth joint; when the tip type robot meets the condition that the six shafts are all vertically downward, the angles of the fourth joint, the fifth joint and the sixth joint are obtained; an inverse kinematics equation is established according to the end coordinates of the tip-type robot and the joint angles corresponding to the fourth joint, the fifth joint and the sixth joint, and the angles of the first joint, the second joint and the third joint are calculated; and taking the solution of the group of angles as a group of approximate solutions of numerical solutions of the tip-type robot, and taking the approximate solutions as initial values of the numerical solutions for iteration to obtain a plurality of numerical solutions. Therefore, all numerical solutions of the tip-type robot can be rapidly solved, all joints of the tip-type robot can be conveniently controlled, and the tip-type robot can smoothly pass through singular points.

Description

Background Art

[0002] The inverse kinematics problem of a joint-type robotic arm is to obtain the angular values of all the robotic arm joint rotations given the position and orientation of the end effector coordinate system of the robotic arm relative to the base coordinate system, as well as all the geometric parameters of the robotic arm links, which is the inverse process of forward kinematics.

[0003] Currently, the wrists of most joint-type six-degree-of-freedom robotic arms on the market satisfy the Pieper criterion, that is, the rotation axes of three adjacent joints of the wrist intersect at a point, so the three adjacent joints of the wrist are decoupled.

[0004] However, the cuspidal robot does not satisfy the Pieper criterion, and its kinematics is very complex. The cuspidal robot is a special robotic configuration. This robot can switch from one inverse kinematic configuration to another without passing through a singular configuration. The cuspidal robot described in the present invention is an industrial robotic configuration with a non-spherical wrist configuration having 6 revolute joints, such as the FANUC CRX series (see the attached Figure 1 ). In this configuration, its joints 4, 5, and 6 do not intersect at a point, resulting in its non-compliance with the Pieper criterion and greatly increasing the difficulty of solving the inverse kinematics. Its kinematics is very complex, and the commonly used numerical solution methods often solve it by iteration, so it can only solve for a unique solution (joint coordinates). Summary of the Invention

[0005] Aiming at the defects in the prior art, the purpose of the present invention is to provide a numerical solution method and device for a cuspidal robot.

[0006] In a first aspect, an embodiment of the present application provides a numerical solution method for a cuspidal robot, including:

[0007] Step 1: Establish a robotic arm joint coordinate system, configure a shoulder-elbow-wrist structure according to the six degrees of freedom of the cuspidal robot, and define joints 0 to 6 from the base to the end;

[0008] Step 2: When the cuspidal robot satisfies that all 6 axes are vertically downward, respectively obtain the joint angles corresponding to joint 4, joint 5, and joint 6;

[0009] Step 3: Establish an inverse kinematics equation based on the end coordinates of the cuspidal robot and the joint angles corresponding to joint 4, joint 5, and joint 6, and calculate the angles corresponding to joint 1, joint 2, and joint 3;

[0010] Step 4: Take the angles corresponding to the calculated first joint, second joint, and third joint, as well as the known fourth joint, fifth joint, and sixth joint, as a set of approximate solutions for the numerical solution of the tip-type robot, and use the approximate solutions as the initial values of the numerical solution for iteration to obtain multiple numerical solutions.

[0011] Optionally, step 1 includes:

[0012] Establish a rectangular coordinate system with the position corresponding to the base of the tip-type robot as the origin; wherein, the end coordinate of the tip-type robot is the target coordinate position that the end of the robotic arm needs to reach.

[0013] Optionally, step 2 includes:

[0014] Control the flange of the 6 axes of the tip-type robot to always face downwards, then the angle q4 of the fourth joint of the tip-type robot is 0;

[0015] The angle q5 of the fifth joint of the tip-type robot = q2 + q3 - π; where: q2 represents the angle of the second joint, and q3 represents the angle of the third joint.

[0016] Optionally, step 3 includes:

[0017] Construct the forward kinematic equation of the tip-type robot, where the calculation formula for the end coordinate of the tip-type robot is as follows:

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

[0019] In the formula: 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 a rotation of q1 degrees around the Z axis, t 12 represents the displacement of the second joint relative to the first joint, roty(q2) represents a rotation of q2 degrees around the Y axis, t 23 represents the displacement of the third joint relative to the second joint, roty(q3) represents a rotation of q3 degrees around the Y axis, t 34 represents the displacement of the fourth joint relative to the third joint, rotx(q4) represents a rotation of q4 degrees around the X axis, roty(q5) represents a rotation of q5 degrees around the Y axis, roty(q6) represents a rotation of q6 degrees around the Y axis, t 6TRepresents the displacement of the end flange relative to the 6th joint; where:

[0020]

[0021]

[0022] Substitute q4 = 0, q5 = q2 + q3 - π into it, and the simplified calculation formula is obtained:

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

[0024] Where: rotz(q1)*roty(q2)*roty(q3)*roty(q5)*roty(q6)*t 6T = R 0T * t 6T

[0025] After further simplification:

[0026] t 0T = t 01 + rotz(q1)*(t 12 + roty(q2)*(t 23 + roty(q3)*(t 34 + t 45 )))+ R 0T * t 6T

[0027] According to the known t 0T , calculate q1, q2, q3 respectively.

[0028] Optionally, step 4 includes:

[0029] Obtain 8 groups of angles corresponding to the 1st joint, the 2nd joint, and the 3rd joint, as well as the corresponding angles of the 4th joint, the 5th joint, and the 6th joint, and construct the pose equation of the tip-type robot according to the 8 groups of solutions, and calculate the numerical solutions of all joints of the tip-type robot.

[0030] In a second aspect, an embodiment of the present application provides a numerical solution device for a tip-type robot, including:

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

[0032] The joint angle acquisition module is used to respectively acquire the joint angles corresponding to the 4th joint, the 5th joint, and the 6th joint when the tip-type robot has all six axes vertically downward;

[0033] The joint angle calculation module is used to establish an inverse kinematics equation based on the end coordinates of the tip-type robot 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;

[0034] The numerical solution calculation module is used to take the calculated angles of the 1st joint, the 2nd joint, and the 3rd joint, and the known angles corresponding to the 4th joint, the 5th joint, and the 6th joint as a set of approximate solutions of the numerical solution of the tip-type robot, and use the approximate solutions as the initial values of the numerical solution for iteration to obtain multiple numerical solutions.

[0035] Optionally, the joint coordinate system establishment module is specifically used for:

[0036] Establish a rectangular coordinate system with the position corresponding to the base of the tip-type robot as the origin; wherein, the end coordinates of the tip-type robot are the target coordinate positions that the execution end of the robotic arm needs to reach.

[0037] Optionally, the joint angle acquisition module is specifically used for:

[0038] Control the flange of the 6 axes of the tip-type robot to always face downward, then the joint angle q4 of the 4th joint of the tip-type robot is 0;

[0039] The joint angle q5 of the 5th joint of the tip-type robot = q2 + q3 - π; where: q2 represents the joint angle of the 2nd joint, q3 represents the joint angle of the 3rd joint,

[0040] Optionally, the joint angle calculation module is specifically used for:

[0041] Construct the forward kinematics equation of the tip-type robot, where the calculation formula for the end coordinates of the tip-type robot is as follows:

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

[0043] Where: t 0T represents the displacement of the end flange relative to the 0th joint, t 01 represents the displacement of the 1st joint relative to the 0th joint, rotz(q1) represents a rotation of q1 degrees about the Z-axis, t 12 represents the displacement of the 2nd joint relative to the 1st joint, roty(q2) represents a rotation of q2 degrees about the Y-axis, t 23 represents the displacement of the 3rd joint relative to the 2nd joint, roty(q3) represents a rotation of q3 degrees about the Y-axis, t 34 represents the displacement of the 4th joint relative to the 3rd joint, rotx(q4) represents a rotation of q4 degrees about the X-axis, roty(q5) represents a rotation of q5 degrees about the Y-axis, roty(q6) represents a rotation of q6 degrees about the Y-axis, t 6T represents the displacement of the end flange relative to the 6th joint; where:

[0044]

[0045]

[0046] Substitute q4 = 0, q5 = q2 + q3 - π into it, and the simplified calculation formula is obtained:

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

[0048] Where: rotz(q1)*roty(q2)*roty(q3)*roty(q5)*roty(q6)*t 6T = R 0T * t 6T

[0049] After further simplification:

[0050] t 0T = t 01 + rotz(q1)*(t 12 + roty(q2)*(t 23 + roty(q3)*(t 34 + t 45 )))+ R 0T * t 6T

[0051] According to the known t 0T , calculate q1, q2, and q3 respectively.

[0052] Optionally, the numerical solution calculation module is specifically configured to:

[0053] Obtain the angles corresponding to the first joint, the second joint, and the third joint in 8 groups, as well as the angles corresponding to the fourth joint, the fifth joint, and the sixth joint, and construct the pose equation of the tip-type robot according to the 8 groups of solutions, and calculate the numerical solutions of all joints of the tip-type robot.

[0054] In a third aspect, an embodiment of the present application provides a tip-type robot numerical solution device, including: a processor and a memory, where an executable program instruction is stored in the memory, and when the processor calls the program instruction in the memory, the processor is used for:

[0055] Execute the steps of the tip-type robot numerical solution method described in any item of the first aspect.

[0056] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium for storing a program, and when the program is executed, the steps of the tip-type robot numerical solution method described in any item of the first aspect are implemented.

[0057] In a fifth aspect, an embodiment of the present application provides a program product, where the program product includes a computer program, the computer program is stored in a readable storage medium, and at least one processor of the robot can read the computer program from the readable storage medium, and the at least one processor executes the computer program to enable the robot to implement the steps of the tip-type robot numerical solution method in the first aspect.

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

[0059] In this application, a robotic arm joint coordinate system is established, and a shoulder-elbow-wrist structure is configured according to the six degrees of freedom of the tip-type robot. Joints 0 to 6 are defined from the base to the end; when the six axes of the tip-type robot are all vertically downward, the joint angles corresponding to the 4th joint, the 5th joint, and the 6th joint are obtained respectively; according to the end coordinates of the tip-type robot and the joint angles corresponding to the 4th joint, the 5th joint, and the 6th joint, an inverse kinematics equation is established to calculate the angles corresponding to the 1st joint, the 2nd joint, and the 3rd joint; the calculated angles corresponding to the 1st joint, the 2nd joint, and the 3rd joint, and the known angles corresponding to the 4th joint, the 5th joint, and the 6th joint are used as a set of approximate solutions of the numerical solutions of the tip-type robot, and the approximate solutions are used as the initial values of the numerical solutions for iteration to obtain multiple numerical solutions. Thus, all the numerical solutions of the tip-type robot can be quickly solved, which is convenient for controlling each joint of the tip-type robot, enabling the tip-type robot to smoothly pass through the singular point. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, the drawings in the following description are only the embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained according to the provided drawings. 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:

[0061] Figure 1 FIG. is a schematic structural diagram of a non-spherical wrist joint tip robot provided by an embodiment of the present application;

[0062] Figure 2 FIG. is a flowchart of a method for solving the numerical values of a tip-type robot provided by an embodiment of the present application;

[0063] Figure 3 FIG. is a schematic structural diagram of a device for solving the numerical values of a tip-type robot provided by an embodiment of the present application;

[0064] Figure 4 FIG. is a schematic structural diagram of a device for solving the numerical values of a tip-type robot provided by an embodiment of the present application;

[0065] Figure 5 FIG. is a schematic structural diagram of a computer-readable storage medium in an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0066] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions in the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings in the embodiments of this application. Apparently, the described embodiments are some, but not all, of the embodiments of this application. All other embodiments obtained by those of ordinary skill in the art based on the embodiments in this application without creative efforts shall fall within the scope of protection of this application.

[0067] It should be noted that when a component is referred to as being "fixed to" another component, it can be directly on the other component or there may also be an intermediate component. When a component is considered to be "connected to" another component, it can be directly connected to the other component or there may be an intermediate component at the same time.

[0068] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those of ordinary skill in the technical field to which this application belongs. The terms used in the specification of this application herein 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.

[0069] The terms "first", "second", "third", "fourth", etc. (if any) in the specification and claims of the present invention and the above-mentioned accompanying drawings are used to distinguish similar objects and do not necessarily have to be used to describe a specific order or sequence. It should be understood that such data can be interchanged under appropriate circumstances 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 "comprising" and "having" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or device that includes a series of steps or units does not necessarily have to be limited to those steps or units clearly listed, but may include other steps or units not clearly listed or inherent to these processes, methods, products, or devices.

[0070] The technical solutions of the present invention and how the technical solutions of this application solve the above technical problems will be described in detail below with specific embodiments. These several specific embodiments below can be combined with each other, and the same or similar concepts or processes may not be repeated in some embodiments.

[0071] The following will describe in detail some embodiments of this application with reference to the accompanying drawings. Without conflict, the following embodiments and the features in the embodiments can be combined with each other.

[0072] Figure 2 It is a flowchart of a numerical solution method for a tip-type robot provided for the embodiments of this application, as Figure 2As shown, the method in this embodiment may include the following steps:

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

[0074] In this embodiment, a rectangular coordinate system can be established with the position corresponding to the base of the tip-type robot as the origin; among them, the end coordinate of the tip-type robot is the target coordinate position that the robotic arm execution end needs to reach.

[0075] Step S202: When the tip-type robot satisfies that all 6 axes are vertically downward, obtain the joint angles corresponding to the 4th joint, the 5th joint, and the 6th joint respectively.

[0076] In this embodiment, because the tip-type (cuspidal) robot does not satisfy the Pieper's rule, its inverse kinematics is very complex. Because in the application scenario, the 6 axes are vertically downward, some simplifications can be made. A sufficient condition for the cuspidal robot to satisfy that the 6 axes are vertically downward is: q4 = 0, q5 = q2 + q3 - M PI .

[0077] Exemplarily, control the flange of the 6 axes of the tip-type robot to always face downward, then the joint angle q4 of the 4th joint of the tip-type robot is 0; the joint angle q5 of the 5th joint of the tip-type robot satisfies q5 = q2 + q3 - π.

[0078] Step S203: Establish an inverse kinematics equation based on the end coordinate of the tip-type robot 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.

[0079] In this embodiment, first construct the forward kinematics equation of the tip-type robot, where the calculation formula for the end coordinate of the tip-type robot is as follows:

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

[0081] In the formula: t 0T represents the displacement of the end flange relative to the 0th joint, t 01 represents the displacement of the 1st joint relative to the 0th joint, rotz(q1) represents a rotation of q1 angle around the Z axis, t12 represents the displacement of the second joint relative to the first joint, roty(q2) represents a rotation of q2 degrees about the Y-axis, t 23 represents the displacement of the third joint relative to the second joint, roty(q3) represents a rotation of q3 degrees about the Y-axis, t 34 represents the displacement of the fourth joint relative to the third joint, rotx(q4) represents a rotation of q4 degrees about the X-axis, roty(q5) represents a rotation of q5 degrees about the Y-axis, roty(q6) represents a rotation of q6 degrees about the Y-axis, t 6T represents the displacement of the end flange relative to the sixth joint; where:

[0082]

[0083]

[0084] Substitute q4 = 0, q5 = q2 + q3 - π into it to obtain the simplified calculation formula:

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

[0086] where: rotz(q1)*roty(q2)*roty(q3)*roty(q5)*roty(q6)*t 6T = R 0T * t 6T

[0087] After further simplification:

[0088] t 0T = t 01 + rotz(q1)*(t 12 + roty(q2)*(t 23 + roty(q3)*(t 34 + t 45 )))+ R 0T * t 6T

[0089] According to the known t 0T , calculate q1, q2, q3 respectively.

[0090] Exemplarily, after obtaining the q1, q2, q3 axes, q5 and q6 can also be solved conversely.

[0091] q5 = q2 + q3 - M PI

[0092] rotz(q1)*roty(q2)*roty(q3)*rotx(q4)*roty(q5)*roty(q6) = R 06

[0093] After simplifying rotz(q6), q6 can be calculated according to Rodrigues' formula.

[0094] Step S204: Take the angles corresponding to the first joint, the second joint, and the third joint obtained by calculation, and the known angles corresponding to the fourth joint, the fifth joint, and the sixth joint as a set of approximate solutions of the numerical solutions of the tip-type robot, and use the approximate solutions as the initial values of the numerical solutions for iteration to obtain multiple numerical solutions.

[0095] In this embodiment, eight sets of angles corresponding to the first joint, the second joint, and the third joint, and the corresponding angles corresponding to the fourth joint, the fifth joint, and the sixth joint are obtained, and the pose equation of the tip-type robot is constructed according to the eight sets of solutions to calculate the numerical solutions of all joints of the tip-type robot.

[0096] In this embodiment, by establishing a robotic arm joint coordinate system and configuring a shoulder-elbow-wrist structure according to the six degrees of freedom of the tip-type robot, the 0th to 6th joints are defined from the base to the end; when the six axes of the tip-type robot are all vertically downward, the joint angles corresponding to the fourth joint, the fifth joint, and the sixth joint are obtained respectively; an inverse kinematics equation is established according to the end coordinates of the tip-type robot and the joint angles corresponding to the fourth joint, the fifth joint, and the sixth joint to calculate the angles corresponding to the first joint, the second joint, and the third joint; take the angles corresponding to the first joint, the second joint, and the third joint obtained by calculation, and the known angles corresponding to the fourth joint, the fifth joint, and the sixth joint as a set of approximate solutions of the numerical solutions of the tip-type robot, and use the approximate solutions as the initial values of the numerical solutions for iteration to obtain multiple numerical solutions. Thus, all numerical solutions of the tip-type robot can be quickly solved, which is convenient for controlling each joint of the tip-type robot and enables the tip-type robot to smoothly pass through the singular point.

[0097] Figure 3 It is a schematic structural diagram of a device for solving numerical solutions of a tip-type robot provided by an embodiment of the present application, as Figure 3As shown in the figure, the device in this embodiment may include: a joint coordinate system establishment module 301, configured to establish a manipulator joint coordinate system, configure a shoulder-elbow-wrist structure according to the six degrees of freedom of the tip-type robot, and define joints 0 to 6 from the base to the end; a joint angle acquisition module 302, configured to, when the tip-type robot satisfies that all six axes are vertically downward, respectively acquire the joint angles corresponding to joint 4, joint 5, and joint 6; a joint angle calculation module 303, configured to establish an inverse kinematics equation according to the tip-type robot end coordinates and the joint angles corresponding to joint 4, joint 5, and joint 6, and calculate the angles corresponding to joint 1, joint 2, and joint 3; a numerical solution calculation module 304, configured to use the calculated angles corresponding to joint 1, joint 2, and joint 3, and the known angles corresponding to joint 4, joint 5, and joint 6 as a set of approximate solutions of the tip-type robot numerical solution, and use the approximate solutions as the initial values of the numerical solution for iteration to obtain multiple numerical solutions.

[0098] Exemplarily, the joint coordinate system establishment module 301 is specifically configured to:

[0099] Establish a rectangular coordinate system with the position corresponding to the base of the tip-type robot as the origin; wherein, the tip-type robot end coordinates are the target coordinate positions that the manipulator execution end needs to reach.

[0100] Exemplarily, the joint angle acquisition module 302 is specifically configured to:

[0101] Control the flange of the 6 axes of the tip-type robot to always face downward, then the joint angle q4 of the tip-type robot is 0;

[0102] The joint angle q5 of the tip-type robot = q2 + q3 - M PI ; where: q2 represents the joint angle of joint 2, and q3 represents the joint angle of joint 3.

[0103] Exemplarily, the joint angle calculation module 303 is specifically configured to:

[0104] Construct the forward kinematics equation of the tip-type robot, where the calculation formula of the tip-type robot end coordinates is as follows:

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

[0106] In the formula: t0T Indicates the displacement of the end flange relative to the 0th joint, t 01 Indicates the displacement of the 1st joint relative to the 0th joint, rotz(q1) represents a rotation of q1 degrees about the Z axis, t 12 Indicates the displacement of the 2nd joint relative to the 1st joint, roty(q2) represents a rotation of q2 degrees about the Y axis, t 23 Indicates the displacement of the 3rd joint relative to the 2nd joint, roty(q3) represents a rotation of q3 degrees about the Y axis, t 34 Indicates the displacement of the 4th joint relative to the 3rd joint, rotx(q4) represents a rotation of q4 degrees about the X axis, roty(q5) represents a rotation of q5 degrees about the Y axis, roty(q6) represents a rotation of q6 degrees about the Y axis, t 6T Indicates the displacement of the end flange relative to the 6th joint; where: <�

[0107] <� <� <�

[0108] <� <� <�

[0109] Substitute q4 = 0, q5 = q2 + q3 - π, to obtain the simplified calculation formula:<� <�

[0110] t<� 0T = t<� 01 + rotz(q1) * (t<� 12 + roty(q2) * (t<� 23 + roty(q3) * (t<� 34 + t<� 45 + roty(q5) * roty(q6) * t<� 6T )))<� <�

[0111] Where: rotz(q1) * roty(q2) * roty(q3) * roty(q5) * roty(q6) * t<� 6T = R<� 0T * t<� 6T <� <�

[0112] After further simplification:<� <�

[0113] t<� 0T = t<� 01 + rotz(q1) * (t<� 12 + roty(q2) * (t<� 23 + roty(q3) * (t<� 34 + t<� 45 )))+ R<� 0T * t<� 6T <� <��

[0114] According to the known t<� 0T Note: There are some angle brackets in the original text that seem to be in an incorrect format (e.g., <�

[0107] ). I've translated them as they are, but it's possible there's an error in the original. If you can correct the original, the translation will be more accurate., calculate q1, q2, and q3 respectively.

[0115] Exemplarily, the numerical solution calculation module 304 is specifically configured to:

[0116] Obtain the angles corresponding to the first joint, the second joint, and the third joint, as well as the angles corresponding to the fourth joint, the fifth joint, and the sixth joint in 8 groups, and construct the pose equation of the tip-type robot according to the 8 groups of solutions, and calculate the numerical solutions of all joints of the tip-type robot.

[0117] In this embodiment, by establishing a manipulator joint coordinate system and configuring a shoulder-elbow-wrist structure according to the six degrees of freedom of the tip-type robot, the 0th to 6th joints are defined from the base to the end; when the tip-type robot satisfies that all 6 axes are vertically downward, the joint angles corresponding to the fourth joint, the fifth joint, and the sixth joint are obtained respectively; according to the end coordinates of the tip-type robot and the joint angles corresponding to the fourth joint, the fifth joint, and the sixth joint, an inverse kinematics equation is established to calculate the angles corresponding to the first joint, the second joint, and the third joint; the calculated angles of the first joint, the second joint, and the third joint, and the known angles corresponding to the fourth joint, the fifth joint, and the sixth joint are used as a set of approximate solutions of the numerical solutions of the tip-type robot, and the approximate solutions are used as the initial values of the numerical solutions for iteration to obtain multiple numerical solutions. Thus, all numerical solutions of the tip-type robot can be quickly solved, which is convenient for controlling each joint of the tip-type robot, so that the tip-type robot can smoothly pass through the singular point.

[0118] Figure 4 FIG. is a schematic structural diagram of a tip-type robot numerical solution device provided by an embodiment of the present application. The tip-type robot numerical solution device 400 in this embodiment may include: a processor 401 and a memory 402.

[0119] A memory 402 for storing programs; the memory 402 may include volatile memory (e.g., random-access memory, 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 402 is used to store computer programs (such as application programs and functional modules for implementing the above methods), computer instructions, etc. The above computer programs, computer instructions, etc. may be partitioned and stored in one or more memories 402. And the above computer programs, computer instructions, data, etc. may be called by the processor 401.

[0120] The above computer programs, computer instructions, etc. may be partitioned and stored in one or more memories 402. And the above computer programs, computer instructions, data, etc. may be called by the processor 401.

[0121] A processor 401 for executing the computer programs stored in the memory 402 to implement each step in the method described in the above embodiments.

[0122] For details, reference may be made to the relevant descriptions in the foregoing method embodiments.

[0123] The processor 401 and the memory 402 may be of an independent structure or an integrated structure integrated together. When the processor 401 and the memory 402 are of an independent structure, the memory 402 and the processor 401 may be coupled and connected through a bus 403.

[0124] The tip-type robot numerical solution device 400 of this embodiment can execute Figure 2 the technical solutions in the method shown, and for the specific implementation process and technical principle, reference may be made to Figure 2 the relevant descriptions in the method shown, which will not be elaborated here.

[0125] Those skilled in the art can understand that various aspects of the present invention can be implemented as a system, a method, or a program product. Therefore, various aspects of the present invention can be specifically implemented in the following forms, namely: a complete hardware implementation, a complete software implementation (including firmware, microcode, etc.), or an implementation combining hardware and software aspects, which can be collectively referred to as "circuit", "module", or "platform" here.

[0126] 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 the user device executes the computer-executable instructions, the user device executes the above various possible methods.

[0127] Among them, the computer-readable medium includes a computer storage medium and a communication medium. The communication medium includes any medium that facilitates the transmission of a computer program from one place to another. The storage medium can be any available medium that can be accessed by a general-purpose 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 a component of the processor. The processor and the storage medium can be located in an ASIC. In addition, the ASIC can be located in the user device. Of course, the processor and the storage medium can also exist as discrete components in the communication device.

[0128] The present application also provides a program product. The program product 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, and at least one processor executes the computer program to enable the server to implement the method according to any one of the above embodiments of the present invention.

[0129] Those of ordinary skill 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. The foregoing program can be stored in a computer-readable storage medium. When the program is executed, it executes the steps including the above method embodiments; and the foregoing storage medium includes: Read-Only Memory (ROM), Random Access Memory (RAM), magnetic disk, optical disk, or other various media that can store program codes.

[0130] Figure 5 is a schematic structural diagram of the computer-readable storage medium in the embodiment of the present invention. Refer to Figure 5As shown, a program product 500 for implementing the above method according to an embodiment of the present invention is described. It may be a portable compact disc read-only memory (CD-ROM), include program code, and can 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 may be any tangible medium that contains or stores a program that can be used by or in conjunction with an instruction execution system, apparatus, or device.

[0131] The program product may adopt any combination of one or more readable media. The readable media may be a readable signal medium or a readable storage medium. The readable storage medium may, for example, but is not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination of the above. More specific examples (non-exhaustive list) of the readable storage medium include: an electrical connection having 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 disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above.

[0132] The computer-readable storage medium may include a data signal propagated in a baseband or as part of a carrier wave, in which the readable program code is carried. Such a propagated data signal may take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. The readable storage medium may also be any readable medium other than the readable storage medium that can send, propagate, or transmit a program for use by or in conjunction with an instruction execution system, apparatus, or device. The program code contained on the readable storage medium may be transmitted using any appropriate medium, including but not limited to wireless, wired, optical fiber cable, RF, etc., or any suitable combination of the above.

[0133] The 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 also including conventional procedural programming languages such as the "C" language or similar programming languages. The program code may be executed entirely on the user's computing device, partially on the user's device, executed as a stand-alone software package, partially on the user's 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's 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).

[0134] The various embodiments in this specification are described in a progressive manner. Each embodiment focuses on the differences from other embodiments, and the same or similar parts among the various embodiments can be referred to each other. The above description of the disclosed embodiments enables those skilled in the art to implement or use the present invention. Various modifications to these embodiments will be obvious to those skilled in the art, 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 herein, but rather will conform to the widest scope consistent with the principles and novel features disclosed herein.

[0135] The specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various deformations or modifications within the scope of the claims, which do not affect the essence of the present invention.

Claims

1. A numerical solution method for a tip-type robot, characterized in that, Including: Step 1: Establish a robotic arm joint coordinate system, configure a shoulder-elbow-wrist structure according to the six degrees of freedom of the tip-type robot, and define the 0th to 6th joints from the base to the end; Step 2: When the tip-type robot has all 6 axes vertically downward, obtain the joint angles corresponding to the 4th joint, 5th joint, and 6th joint respectively; Step 3: Establish an inverse kinematics equation based on the tip-type robot's end coordinates and the joint angles corresponding to the 4th joint, 5th joint, and 6th joint, and calculate the angles corresponding to the 1st joint, 2nd joint, and 3rd joint; Step 4: Take the calculated angles of the 1st joint, 2nd joint, and 3rd joint, and the known angles corresponding to the 4th joint, 5th joint, and 6th joint as a set of approximate solutions of the tip-type robot's numerical solution, and use the approximate solution as the initial value of the numerical solution for iteration to obtain multiple numerical solutions.

2. The numerical solution method for the tip-type robot according to claim 1, characterized in that The said Step 1 includes: Taking the position corresponding to the base of the tip-type robot as the origin to establish a rectangular coordinate system; wherein, the tip-type robot's end coordinates are the target coordinate positions that the robotic arm's execution end needs to reach.

3. The numerical solution method for the tip-type robot according to claim 1, characterized in that The said Step 2 includes: Controlling the flange of the 6 axes of the tip-type robot to always face downward, then the angle q4 of the 4th joint of the tip-type robot is 0; The angle q5 of the 5th joint of the tip-type robot = q2 + q3 - π; where: q2 represents the angle of the 2nd joint, and q3 represents the angle of the 3rd joint.

4. The numerical solution method of the tip-type robot according to claim 3, characterized in that The said Step 3 includes: Constructing the forward kinematics equation of the tip-type robot, wherein the calculation formula of the tip-type robot's end coordinates is as follows: t 0T = t 01 + rotz(q1) * (t 12 + roty(q2) * (t 23 + roty(q3) * (t 34 + rotx(q4)(t 45 + roty(q5) * roty(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 1st joint relative to the 0th joint, rotz(q1) represents a rotation of q1 degrees about the Z-axis, t 12 represents the displacement of the 2nd joint relative to the 1st joint, roty(q2) represents a rotation of q2 degrees about the Y-axis, t 23 represents the displacement of the 3rd joint relative to the 2nd joint, roty(q3) represents a rotation of q3 degrees about the Y-axis, t 34 represents the displacement of the 4th joint relative to the 3rd joint, rotx(q4) represents a rotation of q4 degrees about the X-axis, roty(q5) represents a rotation of q5 degrees about the Y-axis, roty(q6) represents a rotation of q6 degrees about the Y-axis, t 6T represents the displacement of the end flange relative to the 6th joint; where: Substituting q4 = 0, q5 = q2 + q3 - π into it, the simplified calculation formula is obtained: t 0T = t 01 + rotz(q1) * (t 12 + roty(q2) * (t 23 + roty(q3) * (t 34 + t 45 + roty(q5) * roty(q6) * t 6T ))) where: rotz(q1)*roty(q2)*roty(q3)*roty(q5)*roty(q6)*t 6T = R 0T *t 6T After further simplification: t 0T = t 01 + rotz(q1) * (t 12 + roty(q2) * (t 23 + roty(q3) * (t 34 + t 45 ))) + R 0T * t 6T Based on the known t 0T , calculate q1, q2, and q3 respectively.

5. The numerical solution method for a tip-type robot according to any one of claims 1-4, characterized in that, The said Step 4 includes: Obtaining 8 groups of angles corresponding to the 1st joint, 2nd joint, and 3rd joint, and the corresponding angles of the 4th joint, 5th joint, and 6th joint, and constructing the pose equation of the tip-type robot according to the 8 groups of solutions to calculate the numerical solutions of all joints of the tip-type robot.

6. A numerical solution device for a tip-type robot, characterized in that, Including: A joint coordinate system establishment module, used to establish a robotic arm joint coordinate system, configure a shoulder-elbow-wrist structure according to the six degrees of freedom of the tip-type robot, and define the 0th to 6th joints from the base to the end; A joint angle acquisition module, used to obtain the joint angles corresponding to the 4th joint, 5th joint, and 6th joint respectively when the tip-type robot has all 6 axes vertically downward; A joint angle calculation module, used to establish an inverse kinematics equation based on the tip-type robot's end coordinates and the joint angles corresponding to the 4th joint, 5th joint, and 6th joint, and calculate the angles corresponding to the 1st joint, 2nd joint, and 3rd joint; A numerical solution calculation module, used to take the calculated angles of the 1st joint, 2nd joint, and 3rd joint, and the known angles corresponding to the 4th joint, 5th joint, and 6th joint as a set of approximate solutions of the tip-type robot's numerical solution, and use the approximate solution as the initial value of the numerical solution for iteration to obtain multiple numerical solutions.

7. The tip-type robot numerical value solving device according to claim 6, characterized in that, The said joint coordinate system establishment module is specifically used for: Taking the position corresponding to the base of the tip-type robot as the origin to establish a rectangular coordinate system; wherein, the tip-type robot's end coordinates are the target coordinate positions that the robotic arm's execution end needs to reach.

8. The tip-type robot numerical value solving device according to claim 6, characterized in that, The joint angle acquisition module is specifically configured to: Control the flange of the 6 axes of the tip-type robot to always face downward, then the joint angle q4 of the 4th joint of the tip-type robot is 0; The joint angle q5 of the 5th joint of the tip-type robot = q2 + q3 - π; where: q2 represents the joint angle of the 2nd joint, and q3 represents the joint angle of the 3rd joint.

9. A numerical solution device for a tip - type robot, characterized in that, It includes: A processor and a memory, where executable program instructions are stored in the memory. When the processor calls the program instructions in the memory, the processor is used to: Execute the steps of the tip-type robot numerical solution method according to any one of claims 1 to 5.

10. A computer-readable storage medium for storing a program, characterized in that, When the program is executed, the steps of the tip-type robot numerical solution method according to any one of claims 1 to 5 are implemented.

Images