Error compensation and processing method for robot end model dynamic contact force measurement

Abstract

This invention provides an error compensation and processing method for dynamic contact force measurement of a robot end effector, specifically for processing the error of a six-dimensional force sensor output during robot force control tasks. First, the six-dimensional force sensor is dynamically calibrated offline, and a dynamic decoupling-compensator is designed based on the calibration experimental data. Then, the robot is controlled to automatically calibrate online the six-dimensional force sensor bias, end effector mass parameters, six-dimensional force sensor installation error, and end effector inertia parameters. Finally, based on the parameters of the six-dimensional force sensor dynamic decoupling-compensator and the online automatic calibration, real-time error compensation processing is performed on the six-dimensional force sensor measurement output signal during robot force control tasks, including bias removal, dynamic decoupling-compensation, end effector gravity compensation, end effector inertia compensation, and model coordinate system transformation, thereby obtaining accurate dynamic contact force on the end effector.

Description

Technical Field

[0001] This invention relates to robot dynamic force sensing technology, specifically a method for error compensation and processing of dynamic contact forces on the end effector model in robot force control tasks. Background Technology

[0002] In the process of robots performing certain control tasks, such as ground zero-gravity motion simulation for spacecraft on-orbit servicing missions and force control tasks such as remote surgical operations by human-robot collaborative robots, it is necessary to accurately measure the dynamic contact force on the end effector model. This is generally achieved by installing a six-dimensional force sensor between the robot's end effector and the model. Specifically, the support end of the six-dimensional force sensor is mounted on the end flange of the robot, and the corresponding end effector model is mounted on the load end of the six-dimensional force sensor. End effector models typically include various robot end tools and experimental models, such as end-effectors in robot force control operations and satellite models in satellite capture ground zero-gravity simulation experiments. However, during the actual execution of control tasks by the robot, the six-dimensional force / torque data measured by the six-dimensional force sensor installed at the robot's end effector contains multiple error components, mainly including: six-dimensional force sensor bias, six-dimensional force sensor installation error, end effector model gravity / torque, end effector model inertial force / torque, and six-dimensional force sensor dynamic characteristic error. Existing robot end-effector force measurement and compensation techniques typically only consider the bias and gravity / torque errors in the six-dimensional force sensor measurement results. A few studies have performed inertial compensation on the six-dimensional force sensor output, but most neglect the installation error and dynamic characteristic error of the six-dimensional force sensor, leading to inaccurate dynamic contact force measurements. For example, the installation error angle of the six-dimensional force sensor can cause errors in the actual attitude matrix of the sensor, thus affecting the coordinate system transformation of model gravity compensation and contact force. Poor dynamic characteristics of the six-dimensional force sensor itself can result in significant dynamic errors in the measurement of dynamic contact force, as well as model gravity compensation and inertial force compensation. Therefore, in applications requiring high accuracy in measuring the dynamic contact force of the robot end-effector model, comprehensive error compensation of the six-dimensional force sensor measurement results is necessary to improve measurement accuracy. As mentioned earlier, the dynamic measurement error of the six-dimensional force sensor mainly originates from the sensor's installation error, its own dynamic characteristics, and the characteristic parameters of the end-effector model. Therefore, to compensate for various error components in the six-dimensional force sensor measurement results in real time during robot control, it is necessary to obtain the characteristic parameters of the six-dimensional force sensor and the end-effector model in advance. Existing methods often require offline calibration of the characteristic parameters of the six-dimensional force sensor and end-effector model through a series of experiments. This results in repeated, time-consuming, and labor-intensive calibration work when the six-dimensional force sensor or end-effector model is replaced, leading to low efficiency.

[0003] To address the aforementioned issues, this invention proposes an error compensation and processing method for dynamic contact force measurement of a robot end effector. This method involves obtaining the dynamic characteristics of a six-dimensional force sensor through offline calibration and acquiring other characteristic parameters of the six-dimensional force sensor and the end effector through online automatic robot calibration. In actual dynamic contact force measurement of the robot end effector, accurate dynamic contact force data is obtained by performing offset compensation, dynamic characteristic compensation, gravity compensation, inertial force compensation, and force coordinate system transformation on the measurement results of the six-dimensional force sensor. Summary of the Invention

[0004] This invention addresses the problem in robot force control tasks where the actual output of a six-dimensional force sensor installed on the robot's end effector flange contains multiple error components, including the six-dimensional force sensor bias, dynamic error, end effector gravity / torque, and end effector inertial force / torque, which interfere with the measurement of the dynamic contact force on the end effector. The invention proposes a method for compensating and processing the error in the six-dimensional force sensor output signal to accurately measure the dynamic contact force on the end effector.

[0005] The technical solution adopted in this invention is as follows: First, an offline dynamic calibration experiment is conducted on a six-dimensional force sensor with an end-effector model. Based on the dynamic calibration experiment data, a dynamic decoupling-compensator for the six-dimensional force sensor is designed to dynamically correct the dynamic errors caused by the characteristics of the six-dimensional force sensor itself. Then, the robot automatically controls the online automatic calibration of the characteristic parameters of the six-dimensional force sensor and the end-effector model, including the installation error of the six-dimensional force sensor, the bias of the six-dimensional force sensor, the mass parameters of the end-effector model, and the inertial parameters of the end-effector model. Finally, based on the characteristic parameters of the six-dimensional force sensor dynamic decoupling-compensator and the online automatic calibration, the robot force control task sequentially performs real-time error compensation processing on the measured output signal of the six-dimensional force sensor, including bias removal, dynamic decoupling-compensation, end-effector model gravity compensation, end-effector model inertial compensation, and model coordinate system transformation, thereby obtaining accurate dynamic contact force data of the end-effector model.

[0006] The technical process of this invention is as follows: offline dynamic calibration of the six-dimensional force sensor 1 → online automatic calibration of characteristic parameters of the model contact force measurement system 2 → real-time error compensation processing of dynamic contact force 3. The method of this invention uses a world coordinate system C0, a robot base coordinate system C1, a robot end flange coordinate system C2, a sensor coordinate system C3, and a model coordinate system C4 for auxiliary explanation. Specifically, the world coordinate system C0 and the robot base coordinate system C1 have the same axis directions and are both fixed coordinate systems; the robot end flange coordinate system C2 is located on the mounting flange of the robot end; the sensor coordinate system C3 is the measurement coordinate system of the six-dimensional force sensor; the origin of the model coordinate system C4 is located at the centroid of the end model; the relationship between the robot end flange coordinate system C2, the sensor coordinate system C3, and the model coordinate system C4 is fixed by the installation relationship between the robot end flange, the six-dimensional force sensor, and the end model, and all move with the robot end; typically, when installing the six-dimensional force sensor and the end model, it is preferable that the coordinate axis directions of the sensor coordinate system C3 and the model coordinate system C4 are the same as the coordinate axis directions of the robot end flange coordinate system C2.

