CN113954078A - Force control joint control method and device, robot and readable storage medium - Google Patents
Force control joint control method and device, robot and readable storage medium Download PDFInfo
- Publication number
- CN113954078A CN113954078A CN202111360702.7A CN202111360702A CN113954078A CN 113954078 A CN113954078 A CN 113954078A CN 202111360702 A CN202111360702 A CN 202111360702A CN 113954078 A CN113954078 A CN 113954078A
- Authority
- CN
- China
- Prior art keywords
- moment
- current
- joint
- force
- value
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 46
- 238000001914 filtration Methods 0.000 claims abstract description 31
- 238000005259 measurement Methods 0.000 claims abstract description 27
- 238000004364 calculation method Methods 0.000 claims description 14
- 238000004590 computer program Methods 0.000 claims description 10
- 230000014509 gene expression Effects 0.000 claims description 6
- 238000013016 damping Methods 0.000 claims description 4
- 230000002452 interceptive effect Effects 0.000 abstract 1
- 238000010586 diagram Methods 0.000 description 6
- 239000011159 matrix material Substances 0.000 description 6
- 230000006870 function Effects 0.000 description 5
- 230000000694 effects Effects 0.000 description 3
- 239000000126 substance Substances 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 230000010355 oscillation Effects 0.000 description 2
- 101000802640 Homo sapiens Lactosylceramide 4-alpha-galactosyltransferase Proteins 0.000 description 1
- 102100035838 Lactosylceramide 4-alpha-galactosyltransferase Human genes 0.000 description 1
- 230000002411 adverse Effects 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 239000012636 effector Substances 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000010399 physical interaction Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
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/1656—Programme controls characterised by programming, planning systems for manipulators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
- B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
- B25J13/087—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices for sensing other physical parameters, e.g. electrical or chemical properties
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J17/00—Joints
- B25J17/02—Wrist joints
-
- 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
Abstract
The embodiment of the application provides a force control joint control method, a force control joint control device, a robot and a readable storage medium, wherein the method comprises the following steps: based on the obtained moment measurement value at the previous moment and the nominal dynamic model of the force control joint, a moment estimation value and a moment differential estimation value at the current moment are obtained through Kalman filtering; obtaining current control quantity at the current moment based on a preset moment controller according to the expected moment, the moment estimation value and the moment differential estimation value at the current moment; obtaining a disturbance compensation quantity at the current moment according to the nominal dynamic model, the input current of the force control joint motor at the previous moment and the moment estimated value; and calculating the input current required by the motor at the current moment according to the current control quantity and the disturbance compensation quantity so as to control the force control joint to execute corresponding operation. The method has better system robustness under the condition of realizing high-precision moment control, and can be applied to various interactive scenes and the like.
Description
Technical Field
The application relates to the technical field of robots, in particular to a force control joint control method and device, a robot and a readable storage medium.
Background
In order to pursue high speed and high precision, a control mode of a traditional robot is mainly position control, and the robot can only move along a track planned in advance in a structured environment to meet specific application requirements. However, as the application scenarios of the robot are continuously expanded, the simple position control cannot meet the application requirements of the robot, and especially in a rigid environment, because unavoidable modeling errors and uncertainties can cause sudden changes of contact force, the simple position control easily causes unstable behaviors (such as oscillation) of the system. Therefore, the robot needs to be able to accurately detect the magnitude of the contact force and control the contact force to have the capability of interacting with the outside world in an unstructured environment.
Although the robot only needs to sense the stress condition of the end effector in some industrial scenes, such as grinding, assembly and the like of the industrial robot, in other scenes, the whole body of the robot needs to be capable of sensing external force and performing joint moment control, such as whole body collision detection, man-machine physical interaction and the like of the robot. In addition, the kinematic model of the robot can only partially reflect the actual motion state of the robot, and in order to realize high-performance motion control, a control algorithm is often designed based on the dynamic model of the robot, and joint torque is used as the control input of the system at the moment. At present, the most widely applied torque measurement mode is that a torque sensor based on a resistance strain gauge type is installed at a joint, however, the measured value of the torque measurement mode is accompanied by a large amount of noise, the force control precision is reduced, adverse effects on the adjustment of control parameters are caused due to poor signal quality, and then phenomena such as over-adjustment, oscillation and the like occur.
Disclosure of Invention
The embodiment of the application provides a force control joint control method, wherein a torque sensor is arranged at the force control joint and used for measuring to obtain a torque measured value at a corresponding moment; the method comprises the following steps:
based on the obtained moment measurement value at the previous moment and the nominal dynamic model of the force control joint, obtaining a moment estimation value and a moment differential estimation value at the current moment through Kalman filtering;
obtaining current control quantity at the current moment according to the expected moment of the force control joint at the current moment, the moment estimated value and the moment differential estimated value based on a preset moment controller;
obtaining the disturbance compensation quantity at the current moment according to the nominal dynamic model, the input current of the force control joint motor at the previous moment and the moment estimation value;
and calculating to obtain the input current required by the force control joint motor at the current moment according to the current control quantity and the disturbance compensation quantity, and controlling the force control joint to move according to the input current.
In some embodiments, the nominal dynamical model of the force-controlled joint is obtained by a pre-construction comprising:
decoupling the force control joint into a motor end and a load end based on the torque sensor, and respectively establishing a dynamic model of the motor end and the load end;
and constructing a nominal dynamic model when the load end is fixed according to the dynamic models of the motor end and the load end.
In some embodiments, the obtaining the moment estimation value and the moment differential estimation value at the current moment through kalman filtering includes:
obtaining a moment predicted value and a moment differential predicted value at the current moment by utilizing a first state equation according to the motor input current, the moment predicted value and the moment differential predicted value of the force control joint at the previous moment, wherein the first state equation is constructed in advance through the nominal dynamic model;
calculating the error covariance between the moment predicted value and the moment real value at the current moment according to the error covariance between the moment estimated value and the moment real value at the previous moment and the process noise, and calculating the gain of the Kalman filter at the current moment according to the error covariance between the moment predicted value and the moment real value at the current moment and the measurement noise at the previous moment;
and calculating a moment estimation value and a moment differential estimation value at the current moment by using a second state equation according to the moment predicted value, the moment differential predicted value, the gain of the Kalman filter and the moment measurement value at the previous moment, wherein the second state equation is constructed in advance through a system measurement model.
In some embodiments, the preset torque controller is a PD-feedforward controller, and the obtaining the current control amount at the current moment includes:
obtaining an expected moment differential according to the expected moment of the force control joint at the current moment;
and obtaining the current control quantity at the current moment through the PD-feedforward controller according to the deviation between the expected torque and the torque estimation value at the current moment and the deviation between the expected torque differential and the torque differential estimation value.
In some embodiments, the obtaining the disturbance compensation amount at the current time includes:
obtaining a first disturbance compensation component through a preset low-pass filtering model according to the inverse model of the nominal dynamic model and the moment estimation value;
obtaining a second disturbance compensation component through a preset low-pass filtering model according to the input current of the force control joint motor at the previous moment;
and adding the first disturbance compensation component and the second disturbance compensation component to obtain a disturbance compensation quantity.
In some embodiments, the expression for the nominal kinetic model of the force controlled joint is as follows:
wherein, taus＝KsΔθ；
Δθ＝θm-θl；
Wherein, Pn(s) represents the nominal dynamics model, i(s) represents the input current of the force-controlled joint motor, τs(s) represents the output torque of the force control joint, beta represents the equivalent torque coefficient of the motor, and KsRepresenting the stiffness of the torque sensor, JmRepresenting the moment of inertia at the motor end, BmRepresenting the damping term, theta, of the motormIndicating the angle of rotation, theta, of the motorlRepresents the rotation angle of the load, Δ θ represents the deformation amount of the spring modeled by the torque sensor, and s represents a complex variable in the laplace transform.
In some embodiments, the PD-feedforward controller has the following expression:
where k denotes the kth time, iPD(k) A current control quantity, τ, output by the PD-feedforward controllerref(k) For the desired moment of the force-controlled joint,
for the desired moment differential, KPAnd KDProportional and differential link coefficients, tau, in the PD-feedforward controllerk(k) In order to be an estimate of the torque,
beta represents an equivalent torque coefficient of the motor as a torque differential estimation value.
The embodiment of the application provides a force control joint control device, wherein a torque sensor is arranged at a force control joint and used for measuring to obtain a torque measurement value at a corresponding moment; the method comprises the following steps:
the filtering module is used for obtaining a moment estimated value and a moment differential estimated value at the current moment through Kalman filtering based on the obtained moment measured value at the previous moment and the nominal dynamic model of the force control joint;
the calculation module is used for obtaining the current control quantity at the current moment according to the expected moment of the force control joint at the current moment, the moment estimation value and the moment differential estimation value based on a preset moment controller;
the compensation module is used for obtaining disturbance compensation quantity at the current moment according to the nominal dynamic model, the moment estimation value at the previous moment and the input current of the force control joint motor;
and the control module is used for calculating the input current of the force control joint motor at the current moment according to the current control quantity and the disturbance compensation quantity and controlling the force control joint to move according to the input current.
Embodiments of the present application further provide a robot, which includes a force-controlled joint, a processor, and a memory, where the memory stores a computer program, and the processor is configured to execute the computer program to implement the above-mentioned force-controlled joint control method, so as to control the force-controlled joint to move.
Embodiments of the present application also provide a readable storage medium storing a computer program which, when executed on a processor, implements the force-controlled joint control method described above.
The embodiment of the application has the following beneficial effects:
according to the force control joint control method, the moment estimation value and the moment differential estimation value are obtained through Kalman filtering, disturbance observation and compensation in a force control joint system are achieved through disturbance observation, and the current required to be input to a joint motor is calculated, so that accurate force control of the force control joint is achieved.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings that are required to be used in the embodiments will be briefly described below, it should be understood that the following drawings only illustrate some embodiments of the present application and therefore should not be considered as limiting the scope, and for those skilled in the art, other related drawings can be obtained from the drawings without inventive effort.
FIG. 1 illustrates a first flowchart of a force-controlled joint control method of an embodiment of the present application;
FIG. 2 is a system architecture diagram illustrating a force control joint control method according to an embodiment of the present application;
FIG. 3 illustrates a second flowchart of a force-controlled joint control method of an embodiment of the present application;
FIG. 4 illustrates a third flowchart of a force-controlled joint control method of an embodiment of the present application;
fig. 5 shows a schematic structural diagram of a force control joint control device according to an embodiment of the present application.
Detailed Description
The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are only a part of the embodiments of the present application, and not all of the embodiments.
The components of the embodiments of the present application, generally described and illustrated in the figures herein, can be arranged and designed in a wide variety of different configurations. Thus, the following detailed description of the embodiments of the present application, presented in the accompanying drawings, is not intended to limit the scope of the claimed application, but is merely representative of selected embodiments of the application. All other embodiments, which can be derived by a person skilled in the art from the embodiments of the present application without making any creative effort, shall fall within the protection scope of the present application.
Hereinafter, the terms "including", "having", and their derivatives, which may be used in various embodiments of the present application, are intended to indicate only specific features, numbers, steps, operations, elements, components, or combinations of the foregoing, and should not be construed as first excluding the existence of, or adding to, one or more other features, numbers, steps, operations, elements, components, or combinations of the foregoing.
Furthermore, the terms "first," "second," "third," and the like are used solely to distinguish one from another and are not to be construed as indicating or implying relative importance.
Unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art to which the various embodiments of the present application belong. The terms (such as those defined in commonly used dictionaries) should be interpreted as having a meaning that is consistent with their contextual meaning in the relevant art and will not be interpreted in an idealized or overly formal sense unless expressly so defined herein in various embodiments.
Example 1
Fig. 1 is a first flowchart of a force-controlled joint control method according to the present embodiment.
In this embodiment, the force control joint means that the joint of the robot can directly control the output force or the torque. The force control joint is provided with a torque sensor for measuring the torque of the force control joint at the corresponding moment in real time, for example, the torque sensor may be a sensor based on a resistance strain gauge. It should be understood that the above-mentioned time refers to the command control cycle of the robot, and the execution of one task usually includes many command control cycles.
In the embodiment, the moment sensor is modeled into an ideal spring with certain rigidity, and the rigidity of the moment sensor at the joint is considered to be obviously lower than the rigidity of other parts in the force control joint, so that the force control joint can be decoupled from the motor end and the load end, respective dynamic models are established based on the motor end and the load end respectively, and then the dynamic model is utilized to establish the nominal dynamic model of the force control joint for subsequent control. The motor end refers to a motor side for driving the joint to rotate, and the load end refers to a part which is connected with the motor and is used for being in direct contact with the external environment.
Exemplarily, in one embodiment, the following motor-end dynamics model may be constructed:
wherein, JmRepresenting the moment of inertia at the motor end, BmRepresenting the damping term, theta, of the motorm、
And
respectively representing the rotation angle, rotation speed and rotation acceleration of the motor, KsRepresenting the stiffness, theta, of the torque sensorlRepresenting the angle of rotation, tau, of the loaddmUncertainty factors representing unmodeled presence at the motor end, e.g. coulomb friction, τmDenotes the output torque of the motor, which has a relation τ with the current i input to the motormβ is an equivalent moment coefficient at the motor end.
Accordingly, the dynamic model of the load end satisfies:
wherein, JlRepresenting the moment of inertia of the load side, BlA damping term representing the load is represented by,
and
respectively representing the angular speed and acceleration of rotation, tau, of the loadextRepresenting the moment from the outside environment to which the load is subjected.
Further, based on the above-described dynamic models of the motor end and the load end, in order to analyze the torque control on the motor side, the load end is considered as a stationary state here, and then a nominal dynamic model is established when the load end is fixed. In one embodiment, the following nominal kinetic model can be constructed using the two kinetic models:
wherein, taus＝KsΔθ；
Δθ＝θm-θl；
Wherein, Pn(s) a nominal dynamics model, i(s) an input current of the force-controlled joint motor, τs(s) represents the output torque of the force control joint, and delta theta represents the deformation quantity of an ideal spring modeled by the torque sensor, and is the difference of the rotation angle between the motor and the load; s represents a complex variable in the laplace transform.
Therefore, after the nominal dynamic model of the force control joint is constructed, the moment control is realized by combining the Kalman filter and the disturbance observer together, on one hand, the moment estimation value and the moment differential estimation value of the force control joint obtained through Kalman filtering can enable the filtered result to be smoother, and the lag is smaller compared with the result obtained by adopting low-pass filtering; on the other hand, disturbance observation is utilized to realize observation and compensation of disturbance in the force control joint system, so that the robustness of the system to external disturbance is improved, the accuracy of a model used in Kalman filtering is improved, and the estimation precision is higher.
In order to facilitate the moment analysis, the embodiment constructs a discrete state equation of the moment control system, and then performs the optimal estimation of the system state based on the kalman filter, so as to obtain the optimal estimation of the moment control related quantity at each moment. Exemplarily, the discrete state equation comprises a first state equation and a second state equation, wherein the first state equation can be used for reflecting the relationship between the system state at the last moment and the current moment; the second state equation is an observation equation reflecting a relationship between the measured value and the estimated value of the system state.
In one embodiment, the first equation of state may be constructed from the above nominal dynamical model, and the second equation of state may be constructed from the current system measurement model, specifically, the expressions of the two equations of state are as follows:
x(k)＝Ax(k-1)+Bu(k-1)+w(k-1)；
y(k)＝Hx(k)+n(k)；
where x (k) is the system state at time k, and in this embodiment, as shown in fig. 2, the system state mainly refers to the torque value output by the motor and the differential of the torque value; u (k) is the control variable of the system at time k, here mainly the input current i for driving the motor to generate the torquePD(k) (ii) a A and B are derived from a nominal kinetic model Pn(s) system parameters obtained when converted to discrete form; w (k-1) represents the process noise at time k-1, taking into account some noise that may be present during data transfer, etc. y (k) is a measured value of the moment at the moment k, can be measured by a moment sensor and comprises a moment value of measurement noise n (k); h is a system parameter matrix of the measurement system, which may be set as a matrix H ═ 10]。
It can be understood that when the torque sensor is used for torque measurement, there may be a great amount of noise accompanied with the finally obtained measurement value due to the influence of various factors such as external environment, signal sampling and signal conversion, and therefore, the force control accuracy can be further improved by taking the measurement noise into consideration.
Based on the system state equation, the present embodiment performs optimal estimation on the moment and the moment differential through kalman filtering. The following description will be made in conjunction with specific steps of the force control joint control method. In the control process, the force control joint control method exemplarily comprises the following steps:
and step S110, obtaining a moment estimated value and a moment differential estimated value at the current moment through Kalman filtering based on the obtained moment measured value at the previous moment and the nominal dynamic model of the force control joint.
As shown in fig. 3, in one embodiment, the step S110 includes the following sub-steps:
and a substep S210, obtaining a moment predicted value and a moment differential predicted value at the current moment by utilizing a first state equation according to the motor input current, the moment predicted value and the moment differential predicted value of the force control joint at the previous moment.
For example, the moment predicted value and the moment differential predicted value at the current moment may be performed based on the first state equation and the motor input current, the moment predicted value, and the like of the force control joint at the previous moment, and may be specifically calculated by the following formula:
wherein the content of the first and second substances,
and
respectively representing predicted torque values at the moment k and the moment k-1, and u (k-1) representing a system control quantity at the previous moment, namely a motor input current iPD(k-1)。
And a substep S220 of calculating the error covariance between the moment predicted value and the moment true value at the current moment according to the error covariance between the moment estimated value and the moment true value at the previous moment and the process noise, and calculating the gain of the Kalman filter according to the error covariance between the moment predicted value and the moment true value at the current moment and the measurement noise at the previous moment.
According to the kalman filter principle, the gain coefficient of the kalman filter can be calculated by the error covariance between the predicted value and the true value of the system state, the noise of the measurement system, and the like. Exemplarily, the calculation formula of the gain of the kalman filter is as follows:
K(k)＝P-(k)HT(HP-(k)HT+R)-1；
where K (k) denotes the gain of the Kalman filter at time k, P-(k) A covariance matrix representing the prediction error of the system state at time k, i.e. the error covariance between the predicted value and the true value of the system state at time k, R represents the covariance of the measurement noise n (k) at time k.
Covariance for prediction error as described aboveMatrix P-(k) Exemplarily, the calculation formula is as follows:
P-(k)＝AP(k-1)AT+Q；
p (k-1) is a covariance matrix of an estimation error of the system state at the time k-1, namely an error covariance between an estimation value and a true value of the system state at the time k-1; q represents the covariance of the process noise w (k). In this embodiment, the covariance of the prediction error includes an error covariance between the predicted value of the moment and the true value of the moment, and an error covariance between the predicted value and the true value of the moment differential.
And a substep S230 of calculating a moment estimation value and a moment differential estimation value at the current moment by using a second state equation according to the moment prediction value, the moment differential prediction value, the gain of the kalman filter and the moment measurement value at the previous moment.
Illustratively, based on the second state equation described above, the estimated value may be calculated from the predicted value and the measured value of the system state as follows:
wherein the content of the first and second substances,
is an estimate of the Kalman filter output, i.e., in this embodiment
Further, after the control is performed based on the estimated value at the time k, the error covariance between the estimated value of the system state at the time k and the true value, that is, the covariance matrix p (k) of the estimated error, may be obtained, where p (k) may be used to calculate the estimated value of the system state at the next time, and then the above process is repeated.
Exemplarily, the calculation formula of the covariance matrix p (k) of the estimation error is as follows:
P(k)＝(I-K(k)H)P-(k)。
it is to be understood that, in the present embodiment, the covariance of the estimated error includes the covariance of the error between the estimated value of the moment and the true value, and between the estimated value of the moment differential and the true value.
Therefore, the torque predicted value and the torque differential predicted value at the current moment can be calculated based on the steps, and the current required by the motor can be calculated according to the torque predicted value and the torque differential predicted value.
And step S120, obtaining the current control quantity at the current moment based on a preset moment controller according to the expected moment, the moment estimation value and the moment differential estimation value of the force control joint at the current moment.
The torque controller is used for calculating the current required to be input to the motor, in this embodiment, proportional-derivative control (PD control) is adopted for feedback control, and feed-forward control is added to achieve a better control effect, at this time, a controller formed by PD feedback control and feed-forward control may be referred to as a PD-feed-forward controller. It should be understood that the torque controller may also adopt other controls, such as proportional-integral control (i.e. PI control), proportional-integral-derivative control (i.e. PID control), etc., and whether to add feed-forward control, which may be specifically selected according to actual requirements, and is not limited herein, and accordingly, the control amount related to the torque may also be adaptively adjusted.
For the above PD-feedforward controller, in one embodiment, the expression is as follows:
where k denotes the kth time, iPD(k) A current control quantity, τ, output by the PD-feedforward controllerref(k) For the desired moment of the force-controlled joint,
for the derivative of the desired moment, KPAnd KDProportional and differential link coefficients, tau, in the PD-feedforward controllerk(k) In order to be an estimate of the torque,
is a torque differential estimate.
Exemplarily, taking the above PD-feedforward controller as an example, as shown in the figure, for the current control amount at the current moment, the desired torque τ of the joint at the current moment can be controlled by first controlling the desired torque τ according to the forcerefObtaining a desired moment differential
Then, the desired moment τ is determined according to the current timerefWith torque estimate τkDeviation therebetween, and the expected torque derivative
Differential estimate from moment
The deviation between the current control values is substituted into the PD-feedforward controller, and the current control quantity i at the current moment can be calculatedPD。
And step S130, obtaining the disturbance compensation quantity at the current moment according to the nominal dynamic model, the input current of the force control joint motor at the previous moment and the moment estimated value.
The present embodiment compensates for the disturbance at the current time by using the system state at the previous time, which is mainly implemented by a low-pass filter Q, as shown in fig. 2. For example, in one embodiment, a model of a low pass filter may be employed as follows:
where Q(s) represents a low pass filter model, ωqThe cut-off frequency of the low-pass filter.
Exemplarily, as shown in fig. 4, the step S130 may include the following sub-steps:
and a substep S310, obtaining a first disturbance compensation component through a preset low-pass filtering model according to the inverse model of the nominal dynamic model and the moment estimation value.
And a substep S320, obtaining a second disturbance compensation component through a preset low-pass filtering model according to the input current of the force control joint motor at the previous moment.
And a substep S330, adding the first disturbance compensation component and the second disturbance compensation component to obtain a disturbance compensation quantity.
In this embodiment, the disturbance compensation amount mainly includes two parts, namely, the disturbance compensation with respect to the current input to the motor, i.e., the first disturbance compensation component, and the disturbance compensation with respect to the nominal dynamic model and the torque estimation value, i.e., the second disturbance compensation component.
Exemplarily, for the calculation process of the disturbance compensation amount, if the expression is used, there are:
wherein the content of the first and second substances,
representing a disturbance compensation quantity; i(s) is the total current value, wherein the input current of the force control joint motor at the previous moment is mainly adopted;
is a nominal model PnThe inverse model of(s) can obtain the disturbance compensation value at the current moment by only converting the above formula into a discrete form and substituting the discrete form into the known quantity
And step S140, calculating the input current of the force control joint motor at the current moment according to the current control quantity and the disturbance compensation quantity so as to control the force control joint to execute corresponding operation.
Exemplarily, by controlling the amount i of current to be output by a preset controllerPD(k) And the calculated disturbance compensation quantity of the current moment
And subtracting to obtain the total current i (k) required to be input to the joint motor at the current moment, wherein the specific calculation process is as follows:
and finally, inputting the total current into a motor of the force control joint to drive the motor to rotate so as to control the force control joint to perform corresponding operation. And then entering the next control cycle according to the current task requirement, and repeating the step S110 until the task is completed.
According to the force control joint control method, the moment estimation value and the moment differential estimation value obtained through Kalman filtering are utilized, so that the filtered result is smoother, and the lag is smaller compared with the low-pass filtering result; meanwhile, the disturbance observer is used for observing and compensating disturbance in the force control joint system, so that the robustness of the system to external disturbance is improved, the accuracy of a model used in Kalman filtering is improved, and the estimation precision is higher. It should be understood that the disturbance observation and the kalman filtering in this embodiment are both located in the torque control inner loop, and decoupling with other control loops can be achieved, so that the control algorithm of the outer loop can achieve a better control effect on the basis of the disturbance observation and the kalman filtering.
Example 2
Referring to fig. 5, based on the method of embodiment 1, this embodiment provides a force-controlled joint control device 100, where a torque sensor is disposed at the force-controlled joint for measuring a torque measurement value at a corresponding time. Illustratively, the force-controlled joint control apparatus 100 includes:
the filtering module 110 is configured to obtain a torque estimation value and a torque differential estimation value at a current moment through kalman filtering based on the obtained torque measurement value at the previous moment and the nominal dynamic model of the force-controlled joint;
the calculation module 120 is configured to obtain a current control quantity at a current moment based on a preset torque controller according to an expected torque of the force-controlled joint at the current moment, the torque estimation value, and the torque differential estimation value;
the compensation module 130 is configured to obtain a disturbance compensation amount at a current moment according to the nominal dynamic model, the moment estimation value at a previous moment, and the input current of the force-controlled joint motor;
the calculation module 120 is further configured to calculate an input current of the force-controlled joint motor at the current moment according to the current control amount and the disturbance compensation amount;
and the control module 140 is used for controlling the force control joint to move according to the input current.
In one embodiment, the filtering module 110 includes: the equation establishing sub-module 111, the predicted value calculating sub-module 112, the true value calculating sub-module 113, the gain calculating sub-module 114, and the estimated value calculating sub-module 115 (not shown), specifically, the equation establishing sub-module 111 is configured to obtain a first state equation through the nominal kinetic model construction, and construct a second state equation through the system measurement model. The predicted value calculation submodule 112 is configured to obtain a predicted value of the moment and a predicted value of the moment differential at the current time by using a first state equation according to the motor input current, the moment estimated value, and the moment differential estimated value of the force control joint at the previous time. The actual value calculation submodule 113 is configured to obtain the actual value of the moment at the previous time by using the second state equation according to the moment measurement value and the measurement noise at the previous time. The gain calculation submodule 114 is configured to calculate a kalman filter gain according to an error between the predicted moment value and the true moment value at the previous time. The estimated value calculating submodule 115 is configured to calculate a torque estimated value and a torque differential estimated value at the current time according to the torque predicted value, the torque differential predicted value, the torque measured value at the previous time, and the kalman filter gain at the current time.
In one embodiment, the compensation module 130 includes a component calculation submodule 131 and a component addition submodule 132, specifically, the component calculation submodule 131 is configured to obtain a first disturbance compensation component through a preset low-pass filtering model according to an inverse model of the nominal dynamic model and the moment estimation value; and the second disturbance compensation component is obtained by passing the input current of the force control joint motor at the previous moment through a preset low-pass filtering model. The component adding sub-module 132 is configured to add the first disturbance compensation component and the second disturbance compensation component to obtain a disturbance compensation amount.
It is to be understood that the apparatus of the present embodiment corresponds to the method of embodiment 1 described above, and the alternatives of embodiment 1 described above are equally applicable to the present embodiment, and therefore, the description thereof will not be repeated.
The application further provides a robot, exemplarily comprising a force control joint, a processor and a memory, wherein the memory stores a computer program, and the processor causes the robot to execute the functions of the above-mentioned force control joint control method or the above-mentioned modules in the force control joint control device by executing the computer program, so as to control the force control joint to execute the corresponding operation.
The application also provides a readable storage medium for storing the computer program used in the terminal device.
In the embodiments provided in the present application, it should be understood that the disclosed apparatus and method can be implemented in other ways. The apparatus embodiments described above are merely illustrative and, for example, the flowchart and block diagrams in the figures illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods and computer program products according to various embodiments of the present application. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code, which comprises one or more executable instructions for implementing the specified logical function(s). It should also be noted that, in alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It will also be noted that each block of the block diagrams and/or flowchart illustration, and combinations of blocks in the block diagrams and/or flowchart illustration, can be implemented by special purpose hardware-based systems which perform the specified functions or acts, or combinations of special purpose hardware and computer instructions.
In addition, each functional module or unit in each embodiment of the present application may be integrated together to form an independent part, or each module may exist separately, or two or more modules may be integrated to form an independent part.
The functions, if implemented in the form of software functional modules and sold or used as a stand-alone product, may be stored in a computer readable storage medium. Based on such understanding, the technical solution of the present application or portions thereof that substantially contribute to the prior art may be embodied in the form of a software product stored in a storage medium and including instructions for causing a computer device (which may be a smart phone, a personal computer, a server, or a network device) to execute all or part of the steps of the method according to the embodiments of the present application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.
The above description is only for the specific embodiments of the present application, but the scope of the present application is not limited thereto, and any person skilled in the art can easily conceive of the changes or substitutions within the technical scope of the present application, and shall be covered by the scope of the present application.
Claims (10)
1. A force control joint control method is characterized in that a torque sensor is arranged at the force control joint and used for measuring to obtain a torque measured value at a corresponding moment; the method comprises the following steps:
based on the obtained moment measurement value at the previous moment and the nominal dynamic model of the force control joint, obtaining a moment estimation value and a moment differential estimation value at the current moment through Kalman filtering;
obtaining current control quantity at the current moment according to the expected moment of the force control joint at the current moment, the moment estimated value and the moment differential estimated value based on a preset moment controller;
obtaining the disturbance compensation quantity at the current moment according to the nominal dynamic model, the input current of the force control joint motor at the previous moment and the moment estimation value;
and calculating to obtain the input current required by the force control joint motor at the current moment according to the current control quantity and the disturbance compensation quantity so as to control the force control joint to execute corresponding operation.
2. The force-controlled joint control method of claim 1, wherein the nominal dynamical model of the force-controlled joint is pre-constructed and comprises:
decoupling the force control joint into a motor end and a load end based on the torque sensor, and respectively establishing a dynamic model of the motor end and the load end;
and constructing a nominal dynamic model when the load end is fixed according to the dynamic models of the motor end and the load end.
3. The force-controlled joint control method according to claim 1, wherein the obtaining of the moment estimation value and the moment differential estimation value at the current time through kalman filtering includes:
obtaining a moment predicted value and a moment differential predicted value at the current moment by utilizing a first state equation according to the motor input current, the moment predicted value and the moment differential predicted value of the force control joint at the previous moment, wherein the first state equation is constructed in advance through the nominal dynamic model;
calculating the error covariance between the moment predicted value and the moment real value at the current moment according to the error covariance between the moment estimated value and the moment real value at the previous moment and the process noise, and calculating the gain of the Kalman filter according to the error covariance between the moment predicted value and the moment real value at the current moment and the measurement noise at the previous moment;
and calculating a moment estimation value and a moment differential estimation value at the current moment by using a second state equation according to the moment predicted value, the moment differential predicted value, the gain of the Kalman filter and the moment measurement value at the previous moment, wherein the second state equation is constructed in advance through a system measurement model.
4. The force-controlled joint control method according to claim 1, wherein the preset torque controller is a PD-feedforward controller, and the obtaining the current control amount at the current moment comprises:
obtaining an expected moment differential according to the expected moment of the force control joint at the current moment;
and obtaining the current control quantity at the current moment through the PD-feedforward controller according to the deviation between the expected torque and the torque estimation value at the current moment and the deviation between the expected torque differential and the torque differential estimation value.
5. The force-controlled joint control method according to claim 1, wherein the obtaining the disturbance compensation amount at the current time includes:
obtaining a first disturbance compensation component through a preset low-pass filtering model according to the inverse model of the nominal dynamic model and the moment estimation value;
obtaining a second disturbance compensation component through a preset low-pass filtering model according to the input current of the force control joint motor at the previous moment;
and adding the first disturbance compensation component and the second disturbance compensation component to obtain a disturbance compensation quantity.
6. The force-controlled joint control method of claim 2, wherein the nominal dynamical model of the force-controlled joint is expressed as follows:
wherein, taus＝KsΔθ；
Δθ＝θm-θl；
Wherein, Pn(s) represents the nominal dynamics model, i(s) represents the input current of the force-controlled joint motor, τs(s) represents the output torque of the force control joint, beta represents the equivalent torque coefficient of the motor, and KsRepresenting the stiffness of the torque sensor, JmRepresenting the moment of inertia at the motor end, BmRepresenting the damping term, theta, of the motormIndicating the angle of rotation, theta, of the motorlRepresents the rotation angle of the load, Δ θ represents the deformation amount of the spring modeled by the torque sensor, and s represents a complex variable in the laplace transform.
7. The force-controlled joint control method of claim 4, wherein the PD-feedforward controller has the expression:
where k denotes the kth time, iPD(k) A current control quantity, τ, output by the PD-feedforward controllerref(k) For the desired moment of the force-controlled joint,
for the desired moment differential, KPAnd KDProportional and differential link coefficients, tau, in the PD-feedforward controllerk(k) In order to be an estimate of the torque,
beta represents an equivalent torque coefficient of the motor as a torque differential estimation value.
8. A force control joint control device is characterized in that a torque sensor is arranged at a force control joint and used for measuring to obtain a torque measured value at a corresponding moment; the method comprises the following steps:
the filtering module is used for obtaining a moment estimated value and a moment differential estimated value at the current moment through Kalman filtering based on the obtained moment measured value at the previous moment and the nominal dynamic model of the force control joint;
the calculation module is used for obtaining the current control quantity at the current moment according to the expected moment of the force control joint at the current moment, the moment estimation value and the moment differential estimation value based on a preset moment controller;
the compensation module is used for obtaining disturbance compensation quantity at the current moment according to the nominal dynamic model, the moment estimation value at the previous moment and the input current of the force control joint motor;
and the control module is used for calculating the input current of the force control joint motor at the current moment according to the current control quantity and the disturbance compensation quantity and controlling the force control joint to move according to the input current.
9. A robot, characterized in that the robot comprises a force controlled joint, a processor and a memory, the memory storing a computer program for executing the computer program to implement the force controlled joint control method of any of claims 1-7 for controlling the force controlled joint to move.
10. Readable storage medium, characterized in that it stores a computer program which, when executed on a processor, implements the force controlled joint control method according to any of claims 1-7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111360702.7A CN113954078B (en) | 2021-11-17 | 2021-11-17 | Force control joint control method and device, robot and readable storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111360702.7A CN113954078B (en) | 2021-11-17 | 2021-11-17 | Force control joint control method and device, robot and readable storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113954078A true CN113954078A (en) | 2022-01-21 |
| CN113954078B CN113954078B (en) | 2023-02-24 |
Family
ID=79470911
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111360702.7A Active CN113954078B (en) | 2021-11-17 | 2021-11-17 | Force control joint control method and device, robot and readable storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113954078B (en) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2013000856A (en) * | 2011-06-20 | 2013-01-07 | Seiko Epson Corp | Robot control system and robot system |
| CN106313044A (en) * | 2016-09-20 | 2017-01-11 | 华南理工大学 | Feedforward torque compensating method used for industrial robot |
| US20180169864A1 (en) * | 2015-05-21 | 2018-06-21 | Kastanienbaum GmbH | Method and device for open-loop/closed-loop control of an actuator-driven robot joint |
| CN108189036A (en) * | 2018-01-17 | 2018-06-22 | 广州视源电子科技股份有限公司 | Torque Control method, apparatus, robot and storage medium |
| CN109202889A (en) * | 2017-06-30 | 2019-01-15 | 沈阳新松机器人自动化股份有限公司 | A kind of Flexible Multi-joint robot electric current Force control system and method |
| CN111546346A (en) * | 2020-05-29 | 2020-08-18 | 清华大学 | Flexible joint disturbance observation method, moment control method and equipment |
| CN112809667A (en) * | 2020-12-22 | 2021-05-18 | 上海新时达机器人有限公司 | Force control method and device of industrial robot and application of force control device |
| CN112987812A (en) * | 2021-02-24 | 2021-06-18 | 深圳市优必选科技股份有限公司 | Torque control method and device, terminal equipment and storage medium |
-
2021
- 2021-11-17 CN CN202111360702.7A patent/CN113954078B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2013000856A (en) * | 2011-06-20 | 2013-01-07 | Seiko Epson Corp | Robot control system and robot system |
| US20180169864A1 (en) * | 2015-05-21 | 2018-06-21 | Kastanienbaum GmbH | Method and device for open-loop/closed-loop control of an actuator-driven robot joint |
| CN106313044A (en) * | 2016-09-20 | 2017-01-11 | 华南理工大学 | Feedforward torque compensating method used for industrial robot |
| CN109202889A (en) * | 2017-06-30 | 2019-01-15 | 沈阳新松机器人自动化股份有限公司 | A kind of Flexible Multi-joint robot electric current Force control system and method |
| CN108189036A (en) * | 2018-01-17 | 2018-06-22 | 广州视源电子科技股份有限公司 | Torque Control method, apparatus, robot and storage medium |
| CN111546346A (en) * | 2020-05-29 | 2020-08-18 | 清华大学 | Flexible joint disturbance observation method, moment control method and equipment |
| CN112809667A (en) * | 2020-12-22 | 2021-05-18 | 上海新时达机器人有限公司 | Force control method and device of industrial robot and application of force control device |
| CN112987812A (en) * | 2021-02-24 | 2021-06-18 | 深圳市优必选科技股份有限公司 | Torque control method and device, terminal equipment and storage medium |
Non-Patent Citations (1)
| Title |
|---|
| 涂骁: "基于动力学前馈的工业机器人运动控制关键技术研究", 《中国博士学位论文全文数据库 信息科技辑》 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113954078B (en) | 2023-02-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Miranda-Colorado et al. | Experimental parameter identification of flexible joint robot manipulators | |
| CN110467111B (en) | Control of bridge cranes | |
| CN106020124B (en) | Servo motor control device and collision detection method | |
| WO1991005296A1 (en) | Sliding mode control method | |
| CN113305839B (en) | Admittance control method and admittance control system of robot and robot | |
| US11669055B2 (en) | Vibration suppression device, method and computer-readable medium using estimated vibration torque | |
| CN114450131A (en) | Non-derivative model learning system and design for robot system | |
| CN108762088B (en) | Sliding mode control method for hysteresis nonlinear servo motor system | |
| CN112077839A (en) | Motion control method and device for mechanical arm | |
| Hakvoort et al. | Lifted system iterative learning control applied to an industrial robot | |
| Kasac et al. | Frequency‐shifting‐based algebraic approach to stable on‐line parameter identification and state estimation of multirotor UAV | |
| JPH10309684A (en) | Compliance control method of manipulator | |
| Suzumura et al. | A general framework for designing SISO-based motion controller with multiple sensor feedback | |
| CN111015661B (en) | Active vibration control method and system for flexible load of robot | |
| KR102289968B1 (en) | Apparatus for controlling robot manipulator and method therefor | |
| JP6930868B2 (en) | Servo control device, servo control method and system | |
| CN113954078B (en) | Force control joint control method and device, robot and readable storage medium | |
| JP5208610B2 (en) | Characteristic identification method and motor control device for controlling inertial resonance system | |
| Aksman et al. | Force estimation based compliance control of harmonically driven manipulators | |
| CN115236990A (en) | Generalized estimator, generalized interference rejection controller and design method | |
| JP2019219762A (en) | Control device, control method and program | |
| Hatamleh et al. | Quasi-Continuous Second Order Sliding Mode Control of Revolute-Revolute Manipulator with Noisy Feedback Signals & Modelling Uncertainties | |
| JP5037065B2 (en) | Machine control device | |
| CN113297798A (en) | Robot external contact force estimation method based on artificial neural network | |
| Waiboer et al. | Application of a perturbation method for realistic dynamic simulation of industrial robots |
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 | ||
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20231204 Address after: Room 601, 6th Floor, Building 13, No. 3 Jinghai Fifth Road, Beijing Economic and Technological Development Zone (Tongzhou), Daxing District, Beijing, 100176 Patentee after: Beijing Youbixuan Intelligent Robot Co.,Ltd. Address before: 518000 16th and 22nd Floors, C1 Building, Nanshan Zhiyuan, 1001 Xueyuan Avenue, Nanshan District, Shenzhen City, Guangdong Province Patentee before: Shenzhen Youbixuan Technology Co.,Ltd. |