# Robot kinematic parameter error optimized compensation method and device

## A technology of robot kinematics and parameter compensation, which is applied in the fields of instruments, electrical digital data processing, and special data processing applications, and can solve problems such as kinematic inverse analysis failures

Active Publication Date: 2017-02-15

太仓珞石三盛网络科技有限公司 +2

5 Cites 14 Cited by

## AI-Extracted Technical Summary

### Problems solved by technology

[0005] The purpose of the present invention is to provide a robot kinematic parameter error optimization compensation metho...

## Abstract

The invention relates to the field of robot parameter correction technologies, in particular to a robot kinematic parameter error optimized compensation method and a robot kinematic parameter error optimized compensation device. The robot kinematic parameter error optimized compensation method comprises the steps of: constructing a robot kinematic nominal value model according to robot kinematic parameters before calibration, and constructing a robot kinematic calibration value model according to robot kinematic parameters after calibration; calculating a pose of an end effector coordinate system origin according to the robot kinematic calibration value model at a preset joint spatial position; constructing a parameter compensation constrained optimization model, wherein the parameter compensation constrained optimization model is used for constraining a difference value between the pose of the end effector coordinate system origin and a preset object pose to be minimum; and calculating the parameter compensation constrained optimization model to obtain joint spatial coordinate values after optimized compensation. The robot kinematic parameter error optimized compensation method and the robot kinematic parameter error optimized compensation device can solve the problem of inverse kinematics analysis failure caused after the compensation of the kinematic parameters.

Application Domain

Electric testing/monitoringSpecial data processing applications

Technology Topic

Kinematic calibrationRobot kinematics +4

## Image

## Examples

- Experimental program(1)

### Example Embodiment

[0052] The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. The components of the embodiments of the present invention generally described and illustrated in the drawings herein may be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely represents selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative work shall fall within the protection scope of the present invention.

[0053] In the prior art, the process of compensating the identified parameter error into the controller is usually completed by modifying the robot model in the robot controller. The success of the robot model modification directly determines the success or failure of the kinematic calibration. With the increase in the popularity of robots, most non-professional users do not have the authority to modify the robot controller, so it is difficult to write the recognized compensation parameters into the robot controller, which makes the results of parameter identification unable to be applied to actual operations Therefore, the absolute positioning accuracy of the robot cannot be improved.

[0054] On the other hand, even if the user has the permission to modify the robot controller, if the calibrated kinematics parameters do not match the nominal values, for example, the kinematics parameters of the three-axis coordinate system of the calibrated robot wrist joints do not match the nominal values, which may cause The three axes of the wrist joints in the compensated robot model no longer intersect at one point, which will cause the analytical failure of the inverse kinematics of the robot.

[0055] In order to solve the problem that the inverse kinematics solution analysis fails after kinematic parameter compensation in the prior art, an embodiment of the present invention provides a method for optimizing and compensating robot kinematic parameter errors.

[0056] figure 1 It is a flowchart of a method for optimizing and compensating robot kinematic parameter errors in an embodiment of the present invention. figure 1 The execution body of the method shown is the controller in the robot, and the method includes:

[0057] Step S101: According to the kinematic parameters of the robot before and after the calibration, construct the robot kinematics nominal value model and the robot kinematics calibration value model respectively.

[0058] In the robot kinematics nominal value model and the robot kinematics calibration value model, the pose vector at the origin of the end effector coordinate system is used to represent the pose conversion relationship between the end effector coordinate system and the base coordinate system.

[0059] In the embodiment of the present invention, D-H parameters are used to construct the aforementioned robot kinematics nominal value model and robot kinematics calibration value model. Classic D-H parameters are often used to describe the kinematics model of robots. It is a simple method for modeling robot linkages and joints. It can be used for any robot configuration.

[0060] In the solution of the embodiment of the present invention, the method for constructing a robot kinematics model using D-H parameters includes:

[0061] (1) such as figure 2 As shown, the joint axis is taken as the Z axis at the robot joint, and the starting point of the link connected with the joint is used as the coordinate origin to establish the coordinate system of each joint, wherein the established coordinate system of each joint is called the base coordinate system.

[0062] (2) A 4×4 homogeneous transformation matrix constructed by the geometric structure parameters of the robot is used to describe the spatial transformation relationship between two adjacent links.

