KR20230003233A - Manipulator's multi-load adaptive gravity compensation method, device and control device - Google Patents

Abstract

As a multi-load adaptive gravity compensation method of the manipulator, step S1.1 to build the kinematics model of the manipulator, step S1.2 to reconstruct the gravitational term of the dynamics model, step S1.3 to sample the no-load static position, after each tool installation Step S1.4 of performing static position sampling, respectively, step S1.5 of calculating the parameter value to be corrected for each tool, respectively, step S1.6 of calculating the mass and center of mass of each tool, respectively, the currently installed tool is the flange Step S2.1 of calculating the force applied to the tool, and step S2.2 of compensating for the gravity of the tool are included. The method reduces operation steps and improves efficiency and gravity compensation performance.

Description

Manipulator's multi-load adaptive gravity compensation method, device and control device

Cross-citation of related applications

This application claims priority to a Chinese patent application with Application No. 202010466099.X, titled "Multiple Load Adaptive Gravity Compensation Method for Manipulator" filed with the Patent Office of China on May 28, 2020, the entire contents of which are hereby incorporated by reference. included in this application.

The present invention relates to the field of robot technology, and more particularly, to a gravity compensation method, device, and control device for a manipulator.

As the manipulator manufacturing industry and sensor industry gradually develop, manipulators are no longer used only on production lines, but are gradually starting to be used in various fields of life. Conventional industrial manipulators must set a safety range and strictly prohibit workers from entering their work area to prevent workers from being injured during operation. However, in most application situations in daily life, setting a safety range causes a lot of inconvenience, and the efficiency of cooperative operation between humans and machines is not high. In order to realize truly high-efficiency, high-precision collaborative operation between humans and machines by no longer separating the workspaces of humans and machines, a collaborative manipulator was developed. Since the collaborative manipulator can respond to physical contact between the human body and the manipulator through a contact force detection function, the manipulator and manipulator can share a workspace. The development of collaborative manipulators has greatly expanded the application of manipulators in fields such as home care, educational entertainment, health care, and high-end manufacturing, and various aspects of life can be improved by using the manipulator features of high efficiency, high precision, and high stability. .

In the direct teaching process, the zero-force control technology allows the manipulator to adapt to an external force and move as if it is not affected by the manipulator's own gravity. This technique reduces the labor intensity of direct teaching and improves the smoothness in manipulator control. To ensure that zero force control can still be implemented even when the end effector is clamped on the manipulator, parameters are calibrated for the manipulator itself and the tool respectively, and the mass and center of mass of each arm and tool of the manipulator are accurately calculated using a reverse engineering method. Should be. The manipulator self-parameter calibration technique is described in the paper Identifying the dynamic model used by the KUKA LWR: A reverse engineering approach. (C. Gaz, F. Flacco) and Gravity compensation of KUKA LBR IIWA Through Fast Robot Interface. (C. Hou, Y. Zhao), but there are few calibration data for end effector parameters. In some complex applications, the end effector may need to be replaced to complete the manipulator task. In this case, the adaptive compensation method for the gravity of the tool of the manipulator has become the key to smooth cooperative operation so that zero force control of various end effectors can be implemented.

Current manipulator gravity compensation methods generally first measure the mass of the end effector using a measuring device, and then measure the center of mass of the tool by hanging or supporting it. Then, the measured data is introduced into the control system of the manipulator to implement zero force control of the manipulator so that the control system can compensate for the gravity according to the parameters of the tool. However, when measuring the mass and center of mass of a tool, since the tool is isolated from the system, the measured parameters are likely to be neglected during installation and their influence on mass and center of mass is likely to be neglected, and gravity can only be applied to one tool parameter at a time. Because compensation is possible, the program must be stopped when changing tools, so the efficiency is low when a plurality of tools need to be changed frequently.

One object of the present invention is to provide a multi-load adaptive gravity compensation method, device, control device, and readable storage medium of a manipulator that reduces operation steps and increases efficiency and gravity compensation performance.

In order to realize the above object, the technical solution used by the present invention is as follows.

In a first aspect, an embodiment of the present invention provides a multi-load adaptive gravity compensation method for a manipulator, including the following steps.

