Robot contact force detection method and device, electronic equipment and storage medium

Abstract

The invention provides a robot contact force detection method and device, electronic equipment and a storage medium. The robot contact force detection method comprises the steps that a first kinetic equation of a robot is determined; transforming the first kinetic equation to obtain a target model; determining the system state of the robot; and determining the contact force of the tail end of the robot according to the target model and the system state. According to the method for detecting the contact force of the robot, the accuracy of contact force detection is high.

Description

Technical Field

[0001] The present invention relates to the field of robot technology, and in particular to a method, device, electronic equipment and storage medium for detecting contact force of a robot. Background Art

[0002] In the related art, the method for estimating the contact force of the robot end only gives an estimate of the end force through the information of the robot's motor. However, the movement of the robot end has certain uncertainties, such as friction, which leads to low detection accuracy of the contact force of the robot end. Summary of the invention

[0003] The present invention aims to at least solve or improve the technical problem of low accuracy in detecting the contact force of a robot end in the prior art.

[0004] To this end, a first aspect of the present invention provides a method for detecting contact force of a robot.

[0005] A second aspect of the present invention provides a device for detecting contact force of a robot.

[0006] A third aspect of the present invention provides an electronic device.

[0007] A fourth aspect of the present invention provides a storage medium.

[0008] In view of this, according to a first aspect of the present invention, the present invention proposes a method for detecting contact force of a robot, comprising: determining a first dynamic equation of the robot: in, represents the derivative of the momentum of the robot's end, represents the first defined quantity, which is determined by the force on the end of the robot, the centripetal force and Coriolis force of the robot, the gravity moment of the robot, and the friction force on the robot. T represents the transpose of the Jacobian matrix of the robot, f represents the external force at the end of the robot, and v p represents the first noise term; transform the first dynamic equation to obtain the target model: in, represents the generalized contact force of the robot, I N represents the first set, represents the first zero matrix, represents the system state of the robot, k represents the time; the system state of the robot is determined; based on the target model and the system state, the generalized contact force of the robot is determined.

[0009] The method for detecting the contact force of a robot proposed in the present invention comprises first determining a first dynamic equation of the robot, wherein the first dynamic equation is an equation about friction and generalized force during the operation of the robot end, transforming the first dynamic equation to obtain a target model, and obtaining the system state of the robot during the operation of the robot, wherein the system state includes motion parameters of a motor, etc. The contact force of the robot end is determined according to the system state and the target model. Since the dynamic equation of the present invention is about friction and generalized force, the contact force finally determined is associated with friction, thereby improving the accuracy of contact force detection.

[0010] In addition, the robot contact force detection method in the above technical solution provided by the present invention may also have the following additional technical features:

[0011] In some embodiments, optionally, transforming the first kinetic equation to obtain a target model includes: defining the derivative of f as: ω f represents the second noise term, represents the first Gaussian distribution, f represents the external force at the end of the robot, Q c,f represents the first set of rational numbers; ω p and ω f Arranged into vector form: Where ω represents the total noise term, ω p represents the first noise term, ω f represents the second noise term, Represents N+n ext The first real number set of dimension, T represents the transposition operation; the system state of the robot is defined as: x represents the system state of the robot, p represents the momentum of the robot's end, and f represents the external force at the robot's end. Represents N+n ext The first real number set of dimensions, T represents the transposition operation; according to the total noise term and the system state, the first dynamic equation is organized into a state space equation: in, represents the derivative of the robot's system state, 0 N×N represents the second zero matrix, J T represents the transpose of the Jacobian matrix of the robot, I N represents the first set, represents the first zero matrix, represents the third zero matrix, Indicates the system status of the robot, Represents the first defined quantity, ω represents the total noise term; the state space equation is discretized to obtain the target model.

[0012] In this embodiment, the first kinematic equation is transformed to obtain the target model, including: defining a series of parameters, organizing the first kinematic equation into a state-space equation, and performing a discrete transformation on the state-space equation to obtain the target model, thereby reducing and filtering out noise by transforming the first kinematic equation, thereby improving the accuracy of detecting the robot's contact force.

[0013] In some embodiments, optionally, the state space equation is discretized to obtain the target model, including: defining the robot's measurement data as p meas , the formula for determining the measurement data is: Among them, I N represents the first set, represents the first zero matrix, represents the system state of the robot, υ represents the measurement error, represents the third Gaussian distribution; replace Defined as Will Defined as A c ,Will Defined as x, Defined as B c ,Will Defined as u, replace p in the formula of measured data meas Defined as y, Defined as C c ; Describing the state space equation as a discrete system yields: x k+1 =A k x k +B k u k +ω k , where k represents the first moment, k+1 represents the second moment, A represents the first coefficient, B represents the second coefficient, and ω represents the total noise term. represents the second Gaussian distribution; describing the formula of the measured data as a discrete system yields: y k =Cx k +υ k , C represents the third coefficient, υ represents the measurement error; the discretized state space equation and the formula of the measurement data are transformed into the relationship:

[0014] represents the second coefficient, represents the fourth zero matrix, I N represents the first set, exp represents the exponential function, 0 N×N represents the second zero matrix, T S Indicates the sampling period; C = CC , Among them, C represents the third coefficient, R represents the fourth coefficient, k represents the first moment, T S represents the sampling period, R C represents the variance of the third Gaussian distribution; Among them, H represents the fifth coefficient, A c express represents the fifth zero matrix, Q C represents the second set of rational numbers, Indicates A C The transpose of Among them, M 11 represents the first mass inertia matrix, M 12 represents the second mass inertia matrix, M 22 represents the third mass inertia matrix, represents the fifth zero matrix, exp represents the exponential function, H represents the fifth coefficient, T S represents the sampling period, k represents the first moment; Among them, M 11 represents the first mass inertia matrix, M 12 represents the second mass inertia matrix, k represents the first moment, and Q represents the third rational number set; the target model is obtained by sorting.