[0007] The aforementioned offline dynamic calibration 1 of the six-dimensional force sensor involves conducting a dynamic calibration experiment on a six-dimensional force sensor with an end model. This yields dynamic calibration experimental data for the six-dimensional force sensor with the end model, and based on this data, a dynamic decoupling-compensator for the six-dimensional force sensor is designed for real-time dynamic decoupling and compensation of the actual measurement output of the six-dimensional force sensor. The process includes: Dynamic calibration experiment 4 → Dynamic decoupling-compensator design 5.

[0008] Dynamic calibration experiment 4 involves fixing the support end of the six-dimensional force sensor onto a rigid calibration platform with stiffness significantly greater than that of the sensor's sensing element. An end-effector model is mounted on the measuring end of the six-dimensional force sensor. A dynamic excitation force is applied to the end-effector model, and the dynamic excitation force data and the output data of the six-dimensional force sensor are recorded to complete the dynamic calibration experiment. The dynamic excitation force is determined based on the actual loading conditions of the end-effector model, with a step excitation force being preferred. When a step excitation force is inconvenient to apply to the model, an impact excitation force is applied to the end-effector model by striking it with a hammer.

[0009] The design of the dynamic decoupling-compensator 5 is to design a dynamic decoupling-compensator Gc(z) that can simultaneously remove the dynamic error of the main channel and the dynamic error of the interdimensional coupling channel of the six-dimensional force sensor based on the dynamic excitation force data and the output data of the six-dimensional force sensor obtained in the dynamic calibration experiment 4, using existing technologies such as the diagonal dominance dynamic decoupling-compensation method or the iterative dynamic decoupling-compensation method.

[0010] The online automatic calibration of the characteristic parameters of the model contact force measurement system is 2, which is the automatic calibration of the installation error of the six-dimensional force sensor, the offset of the six-dimensional force sensor, the mass parameters of the end model and the inertial parameters of the end model controlled by the robot. The automatic calibration process is as follows: calibration of the six-dimensional force sensor offset and model mass parameters 6 → calibration of the six-dimensional force sensor installation error 7 → calibration of the end model inertial parameters 8.

[0011] The calibration of the six-dimensional force sensor bias and model quality parameters (6) involves the automatic simultaneous calibration of the six-dimensional force sensor bias F under robot control. bias =[V1,V2,V3,V4,V5,V6] T End model quality m f and the position of the model's centroid in the sensor coordinate system C3, Pm = [x m ,y m ,z m ] T The automatic calibration control process for the robot is as follows.

[0012] Step 1: Control the robot to make the u-axis of the robot end flange coordinate system C2 horizontal, where u is the X-axis, Y-axis, or Z-axis of coordinate system C2.

[0013] Step 2: Control the robot to rotate the model around the u-axis of the robot's end flange coordinate system C2 by a step angle φ for one revolution. After remaining stationary for time t0 at each step position, start recording sensor data for time t1 and average the sensor data within time t1 to obtain N. b The average values ​​of the six-dimensional force sensor output data at each step angle position: Fu_1, Fu_2…Fu_N b Where φ is divisible by 180, N b =360 / φ.

[0014] The sensor bias F can then be obtained from the following formula. bias .

[0015]

[0016] For N b The six-dimensional force sensor data, including data Fu_i and Fu_(N) with rotation angles differing by 180°, b The following processing is performed on / 2+i).

[0017]

[0018] Where i = 1, 2, 3, ..., N b / 2.

[0019] Then calculate the end model quality g is the acceleration due to gravity.

[0020] For N b / 2 groups The model mass m obtained by calibrating the model under the condition of rotating the model around the u-axis of the end flange coordinate system C2 is obtained by taking the average. f u .

[0021] The coordinates of the centroid of the end model in the six-dimensional force sensor coordinate system C3 are Pm = [x m ,y m ,z m ] T It is easy to see that for the six-dimensional force sensor output component F caused by gravity of the end model, G =[G x G y G z M x M y M z ]have:

[0022]

[0023] Convert to matrix form:

[0024] Using the above six-dimensional force data Fu_1, Fu_2…Fu_N b (N b =360 / φ) and subtract the bias amount The force and torque data are combined into the following matrix according to the above rules.

[0025]

[0026] Based on the above formula, the position of the model's centroid in the sensor coordinate system C3, obtained by calibration under the condition that the model rotates around the u-axis of coordinate system C2, is obtained using the least squares method:

[0027] Pm u =(G n T ·G n ) -1 ·G n T ·M gn .

[0028] Step 3: Select different axes from the X, Y, and Z axes in the end flange coordinate system C2 as the u-axis, and repeat the above robot control and data processing flow to calculate F. X bias F Y bias F Z bias m fX m f Y m f Z Pm X Pm Y Pm Z And by averaging them, we get:

[0029]

[0030]

[0031]

[0032] Then the model's gravity G = mf·g, where g is the acceleration due to gravity.

[0033] The installation error calibration of the six-dimensional force sensor is to calibrate the installation error ΔR = [Δα, Δβ, Δλ] between the coordinate system C3 of the six-dimensional force sensor and the coordinate system C2 of the robot end flange.

[0034] The automatic calibration process for Δα is as follows:

[0035] Step 1: Control the robot to make the Z-axis of the six-dimensional force sensor coordinate system C3 vertical and the X-axis horizontal;

[0036] Step 2: Control the robot to rotate the end effector model around the X-axis of the six-dimensional force sensor coordinate system C3 by an angle θ, preferably 45°. After it comes to rest, record the output data of the six-dimensional force sensor: F2 = [F x2 ,F y2 ,F z2 M x2 M y2 M z2 ] T ;

[0037] Step 3: Repeat Step 1;

[0038] Step 4: Control the robot to rotate the end effector model around the X-axis of the six-dimensional force sensor coordinate system C3 by an angle of -θ; record the output data of the six-dimensional force sensor F3 = [F] while it is stationary. x3 ,F y3 ,F z3 M x3 M y3 M z3 ] T ;

[0039] Step 5: Based on the Z-axis force data of F2 and F3, we can obtain:

[0040]

[0041] From the above formula, we can calculate: Δα=arcsin[(F z2 -F z3 ) / 2G·sin(θ)].

[0042] Step 6: Select different θ angles, repeat the above steps to calculate Δα, and take the average as the final installation error angle Δα.

[0043] The automatic calibration process for Δβ is as follows:

[0044] Step 1: Control the robot to make the Z-axis of the six-dimensional force sensor coordinate system C3 vertical and the Y-axis horizontal;

[0045] Step 2: Control the robot to rotate the end effector model around the Y-axis of the six-dimensional force sensor coordinate system C3 by an angle θ, preferably 45°. After it comes to rest, record the output data of the six-dimensional force sensor: F4 = [F x4 ,F y4 ,F z4 M x4 M y4 M z4 ] T ;

[0046] Step 3: Repeat Step 1;

