CN116652953A - Error compensation and processing method for dynamic contact force measurement of robot tail end model - Google Patents

Error compensation and processing method for dynamic contact force measurement of robot tail end model Download PDF

Info

Publication number
CN116652953A
CN116652953A CN202310689697.7A CN202310689697A CN116652953A CN 116652953 A CN116652953 A CN 116652953A CN 202310689697 A CN202310689697 A CN 202310689697A CN 116652953 A CN116652953 A CN 116652953A
Authority
CN
China
Prior art keywords
force sensor
dimensional force
coordinate system
model
robot
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202310689697.7A
Other languages
Chinese (zh)
Inventor
杨双龙
黄园民
余成江
黄云志
邵春莉
胡泽志
徐科军
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hefei University of Technology
Original Assignee
Hefei University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hefei University of Technology filed Critical Hefei University of Technology
Priority to CN202310689697.7A priority Critical patent/CN116652953A/en
Publication of CN116652953A publication Critical patent/CN116652953A/en
Pending legal-status Critical Current

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1653—Programme controls characterised by the control loop parameters identification, estimation, stiffness, accuracy, error analysis
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1661—Programme controls characterised by programming, planning systems for manipulators characterised by task planning, object-oriented languages
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Abstract

The invention relates to an error compensation and processing method for measuring dynamic contact force of a robot tail end model, which is used for carrying out error compensation processing on the output of a six-dimensional force sensor in a robot manual control task. Firstly, performing off-line dynamic calibration on a six-dimensional force sensor, and designing a dynamic decoupling-compensator of the six-dimensional force sensor according to calibration experimental data; then, the robot is controlled to automatically calibrate the offset of the six-dimensional force sensor, the quality parameters of the tail end model, the installation errors of the six-dimensional force sensor and the inertial parameters of the tail end model on line; and finally, sequentially performing real-time error compensation processing of unbiasing, dynamic decoupling-compensation, end model gravity compensation, end model inertia compensation and model coordinate system conversion on the six-dimensional force sensor measurement output signal in a robot manual control task according to the six-dimensional force sensor dynamic decoupling-compensator and the parameters of on-line automatic calibration, so as to obtain the accurate dynamic contact force born by the end model.

Description