[0015] In this embodiment, the state-space equation is discretely transformed to obtain a target model, including: defining parameters such as measurement data, and describing the formulas of the state-space equation and the measurement data as a discrete system, then performing a relational transformation on the two, and then obtaining the target model through sorting. The above method fully takes into account errors and noise terms, etc., thereby improving the accuracy of contact force determination.

[0016] In some embodiments, optionally, determining the system state of the robot includes: determining the system state through a Kalman filter.

[0017] In this embodiment, determining the system state of the robot includes determining the system state of the robot through a Kalman filter.

[0018] According to a second aspect of the present invention, the present invention provides a device for detecting contact force of a robot, comprising: a first determination module, for determining a first dynamic equation of the robot: in, represents the derivative of the momentum of the robot's end, represents the first defined quantity, which is determined by the force on the end of the robot, the centripetal force and Coriolis force of the robot, the gravity moment of the robot, and the friction force on the robot. T represents the transpose of the Jacobian matrix of the robot, f represents the external force at the end of the robot, ωp represents the first noise term; the change module is used to transform the first dynamic equation to obtain the target model: in, represents the generalized contact force of the robot, I N represents the first set, represents the first zero matrix, represents the system state of the robot, k represents time; the second determination module is used to determine the system state of the robot; the third determination module is used to determine the generalized contact force of the robot according to the target model and the system state.

[0019] The device for detecting the contact force of a robot proposed in the present invention comprises first determining a first dynamic equation of the robot, wherein the first dynamic equation is an equation about friction and generalized force during the operation of the robot end, transforming the first dynamic equation to obtain a target model, and obtaining the system state of the robot during the operation of the robot, wherein the system state includes motion parameters of a motor, etc. The contact force of the robot end is determined according to the system state and the target model. Since the dynamic equation of the present invention is about friction and generalized force, the contact force finally determined is associated with friction, thereby improving the accuracy of contact force detection.

[0020] In some embodiments, optionally, the variation module includes: a first definition module, used to define the derivative of f as: ω f represents the second noise term, represents the first Gaussian distribution, f represents the external force at the end of the robot, Q c,f represents the first rational number set; the first sorting submodule is used to convert ω p and ω f Arranged into vector form: Where ω represents the total noise term, ω p represents the first noise term, ω f represents the second noise term, Represents N+n ext The first real number set of the dimension, T represents the transposition operation; the second definition submodule is used to define the system state of the robot as: x represents the system state of the robot, p represents the momentum of the robot's end, and f represents the external force at the robot's end. Represents N+n ext The first real number set of dimensions, T represents the transposition operation; the second sorting submodule is used to sort the first dynamic equation into a state space equation according to the total noise term and the system state: in, represents the derivative of the robot's system state, 0 N×Nrepresents the second zero matrix, J T represents the transpose of the Jacobian matrix of the robot, I N represents the first set, represents the first zero matrix, represents the third zero matrix, Indicates the system status of the robot, represents the first defined quantity, ω represents the total noise term; the conversion submodule is used to discretize the state space equation to obtain the target model.

[0021] In this embodiment, the first kinematic equation is transformed to obtain the target model, including: defining a series of parameters, organizing the first kinematic equation into a state-space equation, and performing a discrete transformation on the state-space equation to obtain the target model, thereby reducing and filtering out noise by transforming the first kinematic equation, thereby improving the accuracy of detecting the robot's contact force.

[0022] In some embodiments, the conversion submodule may include: a first definition unit for defining the measurement data of the robot as p meas , the formula for determining the measurement data is: Among them, I N represents the first set, represents the first zero matrix, represents the system state of the robot, υ represents the measurement error, represents the third Gaussian distribution; the second definition unit is used to transform the state space equation Defined as Will Defined as A c ,Will Defined as x, Defined as B c ,Will Defined as u, replace p in the formula of measured data meas Defined as y, Defined as C c ; The first conversion unit is used to describe the state space equation as a discrete system to obtain: x k+1 =A k x k +B k u k +ω k , where k represents the first moment, k+1 represents the second moment, A represents the first coefficient, B represents the second coefficient, and ω represents the total noise term. represents the second Gaussian distribution; the second conversion unit is used to describe the formula of the measured data as a discrete system to obtain: y k =Cx k +vk , C represents the third coefficient, v represents the measurement error; the third conversion unit is used to convert the relationship between the discretized state space equation and the formula of the measurement data: Among them, A represents the first coefficient, B represents the second coefficient, represents the fourth zero matrix, I N represents the first set, exp represents the exponential function, 0 N×N represents the second zero matrix, T S Indicates the sampling period; C = C C , Among them, C represents the third coefficient, R represents the fourth coefficient, k represents the first moment, T S represents the sampling period, R C represents the variance of the third Gaussian distribution; Among them, H represents the fifth coefficient, A c express represents the fifth zero matrix, Q C represents the second set of rational numbers, Indicates A C The transpose of Indicates A C The transpose of Among them, M 11 represents the first mass inertia matrix, M 12 represents the second mass inertia matrix, M 22 represents the third mass inertia matrix, represents the fifth zero matrix, exp represents the exponential function, H represents the fifth coefficient, T S represents the sampling period, k represents the first moment; Among them, M 11 represents the first mass inertia matrix, M 12 represents the second mass inertia matrix, k represents the first moment, and Q represents the third rational number set; a sorting unit is used to sort out the target model.

[0023] In this embodiment, the state-space equation is discretely transformed to obtain a target model, including: defining parameters such as measurement data, and describing the formulas of the state-space equation and the measurement data as a discrete system, then performing a relational transformation on the two, and then obtaining the target model through sorting. The above method fully takes into account errors and noise terms, etc., thereby improving the accuracy of contact force determination.

[0024] In some embodiments, optionally, the second determination module includes: a determination submodule, configured to determine the system state of the robot through a Kalman filter.

[0025] In this embodiment, determining the system state of the robot includes determining the system state of the robot through a Kalman filter.

[0026] In this embodiment, the third determination module includes: determining the system state of the robot through a Kalman filter.