Step S1.1 to build the kinematics model of the manipulator.

Step S1.2 to reconstruct the gravity term of the dynamics model.

Step S1.3 sampling the no-load static position.

Step S1.4 to perform static position sampling after each tool installation, respectively.

Step S1.5 of calculating the parameter value to be corrected in each tool respectively.

Step S1.6 to calculate the mass and center of mass of each tool, respectively.

Step S2.1 to calculate the force applied to the flange by the currently installed tool.

Step S2.2 of compensating the gravity of the tool.

In step S1.1 of the feasible method, the manipulator joint coordinate system is constructed using the standard D-H method.

In step S1.3 of the feasible method, the joint position and moment readings are sampled by moving the manipulator to an arbitrary non-specific position in the workspace in an unloaded state.

In step S1.4 of the feasible method, each tool is installed step by step at the end of the manipulator, and step S1.3 is repeated to perform static position sampling.

In step S1.4 of the method that can be implemented, for each tool, after installing the tool at the end of the manipulator, determining the effective working space of the manipulator corresponding to the current tool according to the size of the tool, based on the effective working space Repeat step S1.3 to perform static position sampling for the tool.

In step S1.5 of the feasible method, the sampling data obtained in steps S1.3 and S1.4 are grouped according to tools and sequentially substituted into the gravity term in step S1.2.

In step S1.6 of the feasible method, for each tool, the corrected parameter value when carrying the tool is compared with the parameter value corrected at no load, and the comparison result, the mass of the tool, and the mass of the tool are compared. According to the relationship between the center of gravity, the mass of the manipulator gripper and the center of mass of the gripper, calculate the mass and center of mass of the tool.

In a second aspect, an embodiment of the present invention provides a multi-load adaptive gravity compensation device for a manipulator, the device including:

A model building module for building a kinematics model of the manipulator;

A gravity reconstruction module for reconstructing the gravity term of the dynamics model;

A position sampling module for sampling a no-load static position;

an additional position sampling module for performing static position sampling after each tool installation, respectively;

A parameter correction module for calculating each parameter value to be corrected in each tool;

A mass and center of mass parameter calculation module for calculating the mass and center of mass of each tool, respectively;

An external force calculation module for calculating the force applied to the flange by the currently installed tool;

It includes a gravity compensation module that compensates for the gravity of the tool.

In a feasible implementation, the model construction module configures the manipulator joint coordinate system using a standard D-H method in a process of building a kinematic model of the manipulator.

In an implementable manner, the position sampling module samples joint position and moment readings by controlling the manipulator to move to an arbitrary non-specific position in a working space in an unloaded state of the manipulator.

In a feasible implementation, the position sampling module sets each tool to the end of the manipulator step by step, and then, for each tool, determines the effective working space of the manipulator currently corresponding to the tool according to the size of the tool, and then the effective working space of the manipulator corresponding to the tool. Based on the working space, the joint position and moment readings are sampled by iterative control to move the manipulator to any non-specific position in the corresponding effective working space.

In a feasible implementation, the parameter correction module groups the sampling data obtained by the position sampling module according to tools, sequentially substitutes them into the gravity terms obtained by the gravity reconstruction module, and calculates the parameters to be corrected for each tool. get variable value

In a feasible implementation, the mass and center-of-mass parameter calculation module calculates the mass and center of mass of each tool, for each tool, the corrected parameter value when the tool is transported is converted into a parameter value corrected without load. The variable value is compared, and the mass and center of mass of the tool are calculated according to the comparison result and the relationship between the mass of the tool, the center of mass of the tool, the mass of the manipulator gripper and the center of mass of the gripper.

In a third aspect, an embodiment of the present invention provides a control device for a manipulator including a memory and a processor, wherein a computer program executable by the processor is stored in the memory, and when the processor executes the computer program , the above-described multi-load adaptive gravity compensation method of the manipulator is implemented.

In a fourth aspect, an embodiment of the present invention provides a readable storage medium in which a computer program is stored, and when the computer program is executed by a processor, the aforementioned multi-load adaptive gravity compensation method of the manipulator is executed.

