CN112975971B - A kind of robot inertia force compensation method - Google Patents

Abstract

The invention discloses a method for compensating inertial force of a robot, including obtaining acceleration, obtaining inertial force, and establishing a mapping between inertial force and acceleration: _Finert = [M]·a, calculating inertial force compensation parameter [M], and making compensation The force F _comp =‑F _inert , the compensation torque T _comp =‑T _inert , is used for inertia force compensation with load measurement, in which, by making the robot end move in a straight line, and establishing a linear motion trajectory parameter equation, the form is simple and easy to implement At the same time, the acceleration changes according to the sine law, the measurement is efficient and concise, which ensures the smooth and simple control of the movement; in addition, by measuring the inertial force F _inert , using the measured acceleration a, the mapping relationship between the inertial force and the acceleration is established , so as to accurately compensate the inertial force, improve the real-time calculation efficiency, and do not need to measure the shape and material of the robot end effector, avoiding the complexity and calculation error of integral calculation of the moment of inertia.

Description

Robot inertia force compensation method

Technical Field

The invention relates to the technical field of robot processing, in particular to a robot inertia force compensation method.

Background

The processing precision of the robot is closely related to acceleration and deceleration motion control and dynamics control of the robot. At present, when an industrial robot is applied to industries such as machining and assembly, the contact force between a robot terminal tool or a workpiece and the external environment needs to be accurately sensed so as to ensure the flexibility of operation. In the existing application, a six-component sensor is usually selected, which can measure three-dimensional orthogonal force and three-dimensional orthogonal moment, three-dimensional orthogonal acceleration and three-dimensional orthogonal angular acceleration in any force system in space; under dynamic conditions, the moment measured by the robot end sensor comprises load gravity, load inertia force and external contact force applied to the load. In a closed-loop force control algorithm, a robot controller corrects the motion of the robot at the next moment according to the force feedback and the pose feedback of a sensor;

in the process of movement of the robot mechanical arm, an inertial mass effect can be generated, which is caused by acceleration generated by a load end (a clamp, an actuator, a sensor and the like) of the sensor; the inertia force can be generated under the influence of the acceleration, and the measurement value of the sensor can be directly influenced;

the traditional load end inertial mass is obtained by performing integral calculation by using a theoretical mechanics theory according to the rigid body shape and the material of a robot end actuator (including a sensor and a clamp), and when the overall shape of the actuator is more regular, the calculation result is higher in precision, but in general, the actuator is formed by assembling a plurality of different components, and the material of each component is also larger in difference, so that the calculation error of the inertial mass M of the actuator is larger; therefore, the inertia force error obtained by multiplying the acceleration value a measured by the sensor by M obtained by theoretical calculation is also large, so that a robot inertia force compensation method is urgently needed to solve the problem in order to meet the requirement of the measurement accuracy of the machining contact force.

Disclosure of Invention

The invention provides a robot inertial force compensation method which can eliminate the influence of load inertial force on a sensor measurement value so as to accurately analyze the external contact force applied to a load, and solves the problems in the prior art.

In order to achieve the purpose, the invention provides the following technical scheme: a robot inertial force compensation method, comprising:

s1, obtaining acceleration, planning the tail end of the robot to do linear motion, determining the starting point and the end point of the robot, and establishing a motion trail parameter equation as

Sampling all time step points in the linear motion, and calculating the acceleration a (t) of all the time step points;

s2, obtaining an inertia force, comprising:

a. method for obtaining sensor three-dimensional force F by using no-load motion of robot_meas＝(F_x，F_y，F_z) And three-way moment T_meas＝(T_x，T_y，T_z)；

b. Identifying the gravity magnitude G and the centroid position C ═ C (C) by using a gravity compensation algorithm_x,C_y,C_z)；

c. Dividing gravity and gravity moment along sensor coordinate systemHydrolyzing to obtain G '═ G'_x,G′_y,G′_z) And T ═ T'_x,T′_y,T′_z)；

d. Measuring a sensor value F in a sensor coordinate system_measAnd T_measRespectively subtracting the gravity value G 'and the gravity moment T' to obtain an inertia force F_inertAnd moment of inertia T_inert；