[0027] According to the third aspect of the present invention, the present invention proposes an electronic device, including a processor and a memory, the memory storing programs or instructions that can be run on the processor, and when the program or instructions are executed by the processor, the steps of the robot contact force detection method provided in the first aspect embodiment are implemented.

[0028] The electronic device proposed in the present invention includes a memory that stores a program or instruction that implements the steps of the robot contact force detection method provided in the first aspect embodiment when executed by a processor. Therefore, it has all the beneficial effects of the robot contact force detection method provided in the first aspect embodiment, which are no longer stated one by one here.

[0029] According to a fourth aspect of the present invention, the present invention proposes a storage medium, on which a program or instruction is stored, and when the program or instruction is executed by a processor, the steps of the robot contact force detection method provided in the first aspect embodiment are implemented.

[0030] The readable storage medium proposed by the present invention stores a program or instruction that implements the steps of the robot contact force detection method provided in the first aspect embodiment when executed by a processor. Therefore, it has all the beneficial effects of the robot contact force detection method provided in the first aspect embodiment, which are no longer stated one by one here.

[0031] Additional aspects and advantages of the present invention will become apparent from the following description or may be learned by practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] The above and / or additional aspects and advantages of the present invention will become apparent and easily understood from the description of the embodiments in conjunction with the following drawings, in which:

[0033] Figure 1 A flow chart showing a method for detecting contact force of a robot provided by one embodiment of the present invention;

[0034] Figure 2 A structural block diagram showing a device for detecting contact force of a robot provided by one embodiment of the present invention;

[0035] Figure 3 A structural block diagram of an electronic device provided by an embodiment of the present invention is shown;

[0036] Figure 4A schematic diagram showing an experiment on a robot using a method for detecting robot contact force provided by an embodiment of the present invention;

[0037] Figure 5 A schematic diagram showing a method for detecting robot contact force provided by an embodiment of the present invention for determining contact force and comparing the contact force measured by a sensor. DETAILED DESCRIPTION

[0038] In order to more clearly understand the above-mentioned purpose, features and advantages of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. It should be noted that the embodiments of the present application and the features in the embodiments can be combined with each other without conflict.

[0039] In the following description, many specific details are set forth to facilitate a full understanding of the present invention. However, the present invention may also be implemented in other ways different from those described herein. Therefore, the protection scope of the present invention is not limited to the specific embodiments disclosed below.

[0040] Refer to the following Figures 1 to 5 The following describes a robot contact force detection method, device, electronic device, and storage medium provided according to some embodiments of the present invention.

[0041] According to a first aspect of the present invention, the present invention provides a method for detecting contact force of a robot. Figure 1 A flow chart showing a method for detecting contact force of a robot provided by an embodiment of the present invention is shown as follows: Figure 1 As shown, the steps of a method for detecting contact force of a robot provided by one embodiment of the present invention are as follows:

[0042] Step 102: Determine the first dynamic equation of the robot.

[0043] Specifically, the first kinetic equation is: in, represents the derivative of the momentum of the robot's end, represents the first defined quantity, which is determined by the force on the end of the robot, the centripetal force and Coriolis force of the robot, the gravity moment of the robot, and the friction force on the robot. T represents the transpose of the Jacobian matrix of the robot, f represents the external force at the end of the robot, ω p represents the first noise term.

[0044] Among them, for common serial robots, such as robots commonly used in general industry, the force analysis of their end flanges is generally the second dynamic equation.

[0045] The second kinetic equation is: Where q represents the angle vector of the robot's end, represents the angular velocity vector of the robot's end, represents the angular acceleration vector of the robot's end, M(q) represents the robot's mass inertia matrix, represents the centripetal force and Coriolis force of the robot, G(q) represents the gravity matrix of the robot, τ fric Represents the friction force received by the robot. The contact force at the end of the robot is converted to the shaft end to produce τ ext , τ mot Represents the force at the end of the robot.

[0046] Among them, for the end of a robot with N degrees of freedom, represents the second set of real numbers with N×N degrees of freedom, The third set of real numbers representing N degrees of freedom, The third set of real numbers representing N degrees of freedom, The third set of real numbers representing the N degrees of freedom.

[0047] Through the Jacobian matrix Indicates n ext ×N degrees of freedom, the fourth real number set can be used to combine the external force f at the end of the robot with the shaft end τ ext Get in touch, Indicates n ext The fifth set of real numbers with degrees of freedom.

[0048] The external force f at the end of the robot is usually 6 and is expressed in Cartesian space as:

[0049] Among them, f x 、f y and f z is the contact force, τ x , τ y and τ z is the contact torque.

[0050] According to the generalized momentum definition of the end of the robot, the dynamics of its end can be modeled and described. For the end of the robot, its momentum is expressed as:

[0051] Among them, p represents momentum, M represents the mass inertia matrix of the robot, The angular velocity vector of the robot's end point.

[0052] right After differentiation, we can get: Will Substituting into the second kinetic equation we can get: in, represents the differential of momentum, represents the differential of the robot's mass inertia matrix, represents the angular velocity vector of the robot’s end, τ mot represents the force on the end of the robot, C represents the centripetal force and Coriolis force of the robot, G represents the gravity matrix of the robot, τ fric Represents the friction force received by the robot. The contact force at the end of the robot is converted to the shaft end to produce τ ext .

[0053] Among them, according to the dynamics of the robot, it can be deduced that is a skew-symmetric matrix, and M is a symmetric matrix, so we can get: Will Substitute into the formula It can be further simplified to: in, represents the differential of momentum, τ mot represents the force on the end of the robot, C represents the centripetal force and Coriolis force of the robot, G represents the gravity matrix of the robot, τ fric Represents the friction force received by the robot. The contact force at the end of the robot is converted to the shaft end to produce τ ext , this formula is used in the present invention to describe the dynamic relationship of the end of the robot.

[0054] Afterwards Simplifying, we get

[0055] Step 104: transform the first dynamics equation to obtain a target model.

[0056] Specifically, the target model is in, represents the generalized contact force of the robot, I N represents the first set, represents the first zero matrix, represents the system state of the robot, and k represents time.

