Joint torque estimation method for articulated robot based on discrete nonsmooth observer

Abstract

The application discloses a joint torque estimation method for a joint robot based on a discrete non-smooth observer, according to a dynamics model of the joint robot, expands a term containing a joint torque of the robot into three variables of a system, and designs a discrete non-smooth observer algorithm based on a discrete non-smooth control theory, so as to simultaneously estimate the joint torque and an angular velocity value of the robot. When estimating the joint torque and the angular velocity of the robot, the discrete non-smooth observer algorithm avoids noise influence of a traditional difference method, the observer only takes the joint position of the robot as an input signal when being designed, so that information demand for an actual system is reduced, the robot torque estimation method improves motion control precision and stability of the robot, and reduces system cost.

Description

Technical Field

[0001] This invention relates to the field of robot force estimation technology, and in particular to a method for estimating joint torque of articulated robots based on a discrete non-smooth observer. Background Technology

[0002] With the development of industrial automation technology, industrial robots are widely used in various industrial settings. However, in some special applications, robots need to interact with their surroundings and manipulated objects. In such cases, the interaction forces between the robot and the external environment must be considered. Traditional position control methods are not suitable, and force control is often required. The torque values ​​of each joint of the robot are crucial information in force control. Force sensors can obtain the robot's joint torques most directly and accurately. However, force sensors are expensive. Therefore, developing and designing a simple and high-performance torque observer is of great significance.

[0003] Many scholars have used state observers for estimation, such as adaptive observers, extended state observers, and nonsmooth observers. Among them, nonsmooth observers have received widespread attention due to their good robustness, fast estimation speed, and high accuracy. Many scholars have used nonsmooth observers to estimate the joint torques of industrial robots and achieved good results. However, the input signal of this observer includes robot joint angle signals and angular velocity signals. Since the robot joint angular velocity is obtained through joint position differential filtering, this increases the computational load of the industrial robot control system.

[0004] There is currently limited research on torque observers that only use robot joint positions as input signals. The known torque observers that only use robot joint positions as input signals have low estimation accuracy and slow calculation speed. Summary of the Invention

[0005] To overcome the shortcomings of existing torque observers in meeting the accuracy requirements of robot motion, this invention provides a joint torque estimation method for articulated robots based on discrete non-smooth observers. This method solves the problem of estimating robot joint torque values ​​in the absence of external torque sensors, thereby improving the motion control accuracy and stability of the robot.

[0006] This invention proposes a method for estimating joint torque in articulated robots based on a discrete nonsmooth observer. First, the robot's dynamic model is reconstructed, expanding the term containing robot joint torque into three system variables: θ1, θ2, and θ3, where θ1 = θ... θ3=M -1 (θ1)τ;θ∈R n×1 ,τ∈R n×1 Where n is the number of robot joints; θ is the robot joint position vector. M(θ) is the derivative of θ, and τ is the joint torque of the robot; M(θ)∈R n×n M(θ) is the robot's inertia matrix. -1 (θ) is the inverse matrix of M(θ);

[0007] Set up a torque observer, and then calculate the estimated robot joint angular velocities at a specified time based on the reconstructed robot dynamics model and the torque observer. and joint torque estimation value

[0008] The torque observer is designed as a discrete non-smooth torque observer, and the formula is expressed as follows:

[0009]

[0010]

[0011]

[0012] Where k1, k2, k3, k4, k5, and k6 all represent positive gains. Let θ1, θ2, θ3 be the estimated values, T be the sampling period of the discrete nonsmooth observer, and t be the sampling period of the observer. k Let t represent time k. k+1 Let θ1(t) represent the time at time k+1. k ) represents the sampled value of θ1 at time k. Let θ1, θ2, and θ3 represent the estimated values ​​at time k, respectively. Let θ1, θ2, and θ3 represent the estimated values ​​at time k+1, respectively; sig and sgn are both functions; f c ∈R n×n The coefficient of friction of the robot is the Coulomb friction coefficient.

[0013] for The inverse matrix, Values ​​for robot joint position vectors The robot's inertia matrix at that time; For calculation items;

[0014]

[0015] Among them, M -1 (θ1) denotes the inverse matrix of M(θ1), where M(θ1) is the robot's inertia matrix when the robot's joint position vector takes the value θ1; C(θ1,θ2)∈R n×n Let G(θ1) ∈ R be the centripetal force and Coriolis force vector of the robot. n×1 f is the robot's gravity vector; v ∈R n×nf is the coefficient of viscous friction of the robot. b ∈R n×1 This represents the robot's friction offset value.

