Dynamic zero point correction and gravity compensation method for six-dimensional force sensor

Abstract

The application provides a dynamic zero point correction and gravity compensation method of a six-dimensional force sensor, which comprises the following steps: 1, determining a pose change matrix of a robot end coordinate system relative to a base coordinate system M 0 T1; 2, determining a coordinate transformation relationship between an end tool and a robot base coordinate; 3, determining force components of end tool gravity in three directions of a sensor coordinate system XYZ; 4, determining a gravity compensation value, and performing zero point correction and compensation error. The application can compensate the force of the end tool gravity on the sensor; after data processing by using an average filtering algorithm, zero point correction can be performed, so that the influence of external interference factors, operation errors and the like can be reduced; the application is suitable for gravity compensation and zero point correction under various postures and motion states of a robot, and is suitable for various types of mechanical arms, end tools and six-dimensional force sensors; and the application is suitable for zero point correction and gravity compensation in a static state and in a robot motion process.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of sensors, in particular to a dynamic zero-point correction and gravity compensation method for a six-dimensional force sensor. BACKGROUND

[0002] In some dangerous and special environments, human-computer interaction technology helps to complete the work that humans are difficult to complete but need human intelligence to judge, and contact force perception and feedback are one of the important technologies to complete such work. Force feedback data directly affects human operation and judgment. When a robot needs to contact operation, the perception of contact force is crucial to the accuracy of operation, and a six-dimensional force sensor is usually installed between the robot end and the end tool to perceive the contact force.

[0003] The six-dimensional force sensor can measure force and torque information in XYZ three directions, and the accuracy of the force data of the six-dimensional force sensor directly affects the performance of the system. The reading of the six-dimensional force sensor installed between the robot end and the end tool is usually not zero in the empty state, so the six-dimensional force sensor needs to be zero-point corrected. The force and torque components of the end tool gravity in each direction change with the robot motion, and the error of the six-dimensional force sensor is different in different postures, so the end tool gravity compensation and zero-point correction of the six-dimensional force sensor in any posture are necessary conditions to ensure the accuracy of the six-dimensional force sensor data and the key technology of human-computer interaction. SUMMARY

[0004] To solve at least one of the above technical problems, the present application provides a dynamic zero-point correction and gravity compensation method for a six-dimensional force sensor.

[0005] A dynamic zero-point correction and gravity compensation method for a six-dimensional force sensor, comprising:

[0006] Step 1: determining the posture change matrix of the robot end coordinate system relative to the base coordinate system

[0007] Step 2: determining the coordinate transformation relationship between the end tool and the robot base coordinate system;

[0008] Step 3: determining the force components of the end tool gravity in the XYZ axis directions of the sensor coordinate system;

[0009] Step 4: determining the gravity compensation value, and performing zero-point correction and error compensation.

[0010] Preferably, step 1 comprises:

[0011] Step 11: establishing the base coordinate system and the joint coordinate systems of the robot by using the D-H method;

[0012] Step 12: define the parameters and variables of each joint connecting rod of the robot, and bring them into the connecting rod transformation matrix to obtain the pose transformation matrix of each joint connecting rod of the robot;

[0013] Step 13: multiply the pose transformation matrices associated with each joint of the robot to obtain the pose change matrix of the robot end coordinate system relative to the base coordinate system

[0014] Preferably, the step 2 comprises:

[0015] Step 21: define the sensor coordinate system S and the end tool coordinate system H;

[0016] Step 22: determine the coordinate transformation relationship between the sensor and the robot end;

[0017] Step 23: determine the coordinate transformation relationship between the end tool and the sensor;

[0018] Step 24: determine the coordinate transformation relationship between the end tool and the robot base coordinate.

[0019] Preferably, in step 21, in the sensor coordinate system S, the center of the upper end plane of the sensor is taken as the coordinate origin, the Z axis passes through the coordinate origin and is perpendicular to the upper end plane of the sensor, the positive direction of the Z axis is vertically upward, the X axis and the Y axis are perpendicular to each other at the coordinate origin and are in the upper end plane of the sensor, and the right-hand rule is followed; the torque direction also follows the right-hand rule.