[0057] Step 106: Determine the system status of the robot.

[0058] Step 108: Determine the generalized contact force of the robot according to the target model and the system state.

[0059] The method for detecting the contact force of a robot provided by the present invention comprises first determining the dynamic equation of the robot end, wherein the dynamic equation is an equation about the friction force and the generalized force during the operation of the robot end, transforming the dynamic equation to obtain a target model, and obtaining the system state of the robot during the operation of the robot, wherein the system state includes the motion parameters of the motor, etc., and determining the contact force of the robot end according to the system state and the target model. Since the dynamic equation of the present invention is about the friction force and the generalized force, the contact force finally determined is associated with the friction force, thereby improving the accuracy of the contact force detection.

[0060] In some embodiments, optionally, the kinetic equation is: The dynamic equations are transformed to obtain the target model, including: arranging the dynamic equations into state space equations: Convert the state space equation to a discrete equation: k+1 =A k x k +B k u k +ω k ;y k =Cx k +υ k ; According to the discrete equation, determine the target model: represents the generalized contact force of the robot, I N represents the first set, represents the first zero matrix, represents the system state of the robot, and k represents time.

[0061] In this embodiment, the kinetic equation is: Among them, ω p Represent the noise of friction in the equation, so as to ensure the accuracy of the dynamic equation, transform the dynamic equation and obtain the target model, including: changing the dynamic equation and organizing the dynamic equation into a state space equation: Then the state space equation is converted into a discrete equation: k+1 =A k x k +B k u k +ω k ;y k =Cx k +υ k ; According to the discrete equation x k+1 =A k x k +B k u k +ω k and k =Cxk +υ k , determine the target model: By transforming the kinematic equations, the noise is reduced and filtered out, thereby improving the accuracy of the detection of the robot's contact force.

[0062] The first kinematic equation is in, represents the derivative of the momentum of the robot's end, represents the first defined quantity, which is determined by the force on the end of the robot, the centripetal force and Coriolis force of the robot, the gravity moment of the robot, and the friction force on the robot. T represents the transpose of the Jacobian matrix of the robot, f represents the external force at the end of the robot, and f is the end force τ of the robot ext Converted to the corresponding quantity at the shaft end, ω p Represents the first noise term. For a more accurate end-to-end dynamic relationship of the robot, ω p The main source of is the uncertainty in the friction model. This first noise term is considered to follow a Gaussian distribution. Represents the fourth Gaussian distribution. For the classical disturbance force observer, the generalized force on the end of the robot is considered to be a constant, and its derivative is also considered to be a noise that obeys the Gaussian distribution: in, represents the derivative of the external force at the end of the robot, ω f represents the second noise term, represents the first Gaussian distribution, f represents the external force at the end of the robot, Q c,f represents the first set of rational numbers.

[0063] Although the generalized force on the end of the robot may not be a constant in reality, we assume it to be a constant to simplify the design in the design of the Kalman filter.

[0064] The two noise terms mentioned above are organized into vector form Where ω represents the total noise term, ω p represents the first noise term, ω f represents the second noise term, Represents N+n ext The first real number set of dimensions, T represents the transpose operation.

[0065] Then the first kinetic equation and Simplifying, we get in, represents the derivative of the robot's system state, 0 N×N represents the second zero matrix, J T represents the transpose of the Jacobian matrix of the robot, IN represents the first set, represents the first zero matrix, represents the third zero matrix, Indicates the system status of the robot, represents the first defined quantity, and ω represents the total noise term.

[0066] In some embodiments, optionally, converting the state-space equation into a discrete equation includes: performing Gaussian distribution on the noise in the state-space equation: Determine the output quantity in the state-space equation: The p meas Defined as y, Defined as C c ; Convert the noise into a discrete equation: x k+1 =A k x k +B k u k +ω k ; Convert the output quantity into a discrete equation: y k =Cx k +υ k .

[0067] In this embodiment, the state space equation is converted into a discrete equation, including: performing Gaussian distribution on the noise in the state space equation: And determine the output quantity in the state space equation: Then p meas Defined as y, Defined as C c ,Will Convert to a discrete equation: k+1 =A k x k +B k u k +ω k ,Will Convert to a discrete equation: k =Cx k +υ k , so that the system state of the robot can be brought in and solved.

[0068] right The parameters in the state space equation are defined. Defined as Will Defined as A c ,Will Defined as x, Defined as B c ,Will Defined as u, where A cis a time-varying matrix, because the robot's terminal Jacobian matrix J(q) is related to the robot's real-time position. And, according to the previous definition of noise, The ω in follows a Gaussian distribution, that is, represents the second Gaussian distribution.

[0069] In the actual system, the robot's real-time position q and real-time axis space speed can be measured, so the corresponding momentum can be calculated. meas Defined as the measured output of the system Among them, I N represents the first set, represents the first zero matrix, represents the system state of the robot, υ represents the measurement error, Represents the third Gaussian distribution: Substitute p in the formula for the measured data meas Defined as y, Defined as C c , and the measurement error υ is also assumed to obey the Gaussian distribution.

[0070] Describing the state space equation as a discrete system yields: k+1 =A k x k +B k u k +ω k , where k represents the first moment, k+1 represents the second moment, A represents the first coefficient, B represents the second coefficient, and ω represents the total noise term. represents the second Gaussian distribution.

[0071] Describing the formula of the measured data as a discrete system yields: k =Cx k +v k , C represents the third coefficient, and υ represents the measurement error.

[0072] Among them, k represents the first moment, that is, this moment, k+1 represents the second moment, that is, the next moment, and the conversion relationship after the discretization of the coefficients is:

[0073] Among them, A represents the first coefficient, B represents the second coefficient, represents the fourth zero matrix, I N represents the first set, exp represents the exponential function, 0 N×N represents the second zero matrix, T S Indicates the sampling period;

[0074] C=CC , Among them, C represents the third coefficient, R represents the fourth coefficient, k represents the first moment, T S represents the sampling period, R C represents the variance of the third Gaussian distribution;