S3, establishing a mapping between inertia force and acceleration: f_inert＝[M]A, calculating an inertial force compensation parameter [ M]；

S4, make the compensation force F_comp＝-F_inertCompensating the moment T_comp＝-T_inertAnd the method is used for compensating the inertia force with load measurement.

Preferably, in step S1, the starting point Ps of the linear motion of the robot end is determined as (x)_s,y_s,z_s) And end point Pe ═ x_e,y_e,z_e) The straight-line distance between two points is

The total time of the linear motion of the tail end of the robot is T, wherein all points on a straight line section between the two points are positioned in the robot working space.

Preferably, in step S1, the trajectory parameter equation S is subjected to isoparameter sampling on t, and the acceleration a (t) is calculated as:

a. sampling parameters of the straight line s (t) to obtain discrete parameters

And a sequence of discrete points

Wherein A is 2 pi L/T²，ω＝2π/T；

b. Calculating speedDegree-discrete point sequence

Deriving t by the linear equation s (t):

wherein at each time sampling point t_iIn the above-mentioned manner,

c. calculating acceleration discrete point sequences

Deriving t by the velocity equation v (t):

wherein at each time sampling point t_iIn the above-mentioned manner,

preferably, in step S2, the robot performs idle motion, that is, only the end effector of the robot moves along the above-mentioned linear track, and does not need to contact with the real workpiece, so as to ensure that the sensor measurement value only includes gravity and inertia force.

Preferably, in step S2, when the gravity and the moment of gravity are resolved along the sensor coordinate system, the direction of the gravity of the robot end effector does not coincide with the Z direction of the sensor coordinate system, and the gravity is projected in three directions of the sensor coordinate system according to the posture of the robot end effector.

Preferably, in step S2, F_measAnd T_measRespectively subtracting the gravity value G 'and the gravity moment T', and specifically:

F_{inert_x}＝F_{meas_x}-G′_x；

F_{inert_y}＝F_{meas_y}-G′_y；

F_{inert_z}＝F_{meas_z}-G′_z；

T_{inert_x}＝T_{meas_x}-T′_x；

T_{inert_y}＝T_{meas_y}-T′_y；

T_{inert_z}＝T_{meas_z}-T′_z。

preferably, in step S3, the discrete series of measurements are formed by a least squares method

And

fitting to obtain an inertia force compensation parameter M, wherein M is mass corresponding to translational motion, M is rotational motion, and M is rotational inertia, and in the F ═ Ma, when R is>(n +1), the least squares method is an over-determined equation whose solution is: m ═ a^Ta)^-1a^TF, wherein the dimension of F is [ R ]]Dimension of a [ R ]]And R is the number of rows of data.

Compared with the prior art, the invention has the beneficial effects that:

in the invention, the tail end of the robot makes linear motion, and a linear motion track parameter equation is established, so that the method has a simple form and is convenient to implement, and meanwhile, the acceleration sine law changes, the measurement is efficient and simple, and the smooth motion is ensured and is simple and controlled; in addition, by measuring the inertial force F_inertThe measured acceleration a is utilized to establish the mapping relation between the inertia force and the acceleration, so that the inertia force is accurately compensated, the real-time calculation efficiency is improved, the shape and the material of the end effector of the robot are not required to be measured, the complexity and the calculation error of integral calculation of the rotational inertia are avoided, the influence of the action of the load inertia force on the measurement value of the sensor is eliminated, and the external contact force applied to the load is accurately analyzed.

Drawings

The accompanying drawings, which are included to provide a further understanding of the invention and are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and together with the description serve to explain the principles of the invention and not to limit the invention.

In the drawings:

FIG. 1 is a general flowchart of a robot inertial force compensation method;

FIG. 2 is a graph of displacement versus time for a straight line trajectory;

FIG. 3 is a graph of velocity versus time for a straight-line trajectory;

FIG. 4 is a graph of acceleration versus time for a straight line trajectory.

Detailed Description

The preferred embodiments of the present invention will be described in conjunction with the accompanying drawings, and it will be understood that they are described herein for the purpose of illustration and explanation and not limitation.