[0047] Step 4: Control the robot to rotate the end effector model around the Y-axis of the six-dimensional force sensor coordinate system C3 by an angle of -θ; record the output data of the six-dimensional force sensor F5 = [F] while it is stationary. x5 ,F y5 ,F z5 M x5 M y5 M z5 ] T ;

[0048] Step 5: Based on the Z-axis force data of F4 and F5, we can obtain:

[0049]

[0050] From the above formula, we can calculate: Δβ=arcsin[(F z4 -F z5 ) / 2G·sin(θ)].

[0051] Step 6: Select different θ angles, repeat the above steps to calculate Δβ, and take the average as the final installation error angle Δβ.

[0052] The automatic calibration process for Δλ is as follows:

[0053] Step 1: Control the robot to make the X-axis vertical and the Z-axis horizontal in the six-dimensional force sensor coordinate system C3;

[0054] Step 2: Control the robot to rotate the end effector model around the Z-axis of the six-dimensional force sensor coordinate system C3 by an angle θ, preferably 45°. After it comes to rest, record the output data of the six-dimensional force sensor: F6 = [F x6 ,F y6 ,F z6 M x6 M y6 M z6 ] T ;

[0055] Step 3: Repeat Step 1;

[0056] Step 4: Control the robot to rotate the end effector model around the Y-axis of the six-dimensional force sensor coordinate system C3 by an angle of -θ; record the output data of the six-dimensional force sensor F7 = [F] while it is stationary. x7 ,F y7 ,F z7 M x7 M y7 M z7 ] T ;

[0057] Step 5: Based on the X-axis force data of F6 and F7, we can obtain...

[0058]

[0059] From the above formula, we can calculate: Δλ=arcsin[(F x6 -F x7 ) / 2G·sin(θ)];

[0060] Step 6: Select different θ angles, repeat the above steps to calculate Δλ, and take the average as the final installation error angle Δλ.

[0061] The end-effector inertial parameter calibration (8) is the inertial tensor matrix I automatically calibrated by the robot. The robot controls the end-effector to perform m uniformly accelerated rotations around its center of mass, where m ≥ 3, and the matrix A is composed of multiple sets of experimental angular acceleration vectors. m The rank is ≥3. The terminal model acceleration can be obtained using the following two methods:

[0062] Method 1: Set the centroid of the end effector model as the origin of the robot tool coordinate system, and have the robot output the acceleration A of the end effector model in the model coordinate system C4. dp =[A x A y A z A rx A ry A rz ] T ;

[0063] Method 2: Install the inertial measurement unit (IMU) on the support end of the six-dimensional force sensor, acquire the IMU output, and convert it into acceleration A in the model coordinate system C4. dp =[A x A y A z A rx A ry A rz ] T .

[0064] The automatic calibration process for the end-effector inertial parameter I is as follows:

[0065] Step 1: Put the robot into the standby position;

[0066] Step 2: Control the model with angular acceleration A r1 The model performs an accelerated rotational motion for duration t2 from the standby position, and the angular acceleration A of the model during the motion is recorded. rx A ry A rz Simultaneously, the output data of the six-dimensional force sensor is recorded and subjected to bias removal and gravity compensation to obtain F. i1 =[F ix1 ,F iy1 ,F iz1 M ix1 M iy1 M iz1 ]

[0067] Step 3: Update the control value of the model's angular acceleration. The new angular acceleration is linearly independent of the angular velocity vector in the previous steps. Repeat the above steps m times.

[0068] Step 4: Utilize m sets of six-dimensional force signals F i1 F i2 …F im The torque data form a matrix M im The matrix A is composed of m sets of terminal model acceleration data. m Then we have:

[0069]

[0070] Multiply the above expression by M on the left. im -1 Right multiply by A m -1 Derived: M im -1 ·I=-A m -1 ,make W m =-A m -1 According to the least squares method, I = (Um T ·U m ) -1 ·U m T ·W m .

[0071] The dynamic contact force real-time error compensation process 3 is based on the previously calibrated end-model mass m. f The dynamic force signals measured in real time by the six-dimensional force sensor during the robot's force-controlled motion are processed by the following parameters: center of mass position Pm, installation error of the six-dimensional force sensor ΔR = [Δα, Δβ, Δλ], and inertial tensor matrix I of the end-effector model. This is to obtain the external dynamic contact force experienced by the end-effector model in its coordinate system C4, which is used for force feedback in the robot's force-controlled tasks. The process includes: signal preprocessing 9 → dynamic error correction 10 → end-effector model gravity compensation 11 → end-effector model inertia compensation 12 → coordinate system transformation 13.

[0072] Signal preprocessing 9 involves subtracting the six-dimensional force sensor bias F from the original six-dimensional force sensor signal L0. bias The signal is then low-pass filtered to remove high-frequency components, resulting in the preprocessed signal L1.

[0073] Dynamic error correction 10 refers to the real-time dynamic decoupling and compensation of the preprocessed signal L1 using the dynamic decoupling-compensator designed in the offline dynamic calibration 1 of the six-dimensional force sensor. The transfer function of the dynamic decoupling-compensator is G. c (z) is used to remove the dynamic measurement error caused by the dynamic characteristics of the six-dimensional force sensor, resulting in the dynamic compensation signal L2 = G. c (z)·L1.

[0074] End-effector gravity compensation 11 refers to obtaining the attitude matrix T of the end-effector in the world coordinate system C0 based on the attitude parameters of the end-effector returned by the robot. f Then, the attitude matrix T of the six-dimensional force sensor in the world coordinate system C0 is calculated. s Then, based on the end-effector model's quality parameters and attitude matrix T... s Calculate the gravity component F of the end model G And provide compensation.

[0075] Based on the actual installation of the robot, the rotation matrix T of the robot's base coordinate system C1 around the world coordinate system C0 can be obtained. r Based on the actual installation of the six-dimensional force sensor, the rotation matrix T of the six-dimensional force sensor coordinate system C3 relative to the robot end flange coordinate system C2 can be obtained. s-f Considering the installation error angle ΔR = [Δα, Δβ, Δλ] of the six-dimensional force sensor, the attitude matrix of the six-dimensional force sensor coordinate system C3 in the world coordinate system C0 is:

[0076]

[0077] From the above formula, the direction cosine of the Z-axis of the world coordinate system C0 in the six-dimensional force sensor coordinate system C3 can be obtained as [N z O z A z Since the gravity direction of the end-effector is parallel to the negative Z-axis of the world coordinate system C0, the direction cosine of the gravity direction of the end-effector in the six-dimensional force sensor coordinate system C3 is -[N]. z O z A z Based on this, the output error of the six-dimensional force sensor caused by the gravity of the end model is obtained:

[0078]

[0079] Subsequently, the gravity compensation signal L3 = L2 - F from the six-dimensional force sensor can be obtained. G .

[0080] End-effector inertia compensation 12 refers to calculating the inertial force / torque F of the end-effector model in the six-dimensional force sensor coordinate system C3 based on the real-time motion acceleration and inertial parameters of the robot end-effector model. inertia =[F ix ,F iy ,F iz M ix M iy M iz ] T And compensation is performed. The end-effector acceleration can be obtained using the following two methods:

[0081] Method 1: Set the centroid of the end effector model as the origin of the robot tool coordinate system, and have the robot output the acceleration A of the end effector model in coordinate system C4. dp =[A x A y A z A rx A ry A rz ] T ;

[0082] Method 2: Install the inertial measurement unit (IMU) on the support end of the sensor, acquire the IMU output, and convert it into acceleration A in coordinate system C4. dp =[A x A y A z A rx A ry A rz ] T .

[0083] The inertial force / moment generated by the end model in the end model coordinate system C4 is:

[0084]

[0085] Based on the coordinates of the centroid of the end model in the six-dimensional force sensor coordinate system, Pm = [x] m ,y m ,z m ] T Then the inertial force / torque F of the end model in the six-dimensional force sensor coordinate system C3 is... inertia for:

[0086]

[0087] Then the external dynamic contact force F of the end model measured by the six-dimensional force sensor can be obtained. sc =L3-F inertia =[F ex ,F ey ,F ez M ex M ey M ez ].

[0088] Coordinate system transformation 13 refers to transforming the external dynamic contact force F in the six-dimensional force sensor coordinate system C3. sc Transform to the end-effector model coordinate system C4 for subsequent robot control tasks; then the external dynamic contact force F in the end-effector model coordinate system C4... mc for:

[0089]

[0090] The advantages of this invention are: It addresses the problem of measuring the dynamic contact force of the end-effector model using a six-dimensional force sensor installed at the robot's end effector in robot control tasks. A complete method for force signal error compensation and processing is proposed, fully considering the potentially negligible installation error and dynamic error of the six-dimensional force sensor in force measurement, to further optimize gravity compensation results and the dynamic measurement accuracy of external contact force. Furthermore, a simple and reliable automated robot calibration method is provided for the constant data required in force signal processing, enabling the robot to automatically calibrate the parameters needed for end-effector model contact force measurement error compensation, thereby improving work efficiency. Attached Figure Description

[0091] Figure 1 This is a flowchart of the technical process of the method of the present invention;

[0092] Figure 2 This is a schematic diagram of the robot end-effector contact force measurement system and its coordinate system according to the method of the present invention;

[0093] Figure 3 This is a schematic diagram of the installation of the six-dimensional force sensor in the dynamic calibration experiment of a specific embodiment of the present invention;

[0094] Figure 4 This is a flowchart illustrating the automatic calibration of the six-dimensional force sensor bias and end-model quality characteristic parameters according to a specific embodiment of the present invention.

[0095] Figure 5 This is a schematic diagram of the gravity components of the end model in the six-dimensional force sensor coordinate system of a specific embodiment of the present invention;

[0096] Figure 6 This is a flowchart of the automatic calibration process for the installation error of a six-dimensional force sensor according to a specific embodiment of the present invention;

[0097] Figure 7 This is a flowchart of the automatic calibration process for the end-model inertia tensor matrix according to a specific embodiment of the present invention;

[0098] Figure 8 This is a schematic diagram of the real-time error compensation processing flow of the dynamic contact force measurement results of the six-dimensional force sensor according to a specific embodiment of the present invention. Detailed Implementation

[0099] The present invention will be further described below with reference to the accompanying drawings:

[0100] The design concept of this invention is as follows: Addressing the problem of accurately sensing the dynamic contact force of the end-effector model during robot motion control using a six-dimensional force sensor mounted on the robot's end flange, the invention first dynamically calibrates the six-dimensional force sensor and designs its dynamic-decoupling compensator accordingly. Then, the robot control automatically calibrates the parameters required for error compensation sequentially. Finally, during the robot's force control task, the error components of the six-dimensional force sensor are compensated in real time, and coordinate system transformation is performed to obtain accurate dynamic contact force data on the end-effector model. Specifically, a dynamic calibration experiment was first conducted on the six-dimensional force sensor. Based on the dynamic calibration experiment data, a dynamic-decoupling compensator for the six-dimensional force sensor was designed to dynamically decouple and compensate the output signal of the six-dimensional force sensor, correcting the dynamic error caused by the dynamic characteristics of the six-dimensional force sensor itself. Then, through automated calibration, the bias of the six-dimensional force sensor, the gravity of the end model, the centroid position of the end model, the installation error angle of the six-dimensional force sensor, and the inertial parameters of the end model were automatically calibrated in sequence. Finally, based on the designed dynamic-decoupling compensator and the error compensation parameters of the robot control automatic calibration, the force signal measured and output by the six-dimensional force sensor was preprocessed, dynamically corrected, compensated for the gravity of the end model, compensated for the inertia of the end model, and transformed into the model coordinate system in the actual robot force control application to obtain accurate dynamic contact force data.

[0101] The flowchart of the technical solution of the present invention is as follows: Figure 1 As shown, the specific process is as follows: offline dynamic calibration of the six-dimensional force sensor 1 → online automatic calibration of characteristic parameters of the model contact force measurement system 2 → real-time error compensation processing of dynamic contact force 3. The method of this invention uses the world coordinate system C0, robot base coordinate system C1, robot end flange coordinate system C2, sensor coordinate system C3, and model coordinate system C4 for auxiliary explanation, as follows: Figure 2 As shown in the diagram, the world coordinate system C0 and the robot base coordinate system C1 have the same axis directions and are both fixed coordinate systems; the robot end flange coordinate system C2 is located on the mounting flange of the robot end; the sensor coordinate system C3 is the measurement coordinate system of the six-dimensional force sensor; the origin of the model coordinate system C4 is located at the centroid of the end model; the relationship between the robot end flange coordinate system C2, the sensor coordinate system C3, and the model coordinate system C4 is fixed by the installation relationship between the robot end flange, the six-dimensional force sensor, and the end model, and all move with the robot end; typically, when installing the six-dimensional force sensor and the end model, it is preferable that the coordinate axis directions of the sensor coordinate system C3 and the model coordinate system C4 are the same as the coordinate axis directions of the robot end flange coordinate system C2.

[0102] The aforementioned offline dynamic calibration 1 of the six-dimensional force sensor involves conducting a dynamic calibration experiment on a six-dimensional force sensor with an end model. This yields dynamic calibration experimental data for the six-dimensional force sensor with the end model, and based on this data, a dynamic decoupling-compensator for the six-dimensional force sensor is designed for real-time dynamic decoupling and compensation of the actual measurement output of the six-dimensional force sensor. The process includes: Dynamic calibration experiment 4 → Dynamic decoupling-compensator design 5.

[0103] Dynamic calibration experiment 4 involves fixing the support end of the six-dimensional force sensor onto a rigid calibration platform with stiffness much greater than that of the sensor's sensing element. The measuring end of the six-dimensional force sensor is fitted with an end-effector model, such as... Figure 3 As shown, a dynamic excitation force is applied to the end model, and the dynamic excitation force data and the output data of the six-dimensional force sensor are recorded to complete the dynamic calibration experiment of the six-dimensional force sensor. The dynamic excitation force is determined according to the actual loading conditions of the end model, with a step excitation force being preferred; when it is inconvenient to apply the step excitation force to the model, an impact excitation force is applied to the end model by striking it with a force hammer.