[0063] (3) Derive the equivalent homogeneous transformation matrix of the end effector coordinate system relative to the base coordinate system based on the adjacent joint coordinate system conversion.

[0064] After the coordinate system of all the connecting rods is specified, the four parameters used to express the relative position and orientation relationship between the coordinate system i-1 and the coordinate system i can be determined, namely the length of the rod ai-1 and the torsion angle of the rod. ɑi-1, joint distance di and joint rotation angle θi, among which, the rod length ai-1, the rod torsion angle ɑi-1, the joint distance di and the joint rotation angle θi are called classic DH parameters. Using the basic homogeneous rotation and homogeneous translation transformation matrix, the homogeneous transformation matrix i-1iT of adjacent coordinate systems i-1 and i can be obtained, namely:

[0065]

[0066] Where cɑi-1 means cos(ɑi-1), sɑi-1 means sin(ɑi-1), cθi means cos(θi), and sθi means sin(θi).

[0067] Therefore, the homogeneous transformation matrix from the robot base coordinate system to the end effector coordinate system can be expressed as:

[0068]

[0069] Among them, n, o, a and p respectively represent the elements in the homogeneous transformation matrix. Accordingly, the nominal value of the kinematic parameters before calibration and the calibration value after calibration can be used to establish the nominal value model of the robot kinematics R n And calibration value model R c. The nominal value model represents the conversion relationship of the end coordinate system relative to the base coordinate system in the controller. The axes of the three coordinate systems of the wrist joint intersect at a point, which can be obtained by the inverse kinematics solution according to the position of the specified end effector coordinate system The analytic solution of the corresponding joint space position; and the calibration value model after the kinematic parameter calibration indicates the accurate conversion relationship between the end coordinate system and the base coordinate system after calibration, and the torsion angle of the rod is not necessarily 0 or 90° , Causing the three axes of the wrist joint to not necessarily intersect, so the inverse solution cannot be obtained through the analytical solution.

[0070] According to the RPY angle method, the homogeneous transformation matrix is converted into the pose P of the origin of the end effector coordinate system in the base coordinate system. Then the x, y and z coordinates are px, py and pz respectively, and the end effector coordinate system is around the base The deflection angle ɑ, pitch angle β and roll angle γ of the coordinate axis x, y, z rotation of the coordinate system can be calculated as:

[0071]

[0072] So far, the pose vector P of the origin of the end effector coordinate system can be used n =[p x p y p z α β γ] T To represent the pose conversion relationship between the end effector coordinate system and the base coordinate system.

[0073] Step S102: Calculate the pose of the origin of the end effector coordinate system according to the kinematic calibration value model under the preset joint space position.

[0074] Wherein, in specific implementation, the robot controller calculates the homogeneous transformation matrix of the end effector coordinate system corresponding to the preset joint space position relative to the base coordinate system in Cartesian space according to the kinematic calibration value model; The homogeneous transformation matrix determines the pose of the origin of the end effector coordinate system.

[0075] Step S103: Construct a parameter compensation constraint optimization model, where the parameter compensation constraint optimization model is used to constrain the difference between the pose of the origin of the end effector coordinate system calculated according to the kinematic calibration value model and the preset target pose The value is the smallest.

[0076] In the solution of the embodiment of the present invention, the step of constructing the parameter compensation constraint optimization model includes:

[0077] (1) The calibration value model R obtained from the first step c Under the premise of a given joint space position, the homogeneous transformation matrix of the end effector coordinate system corresponding to the joint position relative to the base coordinate system in Cartesian space can be calculated, and then the position of the origin of the end effector coordinate system can be obtained. posture. In this constrained optimization problem, the joint position in the calibration value model is used as the variable to be optimized, and the optimization goal is to make the pose of the origin of the end effector coordinate system calculated by the calibration value model as close to the specified target pose as possible. That is, minimize the Euclidean norm of the pose error vector.

[0078] Based on this, the parameter compensation constraint optimization model can be constructed:

[0079] minω(θ)=||P n -f c (θ)|| 2

[0080]

[0081] Among them, ω(θ) represents the Euclidean norm of the pose error vector, that is, the square root of the inner product of the error vector; Pn represents the specified pose vector calculated according to the specified end effector coordinate system conversion matrix [p x p y p z α β γ] T; F c (θ) represents the actual pose vector calculated according to the calibration value model and the joint space position θ; with Respectively represent the minimum and maximum limits of joint i. In a mobile joint, this constraint is expressed as the maximum displacement that the joint can move in the axial direction.

