CN117532623B - Mechanical arm external torque estimation method - Google Patents
Mechanical arm external torque estimation method Download PDFInfo
- Publication number
- CN117532623B CN117532623B CN202410033238.8A CN202410033238A CN117532623B CN 117532623 B CN117532623 B CN 117532623B CN 202410033238 A CN202410033238 A CN 202410033238A CN 117532623 B CN117532623 B CN 117532623B
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- moment
- observer
- external
- generalized momentum
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 92
- 230000003993 interaction Effects 0.000 claims abstract description 15
- 230000008859 change Effects 0.000 claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims description 32
- 230000001133 acceleration Effects 0.000 claims description 15
- 230000008569 process Effects 0.000 claims description 15
- 238000012937 correction Methods 0.000 claims description 13
- 238000011161 development Methods 0.000 claims description 13
- 238000005259 measurement Methods 0.000 claims description 12
- 230000008878 coupling Effects 0.000 claims description 7
- 238000010168 coupling process Methods 0.000 claims description 7
- 238000005859 coupling reaction Methods 0.000 claims description 7
- 238000005070 sampling Methods 0.000 claims description 6
- 238000012795 verification Methods 0.000 claims description 5
- 230000005284 excitation Effects 0.000 claims description 4
- 238000013016 damping Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 238000013507 mapping Methods 0.000 claims description 3
- 230000001360 synchronised effect Effects 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 2
- 230000000694 effects Effects 0.000 abstract description 11
- 238000002474 experimental method Methods 0.000 description 10
- 238000013461 design Methods 0.000 description 8
- 230000009467 reduction Effects 0.000 description 4
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000004069 differentiation Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000003321 amplification Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000000418 atomic force spectrum Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000001627 detrimental effect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000002452 interceptive effect Effects 0.000 description 1
- 238000002955 isolation Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000003199 nucleic acid amplification method Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
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/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
-
- 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/1628—Programme controls characterised by the control loop
- B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
Abstract
The invention provides a method for estimating the external moment of a mechanical arm, which comprises the following steps: establishing a mechanical arm dynamics model by adopting a Newton-Euler method; identifying the mechanical arm dynamic model by adopting an integral parameter identification method; defining generalized momentum of the mechanical arm, deriving the generalized momentum, and combining the identified dynamic model and a dynamic characteristic formula to obtain a discretization model of the generalized momentum system; constructing an external moment observer based on the generalized momentum system discretization model; according to the change of the external torque observation error, a self-adaptive fuzzy gain strategy is designed, and the dynamic adjustment of gain parameters in an external torque observer is carried out to obtain a fuzzy self-adaptive generalized momentum discrete observer; and calculating and obtaining an external moment estimated value of the mechanical arm based on the fuzzy self-adaptive generalized momentum discrete observer. Compared with the traditional generalized momentum observer, the invention has higher external moment observation precision and good effect when being used for man-machine interaction applications such as dragging teaching and the like.
Description
Technical Field
The invention relates to the technical field of mechanical arms, in particular to a mechanical arm external torque estimation method.
Background
With the continuous application and development of robots in human society, the situation that people and robots are required to interact with each other to achieve a target task is unavoidable in the aspect of cooperation of the people and robots. Therefore, interactive control is widely used in unstructured production, such as industrial sanding, collision detection, drag teaching, and the like. The mechanical arm is in contact with the external environment in the scenes, so that the effective measurement or estimation of the external moment of the mechanical arm is of great importance.
Typically, the measurement of interaction force can be achieved by an end six-dimensional force/moment sensor or a joint moment sensor, which is also a type of external force measurement method commonly used at present. However, the price of force/moment sensors is typically high, providing each robotic arm with such a sensor adds significantly to the cost and reliability. Unlike the method based on force/moment sensors as interaction force measurement, sensorless external moment estimation based on mechanical arm dynamics model and observer design provides a solution for this. If the interaction force between the mechanical arm and the external environment can be estimated through the torque observer with higher precision, the data acquisition is simple and feasible, the equipment cost can be saved, and the implementation is easy.
The above related work has been studied extensively in recent years, as in document HADDADIN S, DE LUCA A, ALBU-SCH Ä FFER A, robot protocols: A survey on detection, isolation, and identification [ J ]. IEEE Transactions on Robotics, 2017, 6: 1292-1312 uses motor torque, position, speed and acceleration information provided by the robot controller to make an estimate of the external torque. This calculation method is easy to implement, but since in actual engineering acceleration information is typically obtained by a second differentiation of the position signal, this will lead to an amplification of the measurement noise. For this purpose, reference NA J, JING B, HUANG Y, et al Unknown system dynamics estimator for motion control of nonlinear robotic systems [ J ]. IEEE Transactions on Industrial Electronics, 2019, 67 (5): 3850-3859, by designing filters to reduce the effect of noise on the torque estimation effect, this also indirectly leads to delay problems in the estimated signal, which is detrimental to high dynamic response situations for force control. In order to avoid the limitation caused by directly using the acceleration information, many students further research observation methods based on a disturbance observer, an extended state observer, a sliding mode observer, a Kalman filter and the like so as to improve the estimation effect of the external moment of the mechanical arm. However, such methods have problems associated with errors in force estimation and robot pose configuration, and in addition, their fixed observer gain has certain limitations on the observation effect of external forces under different conditions.
In summary, the invention designs a generalized momentum observer based on a self-adaptive strategy, and realizes the external moment estimation of the mechanical arm in an uncertainty environment so as to solve the defects of the existing external moment estimation method.
Disclosure of Invention
In view of the above problems, the invention provides a mechanical arm external moment estimation method, which has higher external moment observation precision compared with the traditional generalized momentum observer and has good effect when being used for man-machine interaction applications such as dragging teaching and the like.
In order to solve the technical problems, the invention adopts the following technical scheme: a mechanical arm external torque estimation method comprises the following steps: step S1, a mechanical arm dynamics model is established by adopting a Newton-Euler method, wherein the dynamics model is expressed as a group of coupling differential equations in a nonlinear matrix form; s2, identifying the mechanical arm dynamic model by adopting an overall parameter identification method, namely carrying out parameter linearization treatment on the coupling differential equation; step S3, defining generalized momentum of the mechanical arm, deriving the generalized momentum, and combining the identified dynamic model and a dynamic characteristic formula to obtain a generalized momentum system discretization model; s4, constructing an external moment observer based on the generalized momentum system discretization model; s5, according to the change of the external torque observation error, a self-adaptive fuzzy gain strategy is designed, and dynamic adjustment of gain parameters in an external torque observer is carried out, so that a fuzzy self-adaptive generalized momentum discrete observer is obtained; and S6, calculating and obtaining an external moment estimated value of the mechanical arm based on the fuzzy self-adaptive generalized momentum discrete observer.
Preferably, in step S1, the equation of the kinetic model is:
；
in the above-mentioned method, the step of,（Nthe number of joints of the mechanical arm) respectively represent joint position information, speed information and acceleration information of the mechanical arm；/>Representing a symmetric positive definite inertia matrix; />Representing the coriolis force and centrifugal force matrices; />Representing a gravity matrix;frepresents friction, which includes coulomb friction and viscous friction; />Respectively represent the driving moment and the external moment of the joint motor.
As a preferred solution, the step S2 specifically includes: the coupled differential equation is subjected to parameter linearization processing to obtain an equationWherein μ is a column vector containing standard kinetic parameters, H is a regression matrix,>the torque is driven by a joint motor; the partial linear irrelevant columns in the regression matrix H are recombined and arranged to obtain an equationWherein->For the subset consisting of the maximally linearly independent columns of the regression matrix,/for the subset consisting of the maximally linearly independent columns of the regression>Is a kinetic minimum parameter set; by giving an optimized excitation track of a mechanical arm, acquiring joint related parameters on line during the operation of the mechanical arm, and carrying out +_ in an off-line state by using a least square method>Is a single-chip microcomputer.
As a preferenceIn an embodiment, the step S3 specifically includes: the generalized momentum of a mechanical arm is defined asThe generalized momentum p is +.>；
The generalized momentum system model equation for the continuous time domain is as follows:
；
further, define auxiliary variablesSubstituting the generalized momentum system model formula to obtain a generalized momentum system discretization model, wherein the formula is as follows:
；
in the method, in the process of the invention,and->Respectively represent generalized momentum p and external moment +.>At the position ofkAnd the sampling value of time, and h is the sampling period.
Preferably, in step S4, the formula of the external moment observer is as follows:
；
in the method, in the process of the invention,and->Respectively->And->At the position ofkObservation of time of day->Gain parameter for generalized momentum observation error>A symmetric matrix with each element larger than 0; definition of the definitionEstimating an error for the external moment;
the observer error dynamics equation satisfies:
；
wherein I is an identity matrix, and the proper choice is selectedThe observer error dynamics equation is globally asymptotically convergent and the gain parameter +.>And->Two parameters affecting the observer output overshoot and buffeting, respectively.
Preferably, in step S5, the dynamically adjusting the gain parameter in the external torque observer includes: obtaining expert experience and compiling the expert experience into fuzzy logic rules; according to the fuzzy logic rule, a Sugeno type fuzzy inference algorithm is selected to construct a two-dimensional fuzzy logic control system; will observe errorsAnd the amount of change in the observation error +.>Respectively used as fuzzy input of a two-dimensional fuzzy logic control system, obtaining fuzzy output after reasoning, and adding the fuzzy output to gain parameters +.>And->Is a kind of medium.
The method for estimating the external moment of the mechanical arm further comprises the step of verifying the effectiveness of the method for estimating the external moment of the mechanical arm, and specifically comprises the following steps: constructing an industrial robot algorithm development platform; based on the industrial robot algorithm development platform, acquiring moment information acquired by a servo driving unit, and calibrating a torque correction coefficient by using a measurement true value of a moment sensor; acquiring and recording the position and the driving moment of the mechanical arm on line in real time, and identifying a minimum parameter set of a dynamic model by adopting a least square method in an off-line state to obtain a model identification result; carrying out a section of preset track task by controlling the mechanical arm load, and calculating to obtain external torque estimation results of various algorithms; and (3) evaluating the effectiveness of the external moment estimation result by combining various performance indexes, wherein the performance indexes comprise a maximum absolute value error, a root mean square error and deviation.
As a preferable scheme, the industrial robot algorithm development platform comprises a robot body, a servo driving unit and a motion control unit; the robot body is a six-degree-of-freedom serial mechanical arm, a six-dimensional force/moment sensor is arranged at the tail end of the robot body, and a vertical grip is arranged at the tail end of the sensor, so that a user can conveniently perform man-machine interaction with the mechanical arm; the servo driving unit is used for realizing control of the permanent magnet synchronous motor of the mechanical arm joint and has different control modes aiming at the torque, the speed and the position of the joint motor; the motion control unit is realized by adopting a multiple-function controller, and is communicated with the servo driving unit through an industrial Ethernet EtherCAT bus, so that microsecond real-time control and operation of a plurality of servos can be realized.
Preferably, the calibrating the torque correction coefficient by using the measured true value of the torque sensor includes: firstly, controlling a mechanical arm to run on load for a section of preset low-speed track, synchronously acquiring an external force moment value calculated in real time by a mechanical arm dynamics model and a true value acquired by a force/moment sensor during running, and mapping the true value to each joint through Jacobian matrix operation, so that the calibration of a torque correction coefficient k is realized.
Preferably, the method further comprises: based on the fuzzy self-adaptive generalized momentum discrete observer, carrying out dragging teaching verification on the mechanical arm under the condition of a weak/moment sensor by adopting an admittance control algorithm;
the admittance control algorithm has the following formula:
；
in the method, in the process of the invention,wherein->The target pose, the speed and the acceleration of the given mechanical arm are respectively; />The current pose, the current speed and the current acceleration of the mechanical arm are respectively; />Wherein->An outer moment estimated for the algorithm, +.>Then it is the transpose of the mechanical arm jacobian; />Respectively, mass, damping and rigidityA degree matrix parameter.
Compared with the prior art, the invention has the beneficial effects that: modeling mechanical arm dynamics by using a Newton-Euler method, identifying a minimum parameter set to obtain an accurate dynamic model of the mechanical arm, then establishing external torque estimation based on robot generalized momentum and a discrete observer based on the dynamic model, and designing observer gain with self-adaptive adjustment capacity by using a fuzzy logic rule to adapt to different interaction environments; the result shows that the external moment estimation method has higher external moment observation precision compared with the traditional generalized momentum observer, and has good effect when being used for man-machine interaction applications such as dragging teaching and the like.
Drawings
The disclosure of the present invention is described with reference to the accompanying drawings. It is to be understood that the drawings are designed solely for the purposes of illustration and not as a definition of the limits of the invention. In the drawings, like reference numerals are used to refer to like parts. Wherein:
FIG. 1 is a flowchart of a method for estimating the moment of an external force of a mechanical arm according to an embodiment of the present invention;
FIG. 2 is a graph of the blur input E, CE and output in the adaptive blur gain design according to an embodiment of the present invention;
FIG. 3 is a graph showing the result of an identification experiment in the calibration of a torque correction coefficient according to an embodiment of the present invention;
FIG. 4 is a diagram of a three-dimensional motion trajectory of a robotic arm in analysis of an out-of-moment observation in accordance with an embodiment of the present invention;
FIG. 5 is a graph showing the results of the implementation of various algorithms in the experiments of the present invention;
fig. 6 is a graph of contact force in x-direction and y-direction during a drag operation of a robot arm according to an embodiment of the present invention.
Detailed Description
It is to be understood that, according to the technical solution of the present invention, those skilled in the art may propose various alternative structural modes and implementation modes without changing the true spirit of the present invention. Accordingly, the following detailed description and drawings are merely illustrative of the invention and are not intended to be exhaustive or to limit the invention to the precise form disclosed.
An embodiment according to the invention is shown in connection with fig. 1. A mechanical arm external torque estimation method comprises the following steps:
and S1, establishing a mechanical arm dynamics model by adopting a Newton-Euler method, wherein the dynamics model is expressed as a set of coupling differential equations in a nonlinear matrix form.
The mechanical arm dynamics equation can be used to determine the force versus motion relationship, i.e., calculate mechanical arm motion such as position, velocity, acceleration information, etc., from given moment information. Generally, the mechanical arm dynamics system has serious nonlinearity and strong coupling, and the equation of the dynamics model is as follows:
（1）；
in the above-mentioned method, the step of,（Nthe number of joints of the mechanical arm) respectively represents joint position information, speed information and acceleration information of the mechanical arm; />Representing a symmetric positive definite inertia matrix; />Representing the coriolis force and centrifugal force matrices; />Representing a gravity matrix;frepresents friction, which includes coulomb friction and viscous friction; />Respectively represent the driving moment and the external moment of the joint motor.
The friction model considered in the present invention is expressed as:
（2）；
in the method, in the process of the invention,represents the coefficient of friction>And->Respectively, the coulomb friction coefficient and the friction bias term.
In the mechanical arm dynamic model type (1),and->The following kinetics exist:
（3）；
wherein,is->Is a transpose of (a).
And S2, identifying the mechanical arm dynamic model by adopting an integral parameter identification method, namely carrying out parameter linearization treatment on the coupling differential equation.
Since there is always a certain error between the nominal kinetic model parameters and the actual model, this will result in a non-zero result of the actual moment estimate due to model errors even if the robot arm is not acted upon by external forces. To reduce the effect of model errors on observations, it is necessary to obtain a more accurate kinetic model for external moment estimation. Because the mechanical arm has unknown disturbance in the normal operation process and the model has strong nonlinear characteristics, the mechanical arm dynamics adopts an integral parameter identification method, namely, the dynamics equation (1) is subjected to parameter linearization treatment, and the mechanical arm dynamics is expressed in the form of the following parameter linearization separation:
（4）；
wherein μ is a column vector containing standard kinetic parameters, and consists of parameters of each link of the mechanical arm, and taking each link as an example, the column vector contains 15 parameters: inertial parametersJoint quality->First order moment of mass->Motor moment of inertia->Coefficient of friction->、/>The method comprises the steps of carrying out a first treatment on the surface of the H is a regression matrix, which is related to only the arm joint position, velocity and acceleration information. It is noted that the regression matrix H is a non-full order matrix, including zero columns and linear correlation columns, so that the full parameter set cannot be identified. Thus, by reorganizing and sorting the partial linear independent columns in H, it is possible to obtain:
（5）；
in the method, in the process of the invention,a subset of the maximum linear independent columns of the regression matrix; />Is a kinetic minimum parameter set.
For identification purposesBy giving an optimized excitation track of the mechanical arm, information of relevant parameters such as joint motor torque, joint angle, joint angular speed and the like is obtained online during the operation of the mechanical arm, and the mechanical arm is combined with the method (5) to perform the method (I) by using a least square method in an offline state>Is a single-chip microcomputer. Further, by combining the dynamics model characteristics, the dynamics parameter matrix can be obtainedRespectively extracting the external force moment of the mechanical arm and providing a model foundation for the estimation of the external force moment of the mechanical arm.
And step S3, defining and deriving the generalized momentum of the mechanical arm, and combining the identified dynamic model and dynamic characteristic formula to obtain a generalized momentum system model and a generalized momentum system discretization model in a continuous time domain.
The generalized momentum of a mechanical arm is defined as （6）；
The generalized momentum p is derived from time to time by the formula
（7）；
Combining the dynamics model (1) and the dynamics characteristic (3) and substituting the dynamics model into the model (7) to obtain a generalized momentum system model formula under a continuous time domain, wherein the generalized momentum system model formula is as follows:
（8）；
from equation (8), it can be seen that the mechanical arm dynamics equation after the generalized momentum is introduced will not introduce acceleration information, so as to avoid noise interference caused by the secondary differentiation of the mechanical arm joint position information.
Further, define auxiliary variablesSubstituting into the model (8) to obtain a discretization model of the generalized momentum system, wherein the formula is as follows:
（9）；
in the method, in the process of the invention,and->Respectively represent generalized momentum p and external moment +.>At the position ofkAnd the sampling value of time, and h is the sampling period.
And S4, constructing an external moment observer based on the generalized momentum system discretization model.
The formula of the external moment observer is as follows:
（10）；
in the method, in the process of the invention,and->Respectively->And->At the position ofkObservation of time of day->For generalized momentum viewError measurement, gain parameter->A symmetric matrix with each element larger than 0; definition of the definitionEstimating an error for the external moment;
combining equation (9) and equation (10), the observer error dynamics equation satisfies:
（11）；
wherein I is an identity matrix, and the proper choice is selectedThe observer error dynamics equation is globally asymptotically convergent and the gain parameter +.>And->Two parameters affecting the observer output overshoot and buffeting, respectively.
And S5, designing a self-adaptive fuzzy gain strategy according to the change of the external moment observation error, and dynamically adjusting gain parameters in the external moment observer to obtain a fuzzy self-adaptive generalized momentum discrete observer.
Because uncertainty exists when the mechanical arm interacts with an unknown environment, a relatively conservative fixed gain is generally selected for ensuring the robustness of external torque observation, which leads to the problem of buffeting when the system enters a sliding mode. In addition, a fixed gain is ideal for constant external moment estimation, but there are limitations to time-varying scenarios where the robot arm interacts frequently with the environment. Therefore, according to the change of the external torque observation error, the self-adaptive fuzzy gain strategy is designed to realize the dynamic adjustment of gain parameters and the external torque observer, and more accurate external torque estimation is realized in the interaction of the mechanical arm and the uncertain environment.
Specifically, in step S5, the dynamic adjustment of the gain parameter in the external torque observer is performed, including:
(1) Expert experience is obtained and is compiled into fuzzy logic rules.
(2) And according to the fuzzy logic rule, selecting Sugeno type fuzzy inference algorithm to construct a two-dimensional fuzzy logic control system.
(3) Will observe errorsAnd the amount of change in the observation error +.>Respectively used as fuzzy input of a two-dimensional fuzzy logic control system, obtaining fuzzy output after reasoning, and adding the fuzzy output to gain parameters +.>Andis a kind of medium.
The specific implementation method of the fuzzy logic control comprises the following steps:
in the invention, sugeno type fuzzy inference algorithm is selected as a fuzzy inference model, and a fuzzy logic control tool box in MATLAB is adopted for design, and the specific design method is shown in the use description of the MATLAB tool box. The fuzzy inputs E, CE of the two-dimensional fuzzy logic control system are respectively selected as the observation errorsAnd->The output U is designed as a gain parameterAnd->The method comprises the steps of carrying out a first treatment on the surface of the In the experiment, fuzzy input in Sugeno type fuzzy reasoning algorithm is determined according to actual working conditionsThe input ambiguity set is { NB (negative big), NM (negative small), Z (zero), PM (positive small), PB (positive big) }, and the output ambiguity set is { PS (negative big), S (negative small), M (zero), B (positive small), PB (positive big) }. In the embodiment of the present invention, the curved surface diagram of fuzzy input E, CE and output is shown in fig. 2, wherein the gain parameters and the fuzzy logic rule design are consistent, as shown in table 1:
TABLE 1 fuzzy logic rules
It can be seen that when the input amount E, CE increases, the fuzzy logic rule can automatically calculate to obtain a higher gain, so that the external torque estimated value can be ensured to more quickly follow the change of the true value; otherwise, the gain is reduced in a self-adaptive manner, so that buffeting is reduced, and the observation precision of external torque is ensured.
And S6, calculating and obtaining an external moment estimated value of the mechanical arm based on the fuzzy self-adaptive generalized momentum discrete observer.
In order to verify the effectiveness of the mechanical arm external torque observation method provided by the invention, an industrial robot algorithm development platform is constructed for experimental verification. The method specifically comprises the following steps:
(1) And constructing an industrial robot algorithm development platform.
Specifically, the industrial robot algorithm development platform comprises a robot body, a servo driving unit and a motion control unit; the robot body is a six-degree-of-freedom serial mechanical arm, the tail end of the robot body is provided with a six-dimensional force/torque sensor, and the tail end of the sensor is provided with a vertical grip, so that the user can conveniently perform man-machine interaction with the mechanical arm; the servo driving unit is used for realizing control of the permanent magnet synchronous motor of the mechanical arm joint and has different control modes aiming at the torque, the speed and the position of the joint motor; the motion control unit is realized by adopting a multiple-function controller, and is communicated with the servo driving unit through an industrial Ethernet EtherCAT bus, so that microsecond real-time control and operation on a plurality of servos can be realized.
The algorithm development in the experiment is realized by adopting a model-based design method, and the method comprises the following steps: and (3) carrying out algorithm development in a MATLAB/Simulink environment, generating a C++ module which can be read and imported by a ploidy controller through a TE1400 plug-in, and finally realizing algorithm verification in TwainCAT software.
(2) Based on an industrial robot algorithm development platform, acquiring moment information acquired by a servo driving unit, and calibrating a torque correction coefficient by using a measurement true value of a moment sensor;
because the torque information collected by the servo driver is calculated according to the motor current and the torque coefficient set in the driver, an unknown torque correction coefficient k exists between the calculated result and the real torque value. In order to achieve a moment calculation closer to the true value, the torque correction coefficient is calibrated experimentally by means of measured true values of six-dimensional force/moment sensors mounted at the end of the arm.
The following approximate relationship exists between the motor side final output torque and the motor internal current:
（12）
in the method, in the process of the invention,for the final torque output on the motor side, +.>The motor current is K, the torque coefficient is K, and the reduction ratio of the mechanical arm is r. The nominal torque coefficient and reduction ratio of the robot arm are typically provided by the robot arm manufacturer.
The calibration method comprises the following steps: firstly, controlling a mechanical arm to run on load for a section of preset low-speed track, synchronously acquiring external force moment value calculated in real time by a mechanical arm dynamics model and true value acquired by a force/moment sensor during running, and mapping the true value to each joint through Jacobian matrix operation, thereby realizing torque correction coefficientkIs defined by the calibration of (a).
The torque coefficient, reduction ratio and calculated torque correction coefficient of the mechanical arm are shown in table 2:
table 2 torque coefficient, reduction ratio, and torque correction coefficient
(3) And acquiring and recording the position and the driving moment of the mechanical arm on line in real time, and identifying the minimum parameter set of the dynamic model by adopting a least square method in an off-line state to obtain a model identification result.
The experimental result is shown in fig. 3, and the six graphs in fig. 3 respectively correspond to moment values of six joints of the mechanical arm, wherein a solid line represents real torque acquired in the process of executing an excitation track, and a dotted line corresponds to calculated torque of a dynamics model identified by the experiment. From the error curve (dash-dot line), it can be seen that the error of the dynamic model fluctuates in a very small interval, and meets the experimental requirements.
(4) And carrying out a section of preset track task by controlling the mechanical arm load, and calculating to obtain external torque estimation results of various algorithms.
In order to analyze the observation effect of the external moment of the mechanical arm in the working process, a section of preset track task is executed in an experiment by controlling the loading of the mechanical arm, and the three-dimensional running track of the tail end of the mechanical arm in the space is shown in fig. 4.
In order to verify the superiority of the observation method provided by the invention, the external torque estimation results of three algorithms are compared in experiments: a generalized momentum observer GMO, a fixed gain generalized momentum discrete observer GSTO and a fuzzy self-adaptive generalized momentum discrete observer AGSTO; the algorithm parameters are designed as follows:
GMO：=diag{25,25,25,25,25,25}；GSTO：/>=diag{54,54,54,54,54,54}，/>=diag{80,80,80,80,80,80},/>=diag{80,80,80,80,80,80}；AGSTO：/>consistent with GSTO parameters, +.>、/>And calculating according to the fuzzy logic control module.
The implementation results of the algorithms in the experiment are shown in fig. 5 (based on the comparison between the estimates of the external torque of the GMO (broken line), the GSTO (dot-dash line) and the AGSTO (dot-line), and the experimental results of the 1 st to 6 th joints of the mechanical arm are respectively corresponding to the (a) to (f) of fig. 5, wherein the solid line represents the measurement truth value), and compared with the other two algorithms, the designed AGSTO has higher observation precision, is closer to the measurement truth value of the six-dimensional force/torque sensor, and can better reflect the change condition of the external torque. Notably, the small chatter of the observed values can be found in the graph, because the proposed AGSTO can adaptively adjust the gain of the observer, but in order to ensure that the system still has certain robustness in steady state, the gain is not set small enough, so that there is certain chatter; at the same time, noise present in the experiment is also a factor.
(5) And (3) evaluating the effectiveness of the external torque estimation result by combining various performance indexes, wherein the performance indexes comprise a maximum absolute value error, a root mean square error and deviation.
To further quantitatively evaluate the effectiveness of the algorithm, the following algorithm performance index was calculated: maximum absolute value error (MAE), root Mean Square Error (RMSE), and OFFSET (OFFSET), wherein the maximum absolute value error is the absolute value of the maximum error of the measured value and the true value, the root mean square error is the root of the square of the error of the measured value and the true value, and the OFFSET is the absolute value of the average of the error of the measured value and the true value. Table 3 shows the determination of several algorithmsAs a result of the quantitative performance index,to->Respectively represent the external moment of the first joint to the sixth joint of the mechanical arm. Obviously, the proposed observation method is closer to the measurement truth. However, it should be noted that although there is a certain improvement over the conventional algorithm, the algorithm proposed by the present invention still has a certain estimation error due to the unmodeled dynamics and the noise, but the result is acceptable for applications with low force control accuracy requirements, such as drag teaching.
TABLE 3 comparison of Performance indicators for different external force moment estimation methods
Based on an AGSTO (fuzzy self-adaptive generalized momentum discrete observer) algorithm designed by the invention, an admittance control algorithm is adopted to carry out dragging teaching verification on the mechanical arm under the condition of a weak/moment sensor. The man-machine interaction scene is realized by adopting an admittance control algorithm, wherein the admittance control algorithm is as follows: the formula of the admittance control algorithm is as follows:
；
in the method, in the process of the invention,wherein->The target pose, the speed and the acceleration of the given mechanical arm are respectively; />The current pose, the current speed and the current acceleration of the mechanical arm are respectively; />Wherein->An outer moment estimated for the algorithm, +.>Then it is the transpose of the mechanical arm jacobian; />Respectively mass, damping and stiffness matrix parameters.
During the dragging operation of the mechanical arm, the contact force curves along the x-direction and the y-direction are shown in fig. 6 (a) and (b), respectively, where (a) and (b) correspond to the traction forces in the x-direction and the y-direction of the cartesian coordinate system, respectively. The broken line is the true value of the traction force in the x and y directions of the Cartesian coordinate system measured by the sensor, the solid line is the calculated value of the AGSTO observation result according to the invention, in the figure, the measurement result of the six-dimensional force/moment sensor is taken as the true value, and the result of the proposed external force moment estimation method can be seen to be effective.
Conclusion: the invention provides an external torque estimation method based on a fuzzy self-adaptive generalized momentum discrete observer based on a robot generalized momentum and discrete observer design method, and combines a fuzzy logic control idea, and successfully performs a comparison experiment on a six-axis industrial robot, and realizes dragging teaching of a mechanical arm under the condition of a weak/torque sensor; experimental results show that the fuzzy self-adaptive generalized momentum discrete observer has higher external torque observation precision compared with the traditional generalized momentum observer, and has good effect when being used for man-machine interaction applications such as dragging teaching and the like.
In summary, the beneficial effects of the invention include: modeling mechanical arm dynamics by using a Newton-Euler method, identifying a minimum parameter set to obtain an accurate dynamic model of the mechanical arm, then establishing external torque estimation based on robot generalized momentum and a discrete observer based on the dynamic model, and designing observer gain with self-adaptive adjustment capacity by using a fuzzy logic rule to adapt to different interaction environments; the result shows that the external moment estimation method has higher external moment observation precision compared with the traditional generalized momentum observer, and has good effect when being used for man-machine interaction applications such as dragging teaching and the like.
It should be appreciated that the integrated units, if implemented in the form of software functional units and sold or used as stand-alone products, may be stored in a computer readable storage medium. Based on such understanding, the technical solution of the present invention is essentially or a part contributing to the prior art, or all or part of the technical solution may be embodied in the form of a software product stored in a storage medium, comprising several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods of the embodiments of the present invention. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a random access Memory (RAM, random Access Memory), a magnetic disk, or an optical disk, or other various media capable of storing program codes.
The technical scope of the present invention is not limited to the above description, and those skilled in the art may make various changes and modifications to the above-described embodiments without departing from the technical spirit of the present invention, and these changes and modifications should be included in the scope of the present invention.
Claims (7)
1. The mechanical arm external moment estimation method is characterized by comprising the following steps of:
step S1, a mechanical arm dynamics model is established by adopting a Newton-Euler method, wherein the dynamics model is expressed as a group of coupling differential equations in a nonlinear matrix form;
s2, identifying the mechanical arm dynamic model by adopting an overall parameter identification method, namely carrying out parameter linearization treatment on the coupling differential equation;
step S3, defining generalized momentum of the mechanical arm, deriving the generalized momentum, and combining the identified dynamic model and a dynamic characteristic formula to obtain a generalized momentum system discretization model;
s4, constructing an external moment observer based on the generalized momentum system discretization model;
s5, according to the change of the external torque observation error, a self-adaptive fuzzy gain strategy is designed, and dynamic adjustment of gain parameters in an external torque observer is carried out, so that a fuzzy self-adaptive generalized momentum discrete observer is obtained;
step S6, calculating and obtaining an external moment estimated value of the mechanical arm based on the fuzzy self-adaptive generalized momentum discrete observer;
the step S3 specifically includes: the generalized momentum of a mechanical arm is defined asThe generalized momentum p is +.>The method comprises the steps of carrying out a first treatment on the surface of the The generalized momentum system model equation for the continuous time domain is as follows:
；
in the above-mentioned method, the step of,respectively representing joint position information, speed information and acceleration information of the mechanical arm,Nis the number of mechanical arm joints; />Representing a symmetric positive definite inertia matrix; />Representing the coriolis force and centrifugal force matrices;representing a gravity matrix;frepresents friction, which includes coulomb friction and viscous friction; />Respectively representing the driving moment and the external moment of the joint motor;
further, define auxiliary variablesSubstituting the generalized momentum system model formula to obtain a generalized momentum system discretization model, wherein the formula is as follows:
；
in the method, in the process of the invention,and->Respectively represent generalized momentum p and external moment +.>At the position ofkThe sampling value at the moment, h is the sampling period;
in step S4, the formula of the external moment observer is as follows:
；
in the method, in the process of the invention,and->Respectively->And->At the position ofkTime of dayIs>Gain parameter for generalized momentum observation error>For a symmetrical matrix with elements greater than 0, < ->The method comprises the steps of carrying out a first treatment on the surface of the Definitions->Estimating an error for the external moment;
the observer error dynamics equation satisfies:
；
wherein I is an identity matrix, and the proper choice is selectedThe observer error dynamics equation is globally asymptotically convergent and the gain parameter +.>And->Two parameters affecting the output overshoot and buffeting of the observer respectively;
in step S5, the dynamically adjusting the gain parameter in the external torque observer includes: obtaining expert experience and compiling the expert experience into fuzzy logic rules; according to the fuzzy logic rule, a Sugeno type fuzzy inference algorithm is selected to construct a two-dimensional fuzzy logic control system; will observe errorsAnd the amount of change in the observation error +.>Respectively used as fuzzy input of a two-dimensional fuzzy logic control system, obtaining fuzzy output after reasoning, and adding the fuzzy output to gain parameters +.>And->Is a kind of medium.
2. The method according to claim 1, wherein in step S1, the equation of the kinetic model is:
。
3. the method for estimating the external moment of the mechanical arm according to claim 1, wherein the step S2 specifically includes:
the coupled differential equation is subjected to parameter linearization processing to obtain an equationWherein mu is a column vector containing standard kinetic parameters, and H is a regression matrix;
the partial linear irrelevant columns in the regression matrix H are recombined and arranged to obtain an equationWherein->For the subset consisting of the maximally linearly independent columns of the regression matrix,/for the subset consisting of the maximally linearly independent columns of the regression>Is a kinetic minimum parameter set;
the joint related parameters are obtained online during the operation of the mechanical arm by giving an optimized excitation track, and the mechanical arm is performed in an offline state by using a least square methodIs a single-chip microcomputer.
4. The method for estimating the external moment of the mechanical arm according to claim 1, further comprising the step of verifying the effectiveness of the method for estimating the external moment of the mechanical arm, and specifically comprising the steps of:
constructing an industrial robot algorithm development platform;
based on the industrial robot algorithm development platform, acquiring moment information acquired by a servo driving unit, and calibrating a torque correction coefficient by using a measurement true value of a moment sensor;
acquiring and recording the position and the driving moment of the mechanical arm on line in real time, and identifying a minimum parameter set of a dynamic model by adopting a least square method in an off-line state to obtain a model identification result;
carrying out a section of preset track task by controlling the loading of the mechanical arm, and calculating to obtain an external torque estimation result;
and evaluating the effectiveness of the external moment estimation result by combining performance indexes, wherein the performance indexes comprise maximum absolute value error, root mean square error and deviation.
5. The method for estimating external moment of mechanical arm according to claim 4, wherein the industrial robot algorithm development platform comprises a robot body, a servo driving unit and a motion control unit;
the robot body is a six-degree-of-freedom serial mechanical arm, a six-dimensional force sensor or a torque sensor is arranged at the tail end of the robot body, and a vertical grip is arranged at the tail end of the sensor and used for human-computer interaction between a user and the mechanical arm;
the servo driving unit is used for realizing control of the permanent magnet synchronous motor of the mechanical arm joint and has different control modes aiming at the torque, the speed and the position of the joint motor;
the motion control unit is realized by adopting a double-Fu controller, and is communicated with the servo driving unit through an industrial Ethernet EtherCAT bus.
6. The method of claim 4, wherein calibrating the torque correction factor using the measured true value of the torque sensor comprises:
firstly, controlling the mechanical arm to run on load for a section of preset low-speed track, synchronously acquiring an external moment value calculated in real time by a mechanical arm dynamics model and a true value acquired by a force sensor or a moment sensor during running, and mapping the true value to each joint through Jacobian matrix operation, so as to realize the calibration of a torque correction coefficient k.
7. The mechanical arm external moment estimation method according to claim 1, further comprising: based on the fuzzy self-adaptive generalized momentum discrete observer, carrying out dragging teaching verification on the mechanical arm under the condition of no force sensor or moment sensor by adopting an admittance control algorithm;
the admittance control algorithm has the following formula:
；
in the method, in the process of the invention,wherein->The target pose, the speed and the acceleration of the given mechanical arm are respectively; />The current pose, the current speed and the current acceleration of the mechanical arm are respectively; />Wherein->For the estimated external moment +.>Then it is the transpose of the mechanical arm jacobian; />The parameters are mass, damping and stiffness matrix, respectively.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410033238.8A CN117532623B (en) | 2024-01-10 | 2024-01-10 | Mechanical arm external torque estimation method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410033238.8A CN117532623B (en) | 2024-01-10 | 2024-01-10 | Mechanical arm external torque estimation method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117532623A CN117532623A (en) | 2024-02-09 |
| CN117532623B true CN117532623B (en) | 2024-03-29 |
Family
ID=89792393
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410033238.8A Active CN117532623B (en) | 2024-01-10 | 2024-01-10 | Mechanical arm external torque estimation method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117532623B (en) |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH10264057A (en) * | 1997-03-19 | 1998-10-06 | Yaskawa Electric Corp | External force estimating method for mechanical system |
| CN112743541A (en) * | 2020-12-21 | 2021-05-04 | 南京埃斯顿自动化股份有限公司 | Soft floating control method for mechanical arm of powerless/torque sensor |
| CN113199477A (en) * | 2021-04-29 | 2021-08-03 | 浙江工业大学 | Baxter mechanical arm track tracking control method based on reinforcement learning |
| CN115229798A (en) * | 2022-08-22 | 2022-10-25 | 桂林电子科技大学 | Force impedance control method integrating feedforward compensation and variable damping modeling |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP5198514B2 (en) * | 2010-07-22 | 2013-05-15 | 株式会社東芝 | Robot controller |
-
2024
- 2024-01-10 CN CN202410033238.8A patent/CN117532623B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH10264057A (en) * | 1997-03-19 | 1998-10-06 | Yaskawa Electric Corp | External force estimating method for mechanical system |
| CN112743541A (en) * | 2020-12-21 | 2021-05-04 | 南京埃斯顿自动化股份有限公司 | Soft floating control method for mechanical arm of powerless/torque sensor |
| CN113199477A (en) * | 2021-04-29 | 2021-08-03 | 浙江工业大学 | Baxter mechanical arm track tracking control method based on reinforcement learning |
| CN115229798A (en) * | 2022-08-22 | 2022-10-25 | 桂林电子科技大学 | Force impedance control method integrating feedforward compensation and variable damping modeling |
Non-Patent Citations (2)
| Title |
|---|
| 一种自适应扰动观测器的机械手滑模控制研究;马金茹 等;《机床与液压》;20220228;第50卷(第3期);第54-60页 * |
| 基于扰动观测器的机械臂自适应反演滑模控制;司彦娜 等;《计算机测量与控制》;20180831;第26卷(第8期);第102-118页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117532623A (en) | 2024-02-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Wahrburg et al. | Cartesian contact force estimation for robotic manipulators using Kalman filters and the generalized momentum | |
| JP6411380B2 (en) | Method for improving detection of collision between robot and its environment, system and computer program product for implementing the method | |
| CN112743541B (en) | Soft floating control method for mechanical arm of powerless/torque sensor | |
| Park et al. | Adaptive output-feedback control for trajectory tracking of electrically driven non-holonomic mobile robots | |
| Abdellatif et al. | Advanced model-based control of a 6-DOF hexapod robot: A case study | |
| CN111347421A (en) | Method and apparatus for torque estimation | |
| CN113021331B (en) | Seven-degree-of-freedom cooperative robot dynamics modeling and identification method | |
| CN112643670B (en) | Flexible joint control method based on sliding-mode observer | |
| WO2020213062A1 (en) | Motor control device | |
| Lu et al. | Experimental determination of dynamic parameters of robotic arms | |
| Madsen et al. | Model-based on-line estimation of time-varying nonlinear joint stiffness on an e-series universal robots manipulator | |
| CN117532623B (en) | Mechanical arm external torque estimation method | |
| CN104020664B (en) | A kind of flexible mechanical arm interference observer method for designing based on partial differential equation | |
| CN114516050B (en) | Robot external force estimation method and system based on position jacobian condition number optimization | |
| Adam et al. | Robust super-twisting sliding mode controller for the lateral and longitudinal dynamics of rack steering vehicle | |
| CN116690559A (en) | Industrial robot joint moment compensation method based on space-time diagram convolution network | |
| Wahrburg et al. | Cartesian contact force estimation for robotic manipulators-a fault isolation perspective | |
| CN112051736B (en) | Trajectory tracking control system based on single controller | |
| CN112147894B (en) | Wheel type mobile robot active control method based on kinematics and dynamics model | |
| CN114454150A (en) | Arm type robot control method based on composite learning and robot system | |
| Li et al. | End-effector force estimation for robotic manipulators from motor current measurements | |
| Li et al. | Robot Collision Detection and Distinction Based on Convolution Filtering Dynamic Model. | |
| CN110605715A (en) | Robot dynamics parameter identification method independent of joint angular acceleration | |
| Khanesar et al. | Robust Sliding Mode Fuzzy Control of Industrial Robots Using an Extended Kalman Filter Inverse Kinematic Solver. Energies 2022, 15, 1876 | |
| CN113009828B (en) | Anti-interference calibration method for dynamic parameters of complex mechanical 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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |