Inverse Kinematic Solution Method, Device, Equipment and Medium for Joint Angles of a Redundant Manipulator

Abstract

The present application provides a method, apparatus, device and medium for inverse kinematics of joint angles of a redundant robotic arm. The method includes: obtaining an objective function of the redundant robotic arm, where the objective function is an optimization function aiming at making the joint angles of each joint on the redundant robotic arm closest to the preset motion midpoint of each joint; solving the objective function according to the angles of each joint in the previous iteration, using the limit constraint conditions of each joint and the pose constraint conditions of the end joint of the redundant robotic arm to obtain the current iteration angles of each joint; obtaining the actual pose of the end joint according to the current iteration angles of each joint; determining whether the preset joint angle iteration stop condition is satisfied according to the actual pose and the desired pose of the end joint; if the preset joint angle iteration stop condition is satisfied, determining the current iteration angles of each joint when the preset joint angle iteration stop condition is satisfied as the target angles of each joint. Controlling each joint based on the target angles to make each joint closest to the preset motion midpoint.

Description

Technical Field

[0001] The present invention relates to the technical field of robotic arms, and more particularly, to a method, device, equipment, and medium for inverse kinematics of joint angles of a redundant robotic arm. Background Art

[0002] A redundant robotic arm refers to a robotic arm whose degrees of freedom are higher than the dimension of the task space. Generally, such a robotic arm has an infinite number of inverse kinematics solutions, and the inverse kinematics solutions can be limited to a finite number by adding additional constraints. Currently, the Newton method can give a set of inverse kinematics solutions, but this set of inverse kinematics solutions depends on the selection of the initial joint values and iterates based on the current joint angles. When the error function is satisfied, the iteration process is terminated. As a result, although the obtained inverse kinematics solutions can meet the pose requirements of the end effector, the link positions of the robotic arm will be abnormal, and the advantages of the redundant robotic arm cannot be fully utilized. Summary of the Invention

[0003] The purpose of the present invention is to provide a method, device, equipment, and medium for inverse kinematics of joint angles of a redundant robotic arm to address the above-mentioned deficiencies in the prior art. By setting an objective function and adding limit constraints for each joint and pose constraints for the end joint, after controlling each joint based on the target angles of each joint, each joint can be closest to the preset motion midpoint, and the end joint can reach a state close to the desired pose.

[0004] To achieve the above objective, the technical solutions adopted in the embodiments of the present application are as follows:

[0005] In a first aspect, an embodiment of the present application provides a method for inverse kinematics of joint angles of a redundant robotic arm, the method comprising:

[0006] Obtain an objective function of the redundant robotic arm, where the objective function is an optimization function with the objective of making the joint angles of each joint on the redundant robotic arm closest to the preset motion midpoint of each joint;

[0007] Solve the objective function according to the last iteration angles of each joint, using the limit constraints of each joint and the pose constraints of the end joint of the redundant robotic arm, to obtain the current iteration angles of each joint;

[0008] Obtain the actual pose of the end joint according to the current iteration angles of each joint;

[0009] Determine whether a preset joint angle iteration stop condition is satisfied according to the actual pose and the desired pose of the end joint;

[0010] If the preset joint angle iteration stop condition is satisfied, determine the current iteration angles of the respective joints when the preset joint angle iteration stop condition is satisfied as the target angles of the respective joints.

[0011] In an alternative embodiment, the objective function is as shown in the following formula:

[0012]

[0013] Wherein, represents the previous iteration angles of the respective joints, represents the angle change amounts of the respective joints, represents the preset motion midpoints of the respective joints, represents the preset motion ranges of the respective joints, is an integer greater than 1.

[0014] In an alternative embodiment, solving the objective function to obtain the current iteration angles of the respective joints according to the previous iteration angles of the respective joints, using the limit constraint conditions of the respective joints and the pose constraint conditions of the end joints of the redundant manipulator includes:

[0015] Performing a second-order partial derivative on the angle change amounts of the respective joints in the objective function to obtain the Hessian matrix corresponding to the redundant manipulator;

[0016] Performing a first-order partial derivative on the angle change amounts of the respective joints in the objective function to obtain the gradient vector corresponding to the redundant manipulator;

[0017] Performing quadratic programming solution according to the limit constraint conditions of the respective joints, the pose constraint conditions of the end joints, the Hessian matrix, and the gradient vector to obtain the current iteration angles of the respective joints.

[0018] In an alternative embodiment, performing quadratic programming solution according to the limit constraint conditions of the respective joints, the pose constraint conditions of the end joints, the Hessian matrix, and the gradient vector to obtain the current iteration angles of the respective joints includes:

[0019] Performing quadratic programming solution according to the limit constraint conditions of the respective joints, the pose constraint conditions of the end joints, the Hessian matrix, and the gradient vector to obtain the current angular velocities of the respective joints;

[0020] Determining the current iteration angles of the respective joints according to the previous iteration angles and the current angular velocities of the respective joints.

[0021] In an alternative embodiment, the limit constraint conditions of the respective joints are inequality constraint conditions shown in the following formula:

[0022]

[0023] wherein, represents the preset motion lower limit of each of the joints, represents the preset motion upper limit of each of the joints, represents the angle of each joint at the previous iteration, represents the angle change amount of each of the joints.

[0024] In an alternative embodiment, the pose constraint condition of the end joint is as shown in the following formula:

[0025]

[0026] wherein, represents the Jacobian matrix, represents the angle change amount matrix, represents the desired pose matrix of the end joint, represents the actual pose matrix of the end joint, is the inverse function.

[0027] In an alternative embodiment, determining whether to satisfy the preset joint angle iteration stop condition according to the actual pose and the desired pose of the end joint includes:

[0028] Calculating an error value between the actual pose and the desired pose of the end joint;

[0029] If the error value is less than or equal to a preset error threshold, it is determined that the preset joint angle iteration stop condition is satisfied;

[0030] If the error value is greater than the preset error threshold, it is determined that the preset joint angle iteration stop condition is not satisfied, and iteration continues until the actual pose and the desired pose of the end joint satisfy the preset joint angle iteration stop condition.

[0031] In a second aspect, an embodiment of the present application further provides a device for inverse kinematics solution of joint angles of a redundant robotic arm, the device including:

[0032] An acquisition module, configured to acquire an objective function of the redundant robotic arm, where the objective function is an optimization function with the joint angles of each joint on the redundant robotic arm being closest to the preset motion midpoint of each joint as the goal;

[0033] A solution module, configured to solve the objective function according to the angle of each joint at the previous iteration, using the limit constraint condition of each joint and the pose constraint condition of the end joint of the redundant robotic arm, to obtain the current iteration angle of each joint;

[0034] The obtaining module is further configured to obtain the actual pose of the end joint according to the current iterative angles of the joints.

[0035] The determining module is configured to determine whether a preset joint angle iteration stop condition is satisfied according to the actual pose and the desired pose of the end joint.

[0036] The determining module is further configured to, if the preset joint angle iteration stop condition is satisfied, determine that the current iterative angles of the joints when the preset joint angle iteration stop condition is satisfied are the target angles of the joints.

[0037] In a third aspect, an embodiment of the present application further provides an electronic device, including: a processor, a storage medium, and a bus. The storage medium stores program instructions executable by the processor. When the electronic device runs, the processor communicates with the storage medium through the bus. The processor executes the program instructions to perform the steps of the inverse kinematics method for joint angles of a redundant robotic arm according to any one of the first aspects.

[0038] In a fourth aspect, an embodiment of the present application further provides a computer-readable storage medium. A computer program is stored on the storage medium. When the computer program is run by a processor, it performs the steps of the inverse kinematics method for joint angles of a redundant robotic arm according to any one of the first aspects.

[0039] The beneficial effects of the present application are as follows:

[0040] An embodiment of the present application provides an inverse kinematics method, device, equipment, and medium for joint angles of a redundant robotic arm. The method includes: obtaining an objective function of the redundant robotic arm, where the objective function is an optimization function with the objective of making the joint angles of each joint on the redundant robotic arm closest to the preset motion midpoint of each joint. According to the previous iterative angles of each joint, using the limit constraint conditions of each joint and the pose constraint conditions of the end joint of the redundant robotic arm, solving the objective function to obtain the current iterative angles of each joint. Then, according to the current iterative angles of each joint, obtaining the actual pose of the end joint, and determining whether a preset joint angle iteration stop condition is satisfied according to the actual pose and the desired pose of the end joint. If the preset joint angle iteration stop condition is satisfied, determining that the current iterative angles of each joint when the preset joint angle iteration stop condition is satisfied are the target angles of each joint. By setting the objective function and adding the limit constraint conditions of each joint and the pose constraint conditions of the end joint, the method of the present application enables each joint to be closest to the preset motion midpoint after being controlled based on the target angles of each joint, and the end joint can reach a state close to the desired pose, avoiding abnormal situations in the link pose of the redundant robotic arm and being beneficial to the subsequent movement of the redundant robotic arm. Description of the Drawings

[0041] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings required for the embodiments will be briefly introduced below. It should be understood that the following accompanying drawings only show some embodiments of the present invention and should not be regarded as limiting the scope. For those of ordinary skill in the art, without creative efforts, other related accompanying drawings can also be obtained based on these drawings.

[0042] Figure 1 It is a schematic flow chart of a method for inverse kinematics of joint angles of a redundant robotic arm provided by an embodiment of the present application;

[0043] Figure 2 It is a schematic flow chart of another method for inverse kinematics of joint angles of a redundant robotic arm provided by an embodiment of the present application;

[0044] Figure 3 It is a schematic flow chart of yet another method for inverse kinematics of joint angles of a redundant robotic arm provided by an embodiment of the present application;

[0045] Figure 4 It is a schematic flow chart of another method for inverse kinematics of joint angles of a redundant robotic arm provided by an embodiment of the present application;

[0046] Figure 5 It is a schematic diagram of functional modules of a device for inverse kinematics of joint angles of a redundant robotic arm provided by an embodiment of the present application;