[0104] The design of the dynamic decoupling-compensator 5 is to design a dynamic decoupling-compensator Gc(z) that can simultaneously remove the dynamic error of the main channel and the dynamic error of the interdimensional coupling channel of the six-dimensional force sensor based on the dynamic excitation force data and the output data of the six-dimensional force sensor obtained in the dynamic calibration experiment 4, using existing technologies such as the diagonal dominance dynamic decoupling-compensation method or the iterative dynamic decoupling-compensation method.

[0105] The online automatic calibration of the characteristic parameters of the model contact force measurement system is 2, which is the automatic calibration of the installation error of the six-dimensional force sensor, the offset of the six-dimensional force sensor, the mass parameters of the end model and the inertial parameters of the end model controlled by the robot. The automatic calibration process is as follows: calibration of the six-dimensional force sensor offset and model mass parameters 6 → calibration of the six-dimensional force sensor installation error 7 → calibration of the end model inertial parameters 8.

[0106] The calibration of the six-dimensional force sensor bias and model quality parameters (6) involves the automatic simultaneous calibration of the six-dimensional force sensor bias F under robot control. bias =[V1,V2,V3,V4,V5,V6] T End model quality m f and the position of the model's centroid in the sensor coordinate system C3, Pm = [x m ,y m ,z m ] T Its robot automatic calibration control process is as follows: Figure 4 As shown:

[0107] Step 1: Control the robot to make the u-axis of the robot end flange coordinate system C2 horizontal, where u is the X-axis, Y-axis, or Z-axis of coordinate system C2.

[0108] Step 2: Control the robot to rotate the model around the u-axis of the robot's end flange coordinate system C2 by a step angle φ for one revolution. After remaining stationary for time t0 at each step position, start recording sensor data for time t1 and average the sensor data within time t1 to obtain N. b The average values ​​of the six-dimensional force sensor output data at each step angle position: Fu_1, Fu_2…Fu_N b Where φ is divisible by 180, N b =360 / φ.

[0109] The sensor bias F can then be obtained from the following formula. bias .

[0110]

[0111] For N b The six-dimensional force sensor data, including data Fu_i and Fu_(N) with rotation angles differing by 180°, b The following processing is performed on / 2+i).

[0112]

[0113] Where i = 1, 2, 3, ..., N b / 2.

[0114] Then calculate the end model quality g is the acceleration due to gravity.

[0115] For N b / 2 groups The model mass obtained by calibrating the model by taking the average value when rotating it around the u-axis of the end flange coordinate system C2.

[0116] The coordinates of the centroid of the end model in the six-dimensional force sensor coordinate system C3 are Pm = [x m ,y m ,z m ] T ,like Figure 5 As shown; it is easy to see that for the six-dimensional force sensor output component F caused by the gravity of the end model, G =[G x G y G z M x M y M z ]have:

[0117]

[0118] Convert to matrix form:

[0119] Using the above six-dimensional force data Fu_1, Fu_2…Fu_N b (N b =360 / φ) and subtract the bias amount The force and torque data are combined into the following matrix according to the above rules.

[0120]

[0121] Based on the above formula, the position of the model's centroid in the sensor coordinate system C3, obtained by calibration under the condition that the model rotates around the u-axis of coordinate system C2, is obtained using the least squares method:

[0122] Pm u =(G n T ·G n ) -1 ·G n T ·M gn .

[0123] Step 3: Select different axes from the X, Y, and Z axes in the end flange coordinate system C2 as the u-axis, and repeat the above robot control and data processing flow to calculate F. X bias F Y bias F Z bias mf X m f Y m f Z Pm X Pm Y Pm Z And by averaging them, we get:

[0124]

[0125]

[0126]

[0127] Then the model's gravity G = mf·g, where g is the acceleration due to gravity.

[0128] The installation error calibration of the six-dimensional force sensor is to calibrate the installation error ΔR = [Δα, Δβ, Δλ] between the coordinate system C3 of the six-dimensional force sensor and the coordinate system C2 of the robot end flange.

[0129] The automatic calibration process for Δα is as follows: Figure 6 As shown, the details are as follows:

[0130] Step 1: Control the robot to make the Z-axis of the six-dimensional force sensor coordinate system C3 vertical and the X-axis horizontal;

[0131] Step 2: Control the robot to rotate the end effector model around the X-axis of the six-dimensional force sensor coordinate system C3 by an angle θ, preferably 45°. After it comes to rest, record the output data of the six-dimensional force sensor: F2 = [F x2 ,F y2 ,F z2 M x2 M y2 M z2 ] T ;

[0132] Step 3: Repeat Step 1;

[0133] Step 4: Control the robot to rotate the end effector model around the X-axis of the six-dimensional force sensor coordinate system C3 by an angle of -θ; record the output data of the six-dimensional force sensor F3 = [F] while it is stationary. x3 ,F y3 ,F z3 M x3 M y3 M z3 ] T ;

[0134] Step 5: Based on the Z-axis force data of F2 and F3, we can obtain:

[0135]

[0136] From the above formula, we can calculate: Δα=arcsin[(F z2 -F z3 ) / 2G·sin(θ)].

[0137] Step 6: Select different θ angles, repeat the above steps to calculate Δα, and take the average as the final installation error angle Δα.

[0138] The automatic calibration process for Δβ is as follows:

[0139] Step 1: Control the robot to make the Z-axis of the six-dimensional force sensor coordinate system C3 vertical and the Y-axis horizontal;

[0140] Step 2: Control the robot to rotate the end effector model around the Y-axis of the six-dimensional force sensor coordinate system C3 by an angle θ, preferably 45°. After it comes to rest, record the output data of the six-dimensional force sensor: F4 = [F x4 ,F y4 ,F z4 M x4 M y4 M z4 ] T ;

[0141] Step 3: Repeat Step 1;

[0142] Step 4: Control the robot to rotate the end effector model around the Y-axis of the six-dimensional force sensor coordinate system C3 by an angle of -θ; record the output data of the six-dimensional force sensor F5 = [F] while it is stationary. x5 ,F y5 ,F z5 M x5 M y5 M z5 ] T ;

[0143] Step 5: Based on the Z-axis force data of F4 and F5, we can obtain:

[0144]

[0145] From the above formula, we can calculate: Δβ=arcsin[(F z4 -F z5 ) / 2G·sin(θ)].

[0146] Step 6: Select different θ angles, repeat the above steps to calculate Δβ, and take the average as the final installation error angle Δβ.

[0147] The automatic calibration process for Δλ is as follows:

[0148] Step 1: Control the robot to make the X-axis vertical and the Z-axis horizontal in the six-dimensional force sensor coordinate system C3;