One of the beneficial effects of the embodiments provided by the present invention is as follows. After constructing the manipulator joint coordinate system through the D-H method, the center of mass position of each manipulator is set based on the joint coordinate system. In the original gravity term, the items related to the joint position and the items related to the mass and center of mass are split, and after properly combining the parameters to be corrected in the split process, the divided items are put into two matrices, the product of which still satisfies the original gravity term. have to do it Then, after sampling the static position of the manipulator in a no-load state, each tool is installed at the end of the manipulator, and each static position sampling is performed. After grouping the sampling data into different tools, substituting them into the gravity term, using SVD decomposition to obtain the combined parameter values, and finally using the combined object parameter separation method to obtain the mass and center of mass of the tool from the combined parameters. can be extracted. Based on the calibrated parameters and real-time joint position feedback in an unloaded state, the amount of force applied to the flange by the currently installed end effector can be calculated. According to the external force measured on the flange, the system can know which tool is currently installed on the flange, so it can directly measure the external force after the gravity of the tool is compensated using the corrected parameter value, or use the obtained mass and center of mass to configure the manipulator. can be applied to This method simplifies the operation steps in actual application and greatly improves the smoothness of cooperative operation through the method of obtaining tool parameters by pre-calculation. In addition, by using joint position and moment sensors to calibrate the parameters of the tool, the zero-force control performance is improved as the calibrated tool parameters more closely match the kinematics and dynamics of the manipulator.

1 is a flowchart of a multi-load adaptive gravity compensation method of a manipulator provided by an embodiment of the present invention.
2 is an explanatory diagram of a tool and a manipulator end of a multi-load adaptive gravity compensation method of a manipulator provided by an embodiment of the present invention.
3 is a manipulator multi-tool installation explanatory diagram of the manipulator multi-load adaptive gravity compensation method provided by an embodiment of the present invention.
Fig. 4 is a structural explanatory diagram of a multi-load adaptive gravity compensating device for a manipulator provided by an embodiment of the present invention.

The present invention is explained in more detail below by combining specific examples and drawings of the specification.

Referring to Figures 1-3, in the complete manipulator dynamics equation, we include the terms of inertia, centrifugal force and Criolis force terms, gravity term and friction force term. Here, the term of inertia is related to the joint acceleration, the centrifugal force is related to the Criolis force term and the joint velocity, the frictional force term is also related to the joint velocity, and since both the joint acceleration and the joint velocity are zero when the manipulator is at rest, Studies of static positions can only be performed for the gravitational term. Since the construction of the dynamics equation including the gravity term requires the manipulator's joint positions and moments as inputs, there are certain requirements for the manipulator's hardware. Taking the KUKA LBR Med 7 R800 as an example, its manipulator is a 7-axis collaborative manipulator, so each joint is equipped with high-precision position and moment sensors, which meets the requirements for the manipulator hardware configuration of the present invention. Using the KUKA LBR Med 7 R800 7-axis collaborative manipulator as an example, actual operation is explained.

S1.1: A step of building a kinematics model of the manipulator.

In a feasible manner of this embodiment, when constructing the kinematic model of the manipulator, the joint coordinate system configuration of the manipulator uses the classical D-H method (A Kinematic Notation for Lower-Pair Mechanisms Based on Matrices, J. Denavit, R. S. Hartenberg). . In the case of the KUKA LBR Med 7 R800, its D-H parameter table is as follows.

Table of D-H parameters for the KUKA LBR Med 7 R800 joint number

One

0 340

2

0 0

3

0 400

4

0 0

5

0 400

6

0 0

7 0 0 126

는 커넥팅 로드 회전각을 나타내고,

는 커넥팅 로드 길이를 나타내고,

는 커넥팅 로드 편향 거리를 나타내고,

는 관절각을 나타낸다. 관절 좌표계를 구성한 후, 일정한 규칙으로 각 매니퓰레이터의 질량 중심 위치에 대해 질량 중심 좌표계를 설정한다. 질량 중심 좌표계를 설정할 때 좌표계 원점은 각 암의 질량 중심 위치에 위치하고, 회전 각도는 i+1번째 관절의 회전 각도와 일치하면 된다.S1.2 : 역학 모델의 중력항을 재구성하는 단계.here,

Represents the connecting rod rotation angle,

represents the connecting rod length,

represents the connecting rod deflection distance,

represents the joint angle. After configuring the joint coordinate system, the center of mass coordinate system is set for the position of the center of mass of each manipulator according to a certain rule. When setting the center of mass coordinate system, the origin of the coordinate system is located at the center of mass of each arm, and the rotation angle only needs to coincide with the rotation angle of the i+1th joint. S1.2: Step of reconstructing the gravity term of the dynamics model.

In this embodiment, in the case of a manipulator in a stationary state, the gravitational term is equal to the manipulator joint moment, and the formula is expressed as follows.

, 질량

및 질량 중심

과 관련이 있다. 여기서, 질량

및 질량 중심

는 공구의 보정과 직접 관련되어 추출해야 하기 때문에, 중력항

를 하기와 같이 분할해야 한다.As can be seen from the formula, the gravitational term is the joint angle

, mass

and center of mass

is related to where the mass

and center of mass

is directly related to the calibration of the tool and must be extracted, the gravitational term

should be partitioned as follows.

및 질량 중심

는 중력항에서 결합되므로 단독으로 분리될 수 없음을 쉽게 알 수 있다. 따라서 보정할 매개변수의 행렬

를 구성하기 위해서는 반드시 방정식 성립을 충족시키면서 매개변수를 합리적으로 조합하여 보정된 매개변수를 조합된

및

로 변경해야 한다. 매개변수 조합 시에는

의 매개변수 수를 최소화해야만, 선형 방정식 그룹이 국소 최적해에 빠지는 것을 방지해 더 나은 보정 효과를 얻을 수 있다.mass

and center of mass

It is easy to see that since is combined in the gravitational term, it cannot be separated by itself. Hence, the matrix of parameters to calibrate

In order to construct

and

should be changed to When combining parameters

By minimizing the number of parameters of , we can obtain a better correction effect by preventing the group of linear equations from falling into a local optimum.

S1.3: Step of sampling the no-load static position.

In this embodiment, after confirming that the manipulator is in a no-load state, the manipulator is moved to a non-specific position in the work space, and after each joint is stopped, the positions and moments of the joints are collected. The sampling step is repeated so that the sampling points are distributed as widely as possible over the entire working space. In the sampling process, a few sampling points may be very close to the singular position, and the moment feedback may be inaccurate due to the lack of freedom of the manipulator at the singular position, so these sampling points should be removed from the sampling set.

In Fig. 3, S1.4: each step of performing sampling of the static position after each tool installation.

In this embodiment, after each tool is sequentially installed at the end of the manipulator, motion sampling is performed for each tool by repeating step S1.3, and a data set from which specific positions are removed is saved. As the range of movement of the manipulator is limited due to the constant size of the tool itself, an accident of colliding with the manipulator itself or surrounding obstacles may occur during the movement process. It is necessary to reconfirm the effective working space and design a reasonable movement trajectory so that the points can fill the working space as much as possible and at the same time prevent a collision accident. In a possible implementation manner of this embodiment, for each tool, after installing the tool at the end of the manipulator, the effective working space of the manipulator currently corresponding to the tool is determined according to the size of the tool, and then the effective working space is determined. Based on this, repeat step S1.2 to perform static position sampling for the tool.

S1.5: A step of calculating each parameter value to be corrected for each tool.

In this embodiment, the execution of step S1.5 groups all the collected data according to the tool, and stacks the data of each group into the equation group obtained in S12 as follows.

에 스택되며, 관절 모멘트는

에 스택된다. 행렬

및

스택이 모두 확인되었기 때문에, 보정할 매개변수 행렬

는 SVD 분해를 통해 선형 방정식 세트를 구하는 방식으로 얻을 수 있다. 행렬

는 하기의 형식으로 분해될 수 있다.Here, the joint positions of the data set are matrices

, and the joint moment is

stacked on procession

and

Since the stacks are all checked, the parameter matrix to calibrate

can be obtained by obtaining a set of linear equations through SVD decomposition. procession

