Inverse kinematics solving method and device, computer equipment and storage medium

Abstract

The invention relates to an inverse kinematics solving method and device, computer equipment and a storage medium, the method is applied to a mechanical arm of a humanoid robot, the mechanical arm comprises a plurality of joints, the joints are driven by a rotating motor and a linear motor, and the method comprises the steps that a posture transformation matrix of the mechanical arm is obtained; a forward transfer matrix of the mechanical arm is obtained; obtaining a multivariate nonlinear equation set according to the attitude transformation matrix and the forward transfer matrix; according to the Newton Hessian method, obtaining an approximate matrix of the inverse matrix of the multivariate nonlinear equation set; and according to the tail end pose of the mechanical arm, the angles of the multiple joints of the mechanical arm are inversely solved through the approximate matrix. According to the method, the calculation amount of inverse kinematics solution can be reduced, and the solution speed is improved.

Description

Technical Field

[0001] The present invention relates to the field of inverse kinematics solution, and in particular to an inverse kinematics solution method, device, computer equipment and storage medium. Background Art

[0002] With the continuous improvement of information technology, automation and intelligence, multi-degree-of-freedom robotic arms are increasingly used in complex working environments due to their higher flexibility and adaptability. For example, in industrial production, robotic arms are widely used in assembly lines, especially in environments with high precision and high efficiency requirements such as automobile manufacturing and electronic equipment assembly. Robotic arms can continuously perform repetitive tasks such as welding, painting, assembly, handling and packaging, greatly improving production efficiency and product quality.

[0003] The multi-degree-of-freedom manipulator of a humanoid robot is a more complex robotic system. The multi-degree-of-freedom manipulator includes multiple joints, each of which corresponds to a degree of freedom. Different angles and rotation trajectories of the joints correspond to different postures of the end effector of the manipulator. The multi-degree-of-freedom manipulator has more possible postures when performing work, making functions such as path planning and obstacle avoidance more efficient and accurate, and allowing its degree-of-freedom manipulator to simulate the flexible movement of human arms, giving it advantages in many applications.

[0004] For a robotic arm, kinematics solution and inverse kinematics solution are important processes to achieve its control. Among them, kinematics solution refers to determining the position and posture of the end effector according to the angles of each joint of the robotic arm, while inverse kinematics solution is to determine the angles of each joint of the robot according to the position and posture of the end effector.

[0005] Since multi-DOF robotic arms have multiple joints, multiple sets of answers can be obtained by calculating the angles of multiple joints from one end position, which means that the inverse kinematics solution is more difficult. In traditional inverse kinematics solution methods, the inverse solution needs to be calculated based on the position information of the first four joints, and then the angles of the next few joints are determined based on the posture of the robotic arm. The derivation process is complicated and may generate a large number of solutions. It is easy to fall into the local optimum but not the overall optimum. At the same time, the solution generated by the traditional solution may make the movement form of the robotic arm weird, which is quite different from the movement form of humans and cannot meet the needs of users.

[0006] It can be seen that the existing inverse kinematics solution method for multi-degree-of-freedom robotic arms is still imperfect. Summary of the invention

[0007] In order to solve the above technical problem or at least partially solve the above technical problem, the present invention provides an inverse kinematics solution method, device, computer equipment and storage medium.

[0008] In a first aspect, the present invention provides an inverse kinematics solution method, the method is applied to a mechanical arm of a humanoid robot, the mechanical arm includes a plurality of joints, the plurality of joints are driven by a rotary motor and a linear motor, the method comprising:

[0009] Obtaining a posture transformation matrix of the robotic arm;

[0010] Obtaining a forward transfer matrix of the robotic arm;

[0011] Obtaining a multivariate nonlinear equation group according to the posture transformation matrix and the forward transfer matrix;

[0012] According to the Newton-Hessian method, an approximate matrix of the inverse matrix of the multivariate nonlinear equation system is obtained;

[0013] According to the end posture of the robotic arm, the angles of the multiple joints of the robotic arm are inversely solved through the approximate matrix.

[0014] Optionally, the acquiring the posture transformation matrix of the robotic arm includes:

[0015] Obtain the X rotation matrix, Y rotation matrix and Z rotation matrix of the robotic arm;

[0016] Obtain a total rotation matrix according to the X rotation matrix, the Y rotation matrix and the Z rotation matrix;

[0017] The posture transformation matrix is ​​obtained according to the total rotation matrix.

[0018] Optionally, the X rotation matrix is:

[0019]

[0020] The Y rotation matrix is:

[0021]

[0022] The Z rotation matrix is:

[0023]

[0024] The total rotation matrix is:

[0025]

[0026] The posture transformation matrix is:

[0027]

[0028] Among them, α, β and γ are Euler angles, which are used to indicate the end position of the robotic arm.

[0029] Optionally, the acquiring the forward transfer matrix of the robotic arm includes:

[0030] Constructing a DH coordinate system of the robotic arm according to the joint structure of the robotic arm;

[0031] According to the DH coordinate system and the robot arm parameters, DH parameters are obtained;

