Inverse solution method and device for multi-degree-of-freedom mechanical arm

Abstract

The invention provides an inverse solution method and device for a multi-degree-of-freedom mechanical arm. The method comprises the steps that a forward kinematics expression of the mechanical arm is converted into a first expression; randomly sampling the fourth joint angle to determine a current fourth joint angle, and solving the first expression by using the current fourth joint angle to determine a third joint angle; the forward kinematics expression of the mechanical arm is converted into a second expression used for representing the problem of rotation around the ordered double shafts, the second expression is solved, and a first joint angle and a second joint angle are determined; determining a fifth candidate position according to the first joint angle, the second joint angle, the third joint angle and the current fourth joint angle; and when the fifth candidate position does not meet the error allowance condition, sampling the fourth joint angle serving as the variable again, and returning to execute the step of determining the third joint angle. By adopting the inverse solution method and device of the multi-degree-of-freedom mechanical arm, the solution efficiency and the numerical stability of the inverse solution process of the mechanical arm are improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of mechanical arm control, in particular to a multi-degree-of-freedom mechanical arm inverse solution method and device. BACKGROUND

[0002] The core role of the inverse kinematics solution of the serial robot is to inversely solve the motion parameters of each joint, such as angle, displacement, etc., according to the target pose of the end effector, which is the core problem of robot motion planning and control. In the prior art, the following three inverse kinematics solution methods are usually included: the first is an analytical method based on algebraic elimination, which establishes a robot D-H parameter model, deduces a nonlinear equation set between the end pose and the joint angle, and uses algebraic elimination to convert the problem into a high-order polynomial solution; the second is an optimization method based on numerical iteration, which uses numerical optimization algorithms such as Newton-Raphson method and quasi-Newton method to solve the inverse kinematics equation through iterative approximation; the third is a geometric decomposition method, which needs to use Pieper criterion for decoupling for spherical wrist robots.

[0003] However, the analytical method based on algebraic elimination needs to convert the 6R robot inverse kinematics into a 16th order polynomial solution, and this high-order polynomial solving process has the problems of complex solving process and poor numerical stability; the optimization method based on numerical iteration needs to update the joint angle through the inverse or pseudo-inverse of the Jacobian matrix until the end pose error converges, and the convergence depends heavily on the initial guess, which easily leads to the algorithm falling into local optimum or complete divergence, and the Jacobian matrix is prone to ill-conditioned problem near the singular configuration of the robot, resulting in numerical instability; the geometric decomposition method cannot be applied to robots that do not conform to the pieper criterion configuration. SUMMARY

[0004] Therefore, the purpose of the present application is to provide a multi-degree-of-freedom mechanical arm inverse solution method and device to overcome at least one of the above-mentioned defects.

[0005] In the first aspect, the embodiments of the present application provide a multi-degree-of-freedom mechanical arm inverse solution method, comprising: Converting the forward kinematics expression of the mechanical arm into a first expression for characterizing a non-standard screw theory sub-problem, the mechanical arm being a multi-axis mechanical arm that does not conform to the Pieper criterion, the first expression taking the third joint angle and the fourth joint angle as variables; Randomly sampling the fourth joint angle as a variable to determine the current fourth joint angle, and using the current fourth joint angle to solve the first expression to determine the third joint angle; According to the third joint angle and the current fourth joint angle, a forward kinematics expression of the robot arm is converted into a second expression for representing an ordered double-axis rotation problem, the second expression is solved, and the first joint angle and the second joint angle are determined; According to the first joint angle, the second joint angle, the third joint angle, and the current fourth joint angle, a fifth candidate position of the fifth joint at the current fourth joint angle is determined. When the fifth candidate position does not satisfy the error allowance condition, the fourth joint angle as a variable is resampled, and the step of solving the first expression by using the current fourth joint angle to determine the third joint angle is returned to be executed by using the newly determined current fourth joint angle.

[0006] Optionally, the step of converting the forward kinematics expression of the robot arm into the first expression for representing the non-standard screw theory sub-problem comprises: multiplying the fifth joint initial position on both sides of the equation of the forward kinematics expression of the robot arm to obtain a third expression; converting the third expression into a first intermediate expression according to the position invariance principle of the intersection point between the first joint axis and the second joint axis; and simplifying the first intermediate expression according to the length invariance principle of a vector under rotation to obtain the first expression.