[0016] The preferred reconstructed robot dynamics model is:

[0017]

[0018]

[0019] Γ(θ1,θ2)=M -1 (θ1)(C(θ1,θ2)θ2+G(θ1)+f v ·θ2+f b )

[0020] in, The derivative of θ1, Let Γ(θ1,θ2) be the derivative of θ2, and M be the computational term. -1 (θ1) is the inverse matrix of M(θ1); C(θ1,θ2)∈R n×n Let G(θ1) ∈ R be the centripetal force and Coriolis force vector of the robot. n×1 f is the robot's gravity vector; v ∈R n ×n f is the coefficient of viscous friction of the robot. b ∈R n×1 This represents the robot's friction offset value.

[0021] Preferably, the estimated angular velocity of the robot joints at a specified time. The calculation formula is:

[0022]

[0023] This represents the estimated angular velocity of the robot joints at time k. This represents the estimated value of θ2 at time k.

[0024] Preferably, the estimated value of robot joint torque at a specified time. The calculation formula is:

[0025]

[0026] This represents the estimated value of the robot joint torque τ at time k. Indicates the value of the robot joint position vector The robot's inertia matrix at that time, This represents the estimated value of θ1 at time k. This represents the estimated value of θ3 at time k.

[0027] The present invention proposes a system for implementing the joint torque estimation method for articulated robots based on discrete nonsmooth observers. The system is characterized by comprising: a robot dynamics model, a torque observer, a sampling module, and a processor.

[0028] The sampling module is used to acquire the robot joint position vector θ. The processor is connected to the sampling module. The processor substitutes the θ acquired by the sampling module into the robot dynamics model and solves it in conjunction with the torque observer to calculate the estimated value of the robot joint angular velocity at a specified time. and joint torque estimation value

[0029] The present invention proposes a joint torque estimation system for articulated robots based on discrete nonsmooth observers, including a memory containing a computer program. When the computer program is executed, it is used to implement the joint torque estimation method for articulated robots based on discrete nonsmooth observers.

[0030] Preferably, the system further includes a processor connected to a memory, which executes the computer program to implement the articulated robot joint torque estimation method based on a discrete nonsmooth observer.

[0031] The advantages of this invention are:

[0032] The joint torque estimation method for articulated robots based on discrete nonsmooth observers of the present invention can estimate the joint torque and angular velocity values ​​of robots through the joint position, reducing the information requirements of the actual system, avoiding the noise caused by the differential method, and improving the motion control accuracy and stability of the robot.

[0033] Compared to traditional discrete sliding mode observers, the discrete non-smooth torque observer used in this invention adds a linear term, thereby accelerating the convergence speed of the system and improving the observer estimation speed. Attached Figure Description

[0034] Figure 1 This is a flowchart of the joint torque estimation method for articulated robots based on discrete nonsmooth observers proposed in this invention.

[0035] Figure 2(a) is a schematic diagram of the torque estimation results of joint 1 in the embodiment;

[0036] Figure 2(b) is a schematic diagram of the torque estimation results for joint 2 in the embodiment;

[0037] Figure 3 This is a logical structure diagram of the present invention; wherein, θ dThis refers to the angle in the robot's path control instructions. This represents the angular velocity in the robot's path control commands. Detailed Implementation

[0038] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0039] Depend on Figure 1 As shown, the specific implementation steps of the articulated robot joint torque estimation method based on discrete non-smooth observer of the present invention are as follows:

[0040] S1, Reconstruct the dynamic model of the n-degree-of-freedom articulated robot;

[0041] An n-degree-of-freedom articulated robot has n joints, and its dynamic model is as follows:

[0042]

[0043]

[0044] Where, θ∈R n×1 For robot joint position vectors, for The derivative, M(θ)∈R is the derivative of θ; n×n The robot's inertia matrix; Let G(θ) ∈ R be the centripetal force and Coriolis force vector of the robot. n×1 Let τ be the robot's gravity vector, τ∈R n×1 For robot joint torque; f is the joint friction vector of the robot; c ∈R n×n Let f be the Coulomb friction coefficient of the robot. v ∈R n×n f is the coefficient of viscous friction of the robot. b ∈R n×1 This represents the robot's friction offset value.

[0045] sgn is a function. Where x is an algebra;

[0046] Right now:

[0047] It is the derivative of θ.

[0048] When reconstructing the robot's dynamics model, three system variables θ1, θ2, and θ3 are first set, with θ1 = θ. θ3=M -1 (θ1)τ; Substituting the three system variables into the robot dynamics model, the reconstructed robot dynamics model is obtained as follows:

[0049]