[0032] According to the DH parameters, a joint transfer matrix of each joint of the robotic arm is obtained;

[0033] According to the joint transfer matrix, the forward transfer matrix is ​​obtained.

[0034] Optionally, the DH coordinate system of the robotic arm is constructed in the following manner:

[0035] Each joint of the robot arm corresponds to a DH coordinate system, and the coordinate axis of the i-th joint is X i , Y i and Z i ;

[0036] Among them, the coordinate axis X i With coordinate axis Z i-1 Vertical, coordinate axis X i With coordinate axis Z i-1 Intersection, i=0,1,2,…

[0037] Optionally, the DH parameters of the i-th joint include: θ i , d i , l i , α i , offset i ;

[0038] Among them, θ i The coordinate axis X i-1 With coordinate axis X i The angle of

[0039] d i The coordinate axis X i-1 With coordinate axis X i Along the coordinate axis Z i-1 The directed distance of

[0040] l i is the coordinate axis Z i-1 With coordinate axis Z i Along the coordinate axis X i The directed distance of

[0041] α i is the coordinate axis Z i-1 With coordinate axis Z i The angle of

[0042] offset i Used to indicate the rotation direction of the i-th joint.

[0043] i=0,1,2,……。

[0044] Optionally, the forward transfer matrix is:

[0045]

[0046] in, is the forward transfer matrix from the 0th joint to the i-th joint, d i The coordinate axis X i-1 With coordinate axis X i Along the coordinate axis Z i-1 The directed distance, θ i The coordinate axis X i-1 With coordinate axis X i The angle, l i is the coordinate axis Z i-1 With coordinate axis Z i Along the coordinate axis X i The directed distance, α i is the coordinate axis Z i-1 With coordinate axis Z i The angle of , i = 0, 1, 2,…

[0047] Optionally, the approximate matrix of the inverse matrix of the multivariate nonlinear equation system is:

[0048]

[0049] Among them, B k is the approximate matrix of the current step, B K+1 is the approximate matrix for the next step, S k is the variable increment of the current step, y k is the gradient increment of the current step, For S k The matrix transpose of for y k The matrix transpose of .

[0050] Optionally, the inversely solving the angles of the plurality of joints of the robotic arm through the approximate matrix according to the end posture of the robotic arm includes:

[0051] Obtaining angle constraint ranges of the multiple joints;

[0052] Obtaining error limit accuracies of the plurality of joints;

[0053] Obtaining initial angles of the multiple joints;

[0054] According to the angle constraint range, the error limit accuracy, the initial angle and the end position, inversely solve the angles of the multiple joints of the robotic arm through the approximate matrix;

[0055] The initial angle includes multiple groups of fixed initial values, and the fixed initial values ​​are actual measurement values ​​of the robotic arm.

[0056] Optionally, the robotic arm includes a first joint, a second joint, a third joint, a fourth joint, a fifth joint, a sixth joint and a seventh joint,

[0057] The mechanical arm is driven by a rotary motor and a linear motor, and comprises:

[0058] The first joint, the second joint, the third joint and the fifth joint are driven by a rotary motor,

[0059] The fourth joint is driven by the first linear motor,

[0060] The sixth joint and the seventh joint are cooperatively driven by the second linear motor and the third linear motor.

[0061] In a second aspect, an inverse kinematics solving device is provided, the device is applied to a mechanical arm of a humanoid robot, the mechanical arm includes a plurality of joints, the plurality of joints are driven by a rotary motor and a linear motor, the device includes:

[0062] A posture transformation matrix acquisition unit, used to acquire the posture transformation matrix of the robotic arm;

[0063] A forward transfer matrix acquisition unit, used to acquire the forward transfer matrix of the robotic arm;

[0064] An equation group acquisition unit, used for acquiring a multivariate nonlinear equation group according to the posture transformation matrix and the forward transfer matrix;

[0065] An approximate matrix acquisition unit, used for acquiring an approximate matrix of the inverse matrix of the multivariate nonlinear equation group according to the Newton-Hessian method;

[0066] An inverse solution unit is used to inversely solve the angles of the multiple joints of the robotic arm according to the end posture of the robotic arm through the approximate matrix.

[0067] According to a third aspect, a computer device is provided, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements any of the above methods when executing the computer program.

[0068] In a fourth aspect, a computer-readable storage medium is provided, on which a computer program is stored, and when the computer program is executed by a processor, the method as described in any one of the above items is implemented.

[0069] The present invention provides an inverse kinematics solution method, device, computer equipment and storage medium, the method is applied to the mechanical arm of a humanoid robot, the mechanical arm includes multiple joints, the multiple joints are driven by a rotary motor and a linear motor, the method includes: obtaining the posture transformation matrix of the mechanical arm; obtaining the forward transfer matrix of the mechanical arm; obtaining a multivariate nonlinear equation group according to the posture transformation matrix and the forward transfer matrix; obtaining an approximate matrix of the inverse matrix of the multivariate nonlinear equation group according to the Newton-Hessian method; according to the end posture of the mechanical arm, the angles of the multiple joints of the mechanical arm are inversely solved through the approximate matrix. In an embodiment of the present invention, the Newton-Hessian method is used to obtain the approximate matrix of the inverse matrix of the multivariate nonlinear equation group, and the approximate matrix is ​​iteratively updated to reduce the amount of calculation and improve the solution speed. At the same time, due to the strong versatility of the Newton-Hessian method, it can be applied to mechanical arms with different degrees of freedom. For mechanical arms with different degrees of freedom, only the forward transfer matrix of the corresponding degree of freedom needs to be applied, and there is no need to reconstruct the solution method, which has strong applicability. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the invention and, together with the description, serve to explain the principles of the invention.