Error compensation and processing method for dynamic contact force measurement of robot tail end model
Technical Field
The invention relates to a robot dynamic force sensing technology, in particular to an error compensation and processing method for dynamic contact force applied to a tail end model in a robot manual control task.
Background
In the process of the robot executing some control tasks, such as ground zero gravity motion simulation of a spacecraft on-orbit service task, remote surgery of a man-machine cooperative robot and the like, the dynamic contact force born by the tail end model needs to be accurately measured. Model contact force measurements are typically made by mounting six-dimensional force sensors between the robot tip and the model. Namely, the support end of the six-dimensional force sensor is mounted on the end flange of the robot, and then the corresponding end model is mounted on the load end of the six-dimensional force sensor. The end models typically include end tools of various robots, test models, etc., such as end effector tools in robotic control work, satellite models in satellite acquisition ground zero gravity simulation tests, etc. However, in the process of actually performing a control task by the robot, the six-dimensional force/moment data measured by the six-dimensional force sensor installed at the end of the robot is mixed with various error components, and mainly includes: six-dimensional force sensor offset, six-dimensional force sensor installation error, end model gravity/moment, end model inertial force/moment, six-dimensional force sensor dynamic characteristic error. The existing robot tail end force measurement and compensation technology only considers the offset and tail end model gravity/moment error in the six-dimensional force sensor measurement result, few researches carry out inertial compensation on the six-dimensional force sensor measurement output, and the six-dimensional force sensor installation error and dynamic characteristic error are ignored, so that the measurement result of the dynamic contact force is inaccurate. For example, the installation error angle of the six-dimensional force sensor can cause the error of the actual gesture matrix of the six-dimensional force sensor, thereby affecting the gravity compensation of the model and the coordinate system conversion of the contact force; the dynamic characteristic difference of the six-dimensional force sensor can cause a large dynamic error in the measurement of dynamic contact force and the compensation of model gravity and inertial force. Therefore, in the occasion with higher requirement on the measuring precision of the dynamic contact force of the robot terminal model, the measuring result of the six-dimensional force sensor needs to be subjected to omnibearing error compensation so as to improve the measuring precision. As mentioned above, the dynamic measurement error of the six-dimensional force sensor mainly comes from the installation error of the six-dimensional force sensor, the self dynamic characteristics and the characteristic parameters of the end model; therefore, in order to compensate various error components in the measurement results of the six-dimensional force sensor in real time in the robot control, it is necessary to acquire the six-dimensional force sensor and the end model characteristic parameters in advance. The characteristic parameters of the six-dimensional force sensor and the tail end model are usually calibrated in advance through a series of experiments under the off-line condition, so that repeated time-consuming and labor-consuming experiment calibration work is required when the six-dimensional force sensor or the tail end model is replaced, and the efficiency is low.
In order to solve the problems, the invention provides an error compensation and processing method for measuring the dynamic contact force of a robot terminal model, which is characterized in that the dynamic characteristics of a six-dimensional force sensor are obtained through off-line calibration, and other characteristic parameters of the six-dimensional force sensor and the terminal model are obtained through on-line automatic calibration of the robot, so that accurate dynamic contact force data born by the terminal model are obtained through offset compensation, dynamic characteristic compensation, gravity compensation, inertial force compensation and force coordinate system conversion on the measurement result of the six-dimensional force sensor in the dynamic contact force measurement of the actual robot terminal model.
Disclosure of Invention
The invention provides a six-dimensional force sensor output signal error compensation and processing method for solving the problems that in a robot force control task, various error components are mixed in the actual output of a six-dimensional force sensor arranged on a robot end flange, including the offset of the six-dimensional force sensor, the dynamic error of the six-dimensional force sensor, the gravity/moment of an end model, the inertial force/moment of the end model and the interference on the measurement of dynamic contact force borne by the end model, so as to accurately measure the dynamic contact force borne by the end model.
The technical scheme adopted by the invention is as follows: firstly, performing off-line dynamic calibration experiments on a six-dimensional force sensor with an end model, and designing a dynamic decoupling-compensator of the six-dimensional force sensor according to the dynamic calibration experimental data of the six-dimensional force sensor, wherein the dynamic decoupling-compensator is used for dynamically correcting dynamic errors caused by the characteristics of the six-dimensional force sensor; the robot is used for automatically calibrating characteristic parameters of the six-dimensional force sensor and the tail end model on line, wherein the characteristic parameters comprise six-dimensional force sensor installation errors, six-dimensional force sensor offset, tail end model quality parameters and tail end model inertia parameters; and finally, sequentially performing real-time error compensation processing of unbiasing, dynamic decoupling-compensation, end model gravity compensation, end model inertia compensation and model coordinate system conversion on the six-dimensional force sensor measurement output signal in a robot manual control task according to the characteristic parameters of the six-dimensional force sensor dynamic decoupling-compensator and the online automatic calibration, thereby acquiring accurate dynamic contact force data born by the end model.
The technical process of the invention is as follows: the six-dimensional force sensor offline dynamic calibration 1, the online automatic calibration 2 of the characteristic parameters of the model contact force measurement system, and the real-time error compensation treatment 3 of the dynamic contact force. The method of the invention adopts a world coordinate system C0, a robot base coordinate system C1, a robot tail end flange coordinate system C2, a sensor coordinate system C3 and a model coordinate system C4 to carry out auxiliary explanation of the method. Wherein, the directions of the axes of the world coordinate system C0 and the robot base coordinate system C1 are the same and are all fixed coordinate systems; the robot tail end flange coordinate system C2 is positioned on the mounting flange at the tail end of the robot; the sensor coordinate system C3 is a measurement coordinate system of the six-dimensional force sensor; the origin of the model coordinate system C4 is located at the centroid position of the terminal model; the relation among the robot end flange coordinate system C2, the sensor coordinate system C3 and the model coordinate system C4 is fixed by the installation relation among the robot end flange, the six-dimensional force sensor and the end model, and moves along with the robot end; in general, when the six-dimensional force sensor and the end model are mounted, 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 direction of the robot end flange coordinate system C2.
The off-line dynamic calibration 1 of the six-dimensional force sensor is to perform a dynamic calibration experiment on the six-dimensional force sensor with the end model, obtain the dynamic calibration experimental data of the six-dimensional force sensor with the end model, and design a dynamic decoupling-compensating device of the six-dimensional force sensor according to the dynamic calibration experimental data, and the dynamic decoupling-compensating device is used for performing real-time dynamic decoupling-compensation on the actual measurement output of the six-dimensional force sensor. The process comprises the following steps: dynamic calibration experiment 4→dynamic decoupling-compensator design 5.
And a dynamic calibration experiment 4, namely fixedly mounting the support end of the six-dimensional force sensor on a rigid calibration table with the rigidity far greater than that of the sensitive element of the six-dimensional force sensor, mounting a tail end model at the measurement end of the six-dimensional force sensor, applying dynamic excitation force to the tail end model, recording dynamic excitation force data and six-dimensional force sensor output data, and completing the dynamic calibration experiment of the six-dimensional force sensor. The dynamic excitation force is determined according to the loading condition of the actual end model, and the step excitation force is optimized; when the step excitation force is inconvenient to apply to the model, the impact excitation force is applied to the end model by adopting a force hammer knocking method.
The dynamic decoupling-compensator design 5 is to design a dynamic decoupling-compensator Gc (z) capable of simultaneously removing the main channel dynamic error and the inter-dimensional coupling channel dynamic error of the six-dimensional force sensor according to the dynamic excitation force data and the six-dimensional force sensor output data obtained in the dynamic calibration experiment 4 by adopting the prior art, such as a diagonal dominant dynamic decoupling-compensation method or an iterative dynamic decoupling-compensation method.
The characteristic parameters of the model contact force measurement system are automatically calibrated on line 2, namely, the installation errors of the six-dimensional force sensor, the offset of the six-dimensional force sensor, the quality parameters of the end model and the inertial parameters of the end model are automatically calibrated by the control of a robot, and the automatic calibration flow is as follows: six-dimensional force sensor bias and model mass parameter calibration 6, six-dimensional force sensor installation error calibration 7 and end model inertia parameter calibration 8.
Six-dimensional force sensor offset and model quality parameter calibration 6, namely, automatic and simultaneous calibration of six-dimensional force sensor offset F under robot control bias ＝[V1,V2,V3,V4,V5,V6] T End model mass m f And the position pm= [ x ] of the model centroid in the sensor coordinate system C3 m ,y m ,z m ] T The method comprises the steps of carrying out a first treatment on the surface of the The robot automatic calibration control flow is as follows.
Step one: the robot is controlled to level the u-axis of the robot end flange coordinate system C2, u being the X-axis, Y-axis or Z-axis of the coordinate system C2.
Step two: controlling the robot to enable the model to rotate around the u axis of the flange coordinate system C2 of the tail end of the robot in a stepping mode by a stepping angle phi for one circle, starting to record sensor data of t1 time after each stepping position is static for t0 time, and averaging the sensor data in the t1 time to obtain N b Average values Fu_1, fu_ … Fu_N of six-dimensional force sensor output data of each step angle position b The method comprises the steps of carrying out a first treatment on the surface of the Wherein phi is divisible by 180, N b ＝360/φ。
The sensor offset F is obtained by bias 。
For N b Data fu_i and fu_ (N) of 180 ° difference in rotation angle among the six-dimensional force sensor data of group b 2+i) is treated as follows.
Wherein i=1, 2,3, …, N b /2。
Then calculate the end model qualityg is gravitational acceleration.
For N b Group/2Averaging to obtain model mass m obtained by calibration under the condition that the model rotates around the u axis of the terminal flange coordinate system C2 f u 。
Coordinate pm= [ x ] of terminal model centroid in six-dimensional force sensor coordinate system C3 m ,y m ,z m ] T It is easy to know 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 ]The method comprises the following steps:
converting into a matrix form:
using the six-dimensional force data Fu_1, fu_ … Fu_N b (N b =360/Φ) and subtracting the offsetAnd combining the force and moment data into the following matrix according to the rules.
And (3) solving the position of the mass center of the model, which is obtained by calibration under the condition that the model rotates around the u axis of the coordinate system C2, in the sensor coordinate system C3 by adopting a least square method according to the above method:
Pm u ＝(G n T ·G n ) -1 ·G n T ·M gn 。
step three: selecting different axes in X, Y, Z axes in the end flange coordinate system C2 as u axes, and repeating the robot control and data processing flow calculation to obtain 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 respectively averaging to obtain:
the model gravity g=mf·g, G is the gravitational acceleration.
The six-dimensional force sensor installation error calibration 7 is the installation error delta R= [ delta alpha, delta beta, delta lambda ] between the calibration six-dimensional force sensor coordinate system C3 and the robot terminal flange coordinate system C2.
The automatic calibration procedure for Δα is as follows:
step one: controlling the robot to enable the Z axis of the six-dimensional force sensor coordinate system C3 to be vertical and the X axis to be horizontal;
step two: the robot is controlled to enable the tail end model to rotate around the X axis of the six-dimensional force sensor coordinate system C3 by an angle theta, the angle theta is preferably 45 degrees, and after the tail end model is stationary, six-dimensional force sensor output data F2= [ F ] are recorded x2 ,F y2 ,F z2 ,M x2 ,M y2 ,M z2 ] T ；
Step three: repeating the first step;
step four: controlling the robot to enable the tail end model to rotate by an angle theta around the X axis of a six-dimensional force sensor coordinate system C3; recording output data F3= [ F ] of six-dimensional force sensor in static state x3 ,F y3 ,F z3 ,M x3 ,M y3 ,M z3 ] T ；
Step five: z-axis force data from F2, F3 are available:
calculated from the above formula: Δα=arcsin [ (F) z2 -F z3 )/2G·sin(θ)]。
Step six: and selecting different theta angles, repeating the steps to calculate delta alpha, and taking the average as a final installation error angle delta alpha.
The automatic calibration flow of delta beta is as follows:
step one: controlling the robot to enable the Z axis of the six-dimensional force sensor coordinate system C3 to be vertical and the Y axis to be horizontal;
step two: the robot is controlled to enable the tail end model to rotate around the Y axis of the six-dimensional force sensor coordinate system C3 by an angle theta, the angle theta is preferably 45 degrees, and after the tail end model is stationary, six-dimensional force sensor output data F4= [ F ] are recorded x4 ,F y4 ,F z4 ,M x4 ,M y4 ,M z4 ] T ；
Step three: repeating the first step;
step four: controlling the robot to enable the tail end model to rotate by an angle theta around a Y axis of a six-dimensional force sensor coordinate system C3; recording output data F5= [ F ] of six-dimensional force sensor in static state x5 ,F y5 ,F z5 ,M x5 ,M y5 ,M z5 ] T ；
Step five: z-axis force data from F4, F5 are available:
calculated from the above formula: Δβ=arcsin [ (F) z4 -F z5 )/2G·sin(θ)]。
Step six: and selecting different theta angles, repeating the steps to calculate delta beta and taking the average as the final installation error angle delta beta.
The automatic calibration procedure for Δλ is as follows:
step one: controlling the robot to enable the X axis of the six-dimensional force sensor coordinate system C3 to be vertical and the Z axis to be horizontal;
step two: the robot is controlled to enable the tail end model to rotate around the Z axis of the six-dimensional force sensor coordinate system C3 by an angle theta, the angle theta is preferably 45 degrees, and after the tail end model is stationary, six-dimensional force sensor output data F6= [ F ] are recorded x6 ,F y6 ,F z6 ,M x6 ,M y6 ,M z6 ] T ；
Step three: repeating the first step;
step four: controlling the robot to enable the tail end model to rotate by an angle theta around a Y axis of a six-dimensional force sensor coordinate system C3; recording output number of six-dimensional force sensor in static stateData f7= [ F x7 ,F y7 ,F z7 ,M x7 ,M y7 ,M z7 ] T ；
Step five: x-axis force data from F6, F7 are available
Calculated from the above formula: Δλ=arcsin [ (F) x6 -F x7 )/2G·sin(θ)]；
Step six: and selecting different theta angles, repeating the steps to calculate delta lambda and taking the average as a final installation error angle delta lambda.
And 8, calibrating inertial parameters of the end model, namely automatically calibrating an inertial tensor matrix I of the end model by robot control. The robot is controlled to make the terminal model execute m times of uniform acceleration rotation movements around the mass center of the terminal model, m is more than or equal to 3, and a matrix A consisting of a plurality of groups of experimental angular acceleration vectors m The rank of (2) is not less than 3. The end model acceleration can be obtained by two methods:
the method comprises the following steps: setting the centroid position of the end model as the origin of coordinates of a robot tool, and outputting the acceleration A of the end model under a model coordinate system C4 by the robot dp ＝[A x ,A y ,A z ,A rx ,A ry ,A rz ] T ；
The second method is as follows: the inertial measurement unit is arranged at the support end of the six-dimensional force sensor, and the output of the inertial measurement unit is obtained and converted into the acceleration A under the model coordinate system C4 dp ＝[A x ,A y ,A z ,A rx ,A ry ,A rz ] T 。
The automatic calibration flow of the inertial parameters I of the end model is as follows:
step one: controlling the robot to be at a standby position;
step two: control model with angular acceleration A r1 Executing the acceleration rotation motion for a time t2 from the standby position, and recording the model angular acceleration A in the motion process rx 、A ry 、A rz Simultaneously recording the output number of the six-dimensional force sensorThereby de-biasing and gravity compensating to obtain F i1 ＝[F ix1 ,F iy1 ,F iz1 ,M ix1 ,M iy1 ,M iz1 ]
Step three: updating the control value of the model angular acceleration, wherein the new angular acceleration is independent of the angular velocity vector linearity of the previous step, and repeating the previous step m times.
Step four: from m sets of six-dimensional force signals F i1 、F i2 …F im Medium moment data composition matrix M im M groups of end model acceleration data form matrix A m The following steps are:
multiplying the upper formula by M im -1 Right multiplication A m -1 Obtaining: m is M im -1 ·I＝-A m -1 Order-makingW m ＝-A m -1 According to the least square method, i= (U m T ·U m ) -1 ·U m T ·W m 。
The dynamic contact force real-time error compensation process 3 is the terminal model mass m according to the calibration f Centroid position Pm, six-dimensional force sensor mounting error Δr= [ Δα, Δβ, Δλ]The inertial tensor matrix I of the tail end model performs error compensation processing on dynamic force signals measured by the six-dimensional force sensor in real time in robot manual control movement so as to obtain external dynamic contact force born by the tail end model under a coordinate system C4 of the tail end model, and the external dynamic contact force is used for force feedback in the robot manual control task; the process comprises the following steps: signal preprocessing 9, dynamic error correction 10, end model gravity compensation 11, end model inertia compensation 12 and coordinate system conversion 13.
Signal preprocessing 9, namely subtracting the six-dimensional force sensor offset F from the original six-dimensional force sensor signal L0 bias And low-pass filtering to remove signalsThe high frequency component results in a preprocessed signal L1.
The dynamic error correction 10 is to dynamically decouple-compensate the preprocessed signal L1 in real time by adopting a dynamic decoupling-compensator designed in the off-line dynamic calibration 1 of the six-dimensional force sensor, wherein the transfer function of the dynamic decoupling-compensator is G c (z) removing dynamic measurement errors caused by the dynamic characteristics of the six-dimensional force sensor to obtain a dynamic compensation signal L2=G c (z)·L1。
The gravity compensation 11 of the end model is to obtain the attitude matrix T of the end flange in the world coordinate system C0 according to the attitude parameters of the end flange returned by the robot f Then calculate the attitude matrix T of six-dimensional force sensor in the world coordinate system C0 s The method comprises the steps of carrying out a first treatment on the surface of the Then according to the quality parameters of the terminal model and the gesture matrix T s Calculating the gravity component F of the end model G And compensates.
According to the actual installation condition of the robot, a rotation matrix T of the robot base coordinate system C1 around the world coordinate system C0 can be obtained r The method comprises the steps of carrying out a first treatment on the surface of the According to the actual installation condition of the six-dimensional force sensor, a rotation matrix T of a six-dimensional force sensor coordinate system C3 relative to a robot terminal flange coordinate system C2 can be obtained s-f Consider a six-dimensional force sensor mounting error angle Δr= [ Δα, Δβ, Δλ]The pose matrix of the six-dimensional force sensor coordinate system C3 under the world coordinate system C0 is:
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 by the method z O z A z ]. Because the gravity direction of the end model is parallel to the negative Z-axis direction of the world coordinate system C0, the cosine of the gravity direction of the end model in the six-dimensional force sensor coordinate system C3 is- [ N ] z O z A z ]. Accordingly, the six-dimensional force sensor output error caused by the gravity of the end model is obtained:
then, a gravity compensation signal L3=L2-F of the six-dimensional force sensor can be obtained G 。
The end model inertial compensation 12 is to calculate the end model inertial force/moment F under the six-dimensional force sensor coordinate system C3 according to the real-time motion acceleration and inertial parameters of the robot end model inertia ＝[F ix ,F iy ,F iz ,M ix ,M iy ,M iz ] T And compensates. The end model acceleration can be obtained by two methods:
the method comprises the following steps: setting the centroid position of the end model as the origin of coordinates of a robot tool, and outputting the acceleration A of the end model under a coordinate system C4 by the robot dp ＝[A x ,A y ,A z ,A rx ,A ry ,A rz ] T ；
The second method is as follows: the inertial measurement unit is arranged at the supporting end of the sensor, and the output of the inertial measurement unit is obtained and converted into the acceleration A under the coordinate system C4 dp ＝[A x ,A y ,A z ,A rx ,A ry ,A rz ] T 。
The inertial force/moment generated by the end model is in the end model coordinate system C4:
according to the coordinate Pm= [ x ] of the centroid of the end model in the six-dimensional force sensor coordinate system m ,y m ,z m ] T Inertial force/moment F of the end model in the six-dimensional force sensor coordinate system C3 inertia The method comprises the following steps:
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 ]。
The coordinate system is converted 13 to obtain the external dynamic contact force F under the six-dimensional force sensor coordinate system C3 sc Converting into an end model coordinate system C4 so as to carry out subsequent robot control tasks; external dynamic contact force F in the end model coordinate system C4 mc The method comprises the following steps:
the invention has the advantages that: aiming at the problem that a six-dimensional force sensor arranged at the tail end of a robot is required to be adopted to measure the dynamic contact force of a tail end model in a robot control task, a complete force signal error compensation and processing method is provided, and the six-dimensional force sensor installation error and the six-dimensional force sensor dynamic error which can be ignored in force measurement are fully considered, so that the gravity compensation result and the dynamic measurement precision of external contact force are further optimized; and a simple and reliable robot automatic calibration method is provided for constant data needed in force signal processing, so that the robot can automatically calibrate parameters needed in the contact force measurement error compensation of the tail end model, and the working efficiency is improved.
Drawings
FIG. 1 is a technical flow diagram of the method of the present invention;
FIG. 2 is a schematic diagram of a robot end model contact force measurement system and its coordinate system according to the method of the present invention;
FIG. 3 is a schematic diagram of the installation of a six-dimensional force sensor in a dynamic calibration experiment according to an embodiment of the present invention;
FIG. 4 is a flowchart for automatically calibrating the offset of the six-dimensional force sensor and the quality characteristic parameters of the end model according to the embodiment of the invention;
FIG. 5 is a schematic diagram of the gravity components of a tip model in a six-dimensional force sensor coordinate system in accordance with an embodiment of the present invention;
FIG. 6 is a flow chart of automatic calibration of mounting errors of a six-dimensional force sensor in accordance with an embodiment of the present invention;
FIG. 7 is a flow chart of automatic calibration of an end model inertial tensor matrix in accordance with an embodiment of the present invention;
fig. 8 is a schematic diagram of a process flow for real-time error compensation of dynamic contact force measurement results of a six-dimensional force sensor according to an embodiment of the present invention.
Detailed Description
The invention is further described below with reference to the accompanying drawings:
the design idea of the invention is as follows: aiming at the problem that a six-dimensional force sensor arranged on a flange at the tail end of a robot needs to accurately sense the dynamic contact force of a tail end model in the process of controlling the movement of the robot, firstly, the six-dimensional force sensor is dynamically calibrated, a dynamic-decoupling compensator is designed according to the six-dimensional force sensor, then, parameters required by error compensation are automatically calibrated sequentially through the control of the robot, and finally, each error component of the six-dimensional force sensor is compensated in real time in a robot manual control task and is subjected to coordinate system conversion, so that accurate dynamic contact force data born by the tail end model is obtained. Specifically, a dynamic calibration experiment is carried out on the six-dimensional force sensor, a dynamic-decoupling compensator of the six-dimensional force sensor is designed according to dynamic calibration experiment data, the dynamic-decoupling compensator is used for dynamic decoupling compensation of dynamic measurement output signals of the six-dimensional force sensor, and dynamic errors caused by the dynamic characteristics of the six-dimensional force sensor are corrected; then, automatically calibrating offset of the six-dimensional force sensor, gravity of the tail end model, mass center position of the tail end model, installation error angle of the six-dimensional force sensor and inertial parameters of the tail end model in sequence through automatic calibration; finally, the force signals measured and output by the six-dimensional force sensor are preprocessed, dynamic error corrected, end model gravity compensated, end model inertia compensated and model coordinate system converted in sequence in the practical robot power control application according to the designed dynamic-decoupling compensator and the error compensation parameters for automatic calibration of the robot control, so that accurate dynamic contact force data are obtained.
The technical scheme of the invention is shown in the figure 1, and the specific flow is as follows: the six-dimensional force sensor offline dynamic calibration 1, the online automatic calibration 2 of the characteristic parameters of the model contact force measurement system, and the real-time error compensation treatment 3 of the dynamic contact force. The method of the invention adopts a world coordinate system C0, a robot base coordinate system C1, a robot tail end flange coordinate system C2, a sensor coordinate system C3 and a model coordinate system C4 to carry out auxiliary explanation of the method, as shown in figure 2. Wherein, the directions of the axes of the world coordinate system C0 and the robot base coordinate system C1 are the same and are all fixed coordinate systems; the robot tail end flange coordinate system C2 is positioned on the mounting flange at the tail end of the robot; the sensor coordinate system C3 is a measurement coordinate system of the six-dimensional force sensor; the origin of the model coordinate system C4 is located at the centroid position of the terminal model; the relation among the robot end flange coordinate system C2, the sensor coordinate system C3 and the model coordinate system C4 is fixed by the installation relation among the robot end flange, the six-dimensional force sensor and the end model, and moves along with the robot end; in general, when the six-dimensional force sensor and the end model are mounted, 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 direction of the robot end flange coordinate system C2.
The off-line dynamic calibration 1 of the six-dimensional force sensor is to perform a dynamic calibration experiment on the six-dimensional force sensor with the end model, obtain the dynamic calibration experimental data of the six-dimensional force sensor with the end model, and design a dynamic decoupling-compensating device of the six-dimensional force sensor according to the dynamic calibration experimental data, and the dynamic decoupling-compensating device is used for performing real-time dynamic decoupling-compensation on the actual measurement output of the six-dimensional force sensor. The process comprises the following steps: dynamic calibration experiment 4→dynamic decoupling-compensator design 5.
The dynamic calibration experiment 4 is to fixedly mount the support end of the six-dimensional force sensor on a rigid calibration table with the rigidity far greater than that of the sensitive element of the six-dimensional force sensor, and the measurement end of the six-dimensional force sensor is provided with an end model, as shown in fig. 3; and applying dynamic excitation force to the end model, recording dynamic excitation force data and six-dimensional force sensor output data, and completing a six-dimensional force sensor dynamic calibration experiment. The dynamic excitation force is determined according to the loading condition of the actual end model, and the step excitation force is optimized; when the step excitation force is inconvenient to apply to the model, the impact excitation force is applied to the end model by adopting a force hammer knocking method.
The dynamic decoupling-compensator design 5 is to design a dynamic decoupling-compensator Gc (z) capable of simultaneously removing the main channel dynamic error and the inter-dimensional coupling channel dynamic error of the six-dimensional force sensor according to the dynamic excitation force data and the six-dimensional force sensor output data obtained in the dynamic calibration experiment 4 by adopting the prior art, such as a diagonal dominant dynamic decoupling-compensation method or an iterative dynamic decoupling-compensation method.
The characteristic parameters of the model contact force measurement system are automatically calibrated on line 2, namely, the installation errors of the six-dimensional force sensor, the offset of the six-dimensional force sensor, the quality parameters of the end model and the inertial parameters of the end model are automatically calibrated by the control of a robot, and the automatic calibration flow is as follows: six-dimensional force sensor bias and model mass parameter calibration 6, six-dimensional force sensor installation error calibration 7 and end model inertia parameter calibration 8.
Six-dimensional force sensor offset and model quality parameter calibration 6, namely, automatic and simultaneous calibration of six-dimensional force sensor offset F under robot control bias ＝[V1,V2,V3,V4,V5,V6] T End model mass m f And the position pm= [ x ] of the model centroid in the sensor coordinate system C3 m ,y m ,z m ] T The method comprises the steps of carrying out a first treatment on the surface of the The automatic calibration control flow of the robot is shown in fig. 4:
step one: the robot is controlled to level the u-axis of the robot end flange coordinate system C2, u being the X-axis, Y-axis or Z-axis of the coordinate system C2.
Step two: controlling the robot to enable the model to rotate around the u axis of the flange coordinate system C2 of the tail end of the robot in a stepping mode by a stepping angle phi for one circle, starting to record sensor data of t1 time after each stepping position is static for t0 time, and averaging the sensor data in the t1 time to obtain N b Average values Fu_1, fu_ … Fu_N of six-dimensional force sensor output data of each step angle position b The method comprises the steps of carrying out a first treatment on the surface of the Wherein phi is divisible by 180, N b ＝360/φ。
The sensor offset F is obtained by bias 。
For N b Rotation angle mutual difference in data of six-dimensional force sensor180℃data Fu_i and Fu_ (N b 2+i) is treated as follows.
Wherein i=1, 2,3, …, N b /2。
Then calculate the end model qualityg is gravitational acceleration.
For N b Group/2Averaging to obtain the model mass calibrated under the condition that the model rotates around the u axis of the terminal flange coordinate system C2>
Coordinate pm= [ x ] of terminal model centroid in six-dimensional force sensor coordinate system C3 m ,y m ,z m ] T As shown in fig. 5; it is easy to know 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 ]The method comprises the following steps:
converting into a matrix form:
using the six-dimensional force data Fu_1, fu_ … Fu_N b (N b =360/Φ) and subtracting the offsetAnd combining the force and moment data into the following matrix according to the rules.
And (3) solving the position of the mass center of the model, which is obtained by calibration under the condition that the model rotates around the u axis of the coordinate system C2, in the sensor coordinate system C3 by adopting a least square method according to the above method:
Pm u ＝(G n T ·G n ) -1 ·G n T ·M gn 。
step three: selecting different axes in X, Y, Z axes in the end flange coordinate system C2 as u axes, and repeating the robot control and data processing flow calculation to obtain 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 respectively averaging to obtain:
the model gravity g=mf·g, G is the gravitational acceleration.
The six-dimensional force sensor installation error calibration 7 is the installation error delta R= [ delta alpha, delta beta, delta lambda ] between the calibration six-dimensional force sensor coordinate system C3 and the robot terminal flange coordinate system C2.
The automatic calibration flow of Δα is shown in fig. 6, and is specifically as follows:
step one: controlling the robot to enable the Z axis of the six-dimensional force sensor coordinate system C3 to be vertical and the X axis to be horizontal;
step two: the robot is controlled to enable the tail end model to rotate around the X axis of the six-dimensional force sensor coordinate system C3 by an angle theta, the angle theta is preferably 45 degrees, and after the tail end model is stationary, six-dimensional force sensor output data F2= [ F ] are recorded x2 ,F y2 ,F z2 ,M x2 ,M y2 ,M z2 ] T ；
Step three: repeating the first step;
step four: controlling the robot to enable the tail end model to rotate by an angle theta around the X axis of a six-dimensional force sensor coordinate system C3; recording output data F3= [ F ] of six-dimensional force sensor in static state x3 ,F y3 ,F z3 ,M x3 ,M y3 ,M z3 ] T ；
Step five: z-axis force data from F2, F3 are available:
calculated from the above formula: Δα=arcsin [ (F) z2 -F z3 )/2G·sin(θ)]。
Step six: and selecting different theta angles, repeating the steps to calculate delta alpha, and taking the average as a final installation error angle delta alpha.
The automatic calibration flow of delta beta is as follows:
step one: controlling the robot to enable the Z axis of the six-dimensional force sensor coordinate system C3 to be vertical and the Y axis to be horizontal;
step two: the robot is controlled to enable the tail end model to rotate around the Y axis of the six-dimensional force sensor coordinate system C3 by an angle theta, the angle theta is preferably 45 degrees, and after the tail end model is stationary, six-dimensional force sensor output data F4= [ F ] are recorded x4 ,F y4 ,F z4 ,M x4 ,M y4 ,M z4 ] T ；
Step three: repeating the first step;
step four: controlling the robot to enable the tail end model to rotate by an angle theta around a Y axis of a six-dimensional force sensor coordinate system C3; recording output data F5= [ F ] of six-dimensional force sensor in static state x5 ,F y5 ,F z5 ,M x5 ,M y5 ,M z5 ] T ；
Step five: z-axis force data from F4, F5 are available:
calculated from the above formula: Δβ=arcsin [ (F) z4 -F z5 )/2G·sin(θ)]。
Step six: and selecting different theta angles, repeating the steps to calculate delta beta and taking the average as the final installation error angle delta beta.
The automatic calibration procedure for Δλ is as follows:
step one: controlling the robot to enable the X axis of the six-dimensional force sensor coordinate system C3 to be vertical and the Z axis to be horizontal;
step two: the robot is controlled to enable the tail end model to rotate around the Z axis of the six-dimensional force sensor coordinate system C3 by an angle theta, the angle theta is preferably 45 degrees, and after the tail end model is stationary, six-dimensional force sensor output data F6= [ F ] are recorded x6 ,F y6 ,F z6 ,M x6 ,M y6 ,M z6 ] T ；
Step three: repeating the first step;
step four: controlling the robot to enable the tail end model to rotate by an angle theta around a Y axis of a six-dimensional force sensor coordinate system C3; recording output data F7= [ F ] of six-dimensional force sensor in static state x7 ,F y7 ,F z7 ,M x7 ,M y7 ,M z7 ] T ；
Step five: x-axis force data from F6, F7 are available
Calculated from the above formula: Δλ=arcsin [ (F) x6 -F x7 )/2G·sin(θ)]；
Step six: and selecting different theta angles, repeating the steps to calculate delta lambda and taking the average as a final installation error angle delta lambda.
The inertial parameter calibration 8 of the end model is that the inertial tensor of the end model is automatically calibrated by robot controlMatrix I. The robot is controlled to make the terminal model execute m times of uniform acceleration rotation movements around the mass center of the terminal model, m is more than or equal to 3, and a matrix A consisting of a plurality of groups of experimental angular acceleration vectors m The rank of (2) is not less than 3. The end model acceleration can be obtained by two methods:
the method comprises the following steps: setting the centroid position of the end model as the origin of coordinates of a robot tool, and outputting the acceleration A of the end model under a model coordinate system C4 by the robot dp ＝[A x ,A y ,A z ,A rx ,A ry ,A rz ] T ；
The second method is as follows: the inertial measurement unit is arranged at the support end of the six-dimensional force sensor, and the output of the inertial measurement unit is obtained and converted into the acceleration A under the model coordinate system C4 dp ＝[A x ,A y ,A z ,A rx ,A ry ,A rz ] T 。
The automatic calibration flow of the inertial parameters I of the end model is shown in fig. 7, and the specific steps are as follows:
step one: controlling the robot to be at a standby position;
step two: control model with angular acceleration A r1 Executing the acceleration rotation motion for a time t2 from the standby position, and recording the model angular acceleration A in the motion process rx 、A ry 、A rz Simultaneously recording output data of the six-dimensional force sensor, and performing unbiasing and gravity compensation on the output data to obtain F i1 ＝[F ix1 ,F iy1 ,F iz1 ,M ix1 ,M iy1 ,M iz1 ]
Step three: updating the control value of the model angular acceleration, wherein the new angular acceleration is independent of the angular velocity vector linearity of the previous step, and repeating the previous step m times.
Step four: from m sets of six-dimensional force signals F i1 、F i2 …F im Medium moment data composition matrix M im M groups of end model acceleration data form matrix A m The following steps are:
multiplying the upper formula by M im -1 Right multiplication A m -1 Obtaining: m is M im -1 ·I＝-A m -1 Order-makingW m ＝-A m -1 According to the least square method, i= (U m T ·U m ) -1 ·U m T ·W m 。
The dynamic contact force real-time error compensation process 3 is the terminal model mass m according to the calibration f Centroid position Pm, six-dimensional force sensor mounting error Δr= [ Δα, Δβ, Δλ]The inertial tensor matrix I of the tail end model performs error compensation processing on dynamic force signals measured by the six-dimensional force sensor in real time in robot manual control movement so as to obtain external dynamic contact force born by the tail end model under a coordinate system C4 of the tail end model, and the external dynamic contact force is used for force feedback in the robot manual control task; the process comprises the following steps: signal preprocessing 9- & gt dynamic error correction 10- & gt end model gravity compensation 11- & gt end model inertia compensation 12- & gt coordinate system conversion 13, as shown in figure 8.
Signal preprocessing 9, namely subtracting the six-dimensional force sensor offset F from the original six-dimensional force sensor signal L0 bias And low-pass filtering is performed to filter out high-frequency components of the signal to obtain a preprocessed signal L1.
The dynamic error correction 10 is to dynamically decouple-compensate the preprocessed signal L1 in real time by adopting a dynamic decoupling-compensator designed in the off-line dynamic calibration 1 of the six-dimensional force sensor, wherein the transfer function of the dynamic decoupling-compensator is G c (z) removing dynamic measurement errors caused by the dynamic characteristics of the six-dimensional force sensor to obtain a dynamic compensation signal L2=G c (z)·L1。
The gravity compensation 11 of the end model is to obtain the attitude matrix T of the end flange in the world coordinate system C0 according to the attitude parameters of the end flange returned by the robot f Then calculate the attitude matrix T of six-dimensional force sensor in the world coordinate system C0 s The method comprises the steps of carrying out a first treatment on the surface of the Then according to the quality parameters and the pose of the terminal modelState matrix T s Calculating the gravity component F of the end model G And compensates.
According to the actual installation condition of the robot, a rotation matrix T of the robot base coordinate system C1 around the world coordinate system C0 can be obtained r The method comprises the steps of carrying out a first treatment on the surface of the According to the actual installation condition of the six-dimensional force sensor, a rotation matrix T of a six-dimensional force sensor coordinate system C3 relative to a robot terminal flange coordinate system C2 can be obtained s-f Consider a six-dimensional force sensor mounting error angle Δr= [ Δα, Δβ, Δλ]The pose matrix of the six-dimensional force sensor coordinate system C3 under the world coordinate system C0 is:
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 by the method z O z A z ]. Because the gravity direction of the end model is parallel to the negative Z-axis direction of the world coordinate system C0, the cosine of the gravity direction of the end model in the six-dimensional force sensor coordinate system C3 is- [ N ] z O z A z ]. Accordingly, the six-dimensional force sensor output error caused by the gravity of the end model is obtained:
then, a gravity compensation signal L3=L2-F of the six-dimensional force sensor can be obtained G 。
The end model inertial compensation 12 is to calculate the end model inertial force/moment F under the six-dimensional force sensor coordinate system C3 according to the real-time motion acceleration and inertial parameters of the robot end model inertia ＝[F ix ,F iy ,F iz ,M ix ,M iy ,M iz ] T And compensates. The end model acceleration can be obtained by two methods:
the method comprises the following steps: setting the centroid position of the end model as the origin of coordinates of the robot tool, and outputting the end model by the robot under a coordinate system C4Acceleration A dp ＝[A x ,A y ,A z ,A rx ,A ry ,A rz ] T ；
The second method is as follows: the inertial measurement unit is arranged at the supporting end of the sensor, and the output of the inertial measurement unit is obtained and converted into the acceleration A under the coordinate system C4 dp ＝[A x ,A y ,A z ,A rx ,A ry ,A rz ] T 。
The inertial force/moment generated by the end model is in the end model coordinate system C4:
/>
according to the coordinate Pm= [ x ] of the centroid of the end model in the six-dimensional force sensor coordinate system m ,y m ,z m ] T Inertial force/moment F of the end model in the six-dimensional force sensor coordinate system C3 inertia The method comprises the following steps:
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 ]。
The coordinate system is converted 13 to obtain the external dynamic contact force F under the six-dimensional force sensor coordinate system C3 sc Converting into an end model coordinate system C4 so as to carry out subsequent robot control tasks; external dynamic contact force F in the end model coordinate system C4 mc The method comprises the following steps:
/>