[0050]

[0051] Γ(θ1,θ2)=M -1 (θ1)(C(θ1,θ2)+G(θ1)+f v ·θ2+f b )

[0052] in, The derivative of θ1, Γ(θ1,θ2) represents the derivative of θ2; Γ(θ1,θ2) represents the computational term, M -1 (θ1) is the inverse matrix of M(θ1); f c ∈R n×n f c Let f be the Coulomb friction coefficient of the robot, C(θ1,θ2) be the centripetal force and Coriolis force vectors of the robot, and G(θ1) be the gravity vector of the robot; v f is the coefficient of viscous friction of the robot. b This represents the robot's friction offset value.

[0053] M(θ1) represents the robot inertia matrix when the robot joint position vector is θ1, that is, M(θ1) is the value of M(θ) when θ=θ1;

[0054]

[0055] S2, Design a discrete non-smooth torque observer, expressed by the following formula:

[0056]

[0057]

[0058]

[0059] Where k1, k2, k3, k4, k5, and k6 all represent positive gains. Let θ1, θ2, θ3 be the estimated values, T be the sampling period of the discrete nonsmooth observer, and t be the sampling period of the observer. k Let t represent time k. k+1 Let θ1(t) represent the time at time k+1. k) represents the sampled value of θ1 at time k. Let θ1, θ2, θ3 represent the estimated values ​​at time k. Let θ1, θ2, θ3 represent the estimated values ​​of θ1, θ2, θ3 at time k+1.

[0060] sig is a function;

[0061] sig m (x)=|x| m sgn(x); 0 <m≤1;

[0062] Where m is a constant and x is an algebra;

[0063] For m = 2 / 3 and sig of time m (x);

[0064] For m = 1 / 3 and sig of time m (x).

[0065] for sgn(x) at time;

[0066] sgn(θ1(t k )) is x=θ1(t k sgn(x) at time ).

[0067] The discrete non-smooth torque observer used in this embodiment adds a linear term, which speeds up the convergence speed of the system and improves the observer estimation speed.

[0068] S3 estimates the robot's joint angular velocity and joint torque;

[0069]

[0070]

[0071] This represents the estimated angular velocity of the robot joints at time k. This represents the estimated value of the robot joint torque τ at time k.

[0072] The following simulation verification of the joint torque estimation method for articulated robots based on discrete non-smooth observers is carried out with specific embodiments. The experimental robot is a 2-joint robot.

[0073] In this embodiment, the robot's motion is driven by the robot controller. The principle of predicting the robot's joint torque and joint angular velocity using the articulated robot joint torque estimation method based on a discrete non-smooth observer provided by this invention is as follows: Figure 3 As shown.

[0074] In this embodiment, the joint torque estimation method for articulated robots based on discrete non-smooth observers provided by the present invention (hereinafter referred to as the present invention method) and the existing linear method are used to predict the torque changes of each joint of the robot, and the joint torque of the robot is continuously sampled within 10 seconds as a reference value.

[0075] In existing linear methods, the observer is set up as follows:

[0076]

[0077]

[0078]

[0079] Finally, the predicted robot joint torque change curves obtained by the method of this invention and the linear method are shown in Figure 2(a) and Figure 2(b). It can be seen that the prediction results of both the method of this invention and the linear method are not ideal in the first 1 second. However, as time progresses, both the method of this invention and the linear method can fit the reference curve, and the torque change predicted by the method of this invention is basically consistent with the reference curve. Therefore, the method of this invention has higher accuracy. Furthermore, on joint 1, the method of this invention easily overlaps with the reference curve at around 0.8 seconds, while the linear method only stabilizes at around 1.8 seconds. On joint 1, the method of this invention easily overlaps with the reference curve at around 0.9 seconds, while the linear method only stabilizes at around 1.4 seconds. This shows that the method of this invention can adapt to the changing patterns of the robot joints more quickly.

[0080] Of course, those skilled in the art will recognize that the present invention is not limited to the details of the exemplary embodiments described above, but also includes the same or similar structures that can be implemented in other specific forms without departing from the spirit or essential characteristics of the invention. Therefore, the embodiments should be considered illustrative and non-limiting in all respects, and the scope of the invention is defined by the appended claims rather than the foregoing description. Thus, all variations falling within the meaning and scope of equivalents of the claims are intended to be included within the present invention. No reference numerals in the claims should be construed as limiting the scope of the claims.

[0081] Furthermore, it should be understood that although this specification describes embodiments, not every embodiment contains only one independent technical solution. This narrative style is merely for clarity. Those skilled in the art should consider the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