can be decomposed in the following form.

및 우측 특이 행렬

는 모두 직교 행렬이기 때문에, 과결정 방정식

의 경우,

，

로 하면 새로운 수식이 있다.Here, the left singular matrix

and right singular matrix

are all orthogonal matrices, the overdeterministic equation

In the case of,

，

, there is a new formula.

는 대각 행렬이며, 대각 원소 전부는 행렬

의 특이값

이고

이기 때문에,

를 구할 수 있다. 마지막으로

에 따라 행렬

의 보정할 매개변수를 구할 수 있다.In the above formula,

is a diagonal matrix, and all diagonal elements are matrices

singular value of

ego

Because it is

can be obtained. finally

matrix according to

parameters to be calibrated can be obtained.

S1.6: Calculating the mass and center of mass of each tool respectively.

In this embodiment, in the process of executing step S1.5, since the parameters must be corrected after the rigid tool is installed at the end of the manipulator, the mass and center of mass of the last segment rigid body among the corrected parameters are substantially the last segment of the manipulator. It is a parameter that combines segments and tools. Therefore, the mass and center of mass of the tool are used to compare the parameters calibrated with the tool and the parameters calibrated with no load, which in turn characterizes the relationship between the mass and center of mass of the tool and the mass and center of mass of the manipulator gripper. It can be determined by combining the multi-body system center of mass formula. Therefore, in an implementable manner of the present embodiment, for each tool, the parameter value corrected when carrying the tool and the parameter value corrected at no load are compared, and the comparison result and the mass of the tool, the mass of the tool According to the relationship between the center of gravity, the mass of the manipulator gripper and the center of mass of the gripper, calculate the mass and center of mass of the tool.

Here, taking the end of the KUKA LBR Med 7 R800 as an example and referring to FIG. 2 , the formula for the center of mass of the multi-body system corresponding to the tool-clamped manipulator system has the following physical properties.

는 공구와 그리퍼가 결합된 질량 중심이고,

및

는 각각 공구의 질량 및 질량 중심이며,

및

는 각각 그리퍼의 질량 및 질량 중심이다. 상기 공식을 보정된 매개변수 수식과 연립하면, 공구의 질량

및 질량 중심

를 구할 수 있다.here,

is the center of mass of the combined tool and gripper,

and

are the mass and center of mass of the tool, respectively,

and

are the mass and center of mass of the gripper, respectively. Combining the above formula with the calibrated parametric formula, the mass of the tool

and center of mass

can be obtained.

At the same time, the present invention designs a system for automatic selection of tool parameters applied to gravity compensation. The system uses the output of the joint moment and position sensor as input to the system, and internally calculates the force that the current tool exerts on the manipulator end, determines the type of clamped tool, and returns the parameter calculated in S1 is applied to complete the gravity compensation. Detailed implementation steps are as follows.

S2.1: Step of calculating the force applied to the flange (end of the manipulator) by the currently installed tool.

를 계산할 수 있다. 실시간 측정된 관절 모멘트

와

를 상쇄하면, 외력에 의한 관절 모멘트

를 얻을 수 있다. 다시 야코비 행렬을 이용해 외력을 관절 공간에서 작업 공간으로 매핑할 수 있고, 매니퓰레이터 말단(플랜지)이 작업 공간에서 받는 외력을 계산할 수 있다.In this embodiment, the calibrated parameters in the no-load state can be used, and the joint moment by the manipulator body at the current position

can be calculated. Real-time measured joint moments

Wow

If , the joint moment due to the external force

can be obtained. Again, the external force can be mapped from the joint space to the working space using the Jacobian matrix, and the external force received by the manipulator end (flange) in the working space can be calculated.

S2.2: A step of compensating for the gravity of the tool.

In this embodiment, the difference between the tools can be reflected in the value of the external force. For example, if the mass difference between the tools is relatively large, the value of the external force in the XYZ directions may be used as a criterion for classifying the tools. If the mass difference of the tool is small and the center of mass difference is large, the torque in the direction of external force ABC can be considered as a criterion for classification. In the case of a tool with a small difference in mass and center of mass, the same corrected parameters can be applied by considering it as one tool, and a better compensation effect can be obtained. In the case of a manipulator whose input tool parameters are automatically accepted as gravity compensation, by directly writing the mass and center of mass of the tool calculated in step S1.6 into the configuration of the manipulator, the manipulator built-in program can calculate the external force applied to the tool. there is. In the case of a manipulator without a gravity compensation function, the external force applied to the current tool can be calculated by directly applying the parameter calibrated in step S1.5. Thus, the external force detected by the manipulator is the external force compensated for by the tool's gravity, and the control strategy using the external force as an input also ignores the influence of the tool. That is, zero force control is achieved.

In addition, referring to FIG. 4, the present invention further provides a multi-load adaptive gravity compensating device 100 for the manipulator, and multiple loads for the manipulator through each function included in the multi-load adaptive gravity compensating device 100. A module implementing the adaptive gravity compensation method is implemented. Here, the multi-load adaptive gravity compensation device 100 includes a model building module 110, a gravity reconstruction module 120, a position sampling module 130, a parameter correction module 140, a mass and center of mass parameter calculation module 150, an external force calculation module 160 and a gravity compensation module 170.

The model building module 110 is configured to build a kinematics model of the manipulator.

The gravity reconstruction module 120 is configured to reconstruct the gravity terms of the dynamics model.

The position sampling module 130 is configured to sample the no-load static position.

The position sampling module 130 is further configured to each perform static position sampling after each tool installation.

The parameter correction module 140 is configured to calculate each parameter value to be corrected in each tool.

The mass and center of mass parameter calculation module 150 is configured to calculate the mass and center of mass of each tool, respectively.

The external force calculation module 160 is configured to calculate the force applied to the flange by the currently installed tool.

The gravity compensation module 170 is configured to compensate for the gravity of the tool.

In a feasible implementation of this embodiment, the model building module 110 configures the manipulator joint coordinate system using a standard D-H method in the process of building a kinematic model of the manipulator.

In a possible implementation of the present embodiment, the position sampling module 130 controls the manipulator to move to an arbitrary non-specific position in a work space in an unloaded state of the manipulator to sample joint position and moment readings.

In a feasible implementation of the present embodiment, the position sampling module 130 installs each tool step by step at the end of the manipulator, and then determines the effective working space of the manipulator currently corresponding to the tool according to the size of the tool for each tool. determined, and then iteratively controlled to move the manipulator to any non-specific position in the effective working space based on the effective working space to sample the joint position and moment readings.

In a feasible implementation of this embodiment, the parameter correction module 140 groups the sampling data obtained by the position sampling module according to tools, and sequentially substitutes them into the gravity term obtained by the gravity reconstruction module to calculate, Obtain the parameter values to be calibrated for each tool.

It should be noted that in the multiple load adaptive gravity compensation device 100 provided by the embodiment of the present invention, its basic principle and technical effect are the same as the above-described multiple load adaptive gravity compensation method, and for brief description, this embodiment Parts not mentioned in the examples may refer to the description of the multi-load adaptive gravity compensation method.

In addition, the present invention further provides a control device for the manipulator, and the control device includes a memory and a processor. Here, the memory may include one or more computer program products, and the computer program product may include various types of readable storage media such as volatile memory and/or non-volatile memory. The volatile memory may include, for example, random access memory and/or cache memory. The non-volatile memory may include, for example, a read-only memory, a hard disk, or a flash memory. One or more computer programs may be stored in the readable storage medium, and a processor may execute the computer program to implement a function represented by the multi-load adaptive gravity compensation method of the manipulator and/or other desired functions. The readable storage medium may store various types of data, such as various application programs and various data used and/or created by the application programs.

The processor may be implemented using at least one hardware form of a digital signal processor, a field programmable gate array, and a programmable logic array, and the processor may be a central processing unit or other form having data processing capability and/or instruction execution capability. It may be a combination of one or more of the processing devices of the above, and may execute a desired function by controlling other components of the control device. The processor may implement functions represented by the computer program by correspondingly executing the computer program stored in the memory.

In a possible implementation of this embodiment, the multi-load adaptive gravity compensating device 100 of the manipulator may be stored in the memory of the control device in the form of software or firmware, and the multiple loads of the memory may be stored by a processor of the control device. As software function modules and computer programs included in the adaptive gravity compensation device 100 are executed, functions corresponding to the multi-load adaptive gravity compensation method of the manipulator are implemented.

In summary, the above scheme configures the manipulator joint coordinate system through the D-H method, and then sets the position of the center of mass of each manipulator based on the joint coordinate system. In the original gravity term, the items related to the joint position and the items related to the mass and center of mass are divided, and after appropriately combining the parameters to be corrected in the division process, the divided items are put into two matrices, the product of which still satisfies the original gravity term. have to do it Then, after sampling the static position of the manipulator in a no-load state, each tool is installed at the end of the manipulator, and each static position sampling is performed. After grouping the sampling data into different tools, substituting them into the gravity term, using SVD decomposition to obtain the combined parameter values, and finally using the combined object parameter separation method to obtain the mass and center of mass of the tool from the combined parameters. can be extracted. Based on the calibrated parameters and real-time joint position feedback under no-load conditions, the amount of force applied to the flange by the currently installed end effector can be calculated. According to the external force measured on the flange, the system can know which tool is currently installed on the flange, so it can directly measure the external force after the gravity of the tool is compensated using the corrected parameter value, or use the obtained mass and center of mass to configure the manipulator. can be applied to This method simplifies the operation steps in actual application and greatly improves the smoothness of cooperative operation through the method of obtaining tool parameters by pre-calculation. In addition, by using joint position and moment sensors to calibrate tool parameters, the zero-force control performance is improved as the calibrated tool parameters more closely match the kinematics and dynamics of the manipulator.

Finally, the point that needs to be explained is that the above embodiments are not intended to limit the protection scope of the present invention, but only to explain the technical solutions of the present invention, although the present invention will be described in detail with reference to optional implementation methods. Even so, those skilled in the art may change or equivalently replace the technical solution of the present invention without departing from the essence and scope of the technical solution of the present invention.

The manipulator multiple load adaptive gravity compensation method, device, control device, and readable storage medium provided by an embodiment of the present invention configure the manipulator joint coordinate system through the D-H method, and then the center of mass position of each manipulator based on the joint coordinate system. set In the original gravity term, the items related to the joint position and the items related to the mass and center of mass are divided, and after appropriately combining the parameters to be corrected in the division process, the divided items are put into two matrices, the product of which still satisfies the original gravity term. have to do it Then, after sampling the static position of the manipulator in a no-load state, each tool is installed at the end of the manipulator, and each static position sampling is performed. After grouping the sampling data into different tools, substituting them into the gravity term, using SVD decomposition to obtain the combined parameter values, and finally using the combined object parameter separation method to obtain the mass and center of mass of the tool from the combined parameters. can be extracted. Based on the calibrated parameters and real-time joint position feedback under no-load conditions, the amount of force applied to the flange by the currently installed end effector can be calculated. According to the external force measured on the flange, the system can know which tool is currently installed on the flange, so it can directly measure the external force after the gravity of the tool is compensated using the corrected parameter value, or use the obtained mass and center of mass to configure the manipulator. can be applied to This method simplifies the operation steps in actual application and greatly improves the smoothness of cooperative operation through the method of obtaining tool parameters by pre-calculation. In addition, by using joint position and moment sensors to calibrate tool parameters, the zero-force control performance is improved as the calibrated tool parameters more closely match the kinematics and dynamics of the manipulator.

Claims (15)