Claims (3)

1. the method for compensating and processing the dynamic contact force measurement errors of the six-dimensional force sensor mounting errors, sensor dynamic characteristic errors, model gravity errors and model inertia errors in a robot tail end model comprises the following technical processes: the method is characterized by comprising the steps of off-line dynamic calibration of a six-dimensional force sensor, on-line automatic calibration of characteristic parameters of a model contact force measurement system and real-time error compensation treatment of dynamic contact force, and comprises the following steps of:
firstly, performing off-line dynamic calibration experiments on a six-dimensional force sensor with an end model, and designing a dynamic decoupling-compensator of the six-dimensional force sensor according to the dynamic calibration experimental data of the six-dimensional force sensor, wherein the dynamic decoupling-compensator is used for dynamically correcting dynamic errors caused by the characteristics of the six-dimensional force sensor; the on-line automatic calibration is carried out on the offset of the six-dimensional force sensor, the quality parameter of the tail end model, the installation error of the six-dimensional force sensor and the inertial parameter of the tail end model by controlling the robot, so that the parameters required by the dynamic contact force error compensation are obtained; and finally, sequentially performing real-time error compensation processing of unbiasing, dynamic decoupling-compensation, end model gravity compensation, end model inertia compensation and model coordinate system conversion on the six-dimensional force sensor measurement output signal in a robot manual control task according to the six-dimensional force sensor dynamic decoupling-compensator and the parameters of on-line automatic calibration, so as to obtain accurate dynamic contact force data born by the end model.
2. The method for error compensation and processing of dynamic contact force measurement of a robot end model according to claim 1, wherein: the characteristic parameters of the model contact force measurement system are automatically calibrated on line, namely, the installation errors of the six-dimensional force sensor, the offset of the six-dimensional force sensor, the quality parameters of the end model and the inertial parameters of the end model are automatically calibrated by the control of a robot, and the automatic calibration flow is as follows: six-dimensional force sensor bias and model mass parameter calibration, six-dimensional force sensor installation error calibration and end model inertia parameter calibration;
six-dimensional force sensor bias and model quality parameter calibrationNamely, the offset F of the six-dimensional force sensor is automatically calibrated at the same time under the control of a robot bias ＝[V1,V2,V3,V4,V5,V6] T End model mass m f And the position pm= [ x ] of the model centroid in the sensor coordinate system C3 m ,y m ,z m ] T The method comprises the steps of carrying out a first treatment on the surface of the The robot automatic calibration control flow is as follows:
step one: controlling the robot to enable the u axis of the flange coordinate system C2 at the tail end of the robot to be horizontal, wherein u is the X axis, the Y axis or the Z axis of the coordinate system C2;
step two: controlling the robot to enable the model to rotate around the u axis of the flange coordinate system C2 of the tail end of the robot in a stepping mode by a stepping angle phi for one circle, starting to record sensor data of t1 time after each stepping position is static for t0 time, and averaging the sensor data in the t1 time to obtain N b Average values Fu_1, fu_ … Fu_N of six-dimensional force sensor output data of each step angle position b The method comprises the steps of carrying out a first treatment on the surface of the Wherein phi is divisible by 180, N b ＝360/φ；
The sensor offset F is obtained by bias ：
For N b Data fu_i and fu_ (N) of 180 ° difference in rotation angle among the six-dimensional force sensor data of group b 2+i) performing the following treatment;
wherein i=1, 2,3, …, N b 2; then calculate the end model qualityg is gravity acceleration; for N b Group 2>Averaging to obtain a model around the end flange coordinate system C2Model mass m obtained through calibration under u-axis rotation condition f u ；
Coordinate pm= [ x ] of terminal model centroid in six-dimensional force sensor coordinate system C3 m ,y m ,z m ] T It is easy to know 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 ]The method comprises the following steps:
converting into a matrix form:
using the six-dimensional force data Fu_1, fu_ … Fu_N b (N b =360/Φ) and subtracting the offsetCombining the force and moment data into the following matrix according to the rules;
and (3) solving the position of the mass center of the model, which is obtained by calibration under the condition that the model rotates around the u axis of the coordinate system C2, in the sensor coordinate system C3 by adopting a least square method according to the above method: pm (Pm) u ＝(G n T ·G n ) -1 ·G n T ·M gn ；
Step three: selecting different axes in X, Y, Z axes in the end flange coordinate system C2 as u axes, and repeating the robot control and data processing flow calculation to obtain 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 respectively averaging to obtain:
the model gravity g=mf·g, G is the gravitational acceleration;
calibration of the installation error of the six-dimensional force sensor, namely calibration of the installation error delta R= [ delta alpha, delta beta, delta lambda ] between the coordinate system C3 of the six-dimensional force sensor and the coordinate system C2 of the flange at the tail end of the robot;
the automatic calibration procedure for Δα is as follows:
step one: controlling the robot to enable the Z axis of the six-dimensional force sensor coordinate system C3 to be vertical and the X axis to be horizontal;
step two: the robot is controlled to enable the tail end model to rotate around the X axis of the six-dimensional force sensor coordinate system C3 by an angle theta, the angle theta is preferably 45 degrees, and after the tail end model is stationary, six-dimensional force sensor output data F2= [ F ] are recorded x2 ,F y2 ,F z2 ,M x2 ,M y2 ,M z2 ] T ；
Step three: repeating the first step;
step four: controlling the robot to enable the tail end model to rotate by an angle theta around the X axis of a six-dimensional force sensor coordinate system C3; recording output data F3= [ F ] of six-dimensional force sensor in static state x3 ,F y3 ,F z3 ,M x3 ,M y3 ,M z3 ] T ；
Step five: z-axis force data from F2, F3 are available:
calculated from the above formula: Δα=arcsin [ (F) z2 -F z3 )/2G·sin(θ)]；
Step six: selecting different theta angles, repeating the steps to calculate delta alpha and taking the average as a final installation error angle delta alpha;
the automatic calibration flow of delta beta is as follows:
step one: controlling the robot to enable the Z axis of the six-dimensional force sensor coordinate system C3 to be vertical and the Y axis to be horizontal;
Step two: the robot is controlled to enable the tail end model to rotate around the Y axis of the six-dimensional force sensor coordinate system C3 by an angle theta, the angle theta is preferably 45 degrees, and after the tail end model is stationary, six-dimensional force sensor output data F4= [ F ] are recorded x4 ,F y4 ,F z4 ,M x4 ,M y4 ,M z4 ] T ；
Step three: repeating the first step;
step four: controlling the robot to enable the tail end model to rotate by an angle theta around a Y axis of a six-dimensional force sensor coordinate system C3; recording output data F5= [ F ] of six-dimensional force sensor in static state x5 ,F y5 ,F z5 ,M x5 ,M y5 ,M z5 ] T ；
Step five: z-axis force data from F4, F5 are available:
calculated from the above formula: Δβ=arcsin [ (F) z4 -F z5 )/2G·sin(θ)]；
Step six: selecting different theta angles, repeating the steps to calculate delta beta and taking the average as a final installation error angle delta beta;
the automatic calibration procedure for Δλ is as follows:
step one: controlling the robot to enable the X axis of the six-dimensional force sensor coordinate system C3 to be vertical and the Z axis to be horizontal;
step two: the robot is controlled to enable the tail end model to rotate by an angle theta, preferably 45 degrees, around the Z axis of a six-dimensional force sensor coordinate system C3, and the tail end model is stationaryPost-recording six-dimensional force sensor output data F6= [ F x6 ,F y6 ,F z6 ,M x6 ,M y6 ,M z6 ] T ；
Step three: repeating the first step;
step four: controlling the robot to enable the tail end model to rotate by an angle theta around a Y axis of a six-dimensional force sensor coordinate system C3; recording output data F7= [ F ] of six-dimensional force sensor in static state x7 ,F y7 ,F z7 ,M x7 ,M y7 ,M z7 ] T ；
Step five: x-axis force data from F6, F7 are available
Calculated from the above formula: Δλ=arcsin [ (F) x6 -F x7 )/2G·sin(θ)]；
Step six: selecting different theta angles, repeating the steps to calculate delta lambda and taking the average as a final installation error angle delta lambda;
calibrating inertial parameters of the end model, namely automatically calibrating an inertial tensor matrix I of the end model by robot control; the robot is controlled to make the terminal model execute m times of uniform acceleration rotation movements around the mass center of the terminal model, m is more than or equal to 3, and a matrix A consisting of a plurality of groups of experimental angular acceleration vectors m The rank of (2) is more than or equal to 3; the end model acceleration can be obtained by two methods:
the method comprises the following steps: setting the centroid position of the end model as the origin of coordinates of a robot tool, and outputting the acceleration A of the end model under a model coordinate system C4 by the robot dp ＝[A x ,A y ,A z ,A rx ,A ry ,A rz ] T ；
The second method is as follows: the inertial measurement unit is arranged at the support end of the six-dimensional force sensor, and the output of the inertial measurement unit is obtained and converted into the acceleration A under the model coordinate system C4 dp ＝[A x ,A y ,A z ,A rx ,A ry ,A rz ] T ；
The automatic calibration flow of the inertial parameters I of the end model is as follows:
step one: controlling the robot to be at a standby position;
step two: control model with angular acceleration A r1 Executing the acceleration rotation motion for a time t2 from the standby position, and recording the model angular acceleration A in the motion process rx 、A ry 、A rz Simultaneously recording output data of the six-dimensional force sensor, and performing unbiasing and gravity compensation on the output data to obtain F i1 ＝[F ix1 ,F iy1 ,F iz1 ,M ix1 ,M iy1 ,M iz1 ]
Step three: updating a control value of the model angular acceleration, wherein the new angular acceleration is linearly independent of the angular velocity vector of the previous step, and repeating the previous step m times;
step four: from m sets of six-dimensional force signals F i1 、F i2 …F im Medium moment data composition matrix M im M groups of end model acceleration data form matrix A m The following steps are:
multiplying the upper formula by M im -1 Right multiplication A m -1 Obtaining: m is M im -1 ·I＝-A m -1 Order-makingW m ＝-A m -1 According to the least square method, i= (U m T ·U m ) -1 ·U m T ·W m 。
3. The method for error compensation and processing of dynamic contact force measurement of a robot end model according to claim 1, wherein: the dynamic contact force real-time error compensation processing is that the terminal model mass m is calibrated according to the above f Centroid position Pm, six-dimensional force sensor mounting error Δr= [ Δα, Δβ,Δλ]The inertial tensor matrix I of the tail end model performs error compensation processing on dynamic force signals measured by the six-dimensional force sensor in real time in robot manual control movement so as to obtain external dynamic contact force born by the tail end model under a coordinate system C4 of the tail end model, and the external dynamic contact force is used for force feedback in the robot manual control task; the process comprises the following steps: signal preprocessing, dynamic error correction, end model gravity compensation, end model inertia compensation and coordinate system conversion;
Signal preprocessing, namely subtracting the offset F of the six-dimensional force sensor from the original six-dimensional force sensor signal L0 bias And performing low-pass filtering to filter out high-frequency components of the signals to obtain a preprocessed signal L1;
the dynamic error correction is that a dynamic decoupling-compensator designed in the off-line dynamic calibration of the six-dimensional force sensor is adopted to dynamically decouple-compensate the preprocessed signal L1 in real time, and the transfer function of the dynamic decoupling-compensator is G c (z) removing dynamic measurement errors caused by the dynamic characteristics of the six-dimensional force sensor to obtain a dynamic compensation signal L2=G c (z)·L1；
The gravity compensation of the tail end model is that the gesture matrix T of the tail end flange in the world coordinate system C0 is obtained according to the gesture parameters of the tail end flange returned by the robot f Then calculate the attitude matrix T of six-dimensional force sensor in the world coordinate system C0 s The method comprises the steps of carrying out a first treatment on the surface of the Then according to the quality parameters of the terminal model and the gesture matrix T s Calculating the gravity component F of the end model G And compensating;
according to the actual installation condition of the robot, a rotation matrix T of the robot base coordinate system C1 around the world coordinate system C0 can be obtained r The method comprises the steps of carrying out a first treatment on the surface of the According to the actual installation condition of the six-dimensional force sensor, a rotation matrix T of a six-dimensional force sensor coordinate system C3 relative to a robot terminal flange coordinate system C2 can be obtained s-f Consider a six-dimensional force sensor mounting error angle Δr= [ Δα, Δβ, Δλ]The pose matrix of the six-dimensional force sensor coordinate system C3 under the world coordinate system C0 is:
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 by the method z O z A z ]The method comprises the steps of carrying out a first treatment on the surface of the Because the gravity direction of the end model is parallel to the negative Z-axis direction of the world coordinate system C0, the cosine of the gravity direction of the end model in the six-dimensional force sensor coordinate system C3 is- [ N ] z O z A z ]The method comprises the steps of carrying out a first treatment on the surface of the Accordingly, the six-dimensional force sensor output error caused by the gravity of the end model is obtained:
then, a gravity compensation signal L3=L2-F of the six-dimensional force sensor can be obtained G ；
The inertial compensation of the end model is that the inertial force/moment F of the end model under the coordinate system C3 of the six-dimensional force sensor is calculated according to the real-time motion acceleration and the inertial parameter of the end model of the robot inertia ＝[F ix ,F iy ,F iz ,M ix ,M iy ,M iz ] T And compensating; the end model acceleration can be obtained by two methods:
the method comprises the following steps: setting the centroid position of the end model as the origin of coordinates of a robot tool, and outputting the acceleration A of the end model under a coordinate system C4 by the robot dp ＝[A x ,A y ,A z ,A rx ,A ry ,A rz ] T ；
The second method is as follows: the inertial measurement unit is arranged at the supporting end of the sensor, and the output of the inertial measurement unit is obtained and converted into the acceleration A under the coordinate system C4 dp ＝[A x ,A y ,A z ,A rx ,A ry ,A rz ] T ；
The inertial force/moment generated by the end model is in the end model coordinate system C4:
according to the coordinate Pm= [ x ] of the centroid of the end model in the six-dimensional force sensor coordinate system m ,y m ,z m ] T Inertial force/moment F of the end model in the six-dimensional force sensor coordinate system C3 inertia The method comprises the following steps:
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 ]；
The coordinate system conversion is to convert the external dynamic contact force F under the six-dimensional force sensor coordinate system C3 sc Converting into an end model coordinate system C4 so as to carry out subsequent robot control tasks; external dynamic contact force F in the end model coordinate system C4 mc The method comprises the following steps:
CN202310689697.7A 2023-06-09 2023-06-09 Error compensation and processing method for dynamic contact force measurement of robot tail end model Pending CN116652953A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310689697.7A CN116652953A (en) 2023-06-09 2023-06-09 Error compensation and processing method for dynamic contact force measurement of robot tail end model

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310689697.7A CN116652953A (en) 2023-06-09 2023-06-09 Error compensation and processing method for dynamic contact force measurement of robot tail end model