[0071] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0072] Figure 1 The figure shows an application environment diagram of the inverse kinematics solution method according to an embodiment of the present invention;

[0073] Figure 2 Shown is a schematic diagram of a mechanical arm according to an embodiment of the present invention;

[0074] Figure 3 Shown is a flow chart of an inverse kinematics solution method according to an embodiment of the present invention;

[0075] Figure 4 FIG. 4 is a schematic diagram of a DH coordinate system according to an embodiment of the present invention;

[0076] Figure 5 Shown is a schematic diagram of DH parameters of an embodiment of the present invention;

[0077] Figure 6 The figure is a structural block diagram of an inverse kinematics solving device according to an embodiment of the present invention;

[0078] Figure 7 FIG. 2 is a diagram showing the internal structure of a computer device in an embodiment of the present invention. DETAILED DESCRIPTION

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

[0080] Figure 1 This is an application environment diagram of the inverse kinematics solution method in one embodiment. Figure 1 , the inverse kinematics solution method is applied to the inverse kinematics solution system. The inverse kinematics solution method includes a terminal 110 and / or a server 120. The terminal 110 and the server 120 are connected via a network. The terminal 110 can be a desktop terminal or a mobile terminal, and the mobile terminal can be at least one of a mobile phone, a tablet computer, a laptop computer, etc. The server 120 can be implemented by an independent server or a server cluster composed of multiple servers.

[0081] In an embodiment of the present invention, the terminal 110 or the server 120 may be arranged in a control center of the humanoid robot, or may be arranged on a robotic arm of the humanoid robot, or may be arranged outside the humanoid robot to control the robotic arm of the humanoid robot wirelessly or wiredly.

[0082] An inverse kinematics solution method according to an embodiment of the present invention is applied to a mechanical arm of a humanoid robot. The mechanical arm includes a plurality of joints driven by a rotary motor and a linear motor. Figure 2 Shown is a schematic diagram of a robotic arm according to an embodiment of the present invention.

[0083] Figure 2 In the mechanical arm shown, the mechanical arm includes a first joint, a second joint, a third joint, a fourth joint, a fifth joint, a sixth joint and a seventh joint,

[0084] The mechanical arm is driven by a rotary motor and a linear motor, and comprises:

[0085] The first joint, the second joint, the third joint and the fifth joint are driven by a rotary motor,

[0086] The fourth joint is driven by the first linear motor,

[0087] The sixth joint and the seventh joint are cooperatively driven by the second linear motor and the third linear motor.

[0088] Figure 3 FIG. 1 is a flow chart of an inverse kinematics solution method according to an embodiment of the present invention. Figure 3 As shown, the method includes:

[0089] Step 310, obtaining the posture transformation matrix of the robotic arm;

[0090] Step 320, obtaining the forward transfer matrix of the robotic arm;

[0091] Step 330, obtaining a multivariate nonlinear equation system according to the posture transformation matrix and the forward transfer matrix;

[0092] Step 340, obtaining an approximate matrix of the inverse matrix of the multivariate nonlinear equation system according to the Newton-Hessian method;

[0093] Step 350: According to the end posture of the robotic arm, the angles of the multiple joints of the robotic arm are inversely solved by the approximate matrix.

[0094] If the multivariate linear equations are solved directly, if the joints of the robotic arm are high, that is, the robotic arm has a high degree of freedom, the solution process will be very complicated. In the embodiment of the present invention, the Newton-Hessian method is used to obtain the approximate matrix of the inverse matrix of the multivariate nonlinear equations. By iteratively updating the approximate matrix, the amount of calculation is reduced and the solution speed is improved. At the same time, due to the strong versatility of the Newton-Hessian method, it can be applied to robotic arms with different degrees of freedom. For robotic arms with different degrees of freedom, only the forward transfer matrix of the corresponding degrees of freedom needs to be applied, and there is no need to reconstruct the solution method, which has strong applicability.

[0095] In the embodiment of the present invention, in step 310, obtaining the posture transformation matrix of the robotic arm includes:

[0096] Obtain the X rotation matrix, Y rotation matrix and Z rotation matrix of the robotic arm;

[0097] Obtain a total rotation matrix according to the X rotation matrix, the Y rotation matrix and the Z rotation matrix;

[0098] The posture transformation matrix is ​​obtained according to the total rotation matrix.

[0099] In step 310, the X rotation matrix is:

[0100]

[0101] The Y rotation matrix is:

[0102]

[0103] The Z rotation matrix is:

[0104]

[0105] The total rotation matrix is:

[0106]

[0107] The posture transformation matrix is:

[0108]

[0109] Among them, α, β and γ are Euler angles, which are used to indicate the end position of the robotic arm.

[0110] In the embodiment of the present invention, in step 320, obtaining the forward transfer matrix of the robotic arm includes:

[0111] Constructing a DH coordinate system of the robotic arm according to the joint structure of the robotic arm;

[0112] According to the DH coordinate system and the robot arm parameters, DH parameters are obtained;

[0113] According to the DH parameters, a joint transfer matrix of each joint of the robotic arm is obtained;

[0114] According to the joint transfer matrix, the forward transfer matrix is ​​obtained.

[0115] In the embodiment of the present invention, the DH coordinate system of the robotic arm is constructed in the following manner:

[0116] Each joint of the robot arm corresponds to a DH coordinate system, and the coordinate axis of the i-th joint is X i , Y i and Z i ;

[0117] Among them, the coordinate axis X i With coordinate axis Z i-1 Vertical, coordinate axis X i With coordinate axis Z i-1 Intersection, i=0,1,2,…

[0118] The DH coordinate system is a relatively general coordinate system whose coordinate axes can be customized. The DH coordinate system of the embodiment of the present invention is constructed as described above.

[0119] Figure 4 FIG. 4 is a schematic diagram of a DH coordinate system according to an embodiment of the present invention. Figure 4 The mechanical arm in the embodiment shown has 7 degrees of freedom, that is, 7 joints. The mechanical arm of the humanoid robot can be a left arm or a right arm. Figure 4 The embodiment shown is the right arm.

[0120] The DH coordinate system is constructed according to the joint structure of the robot arm. Figure 4 Even if the corresponding right arm is straightened, the joints are not in a straight line. Therefore, in the DH coordinate system constructed based on this robot, the axes of some DH coordinate systems are offset in the X-axis direction. In the DH coordinate system constructed based on other robot arms, there may be no offset in the X-axis direction.

[0121] In the embodiment of the present invention, the DH parameters of the i-th joint include: θ i , d i , l i , α i , offset i ;

[0122] Among them, θ i The coordinate axis X i-1 With coordinate axis X i The angle of

[0123] d i The coordinate axis X i-1 With coordinate axis X i Along the coordinate axis Z i-1 The directed distance of

[0124] l i is the coordinate axis Z i-1 With coordinate axis Z i Along the coordinate axis X i The directed distance of

[0125] α i is the coordinate axis Z i-1 With coordinate axis Z i The angle of

[0126] offset i Used to indicate the rotation direction of the i-th joint.

[0127] i=0,1,2,……。

[0128] α i Direction reference Figure 5 As shown in (a), θ i Direction reference Figure 5 (b).

[0129] The function of the embodiment of the present invention, offset iActually, it does not participate in the subsequent calculations. It is a parameter that can be defined according to needs. The clockwise rotation direction can be set as a positive value, and the counterclockwise rotation direction can be set as a negative value. After this definition, if the joint angle calculated later is a positive value, it means that the joint is rotating in the clockwise direction, and if the calculated joint angle is a negative value, it means that the joint is rotating in the counterclockwise direction.

[0130] Similarly, the counterclockwise rotation direction can be set as a positive value, and the clockwise rotation direction can be set as a negative value.

[0131] In the embodiment of the present invention, the forward transfer matrix is:

[0132]

[0133] in, is the forward transfer matrix from the 0th joint to the i-th joint, d i The coordinate axis X i-1 With coordinate axis X i Along the coordinate axis Z i-1 The directed distance, θ i The coordinate axis X i-1 With coordinate axis X i The angle, l i is the coordinate axis Z i-1 With coordinate axis Z i Along the coordinate axis X i The directed distance, α i is the coordinate axis Z i-1 With coordinate axis Z i The angle of , i = 0, 1, 2,…

[0134] In the embodiment of the present invention, the DH parameters defined above are substituted into the forward transfer matrix. When the number of joints of the robotic arm is different, the defined DH parameters are different, and the forward transfer matrix is ​​also different. Therefore, the method of the embodiment of the present invention only needs to locate different DH parameters for robotic arms with different degrees of freedom / different numbers of joints, and has strong adaptability.

[0135] Step 330, obtaining a multivariate nonlinear equation system according to the posture transformation matrix and the forward transfer matrix, including:

[0136] The posture change matrix N and the forward transfer matrix By constructing an equation, we can obtain a multivariate nonlinear system of equations.

[0137] In the embodiment of the present invention, the obtained set of equations includes 12 multivariate nonlinear equations, so the embodiment of the present invention can actually inverse solve a robotic arm with up to 12 joints.

[0138] In the embodiment of the present invention, when obtaining the approximate matrix, an initial approximate matrix is ​​first constructed according to the Newton-Hessian method.

[0139] x (k+1) =x (k) -uH k g (k)

[0140] Among them, H k is a 4x4 real matrix, u>0, u is the step size. (k) Perform a first-order Taylor expansion of Fourier at (k +1) After substitution, we get