Step S1.1 of building a kinematics model of the manipulator;
Step S1.2 of reconstructing the gravitational term of the dynamics model;
Step S1.3 sampling the no-load static position;
Step S1.4 of performing static position sampling after each tool installation, respectively;
Step S1.5 of calculating each parameter value to be corrected in each tool;
Step S1.6 of calculating the mass and center of mass of each tool, respectively;
Step S2.1 of calculating the force applied to the flange by the currently installed tool;
Compensating for the gravity of the tool, characterized in that it comprises a step S2.2
Manipulator multiple load adaptive gravity compensation method. According to claim 1,
In step S1.1, the standard DH method is used to construct the manipulator joint coordinate system.
Manipulator multiple load adaptive gravity compensation method. According to claim 1 or 2,
In step S1.3, the joint position and moment readings are sampled by moving the manipulator from an unloaded state to an arbitrary non-specific position in the workspace.
Manipulator multiple load adaptive gravity compensation method. According to claim 3,
In step S1.4, each tool is installed step by step at the end of the manipulator, and step S1.3 is repeated to perform static position sampling.
Manipulator multiple load adaptive gravity compensation method. According to claim 4,
In step S1.4, after installing the tool at the end of the manipulator for each tool, determining the effective working space of the manipulator corresponding to the current tool according to the size of the tool, and then, based on the effective working space, step S1. 3 is repeated to perform static position sampling for the tool.
Manipulator multiple load adaptive gravity compensation method. According to any one of claims 1 to 5,
In step S1.5, the sampling data obtained in steps S1.3 and S1.4 are grouped according to the tool and sequentially substituted into the gravity term of step S1.2.
Manipulator multiple load adaptive gravity compensation method. According to any one of claims 1 to 6,
In step S1.6, for each tool, the corrected parameter value at the time of carrying the tool is compared with the parameter value corrected at no load, and the comparison result and the mass of the tool, the center of mass of the tool, and the manipulator gripper are compared. Calculating the mass and center of mass of the tool according to the relationship between the mass of the gripper and the center of mass of the gripper
Manipulator multiple load adaptive gravity compensation method. A model building module for building a kinematics model of the manipulator;
A gravity reconstruction module for reconstructing the gravity term of the dynamics model;
A position sampling module for sampling a no-load static position;
an additional position sampling module for performing static position sampling after each tool installation, respectively;
A parameter correction module for calculating each parameter value to be corrected in each tool;
A mass and center of mass parameter calculation module for calculating the mass and center of mass of each tool, respectively;
An external force calculation module for calculating the force applied to the flange by the currently installed tool;
Characterized in that it comprises a gravity compensation module for compensating for the gravity of the tool
Manipulator multiple load adaptive gravity compensation device. According to claim 8,
Characterized in that the model building module configures the manipulator joint coordinate system using the standard DH method in the process of building the kinematics model of the manipulator.
Manipulator multiple load adaptive gravity compensation device. According to claim 8 or 9,
Characterized in that the position sampling module samples joint position and moment readings by controlling the manipulator to move to an arbitrary non-specific position in the workspace in an unloaded state of the manipulator.
Manipulator multiple load adaptive gravity compensation device. According to claim 10,
After installing each tool step by step at the end of the manipulator, the position sampling module determines the effective working space of the manipulator currently corresponding to the tool according to the size of the tool for each tool, and then, based on the effective working space, Iteratively controlling the manipulator to move to any non-specific position in its effective working space, sampling the joint position and moment readings.
Manipulator multiple load adaptive gravity compensation device. According to any one of claims 8 to 11,
The parameter correction module groups the sampling data obtained by the position sampling module according to tools, calculates by sequentially substituting them into the gravity terms obtained by the gravity reconstruction module, and obtains parameter values to be corrected for each tool. characterized
Manipulator multiple load adaptive gravity compensation device. According to any one of claims 8 to 12,
In the process of calculating the mass and center of mass of each tool, the mass and center of mass parameter calculation module compares the corrected parameter value for each tool with the parameter value corrected when the tool is carried with the parameter value corrected without load, , calculating the mass and center of mass of the tool according to the comparison result and the relationship between the mass of the tool, the center of mass of the tool, the mass of the manipulator gripper and the center of mass of the gripper.
Manipulator multiple load adaptive gravity compensation device. It includes a memory and a processor, and a computer program that can be executed by the processor is stored in the memory,
When the processor executes the computer program, the multi-load adaptive gravity compensation method of the manipulator according to any one of claims 1 to 7 is implemented.
Manipulator's control unit. computer programs are stored,
When the computer program is executed by the processor, the multi-load adaptive gravity compensation method of the manipulator according to any one of claims 1 to 7 is executed.
A readable storage medium.

Images