[0007] Optionally, the step of converting the third expression into the first intermediate expression according to the position invariance principle of the intersection point between the first joint axis and the second joint axis comprises: determining a second joint initial position expression according to the position invariance principle, the second joint initial position expression being an equation that a product of a first joint screw index mapping, a second joint screw index mapping, and a second joint initial position is equal to the second joint initial position; subtracting the second joint initial position from both sides of the equation of the third expression to obtain a distance difference expression; and replacing a target second joint initial position in the distance difference expression with the second joint initial position expression to obtain the first intermediate expression.

[0008] Optionally, the step of solving the first expression by using the current fourth joint angle to determine the third joint angle comprises: updating the first expression by using the current fourth joint angle to convert the first expression into a third expression for representing a single-axis rotation translation problem; and solving the third expression according to a solving method of the single-axis rotation translation problem to determine the third joint angle.

[0009] Optionally, the first expression is an equation that the first distance is equal to the second distance, the first distance is a distance between the first position and the second joint initial position, the second distance is a distance between the second joint initial position and a product of the fifth joint initial position, an inverse of the fifth joint initial pose and a pose after the fifth joint motion, and the first position is a position corresponding to a product of the third joint screw index mapping, the fourth joint screw index mapping and the fifth joint initial position.

[0010] Optionally, the step of converting the forward kinematics expression of the robot arm into the second expression for representing the ordered dual-axis rotation problem according to the third joint angle and the current fourth joint angle comprises: substituting the determined third joint angle and the current fourth joint angle into the third expression to obtain the second expression.

[0011] Optionally, the step of determining the fifth candidate position of the fifth joint under the current fourth joint angle according to the first joint angle, the second joint angle, the third joint angle and the current fourth joint angle comprises: determining the fifth candidate position according to a product of the first joint screw index mapping corresponding to the first joint angle, the second joint screw index mapping corresponding to the second joint angle, the third joint screw index mapping corresponding to the third joint angle, the fourth joint screw index mapping corresponding to the fourth joint angle and the fifth joint initial position.

[0012] Optionally, after determining the third joint angle by solving the first expression with the current fourth joint angle, the method further comprises: determining whether the third joint angle has a solution; and if the third joint angle has no solution, resampling the fourth joint angle as a variable to determine a latest current fourth joint angle, and solving the first expression with the latest current fourth joint angle.

[0013] Optionally, the method further comprises: when the fifth candidate position satisfies the error allowable condition, converting the forward kinematics expression of the robot arm into a fourth expression for representing the ordered dual-axis rotation problem; and solving the fourth expression according to a solving method of the ordered dual-axis rotation problem to determine the fifth joint angle and the sixth joint angle.

[0014] In a second aspect, an inverse solution device of a multi-degree-of-freedom robot arm is also provided in the embodiments of the present application, and the device comprises: A first expression determination module is configured to convert a forward kinematics expression of a robot arm into a first expression for representing a non-standard screw theory sub-problem, the robot arm being a multi-axis robot arm not conforming to a Pieper criterion, and the first expression taking a third joint angle and a fourth joint angle as variables; A first angle determination module is configured to randomly sample the fourth joint angle as a variable to determine a current fourth joint angle, and solve the first expression with the current fourth joint angle to determine the third joint angle. a second angle determination module configured to convert a forward kinematics expression of the robot arm into a second expression for characterizing an ordered two-axis rotation problem according to the third joint angle and the current fourth joint angle, and solve the second expression to determine the first joint angle and the second joint angle; a first position determination module configured to determine a fifth candidate position of the fifth joint at the current fourth joint angle according to the first joint angle, the second joint angle, the third joint angle and the current fourth joint angle; a loop iteration module configured to, when the fifth candidate position does not satisfy the error allowance condition, resample the fourth joint angle as a variable, and return to performing the step of solving the first expression according to the current fourth joint angle to determine the third joint angle.

[0015] The embodiments of the present application have the following beneficial effects: The inverse solution method and device for a multi-degree-of-freedom robot arm provided by the embodiments of the present application can convert a complex nonlinear problem into a series of sub-problems with clear geometric meanings, and solve each sub-problem in turn according to the screw theory, thereby improving the solving efficiency and numerical stability, and solving the problems of low solving efficiency and poor numerical stability in the inverse solution process of the robot arm compared with the inverse solution method for a multi-degree-of-freedom robot arm in the prior art.