[0082] The technologies, shapes, and structures not described in detail in this invention are all known technologies.

Claims

1. A method for estimating joint torque of an articulated robot based on a discrete nonsmooth observer. First, the robot's dynamic model is reconstructed, expanding the term containing robot joint torque into three system variables: θ1, θ2, and θ3, where θ1 = θ. θ3=M -1 (θ1)τ;θ∈R n×1 ,τ∈R n×1 Where n is the number of robot joints; θ is the robot joint position vector. M(θ) is the derivative of θ, and τ is the joint torque of the robot; M(θ)∈R n×n M(θ) is the robot's inertia matrix. -1 (θ) is the inverse matrix of M(θ); Set up a torque observer, and then calculate the estimated robot joint angular velocities at a specified time based on the reconstructed robot dynamics model and the torque observer. and joint torque estimation value Its features are, The torque observer is designed as a discrete non-smooth torque observer, and the formula is expressed as follows: Where k1, k2, k3, k4, k5, and k6 all represent positive gains. Let θ1, θ2, and θ3 be the estimated values, and T be the sampling period of the discrete nonsmooth observer, t. k Let t represent time k. k+1 Let θ1(t) represent the time at time k+1. k ) represents the sampled value of θ1 at time k. Let θ1, θ2, and θ3 represent the estimated values ​​at time k, respectively. Let θ1, θ2, and θ3 represent the estimated values ​​at time k+1, respectively; sig and sgn are both functions; f c ∈R n×n The coefficient of friction of the robot is the Coulomb friction coefficient. for The inverse matrix, Values ​​for robot joint position vectors The robot's inertia matrix at that time; For calculation items; Among them, M -1 (θ1) denotes the inverse matrix of M(θ1), where M(θ1) is the robot's inertia matrix when the robot's joint position vector takes the value θ1; C(θ1, θ2) ∈ R n×n Let G(θ1) ∈ R be the centripetal force and Coriolis force vector of the robot. n×1 f is the robot's gravity vector; v ∈R n×n f is the coefficient of viscous friction of the robot. b ∈R n×1 This represents the robot's friction offset value.

2. The method for estimating joint torque of an articulated robot based on a discrete non-smooth observer as described in claim 1, characterized in that, The reconstructed robot dynamics model is as follows: C(θ1,θ2)=M -1 (θ1)(C(θ1,θ2)θ2+G(θ1)+f v ·θ2+f b ) in, The derivative of θ1, Let Γ(θ1, θ2) be the derivative of θ2, and let M be the computational term. -1 (θ1) is the inverse matrix of M(θ1); C(θ1, θ2) ∈ R n×n Let G(θ1) ∈ R be the centripetal force and Coriolis force vector of the robot. n×1 f is the robot's gravity vector; v ∈R n×n f is the coefficient of viscous friction of the robot. b ∈R n×1 This represents the robot's friction offset value.

3. The method for estimating joint torque of an articulated robot based on a discrete non-smooth observer as described in claim 1, characterized in that, Estimated angular velocity of robot joints at a specified time The calculation formula is: This represents the estimated angular velocity of the robot joints at time k. This represents the estimated value of θ2 at time k.

4. The method for estimating joint torque of an articulated robot based on a discrete non-smooth observer as described in claim 1, characterized in that, Estimated value of robot joint torque at a specified time The calculation formula is: This represents the estimated value of the robot joint torque τ at time k. Represents the value of the robot joint position vector The robot's inertia matrix at that time, This represents the estimated value of θ1 at time k. This represents the estimated value of θ3 at time k.

5. A system for implementing the joint torque estimation method for articulated robots based on discrete nonsmooth observers as described in any one of claims 1-4, characterized in that, The system includes: a robot dynamics model, a torque observer, a sampling module, and a processor; The sampling module is used to acquire the robot joint position vector θ. The processor is connected to the sampling module. The processor substitutes the θ acquired by the sampling module into the robot dynamics model and solves it in conjunction with the torque observer to calculate the estimated value of the robot joint angular velocity at a specified time. and joint torque estimation value 6. A joint torque estimation system for an articulated robot based on a discrete nonsmooth observer, characterized in that, It includes a memory storing a computer program, which, when executed, implements the joint torque estimation method for articulated robots based on a discrete nonsmooth observer as described in any one of claims 1-4.

7. The articulated robot joint torque estimation system based on a discrete non-smooth observer as described in claim 6, characterized in that, It also includes a processor connected to a memory, the processor being used to execute the computer program to implement the joint torque estimation method for articulated robots based on a discrete nonsmooth observer as described in any one of claims 1-4.

Images