[0082] Step S104: Solve the parameter compensation constraint optimization model to obtain joint space coordinate values after optimization and compensation.

[0083] In the embodiment of the present invention, the interior point method may be used to solve the parameter compensation constraint optimization model to obtain the joint space coordinate value after optimization and compensation. Among them, the interior point method has the advantages of fast calculation speed, relatively mature algorithm, and high reliability of results.

[0084] Specifically, the steps of using the interior point method to solve the parameter compensation constraint optimization model to obtain the joint space coordinate values after optimization and compensation include:

[0085] 1. Construct a new unconstrained objective function based on the above parameter compensation constraint optimization model, namely the penalty function

[0086]

[0087] Where r (k) Is the penalty factor, which is a decreasing sequence of positive numbers, namely r (0)r (1)...r (k) And have The penalty factor can greatly increase the speed of model convergence; g i (θ) is the 12 constraints on the joint space position in the original optimization model, namely Etc., are inequalities less than or equal to 0.

[0088] Second, the penalty function Iteratively approach the optimal solution of the original constrained optimization problem step by step in the feasible region, specifically including:

[0089] (1) Take the initial penalty factor r (0)0, allowable error ε>0;

[0090] (2) Take the initial point θ in the feasible region D (0) , Let k=1, the initial point can be obtained by the inverse solution of the kinematic nominal value model according to the given end effector pose;

[0091] (3) From θ (k-1) Start from point, use unconstrained optimization method (such as steepest descent method or Newton method) to solve the penalty function The extreme point θ * (r (k) );

[0092] (4) Check the iteration termination condition, if it is met

[0093]

[0094] or

[0095] ||θ * (r (k) )-θ * (r (k-1) )||≤ε 2 ,

[0096] The iterative calculation is terminated, and θ * (r (k) ) Is the constrained optimal solution of the original optimization function ω(θ), otherwise go to the next step;

[0097] (5) Take r (k+1 )=Cr (k) , Θ (0) =θ * (r (k) ), k=k+1, go to step 3). Decrease coefficient C=0.1-0.5, usually 0.1.

[0098] After iterative calculation, the optimal solution of the parameter compensation constraint optimization model can be obtained, that is, the joint space coordinate value after optimization and compensation.

[0099] Step S105: Substituting the joint space coordinates after the optimization and compensation for the joint space coordinates obtained by the inverse kinematics calculation of the robot kinematics nominal value model.

[0100] In the embodiment of the present invention, the kinematics parameters of the robot before and after calibration are used to construct the nominal value model and the calibration value model of the robot kinematics; according to the specified robot end effector pose and calibration value model, the specified pose and the calibration value model The minimum Euclidean norm of the difference vector of the pose calculated by the value model is used as the optimization target, and the joint motion range of each motion joint is used as the constraint condition to construct the constrained optimization model; the nonlinear multivariate function optimization algorithm is used to solve the model and obtain compensation After the joint space coordinate values, this set of values replaces the joint space coordinates calculated by the inverse kinematics solution of the nominal value model, and the controller operates the robot to move.

[0101] In the robot kinematic parameter error optimization compensation method of the method of the embodiment of the present invention, the kinematic parameter compensation does not need to modify the original parameters in the robot controller, which is convenient for calibration; in addition, the method of the embodiment of the present invention solves the possibility of robot kinematic parameter calibration The three axes of the wrist joint no longer intersect at one point and the inverse solution analysis fails.

[0102] image 3 It is a robot kinematic parameter error optimization compensation device provided by the embodiment of the present invention. Such as image 3 As shown, the device includes: a first model construction unit 201, a first calculation unit 202, a second model construction unit 203, a second calculation unit 204, and a compensation unit 205; wherein,

[0103] The first model construction unit 201 is configured to construct a robot kinematics nominal value model and a robot kinematics calibration value model according to the kinematics parameters before and after the robot calibration, wherein, the robot kinematics nominal value model and the robot In the kinematics calibration model, the pose vector at the origin of the end effector coordinate system is used to express the pose conversion relationship between the end effector coordinate system and the base coordinate system;

[0104] The first calculation unit 202 is configured to calculate the pose of the origin of the end effector coordinate system according to the kinematic calibration value model under the preset joint space position;