[0149] Step 2: Control the robot to rotate the end effector model around the Z-axis of the six-dimensional force sensor coordinate system C3 by an angle θ, preferably 45°. After it comes to rest, record the output data of the six-dimensional force sensor: F6 = [F x6 ,F y6 ,F z6 M x6 M y6 M z6 ] T ;

[0150] Step 3: Repeat Step 1;

[0151] Step 4: Control the robot to rotate the end effector model around the Y-axis of the six-dimensional force sensor coordinate system C3 by an angle of -θ; record the output data of the six-dimensional force sensor F7 = [F] while it is stationary. x7 ,F y7 ,F z7 M x7 M y7 M z7 ] T ;

[0152] Step 5: Based on the X-axis force data of F6 and F7, we can obtain...

[0153]

[0154] From the above formula, we can calculate: Δλ=arcsin[(F x6 -F x7 ) / 2G·sin(θ)];

[0155] Step 6: Select different θ angles, repeat the above steps to calculate Δλ, and take the average as the final installation error angle Δλ.

[0156] The end-effector inertial parameter calibration (8) is the inertial tensor matrix I automatically calibrated by the robot. The robot controls the end-effector to perform m uniformly accelerated rotations around its center of mass, where m ≥ 3, and the matrix A is composed of multiple sets of experimental angular acceleration vectors. m The rank is ≥3. The terminal model acceleration can be obtained using the following two methods:

[0157] Method 1: Set the centroid of the end effector model as the origin of the robot tool coordinate system, and have the robot output the acceleration A of the end effector model in the model coordinate system C4. dp =[A x A y A z A rx A ry A rz ] T ;

[0158] Method 2: Install the inertial measurement unit (IMU) on the support end of the six-dimensional force sensor, acquire the IMU output, and convert it into acceleration A in the model coordinate system C4. dp =[A x A y A z A rx A ry A rz ] T .

[0159] The automatic calibration process for the end-effector inertial parameter I is as follows: Figure 7 As shown, the specific steps are as follows:

[0160] Step 1: Put the robot into the standby position;

[0161] Step 2: Control the model with angular acceleration A r1 The model performs an accelerated rotational motion for duration t2 from the standby position, and the angular acceleration A of the model during the motion is recorded. rx A ry A rz Simultaneously, the output data of the six-dimensional force sensor is recorded and subjected to bias removal and gravity compensation to obtain F. i1 =[F ix1 ,F iy1 ,F iz1 M ix1 M iy1 M iz1 ]

[0162] Step 3: Update the control value of the model's angular acceleration. The new angular acceleration is linearly independent of the angular velocity vector in the previous steps. Repeat the above steps m times.

[0163] Step 4: Utilize m sets of six-dimensional force signals F i1 F i2 …F im The torque data form a matrix M im The matrix A is composed of m sets of terminal model acceleration data. m Then we have:

[0164]

[0165] Multiply the above expression by M on the left. im -1 Right multiply by A m -1 Derived: M im -1 ·I=-A m -1 ,make W m =-A m -1According to the least squares method, I = (U m T ·U m ) -1 ·U m T ·W m .

[0166] The dynamic contact force real-time error compensation process 3 is based on the previously calibrated end-model mass m. f The dynamic force signals measured in real time by the six-dimensional force sensor during robot force control motion are processed using the centroid position Pm, the installation error ΔR = [Δα, Δβ, Δλ] of the six-dimensional force sensor, and the inertial tensor matrix I of the end-effector model to obtain the external dynamic contact force experienced by the end-effector model in its coordinate system C4, which is used for force feedback in robot force control tasks. The process includes: signal preprocessing 9 → dynamic error correction 10 → end-effector model gravity compensation 11 → end-effector model inertia compensation 12 → coordinate system transformation 13, such as Figure 8 As shown.

[0167] Signal preprocessing 9 involves subtracting the six-dimensional force sensor bias F from the original six-dimensional force sensor signal L0. bias The signal is then low-pass filtered to remove high-frequency components, resulting in the preprocessed signal L1.

[0168] Dynamic error correction 10 refers to the real-time dynamic decoupling and compensation of the preprocessed signal L1 using the dynamic decoupling-compensator designed in the offline dynamic calibration 1 of the six-dimensional force sensor. The transfer function of the dynamic decoupling-compensator is G. c (z) is used to remove the dynamic measurement error caused by the dynamic characteristics of the six-dimensional force sensor, resulting in the dynamic compensation signal L2 = G. c (z)·L1.

[0169] End-effector gravity compensation 11 refers to obtaining the attitude matrix T of the end-effector in the world coordinate system C0 based on the attitude parameters of the end-effector returned by the robot. f Then, the attitude matrix T of the six-dimensional force sensor in the world coordinate system C0 is calculated. s Then, based on the end-effector model's quality parameters and attitude matrix T... s Calculate the gravity component F of the end model G And provide compensation.

[0170] Based on the actual installation of the robot, the rotation matrix T of the robot's base coordinate system C1 around the world coordinate system C0 can be obtained. r Based on the actual installation of the six-dimensional force sensor, the rotation matrix T of the six-dimensional force sensor coordinate system C3 relative to the robot end flange coordinate system C2 can be obtained. s-fConsidering the installation error angle ΔR = [Δα, Δβ, Δλ] of the six-dimensional force sensor, the attitude matrix of the six-dimensional force sensor coordinate system C3 in the world coordinate system C0 is:

[0171]

[0172] From the above formula, the direction cosine of the Z-axis of the world coordinate system C0 in the six-dimensional force sensor coordinate system C3 can be obtained as [N z O z A z Since the gravity direction of the end-effector is parallel to the negative Z-axis of the world coordinate system C0, the direction cosine of the gravity direction of the end-effector in the six-dimensional force sensor coordinate system C3 is -[N]. z O z A z Based on this, the output error of the six-dimensional force sensor caused by the gravity of the end model is obtained:

[0173]

[0174] Subsequently, the gravity compensation signal L3 = L2 - F from the six-dimensional force sensor can be obtained. G .

[0175] End-effector inertia compensation 12 refers to calculating the inertial force / torque F of the end-effector model in the six-dimensional force sensor coordinate system C3 based on the real-time motion acceleration and inertial parameters of the robot end-effector model. inertia =[F ix ,F iy ,F iz M ix M iy M iz ] T And compensation is performed. The end-effector acceleration can be obtained using the following two methods:

[0176] Method 1: Set the centroid of the end effector model as the origin of the robot tool coordinate system, and have the robot output the acceleration A of the end effector model in coordinate system C4. dp =[A x A y A z A rx A ry A rz ] T ;

[0177] Method 2: Install the inertial measurement unit (IMU) on the support end of the sensor, acquire the IMU output, and convert it into acceleration A in coordinate system C4. dp =[A x A y A z A rxA ry A rz ] T .

[0178] The inertial force / moment generated by the end model in the end model coordinate system C4 is:

[0179]

[0180] Based on the coordinates of the centroid of the end model in the six-dimensional force sensor coordinate system, Pm = [x] m ,y m ,z m ] T Then the inertial force / torque F of the end model in the six-dimensional force sensor coordinate system C3 is... inertia for:

