Joint angle inverse solution method, device and equipment of redundant mechanical arm and medium

Abstract

The invention provides a joint angle inverse solution method, device and equipment of a redundant mechanical arm and a medium, and the method comprises the steps that a target function of the redundant mechanical arm is obtained, the target function is an optimization function with the joint angle of each joint on the redundant mechanical arm closest to a preset movement midpoint of each joint as a target, and the joint angle of each joint is calculated according to the last iteration angle of each joint; the target function is solved by adopting the limiting constraint condition of each joint and the pose constraint condition of the tail end joint of the redundant mechanical arm, the current iteration angle of each joint is obtained, the actual pose of the tail end joint is obtained according to the current iteration angle of each joint, and the target function is obtained according to the actual pose and the expected pose of the tail end joint. And determining whether a preset joint angle iteration stopping condition is met or not, and if the preset joint angle iteration stopping condition is met, determining that the current iteration angle of each joint is the target angle of each joint when the preset joint angle iteration stopping condition is met. And each joint is controlled based on the target angle, so that each joint is closest to the preset movement midpoint.

Description

Technical Field

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

[0002] A redundant manipulator refers to a manipulator whose degrees of freedom are higher than the dimension of the task space. Generally, this type of manipulator has an infinite number of inverse solutions, which can be limited to a finite number by adding additional constraints. Currently, the Newton method can give a set of inverse solutions, but this inverse solution depends on the selection of the initial value of the joint and is iterated based on the current joint angle. When the error function is satisfied, the iteration process is exited, resulting in the inverse solution that can meet the end position requirements, but it will make the linkage position of the manipulator appear abnormal, and the advantages of the redundant manipulator cannot be fully utilized. Summary of the invention

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

[0004] To achieve the above purpose, the technical solution adopted in the embodiment of the present application is as follows: In a first aspect, an embodiment of the present application provides a method for inverse solution of joint angles of a redundant robotic arm, the method comprising: 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, the current iteration angle of each joint when the preset joint angle iteration stop condition is met is determined to be the target angle of each joint.

[0005] In an optional implementation, the objective function is as shown in the following formula:

[0006] 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.

[0007] In an optional embodiment, according to the last iteration angle of each joint, the limit constraint conditions of each joint and the posture constraint conditions of the end joint of the redundant manipulator are used to solve the objective function 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; A quadratic programming solution is performed 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 iteration angle of each joint.

[0008] In an optional embodiment, 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 iteration angle of each joint includes: 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.

[0009] In an optional implementation, the limit constraint conditions of each joint are inequality constraints shown in the following formula:

[0010] 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.

[0011] In an optional embodiment, the posture constraint condition of the end joint is as shown in the following formula:

[0012] 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.

[0013] In an optional embodiment, determining whether a preset joint angle iteration stop condition is satisfied according to the actual posture and the expected posture of the terminal joint includes: 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.

[0014] In a second aspect, an embodiment of the present application further provides a joint angle inverse solution device for a redundant robotic arm, the device comprising: 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 stopping condition is met, that the current iteration angle of each joint when the preset joint angle iteration stopping condition is met is the target angle of each joint.

[0015] In the third aspect, an embodiment of the present application also provides an electronic device, comprising: 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 through the bus, and the processor executes the program instructions to perform the steps of the inverse solution method of the joint angles of the redundant robotic arm as described in any one of the first aspects.

[0016] In a fourth aspect, an embodiment of the present application further provides a computer-readable storage medium having a computer program stored thereon, 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 described in any one of the first aspects are executed.