[0141] f(x (k+1) )=f(x (k) )+g (k)T (x (k+1) -x (k) )+o(||x (k+1) -x (k) ||)

[0142] =f(x (k) )-ug (k)T H k g (k) +o(‖H k g (k) ||u)

[0143] Among them, g (k)T g (k) The matrix transpose of . When u approaches 0, the second term on the right side of the equation dominates the third term. Therefore, when u is relatively small, in order to ensure that the function f is from x (k) to x (k+1) To be down, you must have:

[0144] g (k)T H k g (k) >0

[0145] The simplest way to ensure that the above formula holds is to ensure that H k is a positive definite matrix. According to the above formulas, we can get an approximate matrix after sorting.

[0146] In the embodiment of the present invention, in step 340, the approximate matrix of the inverse matrix of the multivariate nonlinear equation system is:

[0147]

[0148] Among them, B k is the approximate matrix of the current step, B K+1 is the approximate matrix for the next step, S kis the variable increment of the current step, y k is the gradient increment of the current step, For S k The matrix transpose of for y k The matrix transpose of .

[0149] The Newton-Hessian method itself is an iterative method, so the approximate matrix of the embodiment of the present invention is actually also iterative, that is, B k is the approximate matrix of the current step, B K+1 is the approximate matrix of the next step. Through the iterative method, the amount of calculation can be effectively reduced and the solution speed can be improved.

[0150] In the embodiment of the present invention, in step 350, the inversely solving the angles of the plurality of joints of the robotic arm through the approximate matrix according to the end posture of the robotic arm includes:

[0151] Obtaining angle constraint ranges of the multiple joints;

[0152] Obtaining error limit accuracies of the plurality of joints;

[0153] Obtaining initial angles of the multiple joints;

[0154] According to the angle constraint range, the error limit accuracy, the initial angle and the end position, inversely solve the angles of the multiple joints of the robotic arm through the approximate matrix;

[0155] The initial angle includes multiple groups of fixed initial values, and the fixed initial values ​​are actual measurement values ​​of the robotic arm.

[0156] In an embodiment of the present invention, the method is applied to a humanoid robot. The physical characteristics of the arm of the humanoid robot are to imitate human activities, so the angle constraint range of each joint can refer to the human arm. For example, in a 7-DOF robotic arm, the joints are numbered from 1 to 7, so the physical range of motion of joint 1 cannot be less than 5 degrees; joint 4 cannot be folded back and must be greater than 0 degrees. Joints 6 and 7 are coupled to each other. After decoupling, the angle constraint ranges of joints 6 and 7 are (-40, 40) and (-15, 15) degrees, etc.

[0157] Since the embodiment of the present invention can calculate the joint angles of up to 12 degrees of freedom, when performing the angle constraint range, the angle constraint range of the used joints can refer to the human arm, and the angle constraint range beyond the used degrees of freedom can be infinite. For example, in a 7-degree-of-freedom robot, the joints are numbered from 1 to 12, but only joints 1 to 7 are actually used. In the inverse solution process, the angle constraint range of joints 8 to 12 can be (-10000, 10000).

[0158] In the embodiment of the present invention, the error limit accuracy can be tol=10 -6 .

[0159] In the embodiment of the present invention, the initial angle includes a plurality of groups of fixed initial values, and the fixed initial values ​​are actual measurement values ​​of the robotic arm.

[0160] The method of the embodiment of the present invention limits the angle constraint range of each joint, that is, limits the scope of the inverse solution, so that when the humanoid robot is controlled according to the obtained angle, the joints of the humanoid robot are more consistent with the characteristics of the human body. Limiting the scope of the inverse solution can also avoid local convergence of the algorithm, and avoid unreasonable inverse solution, large error, etc.

[0161] The method of the embodiment of the present invention sets the error limit precision, which can avoid excessive error in the result and avoid waste of computing resources through the error limit precision.

[0162] The method of the embodiment of the present invention will be limited to solving near the initial value when solving the inverse problem. In general, it is sufficient to set a group of initial angles, but in order to avoid the situation where there is no solution, the initial angle in the embodiment of the present invention includes multiple groups of fixed initial values, and the fixed initial values ​​are the actual measured values ​​of the robotic arm, and the actual measured values ​​may also include the values ​​of the last initial state of the robotic arm. Setting the initial angle can ensure that the inverse solution has a suitable solution and avoid wasting computing resources.