[0181]

[0182] Then the external dynamic contact force F of the end model measured by the six-dimensional force sensor can be obtained. sc =L3-F inertia =[F ex ,F ey ,F ez M ex M ey M ez ].

[0183] Coordinate system transformation 13 refers to transforming the external dynamic contact force F in the six-dimensional force sensor coordinate system C3. sc Transform to the end-effector model coordinate system C4 for subsequent robot control tasks; then the external dynamic contact force F in the end-effector model coordinate system C4... mc for:

[0184]

Claims

1. An error compensation and processing method for dynamic contact force measurement of a robot end effector model. This method compensates for multiple error components mixed in the actual output of a six-dimensional force sensor installed on the robot's end effector flange during robot force control tasks. It proposes a compensation method for dynamic contact force measurement errors caused by six-dimensional force sensor installation errors, sensor dynamic characteristic errors, model gravity errors, and model inertia errors. The technical process includes: The system features offline dynamic calibration of a six-dimensional force sensor, online automatic calibration of characteristic parameters of a model contact force measurement system, and real-time error compensation processing for dynamic contact force. First, an offline dynamic calibration experiment was conducted on a six-dimensional force sensor with an end model. Based on the dynamic calibration experiment data of the six-dimensional force sensor, a dynamic decoupling-compensator for the six-dimensional force sensor was designed to dynamically correct the dynamic error caused by the characteristics of the six-dimensional force sensor itself. Secondly, the robot automatically calibrates the installation error of the six-dimensional force sensor, the offset of the six-dimensional force sensor, the mass parameters of the end model, and the inertial parameters of the end model. The automatic calibration process is as follows: calibration of the six-dimensional force sensor offset and model mass parameters → calibration of the six-dimensional force sensor installation error → calibration of the end model inertial parameters. The calibration of the six-dimensional force sensor offset and model mass parameters involves controlling the robot to make the u-axis of the robot's end flange coordinate system C2 horizontal, and causing the model to rotate stepwise around the u-axis of the robot's end flange coordinate system C2, recording sensor data at each step position. u is the X-axis, Y-axis, or Z-axis of coordinate system C2. Through data processing, the six-dimensional force sensor offset is automatically and simultaneously calibrated. End model quality m f and the position of the model's centroid in the sensor coordinate system C3 ; The installation error calibration of the six-dimensional force sensor involves controlling the robot to make each coordinate axis of the six-dimensional force sensor coordinate system C3 horizontal in sequence, and causing the end model to rotate sequentially around the horizontal coordinate axis of the six-dimensional force sensor coordinate system C3. - The angle and record output data of the six-dimensional force sensor are used to calibrate the installation error between the six-dimensional force sensor coordinate system C3 and the robot end flange coordinate system C2 through data processing. ; The calibration of the end-effector inertial parameters involves controlling the robot to make the end-effector rotate around its center of mass m times with uniform acceleration, where m ≥ 3, and the calibration is performed using a matrix composed of multiple sets of experimental angular acceleration vectors. If the rank is ≥3, record the angular acceleration of the model during the motion. , , Using data output from a six-dimensional force sensor, the inertial tensor matrix of the end-effector model is calibrated through data processing. ; Finally, based on the parameters of the six-dimensional force sensor dynamic decoupling-compensator and online automatic calibration, the six-dimensional force sensor measurement output signal is subjected to real-time error compensation processing in the robot force control task, including debiasing, dynamic decoupling-compensation, end-effector gravity compensation, end-effector inertia compensation, and model coordinate system transformation, thereby obtaining accurate dynamic contact force data of the end-effector.

2. The error compensation and processing method for dynamic contact force measurement of a robot end effector model as described in claim 1, characterized in that: The calibration of the six-dimensional force sensor bias and model quality parameters is achieved by automatically and simultaneously calibrating the six-dimensional force sensor bias under robot control. End model quality m f and the position of the model's centroid in the sensor coordinate system C3 The robot's automatic calibration control process is as follows: Step 1: Control the robot to make the u-axis of the robot end flange coordinate system C2 horizontal, where u is the X-axis, Y-axis, or Z-axis of coordinate system C2; Step 2: Control the robot to rotate the model around the u-axis of the robot's end flange coordinate system C2 by a step angle ϕ for one revolution. After remaining stationary for time t0 at each step position, start recording sensor data for time t1 and average the sensor data within time t1 to obtain N. b The average values ​​of the six-dimensional force sensor output data at each step angle position: Fu_1, Fu_2…Fu_N b Where ϕ is divisible by 180, N b =360 / ϕ; The sensor bias F can then be obtained from the following formula. bias : For N b The six-dimensional force sensor data, including data Fu_i and Fu_(N) with rotation angles differing by 180°, b The following processing is performed on / 2+i); Where i = 1, 2, 3, …, N b / 2; then calculate the quality of the final model. g is the acceleration due to gravity; for N b / 2 groups The model mass obtained by calibrating the model by taking the average value when rotating it around the u-axis of the end flange coordinate system C2. ; Coordinates of the centroid of the end model in the six-dimensional force sensor coordinate system C3 It is easy to see that for the six-dimensional force sensor output component F caused by gravity of the end model, G =[G x G y G z M x M y M z ]have: Convert to matrix form: Using the above six-dimensional force data Fu_1, Fu_2…Fu_N b N b =360 / ϕ, minus the bias. The force and torque data are combined into the following matrix according to the above rules; Based on the above formula, the position of the model's centroid in the sensor coordinate system C3, obtained by calibration under the condition that the model rotates around the u-axis of coordinate system C2, is obtained using the least squares method: ; Step 3: Select different axes from the X, Y, and Z axes in the end flange coordinate system C2 as the u-axis, and repeat the above robot control and data processing flow to calculate F. X bias F Y bias F Z bias m f X m f Y m f Z Pm X Pm Y Pm Z And by averaging them, we get: Then the model's gravity G = mf·g, where g is the acceleration due to gravity; The installation error calibration of the six-dimensional force sensor is to calibrate the installation error between the six-dimensional force sensor coordinate system C3 and the robot end flange coordinate system C2. ; The automatic calibration process is as follows: Step 1: Control the robot to make the Z-axis of the six-dimensional force sensor coordinate system C3 vertical and the X-axis horizontal; Step 2: Control the robot to rotate the end effector model around the X-axis of the six-dimensional force sensor coordinate system C3. Angle, after coming to a standstill, record the output data of the six-dimensional force sensor. ; Step 3: Repeat Step 1; Step 4: Control the robot to rotate the end effector model around the X-axis of the six-dimensional force sensor coordinate system C3. Angle; recording the output data of a six-dimensional force sensor in a static state. ; Step 5: Based on the Z-axis force data of F2 and F3, we can obtain: The above formula yields: ; Step Six: Select different Repeat the above steps to calculate the angle. The average value is then taken as the final installation error angle. ; The automatic calibration process is as follows: Step 1: Control the robot to make the Z-axis of the six-dimensional force sensor coordinate system C3 vertical and the Y-axis horizontal; Step 2: Control the robot to rotate the end effector model around the Y-axis of the six-dimensional force sensor coordinate system C3. Angle, after coming to a standstill, record the output data of the six-dimensional force sensor. ; Step 3: Repeat Step 1; Step 4: Control the robot to rotate the end effector model around the Y-axis of the six-dimensional force sensor coordinate system C3. Angle; recording the output data of a six-dimensional force sensor in a static state. ; Step 5: Based on the Z-axis force data of F4 and F5, we can obtain: The above formula yields: ; Step Six: Select different Repeat the above steps to calculate the angle. The average value is then taken as the final installation error angle. ; The automatic calibration process is as follows: Step 1: Control the robot to make the X-axis vertical and the Z-axis horizontal in the six-dimensional force sensor coordinate system C3; Step 2: Control the robot to rotate the end effector model around the Z-axis of the six-dimensional force sensor coordinate system C3. Angle, after coming to a standstill, record the output data of the six-dimensional force sensor. ; Step 3: Repeat Step 1; Step 4: Control the robot to rotate the end effector model around the Y-axis of the six-dimensional force sensor coordinate system C3. Angle; recording the output data of a six-dimensional force sensor in a static state. ; Step 5: Based on the X-axis force data of F6 and F7, we can obtain... The above formula yields: ; Step Six: Select different Repeat the above steps to calculate the angle. The average value is then taken as the final installation error angle. ; The end-effector inertial parameter calibration refers to the automatic calibration of the end-effector's inertial tensor matrix by robot control. The robot is controlled to perform m uniformly accelerated rotational motions around its center of mass, where m ≥ 3, and the matrix is ​​composed of multiple sets of experimental angular acceleration vectors. The rank is ≥3; the terminal model acceleration can be obtained by the following two methods: Method 1: Set the centroid of the end effector model as the origin of the robot tool coordinate system, and have the robot output the acceleration of the end effector model in the model coordinate system C4. ; Method 2: Install the inertial measurement unit (IMU) on the support end of the six-dimensional force sensor, acquire the IMU output, and convert it into acceleration in the model coordinate system C4. ; End model inertial parameters The automatic calibration process is as follows: Step 1: Put the robot into the standby position; Step 2: Control the model with angular acceleration A r1 The model undergoes accelerated rotational motion for duration t2 from its standby position, and the angular acceleration during the motion is recorded. , , Simultaneously, the output data of the six-dimensional force sensor is recorded and subjected to bias removal and gravity compensation to obtain F. i1 =[F ix1 , F iy1 , F iz1 M ix1 M iy1 M iz1 ] Step 3: Update the control value of the model's angular acceleration. The new angular acceleration is linearly independent of the angular velocity vector in the previous steps. Repeat the above steps m times. Step 4: Utilize m sets of six-dimensional force signals F i1 F i2 …F im Medium torque data form a matrix A matrix is ​​formed from m sets of terminal model acceleration data. Then we have: Multiply the above expression on the left right multiplication have to: ,make , According to the least squares method, .