[0017] The beneficial effects of this application are: The embodiments of the present application provide a method, device, equipment and medium for inverse solution of the joint angles of a redundant robotic arm, the method comprising: obtaining an objective function of the redundant robotic arm, the objective function being an optimization function with the joint angles of each joint on the redundant robotic arm being closest to a preset motion midpoint of each joint as the goal; solving the objective function according to the angles of each joint in the previous iteration, using the limit constraints of each joint and the posture constraints of the end joints of the redundant robotic arm to obtain the current iteration angles of each joint; then obtaining the actual posture of the end joint according to the current iteration angles of each joint; and determining whether the preset joint angle iteration stop condition is met according to the actual posture and expected posture of the end joint; if the preset joint angle iteration stop condition is met, determining 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. The method of the present application sets an objective function and adds limit constraints on each joint and posture constraints on the end joint, so that after controlling each joint based on the target angle of each joint, each joint can be made closest to a preset motion midpoint, and the end joint can reach a state close to the desired posture, thereby avoiding abnormal positioning of the connecting rods of the redundant robotic arm and facilitating the subsequent movement of the redundant robotic arm. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0019] Figure 1 A schematic diagram of a flow chart of a method for inverse solution of joint angles of a redundant robotic arm provided in an embodiment of the present application; Figure 2A schematic diagram of a flow chart of another method for inverse solution of joint angles of a redundant robotic arm provided in an embodiment of the present application; Figure 3 A schematic diagram of a flow chart of another method for inverse solution of joint angles of a redundant robotic arm provided in an embodiment of the present application; Figure 4 A schematic diagram of a flow chart of another method for inverse solution of joint angles of a redundant robotic arm provided in an embodiment of the present application; Figure 5 A schematic diagram of functional modules of a joint angle inverse solution device for a redundant robotic arm provided in an embodiment of the present application; Figure 6 A schematic diagram of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0020] In order to make the purpose, 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 in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments.

[0021] Therefore, the following detailed description of the embodiments of the present application provided in the accompanying drawings is not intended to limit the scope of the present application for which protection is sought, but merely represents selected embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in the field without creative work are within the scope of protection of the present application.

[0022] In the description of the present application, it should be noted that if the terms "upper", "lower", etc. appear, the orientation or position relationship indicated is based on the orientation or position relationship shown in the drawings, or is the orientation or position relationship in which the product of the application is usually placed when used. It is only for the convenience of describing the present application and simplifying the description, and does not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present application.

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

[0024] It should be noted that, in the absence of conflict, the features in the embodiments of the present application may be combined with each other.

[0025] A redundant robot arm refers to a robot arm with more degrees of freedom than the minimum number of degrees of freedom required to complete a specific task. For example, in three-dimensional space, a 6-axis (6-DOF) robot arm is commonly used, and a 7-axis (7-DOF) robot arm is called a redundant robot arm. The inverse problem is to solve the angles of each joint given the desired position and posture of the end joint of the robot arm, so that the robot arm can reach the desired position and posture.

[0026] In order to obtain a set of suitable joint angles, so that the angle of each joint is as close as possible to its preset motion midpoint and at the same time meet the posture requirements of the end joint of the robot arm, an embodiment of the present application provides a joint angle inverse solution method for a redundant robot arm, specifically by obtaining the objective function of the redundant robot arm, the objective function is an optimization function with the joint angle of each joint on the redundant robot arm being closest to the preset motion midpoint of each joint as the goal, according to the angle of each joint in the previous iteration, the limit constraint conditions of each joint and the posture constraint conditions of the end joint of the redundant robot arm are used to solve the objective function to obtain the current iteration angle of each joint, and then according to the current iteration angle of each joint, the actual posture of the end joint is obtained, and according to the actual posture and expected posture 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 angle of each joint when the preset joint angle iteration stop condition is met is the target angle of each joint, and at this time, each joint is controlled according to the target angle of each joint, so that each joint can be closest to the preset motion midpoint.

[0027] The inverse solution method for the joint angles of the redundant robotic arm provided by the embodiment of the present application is explained in detail below by using specific examples in conjunction with the accompanying drawings. The inverse 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: a preset inverse solution algorithm for the joint angles of the 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 The flowchart of the joint angle inverse solution method of a redundant robot arm provided in an embodiment of the present application is as follows. Figure 1 As shown, the method includes: S101, obtaining the objective function of the redundant robotic arm.

[0028] The objective function is an optimization function that takes the joint angles of the joints on the redundant robotic arm as close to the preset motion midpoints of the joints.

[0029] In this embodiment, the objective function is an optimization function, the purpose of which is to make the joint angles of each joint on the redundant manipulator as close as possible to the preset motion midpoint of each joint. The preset motion midpoint can be understood as the middle 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°.

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

[0031] S102, 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 robot arm, the objective function is solved to obtain the current iteration angle of each joint.

[0032] Specifically, in the first iteration, the previous iteration angle may be the initially set joint angle, and in subsequent iterations, the previous iteration angle may be the joint angle calculated after the previous iteration ends.

[0033] Each joint has its allowed range of motion. For example, the range of motion of a joint is -90° to 90°. By adding limit constraints to each joint, the final joint angle is within the limit constraint range. The limit constraints can ensure that the joints of the redundant manipulator do not exceed their physical limitations. In addition, adding position constraints to the end joints can ensure that the joint angles obtained can enable the end joints to reach the desired position when solving the objective function.

[0034] By adopting the optimization algorithm, the objective function is minimized under the premise of satisfying the limit constraints and posture constraints, so as to obtain the current iteration angle of each joint.

[0035] S103. Obtain the actual position and posture of the end joint according to the current iteration angle of each joint.

[0036] Specifically, after obtaining the current iteration angle of each joint, the actual position and posture of the end joint can be calculated and mapped according to the current iteration angle of each joint through the kinematic forward solution model of the robot arm. The kinematic forward solution model describes the mathematical relationship between the angles of each joint of the robot arm and the position and posture of the end joint.

[0037] S104: Determine whether a preset joint angle iteration stop condition is met based on the actual posture and expected posture of the terminal joint.

[0038] S105 . If the preset joint angle iteration stopping condition is met, determine that the current iteration angle of each joint when the preset joint angle iteration stopping condition is met is the target angle of each joint.

[0039] Specifically, by comparing the actual position and expected position of the terminal joint, it is determined whether the preset joint angle iteration stopping condition is met. If the preset joint angle iteration stopping condition is met, it means that the current joint angle has been able to make the terminal joint reach a state close to the expected position, and each joint angle is as close as possible to its preset motion midpoint, then the iteration is stopped and the joint angle at this time is used as the final target angle.

[0040] In summary, an embodiment of the present application provides a method for inverse solution of joint angles of a redundant robotic arm, including: obtaining an objective function of the redundant robotic arm, the objective function being an optimization function with the joint angles of each joint on the redundant robotic arm being closest to a preset motion midpoint of each joint as the goal; solving the objective function according to the angles of each joint in the previous iteration, using the limit constraints of each joint and the posture constraints of the end joints of the redundant robotic arm to obtain the current iteration angles of each joint; then obtaining the actual posture of the end joint according to the current iteration angles of each joint; and determining whether the preset joint angle iteration stop conditions are met according to the actual posture and expected posture of the end joint; if the preset joint angle iteration stop conditions are met, determining that the current iteration angles of each joint when the preset joint angle iteration stop conditions are met are the target angles of each joint. The method of the present application sets an objective function and adds limit constraints on each joint and posture constraints on the end joint, so that after controlling each joint based on the target angle of each joint, each joint can be made closest to a preset motion midpoint, and the end joint can reach a state close to the desired posture, thereby avoiding abnormal positioning of the connecting rods of the redundant robotic arm and facilitating the subsequent movement of the redundant robotic arm.

[0041] The embodiment of the present application also provides another possible implementation of the joint angle inverse solution method of the redundant manipulator, and the objective function is shown in the following formula:

[0042] in, Represents the angle of each joint in the last iteration, Expressed as the angle change of each joint, Represents the preset motion midpoint of each joint, Represents the preset range of motion of each joint, is an integer greater than 1.

[0043] In this embodiment, if the redundant manipulator has seven degrees of freedom, then n in the objective function is 6. For example, the preset lower limit of motion of each joint is , the preset upper limit of movement is , lower right corner mark Represents joint numbers 1-6. The preset motion midpoint of each joint is expressed as:

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

[0045] The desired pose of the end joint is expressed as a 4-row 4-column homogeneous matrix: The optimization goal is to minimize the sum of the squares of the distances between each joint and the preset motion midpoint. Since the preset motion ranges of each joint are different, the preset motion ranges of the joints are set to As a scaling factor, the objective function is expressed as:

[0046] in, It is represented by the inverse solution of each joint in the previous iteration, that is, the joint angle. It is expressed as:

[0047] in, Represents the angle of each joint in the last iteration, The angle change of each joint is the variable to be solved iteratively, which represents the angular velocity of each joint. For example, if the joints of the redundant robot arm are set to move for 1 second each time, the angular velocity is the movement angle of the joint.

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

[0049] By setting the objective function, the target angle of each joint obtained by solving the problem is closest to the preset motion midpoint of each joint, avoiding abnormal positioning of the connecting rod of the redundant robot arm, which is beneficial to the subsequent movement of the redundant robot arm.

[0050] The embodiment of the present application also provides another possible implementation of the joint angle inverse solution method of the redundant manipulator, Figure 2 A schematic diagram of a flow chart of another method for inverse solution of joint angles of a redundant robotic arm provided in an embodiment of the present application. Figure 2 As shown, according to the last iteration angle of each joint, the limit constraints of each joint and the posture constraints of the end joints of the redundant manipulator are used to solve the objective function and obtain the current iteration angle of each joint, including: S201. Perform a second-order partial derivative on the angle variation of each joint in the objective function to obtain the Hessian matrix corresponding to the redundant robotic arm.

[0051] 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 target function, and can understand the local shape of the target function. In optimization problems, the Hessian Matrix is ​​used to describe the quadratic approximation of the target function, which plays an important role in judging the convexity of the function and finding the extreme points of the function.

[0052] During the iteration process, the objective function is expanded to obtain the angle change of each joint. - , and - Perform second-order partial derivatives to obtain the Hessian matrix corresponding to the redundant robotic arm.

[0053] S202, 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.

[0054] The gradient vector is a vector composed of the first-order partial derivatives of the objective function with respect to the angle 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 the 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 in the direction in which the objective function decreases.

[0055] Specific and - Perform the first-order partial derivative to obtain the gradient vector corresponding to the redundant robotic arm.

[0056] S203, 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 iteration angle of each joint.

[0057] Among them, the quadratic programming algorithm (Quadratic Programming, QP) is used to solve the current iteration angle of each joint, and the limit constraints of each joint, the posture constraints 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.

[0058] In the method provided in this embodiment, the angle change of each joint in the objective function is subjected to a second-order partial derivative to obtain the Hessian matrix corresponding to the redundant manipulator, and then the angle change of each joint in the objective function is subjected to a first-order partial derivative to obtain the gradient vector corresponding to the redundant manipulator, and finally, a quadratic programming solution is performed 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 iteration angle of each joint. By performing a second-order approximation on the objective function, combining the joint limit and the end joint posture constraints, the problem is converted into a quadratic programming problem for solution, and using the information of the Hessian matrix and the gradient vector, it is possible to more efficiently find the joint angle change that satisfies the constraints and minimizes the objective function, thereby obtaining the joint angle of the current iteration.

[0059] Based on the above embodiment, the embodiment of the present application further provides another possible implementation of the joint angle inverse solution method of the redundant manipulator, Figure 3 A schematic diagram of a flow chart of another method for inverse solution of joint angles of a redundant robotic arm provided in an embodiment of the present application. Figure 3 As shown, according to the position constraints of each joint, the position constraints of the end joint, the Hessian matrix and the gradient vector, the secondary programming solution is performed to obtain the current iteration angle of each joint, including: S301, performing a quadratic programming solution according to the position constraints of each joint, the position constraints of the end joint, the Hessian matrix and the gradient vector to obtain the current angular velocity of each joint.

[0060] S302: Determine the current iteration angle of each joint according to the previous iteration angle and current angular velocity of each joint.

[0061] In this embodiment, the limit constraints of each joint, the posture constraints of the end joints, 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 angle change. After obtaining the current angular velocity of each joint, the current iteration angle of each joint is calculated based on the joint angle obtained in the previous iteration. Specifically, the current angular velocity of each joint is added to the joint angle obtained in the previous iteration to obtain the current iteration angle of each joint.

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

[0063] in, Represents the preset lower limit of motion for each joint, Represents the preset upper limit of motion for each joint. Represents the angle of each joint in the last iteration, Expressed as the angle change of each joint.

[0064] It can be understood that the limit constraints of each joint are met, that is, the current angular velocity of each joint is solved within the limit range, so that the current iteration angle of each joint is within the preset lower limit and the preset upper limit of movement, ensuring that each joint will not exceed its limit range during movement.

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

[0066] in, Expressed as the Jacobian matrix, Expressed as an angle variation matrix, Expressed as the desired pose matrix of the end joint, Represented as the actual pose matrix of the end joint, To find the inverse function.

[0067] Specifically, the Jacobian matrix is ​​the Jacobian matrix corresponding to the iteration angle of each joint during the iteration process. is a three-dimensional velocity vector and a three-dimensional rotation speed vector, so The attitude part is converted into a three-dimensional axis-angle vector.

[0068] In the method provided in this embodiment, a quadratic programming solution is performed 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, and then the current iteration angle of each joint is determined based on the angle of each joint in the previous iteration and the current angular velocity. By converting the problem of solving the joint angle into the problem of solving the joint angular velocity, the quadratic programming method is used to find the optimal angular velocity while satisfying the joint limit and end joint posture constraints, and then combined with the joint angle of the previous iteration, the joint angle of the current iteration is finally obtained.

[0069] The embodiment of the present application also provides another possible implementation of the joint angle inverse solution method of the redundant manipulator, Figure 4 A schematic diagram of a flow chart of another method for inverse solution of joint angles of a redundant robotic arm provided in an embodiment of the present application. Figure 4 As shown, according to the actual posture and expected posture of the end joint, it is determined whether the preset joint angle iteration stop condition is met, including: S401, calculating the error value between the actual posture and the expected posture of the end joint.

[0070] S402: 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.

[0071] S403: If the error value is greater than a 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.

[0072] In this embodiment, the posture of the end joint is usually composed of two parts: position and attitude. The position can be represented by coordinates in three-dimensional space, and the attitude can be described by rotation matrix, Euler angle, quaternion, etc. The expected posture is pre-set according to the task requirements of the redundant manipulator, that is, the target position and attitude that the end of the manipulator needs to reach, which is represented by a 4-row 4-column homogeneous matrix: The actual position and posture are calculated based on the current iteration angle of each joint through the kinematic forward solution model of the robot arm, which is the actual position and posture of the end joint.

[0073] For example, when calculating the error value between the actual posture and the expected posture, for the position error, the square root of the sum of the squares of the differences of each component in the Cartesian coordinates can generally be calculated. For the attitude error, the calculation method varies depending on the different representation methods of the attitude. For example, when the attitude is represented by Euler angles, the difference of the corresponding Euler angle components can be calculated; when the attitude is represented by quaternions, the attitude error can be obtained by calculating a certain distance measure (such as angle difference) between quaternions. Combining the position error and attitude error and their respective weight coefficients, a total error value can be calculated. There is no restriction on the specific calculation method here.

[0074] The preset error threshold is a pre-set value used to measure the acceptable difference between the actual position and expected position of the end joint. It can be determined according to the working accuracy requirements of the redundant robot arm and the actual application scenario.

[0075] When the calculated error value is less than or equal to the preset error threshold, it means that the actual position of the current redundant manipulator end joint is close enough to the expected position, and from the perspective of accuracy, it has reached an acceptable level. At this time, it is considered that the joint angle 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 met and the iteration process can end.

[0076] When the calculated error value is greater than the preset error threshold, it means that there is still a large difference between the actual posture and the expected posture of the current redundant manipulator end joint, and the acceptable accuracy range has not yet been reached. It is considered that the joint angle obtained by the current iteration cannot enable the manipulator to accurately reach the expected posture, and it is necessary to continue the iterative calculation to further adjust the joint angle and reduce the error. Therefore, it is determined that the preset joint angle iteration stop condition is not met, and the iteration process will continue until the actual posture and the expected posture of the end joint meet the preset joint angle iteration stop condition.

[0077] In the method provided in this embodiment, the error value between the actual posture and the expected posture of the end joint is calculated and compared with the preset error threshold to determine whether the preset joint angle iteration stop condition is met, thereby deciding whether the iterative process should continue or end, to ensure that the final joint angle can enable the robotic arm to achieve the desired posture accuracy requirements.

[0078] The following is a corresponding explanation of the joint angle inverse solution device and electronic device for the redundant robotic arm provided by any of the above embodiments of the present application. The specific implementation process and the technical effects produced are the same as those of the corresponding method embodiments mentioned above. For the sake of brief description, the parts not mentioned in this embodiment can refer to the corresponding contents in the method embodiments.

[0079] Figure 5 This is a functional module diagram of a joint angle inverse solution device for a redundant robotic arm provided in an embodiment of the present application. Figure 5 As shown, the joint angle inverse solution device 100 of the redundant mechanical arm includes: An acquisition module 110 is used to acquire an objective function of the redundant robotic arm, where the objective function is an optimization function that takes the joint angles of each joint on the redundant robotic arm as being closest to a preset motion midpoint of each joint; A solving module 120 is used to solve the objective function 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; The acquisition module 110 is also used to acquire the actual position and posture of the end joint according to the current iteration angle of each joint; A determination module 130 is used to determine whether a preset joint angle iteration stop condition is met according to the actual posture and the expected posture of the end joint; The determination module 130 is further configured to determine, if a preset joint angle iteration stopping condition is met, that the current iteration angle of each joint when the preset joint angle iteration stopping condition is met is the target angle of each joint.

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

[0081] in, Represents the angle of each joint in the last iteration, Expressed as the angle change of each joint, Represents the preset motion midpoint of each joint, Represents the preset range of motion of each joint, is an integer greater than 1.

[0082] Optionally, the solution module 120 is also 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 iteration angle of each joint.

[0083] Optionally, the solution module 120 is also used to perform 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; and determine the current iteration angle of each joint based on the previous iteration angle and current angular velocity of each joint.

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

[0085] in, Represents the preset lower limit of motion for each joint, Represents the preset upper limit of motion for each joint. Represents the angle of each joint in the last iteration, Expressed as the angle change of each joint.

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

[0087] in, Expressed as the Jacobian matrix, Expressed as an angle variation matrix, Expressed as the desired pose matrix of the end joint, Represented as the actual pose matrix of the end joint, To find the inverse function.

[0088] Optionally, the determination module 130 is also used to calculate the error value between the actual posture and the expected posture of the terminal 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 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.

[0089] The above-mentioned device is used to execute the method provided by the aforementioned embodiment, and its implementation principle and technical effect are similar, which will not be repeated here.

[0090] The above modules may be one or more integrated circuits configured to implement the above methods, such as one or more application specific integrated circuits (ASICs), or one or more microprocessors, or one or more field programmable gate arrays (FPGAs). For another example, when a module 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 processor that can call program code. For another example, these modules may be integrated together and implemented in the form of a system-on-a-chip (SOC).

[0091] Figure 6 Schematic diagram of an electronic device provided in an embodiment of the present application, which can be used for inverse solution of joint angles of redundant manipulators. Figure 6 As shown, the electronic device includes: a processor 210 , a storage medium 220 , and a bus 230 .

[0092] The storage medium 220 stores machine-readable instructions executable by the processor 210. When the electronic device is running, the processor 210 communicates with the storage medium 220 via the bus 230, and the processor 210 executes the machine-readable instructions to perform the steps of the above method embodiment. The specific implementation method and technical effect are similar and will not be repeated here.

[0093] Optionally, the present application further provides a storage medium 220, on which a computer program is stored, and when the computer program is executed by a processor, the steps of the above method embodiment are executed. The specific implementation method and technical effect are similar and will not be repeated here.

[0094] In the 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 only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation, such as 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 mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

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

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

[0097] The above-mentioned integrated unit implemented in the form of a software functional unit can be stored in a computer-readable storage medium. The above-mentioned software functional unit is stored in a storage medium, including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) or a processor (English: processor) to perform some steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (English: Read-Only Memory, referred to as: ROM), random access memory (English: Random Access Memory, referred to as: RAM), disk or optical disk and other media that can store program codes.

[0098] The above are only specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art who is familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on 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, the current iteration angle of each joint when the preset joint angle iteration stop condition is met is determined to be the target angle of each joint.

2. The method according to claim 1, characterized in that The objective function is shown in the following formula: in, Represented as 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 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; A quadratic programming solution is performed 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 iteration angle of each joint.

4. The method according to claim 3, characterized in that The method of 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 iteration angle of each joint includes: 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.

5. 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.

6. 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.

7. 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.

8. 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 stopping condition is met, that the current iteration angle of each joint when the preset joint angle iteration stopping condition is met is the target angle of each joint.

9. 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 7.

10. 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 joint angle inverse solution method of a redundant robotic arm as claimed in any one of claims 1 to 7 are executed.

Images