[0163] In the embodiment of the present invention, the Newton-Hessian method is used to obtain the approximate matrix of the inverse matrix of the multivariate nonlinear equations. By iteratively updating the approximate matrix, the amount of calculation is reduced and the speed of solution is improved. At the same time, due to the strong versatility of the Newton-Hessian method, it can be applied to mechanical arms with different degrees of freedom. For mechanical arms with different degrees of freedom, only the forward transfer matrix of the corresponding degrees of freedom needs to be applied, and there is no need to reconstruct the solution method, which has strong applicability. In addition, the method of the embodiment of the present invention limits the angle constraint range of each joint, that is, limits the scope of the inverse solution, so that when the humanoid robot is controlled according to the obtained angle, the joints of the humanoid robot are more in line with the characteristics of the human body. Limiting the scope of the inverse solution can also avoid local convergence of the algorithm, avoid unreasonable inverse solution, large error, etc. The method of the embodiment of the present invention sets the error limit accuracy, which can avoid excessive error of the result, and can also avoid waste of computing resources by error limit accuracy. When the method of the embodiment of the present invention is inversely solved, it will be limited to solve near the initial value. In the embodiment of the present invention, the initial angle includes multiple sets of fixed initial values, and the fixed initial value is the actual measurement value of the mechanical arm. The actual measurement value can also include the value of the initial state of the mechanical arm last time. Setting the initial angle can ensure that the inverse solution has a suitable solution and avoid wasting computing resources.

[0164] like Figure 6 As shown, the present invention also provides an inverse kinematics solving device, which is applied to a mechanical arm of a humanoid robot, wherein the mechanical arm includes a plurality of joints, and the plurality of joints are driven by a rotary motor and a linear motor, and the device includes:

[0165] A posture transformation matrix acquisition unit 610, used to acquire the posture transformation matrix of the robotic arm;

[0166] A forward transfer matrix acquisition unit 620, used to acquire the forward transfer matrix of the robotic arm;

[0167] An equation group acquisition unit 630, used to acquire a multivariate nonlinear equation group according to the posture transformation matrix and the forward transfer matrix;

[0168] An approximate matrix acquisition unit 640 is used to acquire an approximate matrix of the inverse matrix of the multivariate nonlinear equation system according to the Newton-Hessian method;

[0169] The inverse solution unit 650 is used to inversely solve the angles of the multiple joints of the robotic arm according to the end posture of the robotic arm through the approximate matrix.

[0170] In the embodiment of the present invention, the posture transformation matrix acquisition unit 610 is further used for:

[0171] Obtain the X rotation matrix, Y rotation matrix and Z rotation matrix of the robotic arm;

[0172] Obtain a total rotation matrix according to the X rotation matrix, the Y rotation matrix and the Z rotation matrix;

[0173] The posture transformation matrix is ​​obtained according to the total rotation matrix.

[0174] In the embodiment of the present invention, the X rotation matrix is:

[0175]

[0176] The Y rotation matrix is:

[0177]

[0178] The Z rotation matrix is:

[0179]

[0180] The total rotation matrix is:

[0181]

[0182] The posture transformation matrix is:

[0183]

[0184] Among them, α, β and γ are Euler angles, which are used to indicate the end position of the robotic arm.

[0185] In the embodiment of the present invention, the forward transfer matrix acquisition unit 620 is further used for:

[0186] Constructing a DH coordinate system of the robotic arm according to the joint structure of the robotic arm;

[0187] According to the DH coordinate system and the robot arm parameters, DH parameters are obtained;

[0188] According to the DH parameters, a joint transfer matrix of each joint of the robotic arm is obtained;

[0189] According to the joint transfer matrix, the forward transfer matrix is ​​obtained.

[0190] In the embodiment of the present invention, the forward transfer matrix acquisition unit 620 is further used to construct the DH coordinate system in the following manner:

[0191] Each joint of the robot arm corresponds to a DH coordinate system, and the coordinate axis of the i-th joint is X i , Y i and Z i ;

[0192] Among them, the coordinate axis X i With coordinate axis Z i-1 Vertical, coordinate axis X i With coordinate axis Z i-1 Intersection, i=0,1,2,…

[0193] In the embodiment of the present invention, the DH parameters of the i-th joint include: θ i , d i , l i , α i , offset i ;

[0194] Among them, θ i The coordinate axis X i-1 With coordinate axis X i The angle of

[0195] d i The coordinate axis X i-1 With coordinate axis X i Along the coordinate axis Z i-1 The directed distance of

[0196] l i is the coordinate axis Z i-1 With coordinate axis Z i Along the coordinate axis Xi The directed distance of

[0197] α i is the coordinate axis Z i-1 With coordinate axis Z i The angle of

[0198] offset i Used to indicate the rotation direction of the i-th joint.

[0199] i=0,1,2,……。

[0200] In the embodiment of the present invention, the forward transfer matrix is:

[0201]

[0202] in, is the forward transfer matrix from the 0th joint to the i-th joint, d i The coordinate axis X i-1 With coordinate axis X i Along the coordinate axis Z i-1 The directed distance, θ i The coordinate axis X i-1 With coordinate axis X i The angle, l i is the coordinate axis Z i-1 With coordinate axis Z i Along the coordinate axis X i The directed distance, α i is the coordinate axis Z i-1 With coordinate axis Z i The angle of , i = 0, 1, 2,…

[0203] In the embodiment of the present invention, the approximate matrix of the inverse matrix of the multivariate nonlinear equation system is:

[0204]

[0205] Among them, B k is the approximate matrix of the current step, B K+1 is the approximate matrix for the next step, S k is the variable increment of the current step, y k is the gradient increment of the current step, For S k The matrix transpose of for y k The matrix transpose of .