3. The error compensation and processing method for dynamic contact force measurement of a robot end effector model as described in claim 1, characterized in that: The dynamic contact force real-time error compensation process is based on the previously calibrated end-model mass m. f Centroid position Pm, six-dimensional force sensor installation error The inertial tensor matrix I of the end-effector model is used to perform error compensation processing on the dynamic force signal measured in real time by the six-dimensional force sensor during the robot's force control motion, so as to obtain the external dynamic contact force experienced by the end-effector model in its coordinate system C4, which is used for force feedback in the robot's force control task. The process includes: signal preprocessing → dynamic error correction → end-model gravity compensation → end-model inertia compensation → coordinate system transformation; Signal preprocessing involves subtracting the six-dimensional force sensor bias from the original six-dimensional force sensor signal L0. The signal is then low-pass filtered to remove high-frequency components, resulting in a preprocessed signal. ; Dynamic error correction refers to the use of a dynamic decoupling-compensator designed in the offline dynamic calibration of a six-dimensional force sensor to correct the preprocessed signal. Real-time dynamic decoupling and compensation are performed, and the transfer function of the dynamic decoupling and compensator is G. c (z) removes the dynamic measurement error caused by the dynamic characteristics of the six-dimensional force sensor itself, and obtains the dynamic compensation signal. ; Gravity compensation for the end-effector model involves obtaining the attitude matrix of the end-effector in the world coordinate system C0 based on the attitude parameters returned by the robot. Then, the attitude matrix of the six-dimensional force sensor in the world coordinate system C0 is calculated. Then, based on the end-model quality parameters and attitude matrix... Calculate the gravity components of the end model And provide compensation; Based on the actual installation of the robot, the rotation matrix of the robot's base coordinate system C1 around the world coordinate system C0 can be obtained. Based on the actual installation of the six-dimensional force sensor, the rotation matrix of the six-dimensional force sensor coordinate system C3 relative to the robot end flange coordinate system C2 can be obtained. Considering the installation error angle of the six-dimensional force sensor Then the attitude matrix of the six-dimensional force sensor coordinate system C3 in the world coordinate system C0 is: From the above equation, the direction cosine of the Z-axis of the world coordinate system C0 in the six-dimensional force sensor coordinate system C3 can be obtained as follows: Since the gravity direction of the end model is parallel to the negative Z-axis of the world coordinate system C0, the direction cosine of the gravity direction of the end model in the six-dimensional force sensor coordinate system C3 is: Based on this, the output error of the six-dimensional force sensor caused by the gravity of the end model is obtained: Subsequently, the gravity compensation signal from the six-dimensional force sensor can be obtained. ; End-effector inertia compensation refers to calculating the inertial force / torque of the end-effector in the six-dimensional force sensor coordinate system C3 based on the real-time motion acceleration and inertial parameters of the robot's end-effector. And compensation is performed; the end-effector acceleration can be obtained using the following two methods: Method 1: Set the centroid of the end effector model as the origin of the robot tool coordinate system, and have the robot output the acceleration of the end effector model in coordinate system C4. ; Method 2: Install the inertial measurement unit (IMU) on the support end of the sensor, acquire the IMU output, and convert it into acceleration in coordinate system C4. ; The inertial force / moment generated by the end model in the end model coordinate system C4 is: Based on the coordinates of the centroid of the end model in the six-dimensional force sensor coordinate system Then the inertial force / torque of the end model in the six-dimensional force sensor coordinate system C3 for: Then the external dynamic contact force of the end model measured by the six-dimensional force sensor can be obtained. ; Coordinate system transformation refers to transforming the external dynamic contact force in the six-dimensional force sensor coordinate system C3. Transform to the end-effector model coordinate system C4 for subsequent robot control tasks; then the external dynamic contact force in the end-effector model coordinate system C4... for: 。

Images