[0047] Figure 6 It is a schematic diagram of an electronic device provided by an embodiment of the present application. Detailed Embodiments

[0048] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention.

[0049] Therefore, the detailed description of the embodiments of the present application provided in the accompanying drawings is not intended to limit the scope of the present application claimed, but merely represents selected embodiments of the present application. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present application without creative efforts fall within the scope of protection of the present application.

[0050] In the description of the present application, it should be noted that if terms such as "upper", "lower", etc. indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, or the orientation or positional relationship in which the product of this application is customarily placed during use, it is only for the convenience of describing the present application and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be construed as a limitation to the present application.

[0051] In addition, terms such as "first", "second", etc. in the description and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects and do not necessarily need to describe a specific order or sequence. It should be understood that such data can be interchanged under appropriate circumstances so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "comprising" and "having" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or device that includes a series of steps or units does not necessarily have to be limited to those steps or units clearly listed, but may include other steps or units not clearly listed or inherent to these processes, methods, products, or devices.

[0052] It should be noted that, without conflict, the features in the embodiments of the present application can be combined with each other.

[0053] A redundant robotic arm refers to a robotic arm whose degree of freedom number is more than the minimum degree of freedom number required to complete a specific task. For example, in three-dimensional space, a 6-axis (six degrees of freedom) robotic arm is commonly used, and a 7-axis (seven degrees of freedom) robotic arm is called a redundant robotic arm. The inverse kinematics problem refers to knowing the desired pose (position and orientation) of the end joint of the robotic arm and solving for the angles of each joint so that the robotic arm can reach the desired position and orientation.

[0054] To obtain a set of appropriate joint angles, making the angle of each joint as close as possible to its preset motion midpoint while meeting the pose requirements of the end joint of the robotic arm, the embodiment of the present application provides an inverse kinematic solution method for the joint angles of a redundant robotic arm. Specifically, by obtaining the objective function of the redundant robotic arm, the objective function is an optimization function with the objective that the joint angles of each joint on the redundant robotic arm are closest to the preset motion midpoints of each joint. According to the angles of each joint in the previous iteration, using the limit constraint conditions of each joint and the pose constraint conditions of the end joint of the redundant robotic arm, the objective function is solved to obtain the current iteration angles of each joint. Then, according to the current iteration angles of each joint, the actual pose of the end joint is obtained, and based on the actual pose and the desired pose of the end joint, it is determined whether the preset joint angle iteration stop condition is met. If the preset joint angle iteration stop condition is met, it is determined that the current iteration angles of each joint when the preset joint angle iteration stop condition is met are the target angles of each joint. At this time, controlling each joint according to the target angles of each joint can make each joint closest to the preset motion midpoint.

[0055] The inverse kinematic solution method for the joint angles of the redundant robotic arm provided by the embodiment of the present application is explained in detail below with specific examples in combination with the accompanying drawings. The inverse kinematic solution method for the joint angles of the redundant robotic arm provided by the embodiment of the present application can be implemented by an electronic device pre-installed with: the inverse kinematic algorithm for the joint angles of the preset redundant robotic arm or detection software, by running the algorithm or software. The electronic device can be, for example, a server or a terminal, and the terminal can be a user computer. Figure 1 It is a schematic flowchart of an inverse kinematic solution method for the joint angles of a redundant robotic arm provided by the embodiment of the present application. As Figure 1 shown, the method includes:

[0056] S101. Obtain the objective function of the redundant robotic arm.

[0057] Among them, the objective function is an optimization function with the objective that the joint angles of each joint on the redundant robotic arm are closest to the preset motion midpoints of each joint.

[0058] In this embodiment, the objective function is an optimization function, and its purpose is to make the joint angles of each joint on the redundant robotic arm as close as possible to the preset motion midpoints of each joint. The preset motion midpoint can be understood as the intermediate angle position of each joint within its allowable motion range. For example, if the motion range of a joint is from 0° to 180°, then the preset motion midpoint is 90°.

[0059] By calculating the target angles of each joint using the objective function, when each joint of the redundant robotic arm moves based on the target angles, each joint can be in a relatively reasonable position, avoiding some joints being too close to their motion limits, thereby improving the motion flexibility and stability of the redundant robotic arm.

[0060] S102. Solve the objective function according to the angles of each joint in the previous iteration, using the limit constraint conditions of each joint and the pose constraint conditions of the end joint of the redundant manipulator, to obtain the current iteration angles of each joint.

[0061] Specifically, in the first iteration, the angle of the previous iteration can be the initially set joint angle. In subsequent iterations, the angle of the previous iteration is the joint angle calculated after the previous iteration ends.

[0062] Each joint has its allowable motion range. For example, the motion range of a certain joint is from -90° to 90°. By adding the limit constraint conditions of each joint, the finally obtained joint angles are within the limit constraint range. The limit constraint conditions can ensure that each joint of the redundant manipulator does not exceed its physical limits. In addition, by adding the pose constraint conditions of the end joint, when solving the objective function, it can be ensured that the obtained joint angles can make the end joint reach the desired pose.