[0020] Preferably, in step 21, in the end tool coordinate system H, the mass center of the end tool is taken as the coordinate origin, the Z axis passes through the coordinate origin and is perpendicular to the bottom contact surface of the end tool, the positive direction of the Z axis is vertically upward, the plane where the X axis and the Y axis are located passes through the coordinate origin and is parallel to the bottom contact surface of the end tool, and the X axis and the Y axis are perpendicular to each other at the coordinate origin and follow the right-hand rule.

[0021] Preferably, in step 22, wherein represents the distance between the coordinate origin of the sensor coordinate system and the coordinate origin of the robot end coordinate system in the Z axis direction.

[0022] Preferably, in step 23, the coordinate transformation matrix between the end tool and the sensor is wherein and respectively represent the projections of the coordinate origin of the end tool coordinate system H to the X axis, the Y axis and the Z axis of the sensor coordinate system S; the rotation matrix between the end tool coordinate system and the sensor coordinate system

[0023] Any of the above solutions is preferably that in step 24, wherein, represents a rotation matrix of the end tool relative to the base coordinate of the robot, represents a position translation matrix of the end tool coordinate system relative to the base coordinate system, n x , o x , a x respectively represent rotation amounts of the X axis of the end tool coordinate system relative to the XYZ three axes of the base coordinate system, n y , o y , a y respectively represent rotation amounts of the Y axis of the end tool coordinate system relative to the XYZ three axes of the base coordinate system, n z , o z , a z respectively represent rotation amounts of the Z axis of the end tool coordinate system relative to the XYZ three axes of the base coordinate system, and respectively represent position translation amounts of the point in the end tool coordinate system relative to the XYZ three axes of the base coordinate system.

[0024] Any of the above solutions is preferably that in step 3, the force components of the gravity G of the end tool in the XYZ axes of the sensor coordinate system wherein, G x , G y , G z respectively represent the force components of the gravity G of the end tool in the XYZ axes of the sensor coordinate system, represents the force components of the gravity G of the end tool in the XYZ axes of the end tool coordinate system, f 0 = |0 0G| T represents the force components of the gravity G of the end tool in the XYZ axes of the base coordinate system.

[0025] Any of the above solutions is preferably that in step 4, the determined gravity compensation value is: wherein, represents the moment components of the gravity G of the end tool in the XYZ axes of the sensor coordinate system, respectively represent the projections of the coordinate origin of the end tool coordinate system H to the X axis, Y axis and Z axis of the sensor coordinate system S.

[0026] Any of the above solutions is preferably that in step 4, the zero point correction is performed by an average filtering algorithm to compensate for errors.

[0027] The dynamic zero point correction and gravity compensation method of the six-dimensional force sensor has the following beneficial effects:

[0028] 1. The gravity of the end tool can be compensated for the force on the sensor;

[0029] 2. By using the average filtering algorithm to process the data and then performing zero correction, the influence of external interference factors, manual operation errors, etc. can be reduced, the error range can be reduced, and the result is more accurate;

[0030] 3. It is suitable for gravity compensation and zero correction in various postures and motion states of the robot, and is suitable for various models of mechanical arms, end tools and six-dimensional force sensors;

[0031] 4. It is also suitable for zero correction and gravity compensation in a stationary state and during robot motion, and can compensate and correct in real time;

[0032] 5. The results of gravity compensation and zero correction are obtained through the transformation matrix in each coordinate system, and during the calculation process, the number of parameter variables is small, the error is smaller, the calculation result is more accurate, and the calculation amount is small. BRIEF DESCRIPTION OF DRAWINGS

[0033] Figure 1 It is a flowchart of a preferred embodiment of the dynamic zero correction and gravity compensation method of the six-dimensional force sensor according to the present application.

[0034] Figure 2 It is a flowchart of step 1 of the embodiment shown in Figure 1 The dynamic zero correction and gravity compensation method of the six-dimensional force sensor according to the present application.

[0035] Figure 3 It is a flowchart of step 2 of the embodiment shown in Figure 1 The dynamic zero correction and gravity compensation method of the six-dimensional force sensor according to the present application.

[0036] Figure 4 It is a first schematic diagram of the sensor coordinate system of the embodiment shown in Figure 1 The dynamic zero correction and gravity compensation method of the six-dimensional force sensor according to the present application.