[0016] In order to make the above purposes, features and advantages of the present application more obvious and easy to understand, the following preferred embodiments are described in detail below, and the accompanying drawings are described as follows. BRIEF DESCRIPTION OF DRAWINGS

[0017] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following will briefly introduce the drawings needed to be used in the embodiments. It should be understood that the following drawings only show some embodiments of the present application, and therefore should not be regarded as a limitation on the scope. For those skilled in the art, other related drawings can also be obtained without creative labor on the basis of these drawings.

[0018] Figure 1 A flowchart of the inverse solution method for a multi-degree-of-freedom robot arm provided by the embodiments of the present application is shown; Figure 2 A structural schematic diagram of a multi-degree-of-freedom robot arm provided by the embodiments of the present application is shown; Figure 3 A flowchart of the determination step of the first expression provided by the embodiments of the present application is shown; Figure 4 A schematic diagram of the third joint angle provided by the embodiments of the present application is shown; Figure 5A structural schematic diagram of a multi-degree-of-freedom robot arm provided by an embodiment of the present application is shown. Figure 6 A structural schematic diagram of an electronic device provided by an embodiment of the present application is shown. DETAILED DESCRIPTION

[0019] To make the objectives, technical solutions, and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be described below in connection with the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. The components of the embodiments of the present application described and shown in the accompanying drawings herein can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present application provided in the accompanying drawings is not intended to limit the scope of the claimed present application, but only represents selected embodiments of the present application. Based on the embodiments of the present application, every other embodiment obtained by a person skilled in the art without creative work falls within the scope of the present application.

[0020] To make the objectives, technical solutions, and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be described below in connection with the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. The components of the embodiments of the present application described and shown in the accompanying drawings herein can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present application provided in the accompanying drawings is not intended to limit the scope of the claimed present application, but only represents selected embodiments of the present application. Based on the embodiments of the present application, every other embodiment obtained by a person skilled in the art without creative work falls within the scope of the present application.

[0021] Please refer to Figure 1 , Figure 1 A flowchart of a multi-degree-of-freedom robot arm inverse solution method provided by an embodiment of the present application is shown. As shown in Figure 1 , the multi-degree-of-freedom robot arm inverse solution method provided by the embodiment of the present application comprises: Step S101, converting a forward kinematics expression of a robot arm into a first expression for characterizing a non-standard screw theory sub-problem.

[0022] The robot arm is a six-degree-of-freedom robot arm of a serial robot, and the robot arm is a multi-axis robot arm that does not meet the Pieper criterion under a specific configuration, i.e., the robot arm does not meet the condition that the axes of the last three joints intersect at a point, nor the condition that there are three adjacent axes parallel to each other.

[0023] A multi-axis robot arm that does not meet the Pieper criterion under a specific configuration will be introduced below with reference to Figure 2 .

[0024] Figure 2 A structural schematic diagram of a multi-degree-of-freedom robot arm provided by an embodiment of the present application is shown, as Figure 2As shown, the robot arm includes six axes, which are the first axis 201, the second axis 202, the third axis 203, the fourth axis 204, the fifth axis 205, and the sixth axis 206 in the order of connection. The specific configuration refers to that the second axis 202 and the third axis 203 of the robot arm are parallel, the fourth axis 204 is perpendicular to the third axis 203, the fifth axis 205 is perpendicular to the fourth axis 204, and the sixth axis 206 is perpendicular to the fifth axis 205. Meanwhile, the robot arm includes a plurality of joints, which include the second joint 212, the third joint 213, the fourth joint 214, and the fifth joint 215. Among them, is a base coordinate system, which is a global fixed reference system of the robot arm and is a reference origin for all motion calculations; is a tool coordinate system, which is a local reference system fixed to the fifth joint of the robot arm and focuses on the actual working point of the tool. Among them, the second axis 202 and the third axis 203 both rotate around direction.

[0025] Meanwhile, the pose of the end effector 216 of the robot arm and the link length between the joints are known, and since the end effector 216 of the robot arm and the fifth joint 215 are on the fifth axis 205, the pose of the fifth joint 215 and the pose of the end effector 216 of the robot arm only differ in distance. Since the distance difference is known, the pose of the fifth joint 215 is also known. The pose of the fifth joint 215 includes the initial pose of the fifth joint and the pose after the motion of the fifth joint. The initial pose of the fifth joint can refer to the pose of the fifth joint in the initial state.

[0026] The determination process of the first expression will be introduced below. Figure 3

[0027] Figure 3 The flow chart of the determination step of the first expression provided by the embodiment of the application is shown in FIG. 1, as shown in the figure, the determination step of the first expression includes: Figure 3 Step S1011, multiply the initial position of the fifth joint on both sides of the forward kinematics expression of the robot arm to obtain a third expression.

[0028] The forward kinematics expression of the robot arm can be determined by the exponential product formula of the serial robot. The exponential product formula can refer to the POE (Product of Exponentials) formula, and the POE formula is: Therefore, the forward kinematics expression of the six-axis robot arm is: wherein, represents the initial pose of the fifth joint, i.e., the pose of the fifth joint in the initial state, is a known 4-order matrix; ​​denotes the pose of the fifth joint after the fifth joint motion, is also a known 4th order matrix; denotes the initial motion screw of each joint (i=1, 2, …, 6); denotes the joint angle of each joint of the robot arm (i=1, 2, …, 6).

[0029] denotes the first joint angle, which can refer to the rotation angle of the first joint around its own axis in the base coordinate system; denotes the second joint angle, which can refer to the rotation angle of the second joint around its own axis in the base coordinate system; denotes the third joint angle, which can refer to the rotation angle of the third joint around its own axis in the base coordinate system; denotes the fourth joint angle, which can refer to the rotation angle of the fourth joint around its own axis in the base coordinate system; denotes the fifth joint angle, which can refer to the rotation angle of the fifth joint around its own axis in the base coordinate system; denotes the sixth joint angle, which can refer to the rotation angle of the sixth joint (end effector) around its own axis in the base coordinate system.

[0030] The goal of the inverse kinematics solution of the robot arm is the above six joint angles, and the known conditions for the inverse kinematics solution of the robot arm are the pose of the end effector (sixth joint) in the initial state, the pose of the end effector (sixth joint) after the motion, and the initial motion screw of each joint , the position of each joint in the initial state. Since the pose of the fifth joint can be determined according to the pose of the end effector, the known conditions for the inverse kinematics solution of the robot arm include the pose of the fifth joint in the initial state and the pose of the fifth joint after the motion. Among them, the pose of the fifth joint in the initial state is called the fifth joint initial pose.

[0031] Specifically, the fifth joint initial position is denoted as: , the third expression can be obtained by multiplying the fifth joint initial position on both sides of the forward kinematics expression of the robot arm: , in the third expression, denotes the first joint screw index mapping, i.e., the screw index mapping of the first joint; denotes the second joint screw index mapping, i.e., the screw index mapping of the second joint; denotes the third joint screw index mapping, i.e., the screw index mapping of the third joint; denotes the fourth joint screw index mapping, i.e., the screw index mapping of the fourth joint.

[0032] Step S1012, according to the position invariance principle of the intersection point between the first joint axis and the second joint axis, the third expression is converted into a first intermediate expression.

[0033] In the first step, the second joint initial position expression is determined according to the position invariance principle.

[0034] The position invariance principle can refer to the position of the intersection point between the axis of the first joint and the axis of the second joint is always invariable, that is, no matter how the first joint and the second joint rotate, the position of the intersection point is invariable. According to this principle, the second joint initial position expression can be constructed.

[0035] The second joint initial position expression is the product of the first joint screw index mapping, the second joint screw index mapping, and the second joint initial position, which is equal to the second joint initial position. The second joint initial position expression can be expressed as: wherein, represents the second joint initial position.

[0036] In the second step, the second joint initial position is subtracted from both sides of the equation of the third expression to obtain a distance difference expression.

[0037] After subtracting the second joint initial position from both sides of the equation of the third expression, the distance difference expression is: .

[0038] In the third step, the target second joint initial position in the distance difference expression is replaced by the second joint initial position expression to obtain a first intermediate expression.

[0039] The target second joint initial position refers to the second joint initial position in the distance difference expression which is on the same side as the multiple joint screw index mappings. The first intermediate expression after replacing the target second joint initial position is expressed as: .

[0040] Further, the first intermediate expression can be arranged to obtain: .

[0041] Step S1013, according to the principle of invariance of vector length under rotation, the first intermediate expression is simplified to obtain the first expression.

[0042] The principle of invariance of vector length under rotation can refer to the length of a vector is invariable no matter how the vector is rotated.

[0043] Taking the norm of both sides of the equation of the first intermediate expression, the first intermediate distance expression is obtained: .

[0044] Then, according to the principle of invariance of vector length under rotation, the left side of the equation can be deleted This yields the first expression.

[0045] The first expression is: The first expression can be described in textual form, for example: the first expression is an equation where the first distance equals the second distance. The first distance is the distance between the first position and the initial position of the second joint. The second distance is the distance between the product of the initial position of the fifth joint, the reciprocal of the initial pose of the fifth joint, and the pose after the fifth joint's movement, and the initial position of the second joint. The first position is the position corresponding to the product of the screw index mapping of the third joint, the screw index mapping of the fourth joint, and the initial position of the fifth joint. The first expression uses the third joint angle and the fourth joint angle as variables. The first distance can be expressed as: The second distance can be expressed as The first position can be represented as: .

[0046] Step S102: Randomly sample the fourth joint angle as a variable to determine the current fourth joint angle, and use the current fourth joint angle to solve the first expression to determine the third joint angle.

[0047] Random sampling is performed within the range of 0° to 360° to determine the current value of the fourth joint angle. Then, the first expression is updated using the current fourth joint angle to transform it into a third expression characterizing the problem of translational rotation about a single axis. The third expression can be represented as: ,in, express ,because but This is also known.

[0048] The following reference Figure 4 Let's introduce the third joint angle. Figure 4 A schematic diagram of the third joint angle provided in an embodiment of this application is shown, as follows: Figure 4 As shown, the fifth joint is composed of Start orbiting the fourth axis ( Rotation to Then by Start around the third axis ( Rotation to , and The distance between them is d. express .

[0049] As can be seen from the form of the third expression, by sampling the fourth joint angle, the first expression can be converted into the third expression representing the single-axis rotation and translation problem (sub-problem 3) in the Paden-Kahan sub-problem in the standard screw theory. The third expression can be solved according to the solving method of the single-axis rotation and translation problem to determine the third joint angle.

[0050] In an embodiment, when the first expression is solved by using the current fourth joint angle, the third joint angle can be solvable or unsolvable. Therefore, after the first expression is solved, it can be determined whether the third joint angle is solvable. If the third joint angle is unsolvable, the step S102 is returned to resample the fourth joint angle as a variable to determine the current fourth joint angle again, and the first expression is solved again by using the latest current fourth joint angle until the third joint angle is solvable, and the step S103 is executed.

[0051] In the step S103, the forward kinematics expression of the robot arm is converted into the second expression for representing the ordered double-axis rotation problem according to the third joint angle and the current fourth joint angle, the second expression is solved, and the first joint angle and the second joint angle are determined.

[0052] Specifically, the fifth joint initial position is multiplied by both sides of the forward kinematics expression of the robot arm, the determined third joint angle and the current fourth joint angle are substituted, and the second expression is obtained, which is represented as: wherein, represents Since , and the current are known, the is also known.

[0053] The second expression is an equation for representing the ordered double-axis rotation problem (sub-problem 2) in the Paden-Kahan sub-problem in the screw theory. The second expression can be solved according to the solving method of the ordered double-axis rotation problem, so as to determine the first joint angle and the second joint angle under the current fourth joint angle.

[0054] In the step S104, the fifth candidate position of the fifth joint under the current fourth joint angle is determined according to the first joint angle, the second joint angle, the third joint angle and the current fourth joint angle.

[0055] The fifth candidate position is determined according to the product of the first joint screw index mapping corresponding to the first joint angle, the second joint screw index mapping corresponding to the second joint angle, the third joint screw index mapping corresponding to the third joint angle, the fourth joint screw index mapping corresponding to the fourth joint angle and the fifth joint initial position.

[0056] For example, the fifth candidate position is denoted as: The fifth candidate position can be expressed as: Since , , , and are known, the value of the fifth candidate position of the fifth joint at the current fourth joint angle can be calculated.

[0057] In an embodiment, after the fifth candidate position is determined, the fifth candidate position can be compared with the actual position of the fifth joint, and it is determined whether the error allowance condition is satisfied according to the comparison result.

[0058] For example, it is determined whether the deviation between the fifth candidate position and the actual position of the fifth joint is less than a set deviation threshold. If the deviation between the fifth candidate position and the actual position of the fifth joint is less than the set deviation threshold, it is determined that the error allowance condition is satisfied, indicating that the current fourth joint angle selected is accurate, and the fifth joint angle and the sixth joint angle can be determined continuously. If the deviation between the fifth candidate position and the actual position of the fifth joint is greater than or equal to the set deviation threshold, it is determined that the error allowance condition is not satisfied, indicating that the current fourth joint angle selected is not accurate enough, and the value of the fourth joint angle needs to be selected again, and the step S105 of solving the first expression by using the current fourth joint angle to determine the third joint angle is executed continuously. The actual position of the fifth joint is determined according to the pose of the fifth joint after movement.

[0059] In step S105, when the fifth candidate position does not satisfy the error allowance condition, the fourth joint angle as a variable is resampled, and the step of solving the first expression by using the current fourth joint angle to determine the third joint angle is executed again by using the fourth joint angle resampled.

[0060] When the fifth candidate position does not satisfy the error allowance condition, it indicates that the fifth candidate position determined at the current fourth joint angle does not match the actual position of the fifth joint, and the fourth joint angle needs to be resampled. Therefore, the step S102 is executed again.

[0061] By analogy, after multiple iterations, when the fifth candidate position satisfies the error allowance condition, the fifth joint angle and the sixth joint angle are determined according to the first joint angle, the second joint angle, the third joint angle and the fourth joint angle.

[0062] When the fifth candidate position satisfies the error allowance condition, the forward kinematics expression of the robot arm is converted into a fourth expression for representing the ordered double-axis rotation problem, and the fourth expression is solved according to the solving method of the ordered double-axis rotation problem to determine the fifth joint angle and the sixth joint angle.

[0063] For example, moving , , , and to the other side of the equation, a second intermediate expression is obtained. Then, multiplying the target joint initial position on both sides of the second intermediate expression, a fourth expression is obtained. The target joint initial position can refer to the position of the target joint in the initial state.

[0064] The target joint is a joint selected from a plurality of joints. The target joint can be the third joint or the fourth joint. When the target joint is the third joint, the fourth expression is: At this time, the target derived position can refer to . In the fourth expression, only and are unknown. It is a double-ordered axis rotation problem (sub-problem 2) under the standard Paden-Kahan sub-problem in the screw theory. Therefore, the fourth expression can be solved by using the solving method of the double-ordered axis rotation problem to determine the fifth joint angle and the sixth joint angle.

[0065] The inverse solution method of the multi-degree-of-freedom robot arm provided in the embodiments of the present application can convert a complex nonlinear problem into a series of sub-problems with clear geometric meaning. Each sub-problem after conversion is solved according to the screw theory, which improves the solving efficiency and numerical stability. Compared with the inverse solution method of the multi-degree-of-freedom robot arm in the prior art, the inverse solution method of the multi-degree-of-freedom robot arm in the embodiments of the present application solves the problems of low solving efficiency and poor numerical stability in the inverse solution process of the robot arm.

[0066] Based on the same inventive concept, the embodiments of the present application also provide an inverse solution device of a multi-degree-of-freedom robot arm corresponding to the inverse solution method of the multi-degree-of-freedom robot arm. Since the principle of solving problems by the device in the embodiments of the present application is similar to the inverse solution method of the multi-degree-of-freedom robot arm described above, the implementation of the device can be referred to the implementation of the method, and the repeated parts will not be described here.

[0067] Please refer to Figure 5 , Figure 5 for a structural schematic diagram of an inverse solution device of a multi-degree-of-freedom robot arm provided by the embodiments of the present application. As shown in Figure 5 , the inverse solution device 400 of the multi-degree-of-freedom robot arm comprises: The first expression determination module 401 is configured to convert the forward kinematics expression of the robot arm into a first expression for characterizing a non-standard screw theory sub-problem, the robot arm being a multi-axis robot arm not conforming to the Pieper criterion, the first expression taking the third joint angle and the fourth joint angle as variables. The first angle determination module 402 is configured to randomly sample the fourth joint angle as a variable to determine a current fourth joint angle, and solve the first expression by using the current fourth joint angle to determine the third joint angle. The second angle determination module 403 is configured to convert the forward kinematics expression of the robot arm into a second expression for characterizing an ordered two-axis rotation problem according to the third joint angle and the current fourth joint angle, and solve the second expression to determine the first joint angle and the second joint angle. The first position determination module 404 is configured to determine a fifth candidate position of the fifth joint under the current fourth joint angle according to the first joint angle, the second joint angle, the third joint angle and the current fourth joint angle. The loop iteration module 405 is configured to, when the fifth candidate position does not satisfy the error allowance condition, resample the fourth joint angle as a variable, and return to the step of solving the first expression by using the current fourth joint angle to determine the third joint angle by using the newly determined current fourth joint angle.