[0075] Where H represents the fifth coefficient, represents the fifth zero matrix, Q C represents the second set of rational numbers;

[0076] Among them, M 11 represents the first mass inertia matrix, M 12 represents the second mass inertia matrix, M 22 represents the third mass inertia matrix, represents the fifth zero matrix, exp represents the exponential function, H represents the fifth coefficient, T S represents the sampling period, k represents the first moment;

[0077] Among them, M 11 represents the first mass inertia matrix, M 12 represents the second mass inertia matrix, k represents the first moment, and Q represents the third rational number set;

[0078] The target model for estimating generalized contact force is obtained.

[0079] In some embodiments, optionally, determining the system state includes: determining the system state through a Kalman filter.

[0080] In this embodiment, determining the system state of the robot includes determining the system state of the robot through a Kalman filter.

[0081] In the above formula, t s represents the sampling period of the system. Based on the discretized formula, X k+1 =A k x k +B k u k +ω k and k =Cx k +v k , a standard Kalman filter can be used to estimate the system state

[0082] The above methods may be implemented in various ways according to specific features and / or example applications. For example, these methods may be implemented by a combination of hardware, firmware, and / or software. For example, in a hardware implementation, the processor may be implemented in one or more application-specific integrated circuits (ASICs), digital signal processors (DSPs), programmable logic devices (PLDs), field programmable gate arrays (FPGAs), controllers, microcontrollers, microprocessors, electronic devices, other equipment units for performing the above functions, and / or combinations thereof.

[0083] like Figure 4 As shown, a six-dimensional torque sensor 404 is installed at the end of the robot 402 for data verification. In the experiment, when the robot 402 is stationary, the external force on the end of the robot is observed by collecting the position, speed and torque signals of the robot 402 shaft end, and the accuracy is verified based on the information collected by the six-dimensional torque sensor 404.

[0084] Figure 5 The test results are shown, wherein the dotted line corresponds to the result measured by the end six-dimensional force sensor, the solid line represents the result determined by the robot contact force detection method provided by the present invention, CCFE X force represents the force in the X direction determined by the present invention, CCFE Y force represents the force in the Y direction determined by the present invention, CCFE Z force represents the force in the Z direction determined by the present invention, sensor Xforce represents the force in the X direction determined by the six-dimensional force sensor, sensor Y force represents the force in the Y direction determined by the six-dimensional force sensor, and sensor Z force represents the force in the Z direction determined by the six-dimensional force sensor.

[0085] By applying external forces in different directions at the same time, it can be seen from the results that the accuracy of the robot contact force detection method provided by the present invention can be controlled at about 5N, and due to the use of the Kalman filter, the result will be smoother than the actual measurement result.

[0086] like Figure 2 As shown, according to the second aspect of the present invention, the present invention provides a robot contact force detection device 200, comprising: a first determination module 202, used to determine the first dynamic equation of the robot: in, represents the derivative of the momentum of the robot's end, represents the first defined quantity, which is determined by the force on the end of the robot, the centripetal force and Coriolis force of the robot, the gravity moment of the robot, and the friction force on the robot. T represents the transpose of the Jacobian matrix of the robot, f represents the external force at the end of the robot, ω p represents the first noise term; the transformation module 204 is used to transform the first dynamic equation to obtain the target model: in, represents the generalized contact force of the robot, i N represents the first set, represents the first zero matrix, represents the system state of the robot, and k represents time; the second determination module 206 is used to determine the system state of the robot; the third determination module 208 is used to determine the generalized contact force of the robot according to the target model and the system state.

[0087] The device for detecting the contact force of a robot provided by the present invention comprises first determining the dynamic equation of the robot end, wherein the dynamic equation is an equation about the friction force and the generalized force during the operation of the robot end, transforming the dynamic equation to obtain a target model, and obtaining the system state of the robot during the operation of the robot, wherein the system state includes the motion parameters of the motor, etc., and determining the contact force of the robot end according to the system state and the target model. Since the dynamic equation of the present invention is about the friction force and the generalized force, the contact force finally determined is associated with the friction force, thereby improving the accuracy of the contact force detection.

[0088] In some embodiments, optionally, the variation module includes: a first definition module, used to define the derivative of f as: ω f represents the second noise term, represents the first Gaussian distribution, f represents the external force at the end of the robot, Q c,f represents the first rational number set; the first sorting submodule is used to convert ω p and ω f Arranged into vector form: Where ω represents the total noise term, ω p represents the first noise term, ω f represents the second noise term, Represents N+n ext The first real number set of the dimension, T represents the transposition operation; the second definition submodule is used to define the system state of the robot as: x represents the system state of the robot, p represents the momentum of the robot's end, and f represents the external force at the robot's end. Represents N+n extThe first real number set of dimensions, T represents the transposition operation; the second sorting submodule is used to sort the first dynamic equation into a state space equation according to the total noise term and the system state: in, represents the derivative of the robot's system state, 0 N×N represents the second zero matrix, J T represents the transpose of the Jacobian matrix of the robot, I N represents the first set, represents the first zero matrix, represents the third zero matrix, Indicates the system status of the robot, represents the first defined quantity, ω represents the total noise term; the conversion submodule is used to discretize the state space equation to obtain the target model.

[0089] In this embodiment, the kinetic equation is: Among them, ω p Represent the noise of friction in the equation, so as to ensure the accuracy of the dynamic equation, transform the dynamic equation and obtain the target model, including: changing the dynamic equation and organizing the dynamic equation into a state space equation: Then the state space equation is converted into a discrete equation: k+1 =A k x k +B k u k +ω k ;y k =Cx k +υ k ; According to the discrete equation x k+1 =A k x k +B k u k +ω k and k =Cx k +u k , determine the target model: By transforming the kinematic equations, the noise is reduced and filtered out, thereby improving the accuracy of the detection of the robot's contact force.