[0037] Figure 5 It is a second schematic diagram of the sensor coordinate system of the embodiment shown in Figure 1 The dynamic zero correction and gravity compensation method of the six-dimensional force sensor according to the present application.

[0038] Figure 6 It is a schematic diagram of the end tool coordinate system of the embodiment shown in Figure 1 The dynamic zero correction and gravity compensation method of the six-dimensional force sensor according to the present application. DETAILED DESCRIPTION

[0039] In order to better understand the present application, the present application is described in detail below in conjunction with specific embodiments.

[0040] Embodiment

[0041] As Figure 1 shown, a dynamic zero-point correction and gravity compensation method of a six-dimensional force sensor, comprising:

[0042] Step 1: determine the pose change matrix of the robot end coordinate system relative to the base coordinate system

[0043] Step 2: determine the coordinate transformation relationship between the end tool and the robot base coordinate;

[0044] Step 3: determine the force component of the end tool gravity in the three directions of the sensor coordinate system XYZ axis.

[0045] Step 4: determine the gravity compensation value, and perform zero-point correction and error compensation.

[0046] Specifically, step 1 comprises:

[0047] Step 11: establish the base coordinate system and the coordinate system of each joint of the robot by using the D-H method;

[0048] Step 12: define the parameters and variables of each joint link of the robot, and bring them into the link transformation matrix to obtain the pose transformation matrix of each joint link of the robot;

[0049] Step 13: multiply the pose transformation matrices associated with each joint of the robot to obtain the pose change matrix of the robot end coordinate system relative to the base coordinate system

[0050] More specifically, the robot has M links, and the M links are connected through M-1 joints. The base coordinate system and the M-1 joint coordinate systems of the robot are established according to the D-H method. Then the parameters and variables of the M links of the robot are defined, including the link length and the link rotation angle of each link; the parameters and variables between the adjacent two links include the joint offset and the joint angle; for the link transformation matrix between the i-1th link and the ith link satisfies the relationship:

[0051] where θ i represents the joint angle of link i, b i-1 represents the link length of link i-1, ɑ i-1 represents the link rotation angle of link i-1, d i represents the joint offset of link i; by bringing the defined parameters and variables of each joint link of the robot into the above relationship, the pose transformation matrix of each joint link of the robot can be obtained Finally, the pose transformation matrix of each joint of the robot is multiplied to obtain the pose transformation matrix of the robot end coordinate system relative to the base coordinate system

[0052] In the embodiment, step 2 comprises:

[0053] Step 21: defining a sensor coordinate system S and an end tool coordinate system H;

[0054] Step 22: determining the coordinate transformation relationship between the sensor and the robot end;

[0055] Step 23: determining the coordinate transformation relationship between the end tool and the sensor;

[0056] Step 24: determining the coordinate transformation relationship between the end tool and the robot base coordinate.

[0057] Specifically, in step 21, as shown in Figure 4 and Figure 5 , in the sensor coordinate system S, the center of the upper end plane of the sensor is taken as the coordinate origin, the Z axis passes through the coordinate origin and is perpendicular to the upper end plane of the sensor, the positive direction of the Z axis is vertically upward, the X axis and the Y axis are perpendicular to each other at the coordinate origin and are in the upper end plane of the sensor, and the right-hand rule is met; the torque direction also meets the right-hand rule. As shown in Figure 6 , in the end tool coordinate system H, the mass center of the end tool is taken as the coordinate origin, the Z axis passes through the coordinate origin and is perpendicular to the bottom contact surface of the end tool, the positive direction of the Z axis is vertically upward, the plane on which the X axis and the Y axis are located passes through the coordinate origin and is parallel to the bottom contact surface of the end tool, the X axis and the Y axis are perpendicular to each other at the coordinate origin, and the right-hand rule is met.

[0058] When the sensor is installed, the end plane thereof is parallel to the end plane of the robot, and the sensor is connected through a flange, so the XYZ three axes of the sensor coordinate system S are coaxial and same-direction with the XYZ three axes of the robot end coordinate system. Let the distance between the coordinate origin of the sensor coordinate system S and the coordinate origin of the robot end coordinate system in the Z axis direction be Then, in step 22, the coordinate transformation matrix between the sensor and the robot end is

[0059]

[0060] When the end tool is installed, the end plane thereof is parallel to the upper end plane of the sensor, is connected through a flange, and the attitude of the end tool coordinate system H is the same as that of the sensor coordinate system S. Let the projections of the coordinate origin of the end tool coordinate system H onto the X axis, the Y axis and the Z axis of the sensor coordinate system S be and Then in step 23, the coordinate transformation matrix between the sensor and the end effector... Rotation matrix between end-effector coordinate system H and sensor coordinate system S

[0061] In step 24, in, This represents the rotation matrix of the end effector relative to the robot's base coordinates. n represents the translation matrix of the end-tool coordinate system relative to the base coordinate system. x o x a x These represent the rotation of the X-axis of the end-effector coordinate system relative to the X, Y, and Z axes of the base coordinate system, respectively. y o y a y These represent the rotation of the Y-axis of the end-effector coordinate system relative to the X, Y, and Z axes of the base coordinate system, respectively. z o z a z These represent the rotation of the Z-axis of the end-effector coordinate system relative to the X, Y, and Z axes of the base coordinate system. and These represent the translation amounts of a point in the end-tool coordinate system relative to the X, Y, and Z axes of the base coordinate system. It should be noted that... In the middle, located The 0 below represents a 1×3 matrix consisting entirely of 0s.

[0062] In this embodiment, preferably, in step 3, the gravity of the end-effector is denoted as G, and the direction of G is along the negative direction of the Z-axis in the base coordinate system. The center of gravity of the end-effector is at the origin of the coordinate system H of the end-effector. Therefore, in the base coordinate system, the force components of the gravity G of the end-effector in the X, Y, and Z directions can be expressed as follows: 0 f = |0 0G| T During robot motion, as the pose of the end effector changes, the components of its gravity G in the XYZ directions of the end effector coordinate system H also change. Correspondingly, the component of the end effector's gravity G acting on the six-dimensional force sensor also changes, and the zero-position value changes continuously. Therefore, before determining the external force acting on the end effector, the components of the end effector's gravity G in the XYZ directions of the sensor coordinate system S can be determined first to perform zero-position correction and gravity compensation on the sensor. Since the rotation matrix of the end effector coordinate system relative to the base coordinate system is... The force components of the gravity G of the end effector in the XYZ directions of the base coordinate system can be expressed as: 0 f = |0 0G| TThe force components of the gravity G of the end tool in the XYZ directions in the end tool coordinate system H are Further, the force components of the gravity G of the end tool in the XYZ directions in the sensor coordinate system S are It should be noted that G x , G y , and G z respectively represent the force components of the gravity G of the end tool in the XYZ directions in the sensor coordinate system S, and since Therefore,

[0063] Since the gravity center of the end tool is at the coordinate origin of the end tool coordinate system H, the moment components of the gravity G of the end tool in the XYZ directions in the end tool coordinate system H are all 0, which can be represented as Further, the components of the gravity G of the end tool in the XYZ directions in the end tool coordinate system H are represented as Further, the moment components of the gravity G of the end tool in the XYZ directions in the sensor coordinate system S are wherein respectively represent the projections of the coordinate origin of the end tool coordinate system H to the X, Y, and Z axes of the sensor coordinate system S. Finally, the gravity compensation value determined in step 4, i.e., the components of the gravity G of the end tool in the XYZ directions in the sensor coordinate system S, are

[0064]

[0065] Further, after the gravity compensation of the six-dimensional force sensor in step 4, due to measurement errors and external factor interference, the zero value of the sensor is not 0, but has a certain error, therefore, the average filtering algorithm is used for zero correction to compensate for the error. Specifically, the data of the six-dimensional force sensor in the first n seconds are taken as the baseline, and the baseline is solved through a loop for compensation. In this embodiment, a certain type of six-dimensional force sensor is experimented, after the gravity compensation, the error range of the zero value of the sensor is between 0.0003 and 0.2, the average filtering algorithm is used for zero correction by taking 7500 data of the sensor in the first 3 seconds, and after the correction, the error of the zero value of the sensor is reduced to between 10 -6 and 10 -2 .

[0066] It should be noted that the above examples are only used to illustrate the technical solutions of the present application, but not to limit the present application; although the foregoing examples have been described in detail, those skilled in the art should understand that the technical solutions recorded in the foregoing examples can be modified, or some or all of the technical features thereof can be replaced equivalently, and these replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the present application.