[0063] By using an optimization algorithm, on the premise of satisfying the limit constraint conditions and pose constraint conditions, minimize the objective function, so as to obtain the current iteration angles of each joint.

[0064] S103. Obtain the actual pose of the end joint according to the current iteration angles of each joint.

[0065] Specifically, after obtaining the current iteration angles of each joint, through the forward kinematic model of the manipulator, the actual pose of the end joint can be calculated and mapped according to the current iteration angles of each joint. The forward kinematic model describes the mathematical relationship between the angles of each joint of the manipulator and the pose of the end joint.

[0066] S104. Determine whether the preset joint angle iteration stop condition is satisfied according to the actual pose and the desired pose of the end joint.

[0067] S105. If the preset joint angle iteration stop condition is satisfied, determine that the current iteration angles of each joint when the preset joint angle iteration stop condition is satisfied are the target angles of each joint.

[0068] Specifically, by comparing the actual pose and the desired pose of the end joint, determine whether the preset joint angle iteration stop condition is satisfied. If the preset joint angle iteration stop condition is satisfied, it means that the current joint angles can already make the end joint reach a state close to the desired pose, and each joint angle is also as close as possible to its preset motion midpoint. Then stop the iteration and use the joint angles at this time as the final target angles.

[0069] In summary, the embodiment of the present application provides an inverse kinematic solution method for the joint angles of a redundant manipulator, including: obtaining the objective function of the redundant manipulator, where the objective function is an optimization function with the objective of making the joint angles of each joint on the redundant manipulator closest to the preset motion midpoint of each joint. According to the angles of each joint in the previous iteration, using the limit constraint conditions of each joint and the pose constraint conditions of the end joint of the redundant manipulator, the objective function is solved to obtain the current iteration angles of each joint. Then, according to the current iteration angles of each joint, the actual pose of the end joint is obtained, and based on the actual pose and the desired pose of the end joint, it is determined whether the preset joint angle iteration stop condition is satisfied. If the preset joint angle iteration stop condition is satisfied, the current iteration angles of each joint when the preset joint angle iteration stop condition is satisfied are determined as the target angles of each joint. The method of the present application, by setting the objective function and adding the limit constraint conditions of each joint and the pose constraint conditions of the end joint, enables each joint to be closest to the preset motion midpoint after being controlled based on the target angles of each joint, and the end joint can reach a state close to the desired pose, avoiding abnormal situations in the link pose of the redundant manipulator and being beneficial to the subsequent movement of the redundant manipulator.

[0070] Another possible implementation of the inverse kinematic solution method for the joint angles of a redundant manipulator is also provided in the embodiment of the present application. The objective function is as shown in the following formula:

[0071]

[0072] Wherein, represents the angles of each joint in the previous iteration, represents the angle change of each joint, represents the preset motion midpoint of each joint, represents the preset motion range of each joint, is an integer greater than 1.

[0073] In this embodiment, if the degree of freedom of the redundant manipulator is seven, then n in the objective function is 6 at this time. For example, the preset motion lower limit of each joint is , and the preset motion upper limit is , and the subscript represents joint numbers 1-6. Then the preset motion midpoint of each joint is expressed as:

[0074]

[0075] The preset motion range of each joint is expressed as:

[0076]

[0077] The desired pose of the end joint is represented by a 4x4 homogeneous matrix as: The optimization objective is set to minimize the sum of the squares of the distances of each joint from the preset midpoint of motion. Since the preset motion ranges of the joints are different, the preset motion ranges of the joints are used as the scaling factors, and the objective function is expressed as:

[0078]

[0079] where represents the inverse solution of each joint obtained in the previous iteration, that is, the joint angle. During the iteration process, is expressed as:

[0080]

[0081] where represents the angle of each joint in the previous iteration, represents the angle change of each joint, which is the variable to be solved by iteration and represents the angular velocity of each joint. For example, if it is set that each joint of the redundant manipulator moves for 1 s each time, the magnitude of the angular velocity is the motion angle of the joint at this time.

[0082] Therefore, for a redundant manipulator with seven degrees of freedom, the objective function can finally be expressed as:

[0083]

[0084] By setting the objective function, the target angles of each joint obtained by the solution are closest to the preset midpoints of motion of each joint, avoiding abnormal postures of the links of the redundant manipulator, which is beneficial to the subsequent motion of the redundant manipulator.

[0085] The embodiment of the present application also provides another possible implementation manner of the inverse solution method for the joint angles of the redundant manipulator, Figure 2 which is a schematic flowchart of another inverse solution method for the joint angles of the redundant manipulator provided by the embodiment of the present application. As Figure 2 shown, according to the angles of each joint in the previous iteration, by using the limit constraint conditions of each joint and the pose constraint conditions of the end joint of the redundant manipulator, the objective function is solved to obtain the current iteration angles of each joint, including:

[0086] S201. Perform second-order partial derivatives on the angle changes of each joint in the objective function to obtain the Hessian matrix corresponding to the redundant manipulator.

[0087] In this embodiment, the Hessian Matrix: is a square matrix composed of the second-order partial derivatives of a multivariate function, which is used to describe the local curvature of the function. The Hessian Matrix contains the curvature information of the objective function and can be used to understand the local shape of the objective function. In optimization problems, the Hessian Matrix is used to describe the quadratic approximation of the objective function and plays an important role in judging the convexity of the function, finding the extreme points of the function, etc.

[0088] During the iteration process, the objective function is expanded to obtain the angular change of each joint. - , and the second-order partial derivative of - is taken to obtain the Hessian Matrix corresponding to the redundant manipulator.

[0089] S202. Take the first-order partial derivative of the angular change of each joint in the objective function to obtain the gradient vector corresponding to the redundant manipulator.

[0090] Among them, the gradient vector is a vector composed of the first-order partial derivatives of the objective function with respect to the angular change of each joint. The direction of the gradient vector is the direction in which the objective function rises fastest at the current point, and its opposite direction is the direction in which the objective function drops fastest. In the optimization algorithm, the gradient vector is used to determine the search direction and guide the iteration process to proceed in the direction of decreasing the objective function.

[0091] Specifically, the first-order partial derivative of - is taken to obtain the gradient vector corresponding to the redundant manipulator.

[0092] S203. Perform quadratic programming solution according to the limit constraint conditions of each joint, the pose constraint conditions of the end joint, the Hessian Matrix, and the gradient vector to obtain the current iteration angle of each joint.

[0093] Among them, the quadratic programming algorithm (Quadratic Programming, QP) is used to solve the current iteration angle of each joint. The limit constraint conditions of each joint, the pose constraint conditions of the end joint, the Hessian Matrix, and the gradient vector are input into the QP solver to obtain the current iteration angle of each joint.

[0094] In the method provided in this embodiment, the second-order partial derivative of the angular change of each joint in the objective function is taken to obtain the Hessian matrix corresponding to the redundant manipulator. Then, the first-order partial derivative of the angular change of each joint in the objective function is taken to obtain the gradient vector corresponding to the redundant manipulator. Finally, quadratic programming is solved according to the limit constraint conditions of each joint, the pose constraint conditions of the end joint, the Hessian matrix, and the gradient vector to obtain the current iterative angle of each joint. By performing second-order approximation on the objective function and combining the joint limit and end joint pose constraint conditions, the problem is transformed into a quadratic programming problem for solution. Using the information of the Hessian matrix and the gradient vector, the angular change of the joint that satisfies the constraint conditions and minimizes the objective function can be found more efficiently, thereby obtaining the joint angle of the current iteration.

[0095] Based on the above embodiment, the embodiment of the present application also provides a possible implementation manner of another inverse kinematic solution method for the joint angles of a redundant manipulator. Figure 3 It is a schematic flowchart of another inverse kinematic solution method for the joint angles of a redundant manipulator provided by the embodiment of the present application. As Figure 3 shown, quadratic programming is solved according to the limit constraint conditions of each joint, the pose constraint conditions of the end joint, the Hessian matrix, and the gradient vector to obtain the current iterative angle of each joint, including:

[0096] S301. Quadratic programming is solved according to the limit constraint conditions of each joint, the pose constraint conditions of the end joint, the Hessian matrix, and the gradient vector to obtain the current angular velocity of each joint.

[0097] S302. Determine the current iterative angle of each joint according to the angle of each joint in the previous iteration and the current angular velocity.

[0098] In this embodiment, the limit constraint conditions of each joint, the pose constraint conditions of the end joint, the Hessian matrix, and the gradient vector are input into the QP solver to obtain the current angular velocity of each joint, that is, the current angular change. After obtaining the current angular velocity of each joint, combined with the joint angle obtained in the previous iteration, the current iterative angle of each joint is calculated. Specifically, the current angular velocity of each joint is added to the joint angle obtained in the previous iteration to obtain the current iterative angle of each joint.

[0099] Optionally, the limit constraint conditions of each joint are inequality constraint conditions shown in the following formula:

[0100]

[0101] where represents the preset motion lower limit of each joint, represents the preset motion upper limit of each joint, represents the angle of each joint in the previous iteration, It is expressed as the angular change of each joint.

[0102] It can be understood that the limit constraint conditions of each joint are satisfied, that is, the current angular velocity of each joint obtained by solving is within the limit range, so that the current iterative angle of each joint is within the preset motion lower limit and the preset motion upper limit, ensuring that each joint will not exceed its limit range during the motion process.

[0103] Optionally, the pose constraint condition of the end joint is shown in the following formula:

[0104]

[0105] Where is expressed as the Jacobian matrix, is expressed as the angular change matrix, is expressed as the desired pose matrix of the end joint, is expressed as the actual pose matrix of the end joint, is the inverse function.

[0106] Specifically, the Jacobian matrix is the Jacobian matrix corresponding to the iterative angle of each joint during the iterative process, is a three-dimensional velocity vector and a three-dimensional rotational speed vector, so the attitude part of is converted into a three-dimensional axis-angle vector.