[0090] The first kinematic equation is in, represents the derivative of the momentum of the robot's end, represents the first defined quantity, which is determined by the force on the end of the robot, the centripetal force and Coriolis force of the robot, the gravity moment of the robot, and the friction force on the robot. Trepresents the transpose of the Jacobian matrix of the robot, f represents the external force at the end of the robot, and f is the end force τ of the robot ext Converted to the corresponding quantity at the shaft end, ω p Represents the first noise term. For a more accurate end-to-end dynamic relationship of the robot, ω p The main source of is the uncertainty in the friction model. This first noise term is considered to follow a Gaussian distribution. Represents the fourth Gaussian distribution. For the classical disturbance force observer, the generalized force on the end of the robot is considered to be a constant, and its derivative is also considered to be a noise that obeys the Gaussian distribution: in, represents the derivative of the external force at the end of the robot, ω f represents the second noise term, represents the first Gaussian distribution, f represents the external force at the end of the robot, Q c,f represents the first set of rational numbers.

[0091] Although the generalized force on the end of the robot may not be a constant in reality, we assume it to be a constant to simplify the design in the design of the Kalman filter.

[0092] The two noise terms mentioned above are organized into vector form Where ω represents the total noise term, ω p represents the first noise term, ω f represents the second noise term, Represents N+n ext The first set of real numbers in dimension.

[0093] Then the first kinetic equation and Simplifying, we get in, represents the derivative of the robot's system state, 0 N×N represents the second zero matrix, J T represents the transpose of the robot's Jacobian matrix, represents the first zero matrix, represents the third zero matrix, Indicates the system status of the robot, represents the first defined quantity, and ω represents the total noise term.

[0094] In some embodiments, the conversion submodule may include: a first definition unit for defining the measurement data of the robot as p meas , the formula for determining the measurement data is: Among them, I N represents the first set, represents the first zero matrix, represents the system state of the robot, υ represents the measurement error, represents the third Gaussian distribution; the second definition unit is used to transform the state space equation Defined as Will Defined as A c ,Will Defined as x, Defined as B c ,Will Defined as u, replace p in the formula of measured data meas Defined as y, Defined as C c ; The first conversion unit is used to describe the state space equation as a discrete system to obtain: x k+1 =A k x k +B k u k +ω k , where k represents the first moment, k+1 represents the second moment, A represents the first coefficient, B represents the second coefficient, and ω represents the total noise term. represents the second Gaussian distribution; the second conversion unit is used to describe the formula of the measured data as a discrete system to obtain: y k =Cx k +υ k , C represents the third coefficient, υ represents the measurement error; the third conversion unit is used to convert the relationship between the discretized state space equation and the formula of the measurement data: Among them, A represents the first coefficient, B represents the second coefficient, represents the fourth zero matrix, I N represents the first set, exp represents the exponential function, 0 N×N represents the second zero matrix, T S Indicates the sampling period; C = C C , Among them, C represents the third coefficient, R represents the fourth coefficient, k represents the first moment, T S represents the sampling period, R C represents the variance of the third Gaussian distribution; Where H represents the fifth coefficient, represents the fifth zero matrix, Q C represents the second set of rational numbers; Among them, M 11 represents the first mass inertia matrix, M 12 represents the second mass inertia matrix, M 22 represents the third mass inertia matrix, represents the fifth zero matrix, exp represents the exponential function, H represents the fifth coefficient, TS represents the sampling period, k represents the first moment; Among them, M 11 represents the first mass inertia matrix, M 12 represents the second mass inertia matrix, k represents the first moment, and Q represents the third rational number set; a sorting unit is used to sort out the target model.

[0095] In this embodiment, the state space equation is converted into a discrete equation, including: performing Gaussian distribution on the noise in the state space equation: And determine the output quantity in the state space equation: Then p meas Defined as y, Defined as C c ,Will Convert to a discrete equation: k+1 =A k x k +B k u k +ω k ,Will Convert to a discrete equation: k =Cx k +v k , so that the system state of the robot can be brought in and solved.

[0096] right The parameters in the state space equation are defined. Defined as Will Defined as A c ,Will Defined as x, Defined as B c ,Will Defined as u, where A c is a time-varying matrix, because the robot's terminal Jacobian matrix J(q) is related to the robot's real-time position. And, according to the previous definition of noise, The ω in follows a Gaussian distribution, that is, represents the second Gaussian distribution.

[0097] In the actual system, the robot's real-time position q and real-time axis space speed can be measured, so the corresponding momentum can be calculated. meas Defined as the measured output of the system Among them, I N represents the first set, represents the first zero matrix, represents the system state of the robot, υ represents the measurement error, Represents the third Gaussian distribution: Substitute p in the formula for the measured data meas Defined as y, Defined as C c , and the measurement error υ is also assumed to obey the Gaussian distribution.

[0098] Describing the state space equation as a discrete system yields: k+1 =A k x k +B k u k +ω k , where k represents the first moment, k+1 represents the second moment, A represents the first coefficient, B represents the second coefficient, and ω represents the total noise term. represents the second Gaussian distribution.

[0099] Describing the formula of the measured data as a discrete system yields: k =Cx k +υ k , C represents the third coefficient, and υ represents the measurement error.

[0100] Among them, k represents the first moment, that is, this moment, k+1 represents the second moment, that is, the next moment, and the conversion relationship after the discretization of the coefficients is:

[0101] Among them, A represents the first coefficient, B represents the second coefficient, represents the fourth zero matrix, I N represents the first set, exp represents the exponential function, 0 N×N represents the second zero matrix, T S Indicates the sampling period;

[0102] C=C C , Among them, C represents the third coefficient, R represents the fourth coefficient, k represents the first moment, T S represents the sampling period, R C represents the variance of the third Gaussian distribution;

[0103] Where H represents the fifth coefficient, represents the fifth zero matrix, Q C represents the second set of rational numbers;

[0104] Among them, M 11 represents the first mass inertia matrix, M 12 represents the second mass inertia matrix, M 22represents the third mass inertia matrix, represents the fifth zero matrix, exp represents the exponential function, H represents the fifth coefficient, T S represents the sampling period, k represents the first moment;

[0105] Among them, M 11 represents the first mass inertia matrix, M 12 represents the second mass inertia matrix, k represents the first moment, and Q represents the third rational number set;

[0106] The target model is obtained.

[0107] In some embodiments, optionally, the second determination module includes: a determination submodule, configured to determine the system state of the robot through a Kalman filter.

[0108] In this embodiment, determining the system state of the robot includes determining the system state of the robot through a Kalman filter.

[0109] T in the above formula s represents the sampling period of the system. Based on the discretized formula, x k+1 =A k x k +B k u k +ω k and k =Cx k +υ k , a standard Kalman filter can be used to estimate the system state

[0110] like Figure 3 As shown, according to the third aspect of the present invention, the present invention proposes an electronic device 300, including a processor 302 and a memory 304, the memory 304 stores programs or instructions that can be run on the processor 302, and when the program or instructions are executed by the processor 302, the steps of the robot contact force detection method provided in the first aspect embodiment are implemented.

[0111] The electronic device proposed in the present invention includes a memory that stores a program or instruction that implements the steps of the robot contact force detection method provided in the first aspect embodiment when executed by a processor. Therefore, it has all the beneficial effects of the robot contact force detection method provided in the first aspect embodiment, which are no longer stated one by one here.

[0112] According to a fourth aspect of the present invention, the present invention provides a storage medium storing a program or instruction, which, when executed by a processor, implements the steps of the robot contact force detection method provided in the first aspect embodiment.

[0113] The storage medium provided by the present invention stores a program or instruction that, when executed by a processor, implements the steps of the robot contact force detection method provided in the first aspect embodiment. Therefore, it has all the beneficial effects of the robot contact force detection method provided in the first aspect embodiment, which are no longer stated one by one here.

[0114] A storage medium may be a tangible device that can retain and store instructions for use by an instruction execution device. A computer storage medium may be an electronic storage device, a magnetic storage device, an optical storage device, an electromagnetic storage device, a semiconductor storage device, or any suitable combination of the above devices, but is not limited thereto. A non-exhaustive list of more specific examples of computer storage media includes: portable computer floppy disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), static random access memory (SRAM), portable compact disc read-only memory (CD-ROM), digital universal disk (DVD), memory card, floppy disk, encoding mechanical device (such as a punch card or a groove with a raised structure with instructions recorded) and any suitable combination of the above devices. The computer storage medium used herein should not be understood as a transmission signal itself, such as radio waves or other freely propagating electromagnetic waves, electromagnetic waves propagating through waveguides or other transmission media, or electrical signals transmitted through wires.

[0115] In the present invention, the terms "first", "second", and "third" are used for descriptive purposes only and should not be understood as indicating or implying relative importance; the term "plurality" refers to two or more, unless otherwise clearly defined. The terms "installed", "connected", "connected", "fixed", etc. should be understood in a broad sense. For example, "connected" can be a fixed connection, a detachable connection, or an integral connection; "connected" can be a direct connection or an indirect connection through an intermediate medium. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0116] In the description of the present invention, it is necessary to understand that the directions or positional relationships indicated by the terms "up", "down", "left", "right", "front", "back", etc. are based on the directions or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the components or units referred to must have a specific direction, be constructed and operated in a specific orientation, and therefore, should not be understood as limiting the present invention.

[0117] In the description of this specification, the description of the terms "one embodiment", "some embodiments", "specific embodiments", etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner.

[0118] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A method for detecting contact force of a robot, characterized in that: include: Determine the first dynamic equation of the robot: in, represents the derivative of the momentum of the end of the robot, represents the first defined quantity, which is determined by the force on the end of the robot, the centripetal force and Coriolis force of the robot, the gravity moment of the robot and the friction force on the robot, J T represents the transpose of the Jacobian matrix of the robot, f represents the external force at the end of the robot, ω p represents the first noise term; The first kinetic equation is transformed to obtain the target model: in, represents the generalized contact force of the robot, I n represents the first set, represents the first zero matrix, represents the system state of the robot, and k represents time; determining the system state of the robot; The generalized contact force of the robot is determined according to the target model and the system state.

2. The method for detecting the contact force of a robot according to claim 1, characterized in that: The first kinetic equation is transformed to obtain a target model, including: Define the derivative of f as: ω f represents the second noise term, represents the first Gaussian distribution, f represents the external force at the end of the robot, Q c,f represents the first set of rational numbers; ω p and ω f Arranged into vector form: Where ω represents the total noise term, ω p represents the first noise term, ω f represents the second noise term, Represents N+n ext The first real number set of dimensions, T represents the transpose operation; The system state of the robot is defined as: x = [p T ,f T ] T , x represents the system state of the robot, p represents the momentum of the end of the robot, f represents the external force at the end of the robot, Represents N+n ext The first real number set of dimensions, T represents a transposition operation; According to the total noise term and the system state, the first dynamic equation is organized into a state space equation: in, represents the derivative of the system state of the robot, 0 N×N represents the second zero matrix, J T represents the transpose of the Jacobian matrix of the robot, I N represents the first set, represents the first zero matrix, represents the third zero matrix, represents the system state of the robot, represents the first defined quantity, ω represents the total noise term; The state space equation is discretely transformed to obtain the target model.