Claims

1. A method for dynamic zero-point calibration and gravity compensation of a six-dimensional force sensor, characterized in that: include: Step 1: Determine the pose transformation matrix of the robot's end effector coordinate system relative to the base coordinate system. ; Step 2: Determine the coordinate transformation relationship between the end effector and the robot's base coordinates; Step 3: Determine the force components of the end-effector's gravity in the three directions of the XYZ axes of the sensor coordinate system; Step 4: Determine the gravity compensation value and perform zero-point correction to compensate for errors; Step 1 includes: Step 11: Establish the base coordinate system and the coordinate systems of each joint of the robot using the DH method; Step 12: Define the parameters and variables of each joint link of the robot, and substitute them into the link transformation matrix to obtain the pose transformation matrix of each joint link of the robot; Step 13: Multiply the pose transformation matrices associated with each joint of the robot to obtain the pose transformation matrix of the robot's end effector coordinate system relative to the base coordinate system. ; Step 2 includes: Step 21: Define the sensor coordinate system S and the end-effector coordinate system H; Step 22: Determine the coordinate transformation relationship between the sensor and the robot end effector; Step 23: Determine the coordinate transformation relationship between the end effector and the sensor; Step 24: Determine the coordinate transformation relationship between the end effector and the robot's base coordinates; In step 21, in the sensor coordinate system S, the center of the upper plane of the sensor is taken as the origin. The Z-axis passes through the origin and is perpendicular to the upper plane of the sensor, with the positive direction of the Z-axis being vertically upward. The X-axis and Y-axis lie in the upper plane of the sensor and intersect perpendicularly at the origin, conforming to the right-hand rule. The direction of the torque also conforms to the right-hand rule. In the end-effector coordinate system H, the center of mass of the end-effector is taken as the origin. The Z-axis passes through the origin and is perpendicular to the bottom contact surface of the end-effector, with the positive direction of the Z-axis being vertically upward. The plane containing the X-axis and Y-axis passes through the origin and is parallel to the bottom contact surface of the end-effector. The X-axis and Y-axis intersect perpendicularly at the origin, conforming to the right-hand rule. In step 22, the coordinate transformation matrix between the sensor and the robot end effector... ,in This represents the distance along the Z-axis between the origin of the sensor coordinate system and the origin of the robot end effector coordinate system. In step 23, the coordinate transformation matrix between the end effector and the sensor ,in , and These represent the projections from the origin of the end-effector coordinate system H onto the X, Y, and Z axes of the sensor coordinate system S, respectively; and the rotation matrix between the end-effector coordinate system and the sensor coordinate system. ; In step 24, the coordinate transformation matrix between the end effector and the robot's base coordinates. ,in, This represents the rotation matrix of the end effector relative to the robot's base coordinates. This represents the translation matrix of the end-tool coordinate system relative to the base coordinate system. , , These represent the rotation of the X-axis of the end-effector coordinate system relative to the X, Y, and Z axes of the base coordinate system, respectively. , , These represent the rotation of the Y-axis of the end-effector coordinate system relative to the X, Y, and Z axes of the base coordinate system. , , These represent the rotation of the Z-axis of the end-effector coordinate system relative to the X, Y, and Z axes of the base coordinate system. , and These represent the translation amounts of a point in the end-tool coordinate system relative to the X, Y, and Z axes of the base coordinate system, respectively. In step 3, the force components of the gravity G of the end effector in the three directions of the XYZ axes of the sensor coordinate system are... ,in, , , These represent the force components of the end effector's gravity G in the X, Y, and Z axes of the sensor coordinate system. This represents the force components of the gravity G of the end-effector in the X, Y, and Z axes of the end-effector coordinate system. This represents the force components of the gravity G of the end effector in the three directions of the X, Y, and Z axes in the base coordinate system; In step 4, the determined gravity compensation value is: ,in, This represents the torque components of the end effector's gravity G in the X, Y, and Z axes of the sensor coordinate system. , , These represent the projections from the origin of the end-effector coordinate system H onto the X, Y, and Z axes of the sensor coordinate system S, respectively; zero-point correction is performed using an average filtering algorithm to compensate for errors.

Images