[0068] Please refer to Figure 6 , Figure 6 The electronic device 500 includes a processor 510, a memory 520 and a bus 530. Figure 6 The memory 520 stores machine readable instructions executable by the processor 510.

[0069] When the electronic device 500 is running, the processor 510 and the memory 520 communicate through the bus 530, and the machine readable instructions executed by the processor 510 can perform the steps of the inverse solution method of the multi-freedom robot arm in the method embodiment as shown above Figure 1 The specific implementation can be referred to the method embodiment, and will not be repeated here.

[0070] The embodiment of the present application further provides a computer readable storage medium, the computer readable storage medium stores a computer program, and the computer program is run by a processor to perform the steps of the inverse solution method of the multi-freedom robot arm in the method embodiment as shown above Figure 1 The specific implementation can be referred to the method embodiment, and will not be repeated here.

[0071] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working process of the system, device and unit described above can refer to the corresponding process in the foregoing method embodiments, which will not be repeated here.

[0072] In several embodiments provided in the present application, it should be understood that the disclosed system, device and method can be implemented in other ways. The device embodiments described above are only schematic, for example, the division of the units is only a logical function division, and actual implementation can have another division manner, for example, a plurality of units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the coupling or direct coupling or communication connection between the units shown or discussed can be indirect coupling or communication connection through some communication interfaces, devices or units, and can be electrical, mechanical or other forms.

[0073] The units described as separate components can or can not be physically separate, and the components shown as units can or can not be physical units, that is, they can be located in one place, or can be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the embodiment scheme.

[0074] In addition, the functional units in each embodiment of the present application can be integrated in one processing unit, or each unit can be physically present separately, or two or more units can be integrated in one unit.

[0075] If the functions are realized in the form of software function units and sold or used as independent products, they can be stored in a nonvolatile computer readable storage medium executable by a processor. Based on this understanding, the technical solutions of the present application or the essential part or part of the technical solutions that make contributions to the prior art can be embodied in the form of a software product. The computer software product is stored in a storage medium, including a plurality of instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the method described in each embodiment of the present application. The foregoing storage medium includes: U disk, mobile hard disk, read-only memory (Read-Only Memory, ROM), random access memory (Random Access Memory, RAM), magnetic disk or optical disk and various program code storage media.

[0076] Finally, it should be noted that the above-described embodiments are merely specific embodiments of the present application, which are used to illustrate the technical solutions of the present application, but not to limit the same. The protection scope of the present application is not limited thereto. Although the present application has been described in detail with reference to the foregoing embodiments, it should be understood by those skilled in the art that any person skilled in the art can make modifications or easily think of changes to the technical solutions recorded in the foregoing embodiments, or make equivalent replacements to some of the technical features within the technical scope disclosed by the present application. The modifications, changes or replacements do not make the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application, and should be covered within the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. A method for inverse kinematics of a multi-degree-of-freedom robot arm, characterized by, The method comprises the following steps: Converting the forward kinematics expression of the robot arm into a first expression for representing a non-standard screw theory sub-problem, the robot arm being a multi-axis robot arm not conforming to the Pieper criterion, the first expression taking a third joint angle and a fourth joint angle as variables; Randomly sampling the fourth joint angle as a variable to determine a current fourth joint angle, and solving the first expression by using the current fourth joint angle to determine the third joint angle; Converting the forward kinematics expression of the robot arm into a second expression for representing an ordered two-axis rotation problem according to the third joint angle and the current fourth joint angle, and solving the second expression to determine a first joint angle and a second joint angle; Determining a fifth candidate position of the fifth joint at the current fourth joint angle according to the first joint angle, the second joint angle, the third joint angle and the current fourth joint angle; When the fifth candidate position does not satisfy an error allowance condition, resampling the fourth joint angle as a variable, and returning to the step of solving the first expression by using the current fourth joint angle to determine the third joint angle by using the newly determined current fourth joint angle.

2. The method of claim 1, wherein, The step of converting the forward kinematics expression of the robot arm into a first expression for representing a non-standard screw theory sub-problem comprises the following steps: Multiplying the forward kinematics expression of the robot arm by the fifth joint initial position on both sides of the equation to obtain a third expression; Converting the third expression into a first intermediate expression according to the principle of position invariance of the intersection point between the first joint axis and the second joint axis; Simplifying the first intermediate expression according to the principle of length invariance of a vector under rotation to obtain the first expression.

3. The method of claim 2, wherein, The step of converting the third expression into a first intermediate expression according to the principle of position invariance of the intersection point between the first joint axis and the second joint axis comprises the following steps: Determining a second joint initial position expression according to the principle of position invariance, the second joint initial position expression being an equation that the product of a first joint screw index mapping, a second joint screw index mapping and a second joint initial position is equal to the second joint initial position; Subtracting the second joint initial position from both sides of the equation of the third expression to obtain a distance difference expression; Replacing a target second joint initial position in the distance difference expression with the second joint initial position expression to obtain the first intermediate expression.

4. The method of claim 1, wherein, The step of solving the first expression by using the current fourth joint angle to determine the third joint angle comprises the following steps: Updating the first expression by using the current fourth joint angle to convert the first expression into a third expression for representing a single-axis rotation translation problem; Solving the third expression according to the solving method of the single-axis rotation translation problem to determine the third joint angle.

5. The method of claim 1, wherein, The first expression is an equation that the first distance is equal to the second distance, the first distance is a distance between a first position and a second joint initial position, the second distance is a distance between the second joint initial position and a product of a fifth joint initial position, an inverse of a fifth joint initial pose and a pose after fifth joint motion, the first position is a position corresponding to a product of a third joint screw index mapping, a fourth joint screw index mapping and the fifth joint initial position.

6. The method of claim 2, wherein, The step of converting the forward kinematics expression of the robot arm into a second expression for representing the ordered dual-axis rotation problem according to the third joint angle and the current fourth joint angle comprises: Substituting the determined third joint angle and the current fourth joint angle into the third expression to obtain a second expression.

7. The method of claim 1, wherein, The step of determining a fifth candidate position of the fifth joint under the current fourth joint angle according to the first joint angle, the second joint angle, the third joint angle and the current fourth joint angle comprises: Determining the fifth candidate position according to a product of a first joint screw index mapping corresponding to the first joint angle, a second joint screw index mapping corresponding to the second joint angle, a third joint screw index mapping corresponding to the third joint angle, a fourth joint screw index mapping corresponding to the fourth joint angle and a fifth joint initial position.

8. The method of claim 1, wherein, After solving the first expression by using the current fourth joint angle, determining the third joint angle further comprises: Determining whether the third joint angle has a solution; If the third joint angle has no solution, resampling the fourth joint angle as a variable to determine a latest current fourth joint angle, and solving the first expression by using the latest current fourth joint angle.

9. The method of claim 1, wherein, The method further comprises: When the fifth candidate position satisfies the error allowable condition, converting the forward kinematics expression of the robot arm into a fourth expression for representing the ordered dual-axis rotation problem; Solving the fourth expression according to a solving method of the ordered dual-axis rotation problem to determine a fifth joint angle and a sixth joint angle.

10. An inverse solution device of a multi-degree-of-freedom robot arm, characterized by, Comprise: A first expression determining module is configured to convert a forward kinematics expression of a robot arm into a first expression for representing a non-standard screw theory sub-problem, the robot arm being a multi-axis robot arm not conforming to a Pieper criterion, the first expression taking a third joint angle and a fourth joint angle as variables; A first angle determining module is configured to randomly sample a fourth joint angle as a variable to determine a current fourth joint angle, and solve the first expression by using the current fourth joint angle to determine a third joint angle; A second angle determining module is configured to convert the forward kinematics expression of the robot arm into a second expression for representing an ordered dual-axis rotation problem according to the third joint angle and the current fourth joint angle, and solve the second expression to determine a first joint angle and a second joint angle. The first position determining module is configured to determine a fifth candidate position of a fifth joint at the current fourth joint angle according to the first joint angle, the second joint angle, the third joint angle and the current fourth joint angle. The loop iteration module is configured to, when the fifth candidate position does not satisfy the error allowance condition, resample the fourth joint angle as a variable, return to perform the step of solving the first expression by using the current fourth joint angle to determine the third joint angle by using the newly determined current fourth joint angle.

Images