[0206] In the embodiment of the present invention, the inverse solution unit 650 is further used for:

[0207] Obtaining angle constraint ranges of the multiple joints;

[0208] Obtaining error limit accuracies of the plurality of joints;

[0209] Obtaining initial angles of the multiple joints;

[0210] According to the angle constraint range, the error limit accuracy, the initial angle and the end position, inversely solve the angles of the multiple joints of the robotic arm through the approximate matrix;

[0211] The initial angle includes multiple groups of fixed initial values, and the fixed initial values ​​are actual measurement values ​​of the robotic arm.

[0212] In the embodiment of the present invention, the robotic arm includes a first joint, a second joint, a third joint, a fourth joint, a fifth joint, a sixth joint and a seventh joint.

[0213] The mechanical arm is driven by a rotary motor and a linear motor, and comprises:

[0214] The first joint, the second joint, the third joint and the fifth joint are driven by a rotary motor,

[0215] The fourth joint is driven by the first linear motor,

[0216] The sixth joint and the seventh joint are cooperatively driven by the second linear motor and the third linear motor.

[0217] In the embodiment of the present invention, the Newton-Hessian method is used to obtain the approximate matrix of the inverse matrix of the multivariate nonlinear equations, and the approximate matrix is ​​iteratively updated to reduce the amount of calculation and improve the solution speed. At the same time, due to the strong versatility of the Newton-Hessian method, it can be applied to manipulators with different degrees of freedom. For manipulators with different degrees of freedom, only the forward transfer matrix of the corresponding degrees of freedom needs to be applied, and there is no need to reconstruct the solution method, which has strong applicability.

[0218] An embodiment of the present invention also provides a computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the following method when executing the computer program: the method comprises: obtaining the posture transformation matrix of the robotic arm; obtaining the forward transfer matrix of the robotic arm; obtaining a group of multivariate nonlinear equations based on the posture transformation matrix and the forward transfer matrix; obtaining an approximate matrix of the inverse matrix of the group of multivariate nonlinear equations based on the Newton-Hessian method; and according to the end posture of the robotic arm, inversely solving the angles of the multiple joints of the robotic arm through the approximate matrix.

[0219] An embodiment of the present invention also provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the following method: the method comprises: obtaining a posture transformation matrix of the robotic arm; obtaining a forward transfer matrix of the robotic arm; obtaining a group of multivariate nonlinear equations based on the posture transformation matrix and the forward transfer matrix; obtaining an approximate matrix of the inverse matrix of the group of multivariate nonlinear equations based on the Newton-Hessian method; and according to the end posture of the robotic arm, inversely solving the angles of the multiple joints of the robotic arm through the approximate matrix.

[0220] The above-mentioned inverse kinematics solution method achieves the beneficial effect of being able to solve the technical problems raised in the background technology.

[0221] Figure 3 FIG. 1 is a flow chart of an inverse kinematics solution method in one embodiment. It should be understood that although Figure 3 The steps in the flowchart are shown in sequence as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified in this document, there is no strict order restriction for the execution of these steps, and these steps can be executed in other orders. Moreover, Figure 3 At least part of the steps may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily executed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least part of the sub-steps or stages of other steps.

[0222] Figure 7 The internal structure diagram of a computer device in one embodiment is shown. The computer device may specifically be Figure 1 The terminal 110 or the server 120 in FIG. Figure 7 As shown, the computer device includes a processor, a memory, a network interface, an input device and a display screen connected through a system bus. Among them, the memory includes a non-volatile storage medium and an internal memory. The non-volatile storage medium of the computer device stores an operating system and may also store a computer program. When the computer program is executed by the processor, the processor can implement the inverse kinematics solution method. The internal memory may also store a computer program. When the computer program is executed by the processor, the processor can execute the inverse kinematics solution method. The display screen of the computer device may be a liquid crystal display screen or an electronic ink display screen, and the input device of the computer device may be a touch layer covered on the display screen, or a button, trackball or touchpad provided on the housing of the computer device, or an external keyboard, touchpad or mouse, etc.

[0223] Those skilled in the art will understand that Figure 7 The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present invention, and does not constitute a limitation on the computer device to which the solution of the present invention is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0224] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiments can be completed by instructing the relevant hardware through a computer program, and the program can be stored in a non-volatile computer-readable storage medium. When the program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided by the present invention can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).

[0225] It should be noted that, in this article, relational terms such as "first" and "second" are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0226] The foregoing is merely a specific embodiment of the present invention, which enables those skilled in the art to understand or implement the present invention. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein may 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 the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features claimed herein.

Claims

1. An inverse kinematics solution method, characterized in that: The method is applied to a mechanical arm of a humanoid robot, the mechanical arm comprising a plurality of joints, the plurality of joints being driven by a rotary motor and a linear motor, and the method comprising: Obtaining a posture transformation matrix of the robotic arm; Obtaining a forward transfer matrix of the robotic arm; Obtaining a multivariate nonlinear equation group according to the posture transformation matrix and the forward transfer matrix; According to the Newton-Hessian method, an approximate matrix of the inverse matrix of the multivariate nonlinear equation system is obtained; According to the end posture of the robotic arm, the angles of the multiple joints of the robotic arm are inversely solved through the approximate matrix.