[0105] The second model construction unit 203 is used to construct a parameter compensation constraint optimization model, wherein the parameter compensation constraint optimization model is used to constrain the pose and preset target of the origin of the end effector coordinate system calculated according to the kinematic calibration value model The difference between poses is the smallest;

[0106] The second calculation unit 204 is configured to solve the parameter compensation constraint optimization model, and obtain the joint space coordinate values after optimization and compensation;

[0107] The compensation unit 205 is configured to replace the joint space coordinates obtained by the inverse kinematics calculation of the robot kinematics nominal value model with the joint space coordinate values after optimized compensation.

[0108] In a possible design, the first calculation unit 202 calculates the pose of the origin of the end effector coordinate system according to the kinematic calibration value model under the preset joint space position, which specifically includes executing:

[0109] Calculating, according to the kinematics calibration value model, a homogeneous transformation matrix of the end effector coordinate system corresponding to the preset joint space position relative to the base coordinate system in the Cartesian space;

[0110] According to the homogeneous transformation matrix, the pose of the origin of the end effector coordinate system is determined.

[0111] In a possible design, the parameter compensation constraint optimization model is:

[0112] minω(θ)=||P n -f c (θ)|| 2

[0113]

[0114] Among them, ω(θ) represents the Euclidean norm of the pose error vector; Pn represents the specified pose vector calculated by the end effector coordinate system conversion matrix [p x p y p z α β γ] T; F c (θ) represents the actual pose vector calculated according to the calibration value model and the joint space position θ; with Respectively represent the minimum and maximum limits of joint i. In a mobile joint, this constraint is expressed as the maximum displacement that the joint can move in the axial direction.

[0115] In a possible design, the second calculation unit 204 solves the parameter compensation constraint optimization model to obtain the joint space coordinate values after optimization and compensation, which specifically includes executing:

[0116] The interior point method is used to solve the parameter compensation constraint optimization model, and the joint space coordinate value after optimization and compensation is obtained.

[0117] In a possible design, the compensation unit 205 uses the interior point method to solve the parameter compensation constraint optimization model to obtain the joint space coordinate values after optimization and compensation, which specifically includes executing:

[0118] Construct an unconstrained objective function according to the parameter compensation constraint optimization model:

[0119]

[0120] Where r (k) Is the penalty factor, which is a decreasing sequence of positive numbers, namely r (0)r (1)...r (k) And have g i (θ) is the constraint condition in the parameter compensation constraint optimization model;

[0121] The unconstrained objective function is iterated to obtain the joint space coordinate value after optimization and compensation.

[0122] The computer program product of the method for optimizing and compensating robot kinematic parameter errors provided by the embodiment of the present invention includes a computer-readable storage medium storing program code, and the instructions included in the program code can be used to execute the instructions described in the previous method embodiments For the specific implementation, please refer to the method embodiment, which will not be repeated here.

[0123] Those skilled in the art can clearly understand that, for convenience and concise description, the specific working process of the above-described system, device, and unit can refer to the corresponding process in the foregoing method embodiment, and details are not described herein again.

[0124] In the several embodiments provided in this application, it should be understood that the disclosed system, device, and method may be implemented in other ways. The device embodiments described above are merely illustrative. For example, the division of the units is only a logical function division, and there may be other divisions in actual implementation. For example, multiple units or components may be combined or It can be integrated into another system, or some features can be ignored or not implemented. In addition, the displayed or discussed mutual coupling or direct coupling or communication connection may be through some communication interfaces, indirect coupling or communication connection of devices or units, and may be in electrical, mechanical or other forms.

[0125] If the function is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer readable storage medium. Based on this understanding, the technical solution of the present invention essentially or the part that contributes to the existing technology or the part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including Several instructions are used to make a computer device (which may be a personal computer, a server, or a network device, etc.) execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage media include: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), magnetic disk or optical disk and other media that can store program code .

[0126] The above are only specific embodiments of the present invention, but the protection scope of the present invention is not limited to this. Any person skilled in the art can easily think of changes or substitutions within the technical scope disclosed by the present invention. It should 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.

## PUM

## Description & Claims & Application Information

We can also present the details of the Description, Claims and Application information to help users get a comprehensive understanding of the technical details of the patent, such as background art, summary of invention, brief description of drawings, description of embodiments, and other original content. On the other hand, users can also determine the specific scope of protection of the technology through the list of claims; as well as understand the changes in the life cycle of the technology with the presentation of the patent timeline. Login to view more.