[0107] In the method provided in this embodiment, quadratic programming is solved according to the limit constraint conditions of each joint, the pose constraint conditions of the end joint, the Hessian matrix and the gradient vector to obtain the current angular velocity of each joint, and then according to the previous iterative angle and the current angular velocity of each joint, the current iterative angle of each joint is determined. By transforming the problem of solving the joint angle into the problem of solving the joint angular velocity, the optimal angular velocity is found by using the quadratic programming method under the conditions of satisfying the joint limit and the pose constraint conditions of the end joint, and then combined with the joint angle of the previous iteration, the joint angle of the current iteration is finally obtained.

[0108] Another possible implementation manner of the inverse kinematics method for the joints of a redundant manipulator is also provided in the embodiments of the present application. Figure 4 is a schematic flow chart of another inverse kinematics method for the joints of a redundant manipulator provided in the embodiments of the present application. As Figure 4 shown, according to the actual pose and the desired pose of the end joint, it is determined whether the preset joint angle iteration stop condition is satisfied, including:

[0109] S401. Calculate the error value between the actual pose and the desired pose of the end joint.

[0110] S402. If the error value is less than or equal to the preset error threshold, it is determined that the preset joint angle iteration stop condition is satisfied.

[0111] S403. If the error value is greater than the preset error threshold, it is determined that the preset joint angle iteration stop condition is not satisfied, and the iteration continues until the actual pose and the desired pose of the end joint satisfy the preset joint angle iteration stop condition.

[0112] In this embodiment, the pose of the end joint generally consists of two parts: position and orientation. The position can be represented by coordinates in a three-dimensional space, and the orientation can be described by a rotation matrix, Euler angles, quaternions, etc. The desired pose is preset according to the task requirements of the redundant manipulator, that is, the target position and orientation that the end of the manipulator needs to reach, and is represented by a 4x4 homogeneous matrix as: ; The actual pose is the actual position and orientation of the end joint calculated through the forward kinematic model of the manipulator according to the current iteration angles of each joint.

[0113] Exemplarily, when calculating the error value between the actual pose and the desired pose, for the position error, generally, the square root of the sum of the squares of the differences of each component in the Cartesian coordinates can be calculated. For the orientation error, the calculation method is different according to the different representation methods of the orientation. For example, when using Euler angles to represent the orientation, the differences of the corresponding Euler angle components can be calculated; when using quaternions to represent the orientation, the orientation error can be obtained by calculating a certain distance metric (such as the angle difference) between the quaternions. Combining the position error, the orientation error, and their respective weight coefficients, a total error value can be calculated. The specific calculation method is not limited here.

[0114] The preset error threshold is a preset value used to measure the acceptable degree of difference between the actual pose and the desired pose of the end joint. It can be determined according to the working precision requirements of the redundant manipulator and the actual application scenario.

[0115] When the calculated error value is less than or equal to the preset error threshold, it indicates that the actual pose of the end joint of the current redundant manipulator is close enough to the desired pose. From the perspective of precision, it has reached an acceptable level. At this time, it is considered that the joint angles obtained by iterative solution can meet the task requirements of the redundant manipulator, so it is determined that the preset joint angle iteration stop condition is satisfied, and the iteration process can end.

[0116] When the calculated error value is greater than the preset error threshold, it indicates that there is still a large difference between the actual pose and the desired pose of the current redundant robotic arm's end joint, and the acceptable accuracy range has not been reached. It is considered that the joint angles obtained in the current iteration cannot make the robotic arm accurately reach the desired pose, and iterative calculations need to be continued to further adjust the joint angles and reduce the error. Therefore, it is determined that the preset joint angle iteration stop condition is not satisfied, and the iterative process will continue until the actual pose and the desired pose of the end joint meet the preset joint angle iteration stop condition.

[0117] In the method provided in this embodiment, by calculating the error value between the actual pose and the desired pose of the end joint and comparing it with the preset error threshold, it is determined whether the preset joint angle iteration stop condition is satisfied, thereby deciding whether the iterative process continues or ends, so as to ensure that the finally obtained joint angles can make the robotic arm meet the desired pose accuracy requirements.

[0118] The following continues to explain the joint angle inverse solution device and the electronic device of the redundant robotic arm provided in any of the above embodiments of the present application. The specific implementation process and the resulting technical effects are the same as those of the corresponding method embodiments described above. For a brief description, for the parts not mentioned in this embodiment, reference can be made to the corresponding content in the method embodiments.

[0119] Figure 5 It is a schematic diagram of the functional modules of a joint angle inverse solution device for a redundant robotic arm provided in an embodiment of the present application. As Figure 5 shown, the joint angle inverse solution device 100 of the redundant robotic arm includes:

[0120] An acquisition module 110, configured to acquire the objective function of the redundant robotic arm, where the objective function is an optimization function with the goal of the joint angles of each joint on the redundant robotic arm being closest to the preset motion midpoint of each joint;

[0121] A solution module 120, configured to solve the objective function according to the angles of each joint in the previous iteration, using the limit constraint conditions of each joint and the pose constraint conditions of the end joint of the redundant robotic arm, to obtain the current iteration angles of each joint;

[0122] The acquisition module 110 is further configured to acquire the actual pose of the end joint according to the current iteration angles of each joint;

[0123] A determination module 130, configured to determine whether the preset joint angle iteration stop condition is satisfied according to the actual pose and the desired pose of the end joint;

[0124] The determination module 130 is further configured to, if the preset joint angle iteration stop condition is satisfied, determine that the current iteration angles of each joint when the preset joint angle iteration stop condition is satisfied are the target angles of each joint.

[0125] Optionally, the objective function is as shown in the following formula:

[0126]

[0127] where, represents the angle of each joint in the previous iteration, represents the angle change of each joint, represents the preset motion midpoint of each joint, represents the preset motion range of each joint, is an integer greater than 1.

[0128] Optionally, the solving module 120 is further configured to perform second-order partial derivatives on the angle changes of each joint in the objective function to obtain the Hessian matrix corresponding to the redundant manipulator; perform first-order partial derivatives on the angle changes of each joint in the objective function to obtain the gradient vector corresponding to the redundant manipulator; and perform quadratic programming solution according to the limit constraint conditions of each joint, the pose constraint conditions of the end joint, the Hessian matrix and the gradient vector to obtain the current iteration angle of each joint.

[0129] Optionally, the solving module 120 is further configured to perform quadratic programming solution according to the limit constraint conditions of each joint, the pose constraint conditions of the end joint, the Hessian matrix and the gradient vector to obtain the current angular velocity of each joint; and determine the current iteration angle of each joint according to the angle of each joint in the previous iteration and the current angular velocity.

[0130] Optionally, the limit constraint condition of each joint is the inequality constraint condition shown in the following formula:

[0131]

[0132] where, represents the preset motion lower limit of each joint, represents the preset motion upper limit of each joint, represents the angle of each joint in the previous iteration, represents the angle change of each joint.

[0133] Optionally, the pose constraint condition of the end joint is as shown in the following formula:

[0134]

[0135] where, represents the Jacobian matrix, represents the angle change matrix, represents the desired pose matrix of the end joint, represents the actual pose matrix of the end joint, is the inverse function.

[0136] Optionally, the determination module 130 is further configured to calculate the error value between the actual pose and the desired pose of the end joint; if the error value is less than or equal to a preset error threshold, it is determined that the preset joint angle iteration stop condition is satisfied; if the error value is greater than the preset error threshold, it is determined that the preset joint angle iteration stop condition is not satisfied, and the iteration continues until the actual pose and the desired pose of the end joint satisfy the preset joint angle iteration stop condition.

[0137] The above device is used to execute the method provided in the foregoing embodiment, and its implementation principle and technical effects are similar, which will not be elaborated here.

[0138] The above modules may be one or more integrated circuits configured to implement the above method. For example: one or more application specific integrated circuits (ASICs), or, one or more microprocessors, or, one or more field programmable gate arrays (FPGAs), etc. Again, when a certain module above is implemented in the form of a processing element scheduling program code, the processing element may be a general-purpose processor, such as a central processing unit (CPU) or other processors that can call program code. Again, these modules may be integrated together and implemented in the form of a system-on-a-chip (SOC).

[0139] Figure 6 It is a schematic diagram of an electronic device provided in an embodiment of the present application. This electronic device can be used for the inverse kinematics of the joint angles of a redundant robotic arm. As Figure 6 shown, the electronic device includes: a processor 210, a storage medium 220, and a bus 230.

[0140] The storage medium 220 stores machine-readable instructions executable by the processor 210. When the electronic device runs, the processor 210 communicates with the storage medium 220 through the bus 230, and the processor 210 executes the machine-readable instructions to execute the steps of the above method embodiment. The specific implementation manner and technical effects are similar and will not be elaborated here.

[0141] Optionally, the present application further provides a storage medium 220, on which a computer program is stored. When the computer program is run by the processor, it executes the steps of the above method embodiment. The specific implementation manner and technical effects are similar and will not be elaborated here.

[0142] In several embodiments provided by the present invention, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are merely illustrative. For example, the division of the units is only a logical function division. In actual implementation, there can be other division methods. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the couplings or direct couplings or communication connections shown or discussed with each other can be through some interfaces. The indirect couplings or communication connections of devices or units can be in electrical, mechanical or other forms.

[0143] The units described as separate components may or may not be physically separated. The components shown as units may or may not be physical units, that is, they can be located in one place or distributed to multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0144] In addition, in each embodiment of the present invention, the functional units can be integrated in a processing unit, or each unit can exist physically alone, or two or more units can be integrated in one unit. The above-mentioned integrated units can be implemented in the form of hardware or in the form of hardware plus software functional units.

[0145] The above-mentioned integrated units implemented in the form of software functional units can be stored in a computer-readable storage medium. The above-mentioned software functional units stored in a storage medium include several instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) or a processor (English: processor) to execute some steps of the methods described in each embodiment of the present invention. The foregoing storage medium includes: various media such as USB flash drives, mobile hard disks, read-only memories (English: Read-Only Memory, abbreviated as: ROM), random access memories (English: Random Access Memory, abbreviated as: RAM), magnetic disks or optical discs that can store program codes.

[0146] The above is only the specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention can easily think of changes or substitutions, which should all be covered within the protection scope of the present invention. Therefore, the protection scope of the present invention should be subject to the protection scope of the claims.

Claims

1. A method for inverse solution of joint angles of a redundant robotic arm, characterized in that: The method comprises: Obtaining an objective function of the redundant robotic arm, wherein the objective function is an optimization function with the joint angle of each joint on the redundant robotic arm being closest to a preset motion midpoint of each joint as a target; According to the last iteration angle of each joint, the objective function is solved by using the limit constraint conditions of each joint and the posture constraint conditions of the end joint of the redundant manipulator to obtain the current iteration angle of each joint; According to the current iteration angle of each joint, the actual position and posture of the end joint are obtained; Determining whether a preset joint angle iteration stop condition is met according to the actual posture and the expected posture of the terminal joint; If the preset joint angle iteration stop condition is met, then determining the current iteration angle of each joint when the preset joint angle iteration stop condition is met is the target angle of each joint; The objective function is solved according to the last iteration angle of each joint, using the limit constraint conditions of each joint and the posture constraint conditions of the end joint of the redundant manipulator to obtain the current iteration angle of each joint, including: Performing a second-order partial derivative on the angle variation of each joint in the objective function to obtain a Hessian matrix corresponding to the redundant robotic arm; Performing a first-order partial derivative on the angle variation of each joint in the objective function to obtain a gradient vector corresponding to the redundant robotic arm; Performing a quadratic programming solution according to the position constraint conditions of each joint, the position constraint conditions of the end joint, the Hessian matrix and the gradient vector to obtain the current angular velocity of each joint; The current iteration angle of each joint is determined according to the previous iteration angle and the current angular velocity of each joint.

2. The method according to claim 1, characterized in that: The objective function is shown in the following formula: in, It is represented by the angle of each joint in the previous iteration, It is expressed as the angle change of each joint, It is represented as the preset motion midpoint of each joint, It is represented by the preset range of motion of each joint. is an integer greater than 1.

3. The method according to claim 1, characterized in that The limit constraints of each joint are inequality constraints shown in the following formula: in, It is represented as the preset lower limit of movement of each joint, It represents the preset upper limit of movement of each joint. It is represented by the angle of each joint in the previous iteration, It is expressed as the angle change of each joint.

4. The method according to claim 1, characterized in that The position constraint condition of the end joint is as shown in the following formula: in, Expressed as the Jacobian matrix, Expressed as an angle variation matrix, is represented as the desired pose matrix of the end joint, is represented as the actual pose matrix of the end joint, To find the inverse function.

5. The method according to claim 1, characterized in that: The step of determining whether a preset joint angle iteration stop condition is satisfied according to the actual posture and the expected posture of the end joint comprises: Calculating the error between the actual position and the expected position of the end joint; If the error value is less than or equal to a preset error threshold, it is determined that a preset joint angle iteration stop condition is met; If the error value is greater than the preset error threshold, it is determined that the preset joint angle iteration stop condition is not met, and the iteration is continued until the actual posture and the expected posture of the terminal joint meet the preset joint angle iteration stop condition.

6. A joint angle inverse solution device for a redundant robotic arm, characterized in that: The device comprises: An acquisition module, used for acquiring an objective function of a redundant robotic arm, wherein the objective function is an optimization function with the joint angle of each joint on the redundant robotic arm being closest to a preset motion midpoint of each joint as a target; A solution module, used to solve the objective function according to the last iteration angle of each joint, using the limit constraints of each joint and the posture constraints of the end joint of the redundant manipulator, to obtain the current iteration angle of each joint; The acquisition module is further used to acquire the actual position and posture of the end joint according to the current iteration angle of each joint; A determination module, used to determine whether a preset joint angle iteration stop condition is met according to the actual posture and expected posture of the terminal joint; The determination module is further configured to determine, if the preset joint angle iteration stop condition is met, that the current iteration angle of each joint when the preset joint angle iteration stop condition is met is the target angle of each joint; The solution module is specifically used to perform a second-order partial derivative on the angle change of each joint in the objective function to obtain the Hessian matrix corresponding to the redundant robotic arm; perform a first-order partial derivative on the angle change of each joint in the objective function to obtain the gradient vector corresponding to the redundant robotic arm; perform a quadratic programming solution based on the limit constraints of each joint, the posture constraints of the end joints, the Hessian matrix and the gradient vector to obtain the current angular velocity of each joint; determine the current iteration angle of each joint based on the previous iteration angle and the current angular velocity of each joint.

7. An electronic device, characterized in that: include: A processor, a storage medium and a bus, wherein the storage medium stores program instructions executable by the processor, and when the electronic device is running, the processor communicates with the storage medium via the bus, and the processor executes the program instructions to perform the steps of the joint angle inverse solution method of the redundant robotic arm as described in any one of claims 1 to 5.

8. A computer-readable storage medium, characterized in that: The storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the method for inverse solution of joint angles of a redundant robotic arm as claimed in any one of claims 1 to 5 are executed.

Images