Example (b): as shown in fig. 1, a robot inertial force compensation method for accurately compensating an inertial force by measuring the inertial force and using a measured acceleration to establish a mapping relationship between the inertial force and the acceleration, includes:

s1, obtaining acceleration, planning the tail end of the robot to do linear motion, determining the starting point and the end point of the robot, and establishing a motion trail parameter equation as

Sampling all time step points in the linear motion, and calculating the acceleration a (t) of all the time step points;

first, the starting point Ps of the linear motion of the robot end is determined as (x)_s,y_s,z_s) And end point Pe ═ x_e,y_e,z_e) Calculating the linear distance between the two

Determining the total linear motion time of the tail end of the robot as T, wherein the distance between the two points is equal to the distance between the two points and is located in the robot working space, in the embodiment, L is 0.8m, and T is 8 s;

performing equal parameter sampling on t by using a track parameter equation s, and calculating an acceleration a (t) specifically as follows:

a. as shown in FIG. 1, for the displacement curve, the parameter sampling is performed on the straight line s (t) to obtain the discrete parameter

And a sequence of discrete points

Wherein A is 2 pi L/T²，ω＝2π/T；

b. As shown in FIG. 3, a velocity discrete point sequence is calculated for the velocity profile

Deriving t by the linear equation s (t):

wherein at each time sampling point t_iIn the above-mentioned manner,

c. as shown in FIG. 4, a sequence of acceleration discrete points is calculated for the acceleration curve

Deriving t by the velocity equation v (t):

wherein at each time sampling point t_iIn the above-mentioned manner,

s2, obtaining an inertia force, comprising:

a. method for obtaining sensor three-dimensional force F by using no-load motion of robot_meas＝(F_x，F_y，F_z) And three-way moment T_meas＝(T_x，T_y，T_z) (ii) a In order to ensure that the measured value of the sensor only contains gravity and inertia force, the robot does no-load motion, namely, the tail end of the robot only moves along the linear track without contacting with a real workpiece;

b. identifying the gravity magnitude G and the centroid position C ═ C (C) by using a gravity compensation algorithm_x,C_y,C_z) Wherein the Gravity Compensation algorithm adopts an algorithm disclosed in the document Bias Estimation and Gravity Compensation For Force-Torque Sensors;

c. decomposing gravity and gravity moment along the sensor coordinate system to obtain G '═ G'_x,G′_y,G′_z) And T ═ T'_x,T′_y,T′_z)；

When the gravity and the gravity moment are decomposed along a sensor coordinate system, wherein the gravity direction of the robot end effector is inconsistent with the Z direction of the sensor coordinate system, and the gravity is projected to three directions of the sensor coordinate system according to the posture of the robot end effector;

d. measuring a sensor value F in a sensor coordinate system_measAnd T_measRespectively subtracting the gravity value G 'and the gravity moment T' to obtain an inertia force F_inertAnd moment of inertia T_inertThe method specifically comprises the following steps:

F_{inert_x}＝F_{meas_x}-G′_x；

F_{inert_y}＝F_{meas_y}-G′_y；

F_{inert_z}＝F_{meas_z}-G′_z；

T_{inert_x}＝T_{meas_x}-T′_x；

T_{inert_y}＝T_{meas_y}-T′_y；

T_{inert_z}＝T_{meas_z}-T′_z。

s3, establishing a mapping between inertia force and acceleration: f_inert＝[M]A, calculating an inertial force compensation parameter [ M]；

Wherein, in step S3, the discrete sequence of measurement values is determined by the least squares method

And

fitting to obtain an inertia force compensation parameter M, wherein M is mass corresponding to translational motion, M is rotational motion, and M is rotational inertia, and in the F ═ Ma, when R is>(n +1), the least squares method is an over-determined equation whose solution is: m ═ a^Ta)^-1a^TF, wherein the dimension of F is [ R ]]Dimension of a [ R ]]R is the number of rows of data;

s4, make the compensation force F_comp＝-F_inertCompensating the moment T_comp＝-T_inertThe method is used for inertial force compensation with load measurement, and the calculation efficiency of compensation implementation is improved.

Finally, it should be noted that: although the present invention has been described in detail with reference to the foregoing embodiments, it will be apparent to those skilled in the art that changes may be made in the embodiments and/or equivalents thereof without departing from the spirit and scope of the invention. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims (7)

1. A robot inertial force compensation method is characterized by comprising the following steps:

s1, obtaining acceleration, planning the tail end of the robot to do linear motion, determining the starting point and the end point of the robot, and establishing a motion trail parameter equation as

Sampling all time step points in the linear motion, and calculating the acceleration a (t) of all the time step points;

s2, obtaining an inertia force, comprising:

a. method for obtaining sensor three-dimensional force F by using no-load motion of robot_meas＝(F_x，F_y，F_z) And three-way moment T_meas＝(T_x，T_y，T_z)；

b. Identifying the gravity magnitude G and the centroid position C ═ C (C) by using a gravity compensation algorithm_x,C_y,C_z)；

c. Decomposing gravity and gravity moment along the sensor coordinate system to obtain G '═ G'_x,G′_y,G′_z) And T ═ T'_x,T′_y,T′_z)；

d. Measuring a sensor value F in a sensor coordinate system_measAnd T_measRespectively subtracting the gravity value G 'and the gravity moment T' to obtain an inertia force F_inertAnd moment of inertia T_inert；

S3, establishing a mapping between inertia force and acceleration: f_inert＝[M]A, calculating an inertial force compensation parameter [ M]；

S4, make the compensation force F_comp＝-F_inertCompensating the moment T_comp＝-T_inertAnd the method is used for compensating the inertia force with load measurement.

2. The method of claim 1, wherein the method further comprises: in step S1, the start point Ps of the linear motion of the robot end is determined as (x)_s,y_s,z_s) And end point Pe ═ x_e,y_e,z_e) The straight-line distance between two points is

The total time of the linear motion of the tail end of the robot is T, wherein the straight line between the two pointsAll points on the line segment are positioned in the robot work space.

3. The method of claim 2, wherein the method further comprises: in step S1, the trajectory parameter equation S is subjected to equal parameter sampling on t, and the acceleration a (t) is calculated as:

a. sampling parameters of the straight line s (t) to obtain discrete parameters

And a sequence of discrete points

Wherein A is 2 pi L/T²，ω＝2π/T；

b. Computing a sequence of velocity discrete points

Deriving t by the linear equation s (t):

wherein at each time sampling point t_iIn the above-mentioned manner,

c. calculating acceleration discrete point sequences

Deriving t by the velocity equation v (t):

wherein at each time sampling point t_iIn the above-mentioned manner,

4. the method of claim 1, wherein the method further comprises: in step S2, the robot performs idle motion, that is, only the actuator at the end of the robot moves along the above-mentioned linear track, and does not need to contact with the real workpiece, so as to ensure that the sensor measurement value only includes gravity and inertia force.

5. The method of claim 1, wherein the method further comprises: in step S2, when the gravity and the moment of gravity are resolved along the sensor coordinate system, the direction of the gravity of the robot end effector does not coincide with the Z direction of the sensor coordinate system, and the gravity is projected in three directions of the sensor coordinate system according to the posture of the robot end effector.

6. The method of claim 1, wherein the method further comprises: in step S2, F_measAnd T_measRespectively subtracting the gravity value G 'and the gravity moment T', and specifically:

F_{inert_x}＝F_{meas_x}-G′_x；

F_{inert_y}＝F_{meas_y}-G′_y；

F_{inert_z}＝F_{meas_z}-G′_z；

T_{inert_x}＝T_{meas_x}-T′_x；

T_{inert_y}＝T_{meas_y}-T′_y；

T_{inert_z}＝T_{meas_z}-T′_z。

7. the method of claim 1, wherein the method further comprises: in step S3, the discrete sequence of measurements is determined by a least squares method

And

fitting to obtain an inertia force compensation parameter M, wherein M is mass corresponding to translational motion, M is rotational motion, and M is rotational inertia, and in the F ═ Ma, when R is>(n +1), the least squares method is an over-determined equation whose solution is: m ═ a^Ta)^-1a^TF, wherein the dimension of F is [ R ]]Dimension of a [ R ]]And R is the number of rows of data.

Images