3. The method for detecting the contact force of a robot according to claim 2, characterized in that: The state space equation is discretized to obtain the target model, including: The measurement data of the robot is defined as p meas , the formula for determining the measurement data is: Among them, I N represents the first set, represents the first zero matrix, represents the system state of the robot, v represents the measurement error, represents the third Gaussian distribution; The state space equation Defined as Will Defined as A c ,Will Defined as x, Defined as B c ,Will Defined as u, replace p in the formula of the measurement data meas Defined as y, Defined as C c ; Describing the state space equation as a discrete system yields: k+1 =A k x k +B k u k +ω k , where k represents the first moment, k+1 represents the second moment, A represents the first coefficient, B represents the second coefficient, and ω represents the total noise term. represents the second Gaussian distribution; Describing the formula of the measured data as a discrete system yields: k =Cx k +υ k , C represents the third coefficient, υ represents the measurement error; The relationship between the discrete state space equation and the measurement data formula is transformed: Wherein, A represents the first coefficient, B represents the second coefficient, represents the fourth zero matrix, I N represents the first set, exp represents the exponential function, 0 N×N Denotes the second zero matrix, T S Indicates the sampling period; C=C C , Wherein, C represents the third coefficient, R represents the fourth coefficient, k represents the first moment, T S represents the sampling period, R C represents the variance of the third Gaussian distribution; Among them, H represents the fifth coefficient, A c express represents the fifth zero matrix, Q C represents the second set of rational numbers, Indicates A C The transpose of Among them, M 11 represents the first mass inertia matrix, M 12 represents the second mass inertia matrix, M 22 represents the third mass inertia matrix, represents the fifth zero matrix, exp represents the exponential function, H represents the fifth coefficient, Ts S represents the sampling period, k represents the first moment; Among them, M 11 represents the first mass inertia matrix, M 12 represents the second mass inertia matrix, k represents the first moment, and Q represents a third rational number set; The target model is obtained by sorting.

4. The method for detecting the contact force of a robot according to any one of claims 1 to 3, characterized in that: Determining the system state of the robot includes: The system state of the robot is determined by a Kalman filter.

5. A robot contact force detection device, characterized in that: include: The first determination module is used to determine the first dynamic equation of the robot: in, represents the derivative of the momentum of the end of the robot, represents the first defined quantity, which is determined by the force on the end of the robot, the centripetal force and Coriolis force of the robot, the gravity moment of the robot and the friction force on the robot, J T represents the transpose of the Jacobian matrix of the robot, f represents the external force at the end of the robot, ω p represents the first noise term; The transformation module is used to transform the first kinetic equation to obtain a target model: in, represents the generalized contact force of the robot, I N represents the first set, represents the first zero matrix, represents the system state of the robot, and k represents time; A second determination module, used to determine the system state of the robot; The third determination module is used to determine the generalized contact force of the robot according to the target model and the system state.

6. The robot contact force detection device according to claim 5, characterized in that: The change module includes: The first definition module is used to define the derivative of f as: ω f represents the second noise term, represents the first Gaussian distribution, f represents the external force at the end of the robot, Q c,f represents the first set of rational numbers; The first sorting submodule is used to convert ω p and ω f Arranged into vector form: Where ω represents the total noise term, ω p represents the first noise term, ω f represents the second noise term, Represents N+n ext The first real number set of dimensions, T represents the transpose operation; The second definition submodule is used to define the system state of the robot as: x = [p T ,f T ] T , x represents the system state of the robot, p represents the momentum of the end of the robot, f represents the external force at the end of the robot, Represents N+n ext The first real number set of dimensions, T represents a transposition operation; The second sorting submodule is used to sort the first dynamic equation into a state space equation according to the total noise term and the system state: in, represents the derivative of the system state of the robot, 0 N×N represents the second zero matrix, J T represents the transpose of the Jacobian matrix of the robot, I N represents the first set, represents the first zero matrix, represents the third zero matrix, represents the system state of the robot, represents the first defined quantity, ω represents the total noise term; The conversion submodule is used to perform discrete conversion on the state space equation to obtain the target model.

7. The robot contact force detection device according to claim 6, characterized in that: The conversion submodule comprises: The first definition unit is used to define the measurement data of the robot as p meas , the formula for determining the measurement data is: Among them, I N represents the first set, represents the first zero matrix, represents the system state of the robot, v represents the measurement error, represents the third Gaussian distribution; The second definition unit is used to transform the state space equation Defined as Will Defined as A c ,Will Defined as x, Defined as B c ,Will Defined as u, replace p in the formula of the measurement data meas Defined as y, Defined as C c ; The first conversion unit is used to describe the state space equation as a discrete system to obtain: k+1 =A k x k +B k u k +ω k , where k represents the first moment, k+1 represents the second moment, A represents the first coefficient, B represents the second coefficient, and ω represents the total noise term. represents the second Gaussian distribution; The second conversion unit is used to describe the formula of the measurement data as a discrete system to obtain: k =Cx k +υ k , C represents the third coefficient, υ represents the measurement error; The third conversion unit is used to perform relationship conversion between the discretized state space equation and the formula of the measurement data: Wherein, A represents the first coefficient, B represents the second coefficient, represents the fourth zero matrix, I N represents the first set, exp represents the exponential function, 0 N×N Denotes the second zero matrix, T S Indicates the sampling period; C=C C , Wherein, C represents the third coefficient, R represents the fourth coefficient, k represents the first moment, T S represents the sampling period, R C represents the variance of the third Gaussian distribution; Among them, H represents the fifth coefficient, A c express represents the fifth zero matrix, Q C represents the second set of rational numbers, Indicates A C The transpose of Indicates A C The transpose of Among them, M 11 represents the first mass inertia matrix, M 12 represents the second mass inertia matrix, M 22 represents the third mass inertia matrix, represents the fifth zero matrix, exp represents the exponential function, H represents the fifth coefficient, T S represents the sampling period, k represents the first moment; Among them, M 11 represents the first mass inertia matrix, M 12 represents the second mass inertia matrix, k represents the first moment, and Q represents a third rational number set; A sorting unit is used to sort and obtain the target model.

8. The robot contact force detection device according to any one of claims 5 to 7, characterized in that: The second determining module comprises: The determination submodule is used to determine the system state of the robot through a Kalman filter.

9. An electronic device, characterized in that: The method comprises a processor and a memory, wherein the memory stores a program or instruction that can be run on the processor, and when the program or instruction is executed by the processor, the steps of the method for detecting the contact force of a robot according to any one of claims 1 to 4 are implemented.

10. A storage medium, characterized in that: The storage medium stores a program or an instruction, and when the program or the instruction is executed by the processor, the steps of the method for detecting the contact force of a robot according to any one of claims 1 to 4 are implemented.

Images