2. The method according to claim 1, characterized in that The step of obtaining a posture transformation matrix of the robotic arm includes: Obtain the X rotation matrix, Y rotation matrix and Z rotation matrix of the robotic arm; Obtain a total rotation matrix according to the X rotation matrix, the Y rotation matrix and the Z rotation matrix; The posture transformation matrix is ​​obtained according to the total rotation matrix.

3. The method according to claim 2, characterized in that The X rotation matrix is: The Y rotation matrix is: The Z rotation matrix is: The total rotation matrix is: The posture transformation matrix is: Among them, α, β and γ are Euler angles, which are used to indicate the end position of the robotic arm.

4. The method according to claim 1, characterized in that: The obtaining of the forward transfer matrix of the robotic arm comprises: Constructing a DH coordinate system of the robotic arm according to the joint structure of the robotic arm; According to the DH coordinate system and the robot arm parameters, DH parameters are obtained; According to the DH parameters, a joint transfer matrix of each joint of the robotic arm is obtained; According to the joint transfer matrix, the forward transfer matrix is ​​obtained.

5. The method according to claim 4, characterized in that The DH coordinate system of the robot arm is constructed as follows: Each joint of the robot arm corresponds to a DH coordinate system, and the coordinate axis of the i-th joint is X i , Y i and Z i ; Among them, the coordinate axis X i With coordinate axis Z i-1 Vertical, coordinate axis X i With coordinate axis Z i-1 Intersection, i=0,1,2,… 6. The method according to claim 5, characterized in that The DH parameters of the i-th joint include: θ i , d i , l i , α i , offset i ; Among them, θ i The coordinate axis X i-1 With coordinate axis X i The angle of d i The coordinate axis X i-1 With coordinate axis X i Along the coordinate axis Z i-1 The directed distance of l i is the coordinate axis Z i-1 With coordinate axis Z i Along the coordinate axis X i The directed distance of α i is the coordinate axis Z i-1 With coordinate axis Z i The angle of offset i Used to indicate the rotation direction of the i-th joint. i＝0,1,2,……。 7. The method according to claim 6, characterized in that The forward transfer matrix is: in, is the forward transfer matrix from the 0th joint to the i-th joint, d i The coordinate axis X i-1 With coordinate axis X i Along the coordinate axis Z i-1 The directed distance, θ i The coordinate axis X i-1 With coordinate axis X i The angle, l i is the coordinate axis Z i-1 With coordinate axis Z i Along the coordinate axis X i The directed distance, α i is the coordinate axis Z i-1 With coordinate axis Z i The angle of , i = 0, 1, 2,… 8. The method according to claim 5, characterized in that The approximate matrix of the inverse matrix of the multivariate nonlinear equation system is: Among them, B k is the approximate matrix of the current step, B K+1 is the approximate matrix for the next step, S k is the variable increment of the current step, y k is the gradient increment of the current step, For S k The matrix transpose of for y k The matrix transpose of .

9. The method according to claim 1, characterized in that: The inversely solving the angles of the plurality of joints of the robotic arm through the approximate matrix according to the end posture of the robotic arm comprises: Obtaining angle constraint ranges of the multiple joints; Obtaining error limit accuracies of the plurality of joints; Obtaining initial angles of the multiple joints; According to the angle constraint range, the error limit accuracy, the initial angle and the end position, inversely solve the angles of the multiple joints of the robotic arm through the approximate matrix; The initial angle includes multiple groups of fixed initial values, and the fixed initial values ​​are actual measurement values ​​of the robotic arm.

10. The method according to claim 1, characterized in that The robotic arm includes a first joint, a second joint, a third joint, a fourth joint, a fifth joint, a sixth joint and a seventh joint. The mechanical arm is driven by a rotary motor and a linear motor, and comprises: The first joint, the second joint, the third joint and the fifth joint are driven by a rotary motor, The fourth joint is driven by the first linear motor, The sixth joint and the seventh joint are cooperatively driven by the second linear motor and the third linear motor.

11. An inverse kinematics solving device, characterized in that: The device is applied to a mechanical arm of a humanoid robot, the mechanical arm comprises a plurality of joints, the plurality of joints are driven by a rotary motor and a linear motor, and the device comprises: A posture transformation matrix acquisition unit, used to acquire the posture transformation matrix of the robotic arm; A forward transfer matrix acquisition unit, used to acquire the forward transfer matrix of the robotic arm; An equation group acquisition unit, used for acquiring a multivariate nonlinear equation group according to the posture transformation matrix and the forward transfer matrix; An approximate matrix acquisition unit, used for acquiring an approximate matrix of the inverse matrix of the multivariate nonlinear equation group according to the Newton-Hessian method; An inverse solution unit is used to inversely solve the angles of the multiple joints of the robotic arm according to the end posture of the robotic arm through the approximate matrix.

12. A computer device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the method according to any one of claims 1 to 10 is implemented.

13. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 10 is implemented.

Images