Publications (1)

Publication Number Publication Date
CN116652953A true CN116652953A (en) 2023-08-29

Family

ID=87711634

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310689697.7A Pending CN116652953A (en) 2023-06-09 2023-06-09 Error compensation and processing method for dynamic contact force measurement of robot tail end model

Country Status (1)

Country Link
CN (1) CN116652953A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116901087A (en) * 2023-09-13 2023-10-20 真健康（北京）医疗科技有限公司 Method and equipment for determining tail end force and moment of puncture operation robot

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116901087A (en) * 2023-09-13 2023-10-20 真健康（北京）医疗科技有限公司 Method and equipment for determining tail end force and moment of puncture operation robot
CN116901087B (en) * 2023-09-13 2023-11-17 真健康（北京）医疗科技有限公司 Method and equipment for determining tail end force and moment of puncture operation robot

Similar Documents

Publication Publication Date Title
CN109822574B (en) Industrial robot end six-dimensional force sensor calibration method
CN108297101B (en) Multi-joint-arm series robot end pose error detection and dynamic compensation method
CN107042528B (en) A kind of Kinematic Calibration system and method for industrial robot
CN110757504B (en) Positioning error compensation method of high-precision movable robot
US9969088B2 (en) Force sensor correcting method
CN109877840B (en) Double-mechanical-arm calibration method based on camera optical axis constraint
CN107817682B (en) Space manipulator on-orbit calibration method and system based on hand-eye camera
US20160298959A1 (en) Calibration of motion systems
CN110666799A (en) Six-degree-of-freedom series robot compliance control method based on gravity compensation
CN110802585B (en) Mechanical arm tail end sensor compensation method and contact force/moment measurement method
CN113386136B (en) Robot posture correction method and system based on standard spherical array target estimation
CN116652953A (en) Error compensation and processing method for dynamic contact force measurement of robot tail end model
CN112504298B (en) GNSS-assisted DVL error calibration method
WO2022183761A1 (en) Spatial pose real-time measurement and adjustment method based on joint calibration
CN110715769A (en) Method for calibrating stress point position of weighing sensor of multi-point method centroid measuring equipment
CN111489399B (en) Device and method for calibrating installation parameters of visual tracking assembly
CN109062139B (en) Robot linear axis positioning error compensation method based on data driving
CN114216456A (en) Attitude measurement method based on IMU and robot body parameter fusion
CN113211445A (en) Robot parameter calibration method, device, equipment and storage medium
CN113319855B (en) Gravity compensation method under compliance control mode of multi-joint diagnosis and treatment robot
CN113513999B (en) Large-view-field real-time deformation measurement system and method for static test of spacecraft structure
CN114029982A (en) Hand-eye calibration device and calibration method of camera outside robot arm
US20150081100A1 (en) Method for calibrating kinematics
JPH10301609A (en) Position error detection method for robot and device therefor
CN111360585B (en) Method for acquiring real-time position error of cutter end in robot milling system

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination