Robot adaptive contact force estimation method based on K-B viscoelastic model

Abstract

The invention relates to the technical field of robot control, in particular to a robot adaptive contact force estimation method based on a K-B viscoelastic model. Comprising the following steps: S1, constructing a robot and an environment model; s2, selecting a K-B viscoelastic model for estimating the contact force between the robot end tool and the environment; s3, constructing a contact force adaptive estimation scheme, and taking an obtained control and estimation rule as a control target; s4, obtaining a control rate and an adaptive rate for realizing a control target through the progressive convergence proof of the tracking error and the stability proof of the closed-loop system; and S5, obtaining a stable robot and environment model based on the control rate and the adaptive rate obtained in the step S4, and estimating the contact force between the robot and the environment. According to the method, the contact force between the robot and the environment can be accurately estimated, the global stability of the system is ensured, the asymptotic convergence of position and speed tracking errors is realized, and the bounded force estimation error is ensured.

Description

Technical Field

[0001] The invention relates to the technical field of robot control, and more particularly to a robot adaptive contact force estimation method based on a KB viscoelastic model. Background Art

[0002] Robot contact force estimation is a key technology for achieving safe interaction between robots and the environment. Traditional methods usually rely on force or torque sensors installed on the joints or ends of the robot to directly measure the contact force. However, the method of using force or torque sensors installed on the joints or ends of the robot to directly measure the contact force has problems such as high hardware cost, increased system complexity and insufficient robustness. In practical applications, force / torque sensors are affected by environmental noise, vibration and temperature changes, which may lead to unstable measurement results.

[0003] In recent years, contact force estimation methods based on the joint position, velocity and acceleration of robots have gradually attracted attention. Some methods have begun to derive contact force through dynamic models. However, the contact force estimation methods ignore the fact that the contact behavior between the robot and the environment often has viscoelastic properties, that is, the contact force is not only related to the deformation, but also closely related to the rate of change of deformation, and traditional dynamic models are difficult to accurately characterize this characteristic.

[0004] Therefore, how to combine the traditional dynamic model with the viscoelastic properties without using a force / torque sensor and design a robot adaptive contact force estimation method based on the KB viscoelastic model is an urgent problem that technicians in this field need to solve. Summary of the invention

[0005] In view of this, the present invention provides a robot adaptive contact force estimation method based on the KB viscoelastic model, aiming to solve the problem of combining the traditional dynamic model with the viscoelastic characteristics when estimating the robot adaptive contact force without using a force / torque sensor.

[0006] In order to achieve the above object, the present invention adopts the following technical solution:

[0007] The robot adaptive contact force estimation method based on the KB viscoelastic model includes the following steps:

[0008] S1. Build robot and environment models based on robot dynamics equations;

[0009] S2, selecting the KB viscoelastic model based on the robot and environment models to estimate the contact force between the robot end tool and the environment;

[0010] S3, constructing a contact force adaptive estimation scheme based on the estimated value of the contact force between the robot end tool and the environment, and taking the obtained control and estimation laws as the control objectives;

[0011] S4. By proving the asymptotic convergence of the tracking error in the contact force adaptive estimation scheme and the stability of the closed-loop system, the control rate and the adaptive rate for achieving the control target are obtained;

[0012] S5. Based on the control rate and adaptation rate obtained in S4, a stable robot and environment model is obtained, which is used to estimate the contact force between the robot and the environment.

[0013] Furthermore, in S1, the robot and environment model is constructed, and the dynamic equation of the n-DOF robot with the position coordinate q of its joint is:

[0014]

[0015] Among them, q, Respectively represent the position, velocity and acceleration of the joint, M(q) is the n×n inertia matrix, is the n×1 Coriolis force and centrifugal force vector, G(q) is the n×1 gravity vector, τ is the joint torque control vector of the actuator, F e is an m×1 vector consisting of the contact force and torque applied to the robot end effector, and J(q) is the m×n task space Jacobian matrix.

[0016] Furthermore, in S1, in the dynamic equation of the n-DOF robot with the position coordinate q of the joint, M(q), The definitions of G(q) and J(q) are:

[0017]

[0018]

[0019]

[0020]

[0021] Among them, l1, l2, m1, m2 and g are the parameters of the robot, q1 and q2 are the two positions of the joint in space, and are the velocities of the two joints.

[0022] Furthermore, in S2, when the KB viscoelastic model estimates the contact force between the robot end tool and the environment, its contact force dynamics equation is:

[0023]

[0024] in, is the estimated value of the contact force between the robot end tool and the environment, F is the contact force applied to the robot end effector, x and They are the Cartesian deformation and its derivative of the KB viscoelastic model, and the identification parameters of the γ, α and βK-B viscoelastic model.

[0025] Furthermore, in S2, according to the damper and spring parameters, the identification parameters γ, α and β of the KB viscoelastic model in the contact force dynamics equation are defined as: Where k1 and k2 are the spring coefficients and b is the damping coefficient.

[0026] Furthermore, in S3, when constructing the adaptive contact force estimation scheme, it is assumed that F e The estimated value of the force between the two for:

[0027]

[0028] The tracking error of the joint trajectory is defined as:

[0029] e(t)=q(t)-q d (t)

[0030] Among them, e(t) is the tracking error of the joint trajectory, q(t) is the actual trajectory of the joint in space, and q d (t) is the second-order differentiable expected trajectory of the joint in space.

[0031] Furthermore, in S3, when constructing the contact force adaptive estimation scheme, the control and estimation laws are obtained as control objectives to achieve tracking position error e(t) and velocity error Converges to zero while ensuring the estimated contact forces The boundedness of .

[0032] Furthermore, in S4, the control rate and the adaptive rate used to verify whether the control target of S3 is achieved are:

[0033]

[0034]

[0035] Where τ is the control rate to achieve the control target or the control vector of the actuator joint torque, are the n×1 Coriolis force and centrifugal force vectors, G(q) is the n×1 gravity vector, To achieve the adaptive rate of the control target or the adaptive estimation error of the contact force, q, Respectively represent the position, velocity and acceleration of the joint, M(q) is the n×n inertia matrix, is the first-order derivative of the n×n inertia matrix, J(q) is the m×n task space Jacobian matrix, Γ, K and K f is a constant positive definite matrix, F e The estimated value of F e is an m×1 vector consisting of the contact force and torque applied to the robot end effector, Defined as: e(t) is the tracking error of the joint trajectory, is the first-order derivative of the joint trajectory tracking error with respect to time t, and T is the transpose of the matrix.

[0036] Furthermore, in S5, based on the adaptive rate expression of S4 and F e Replace with The obtained stable dynamic equation is expressed as:

[0037]

[0038] in, To achieve the adaptive rate of the control target or the adaptive estimation error of the contact force, γ, α and β are the identification parameters of the KB viscoelastic model. is the estimated value of the contact force, x and are the Cartesian position and velocity of the robot.

[0039] It can be seen from the above technical solutions that, compared with the prior art, the present invention has the following beneficial effects:

[0040] (1) The robot adaptive contact force estimation method of the present invention utilizes the current, speed and other data obtained by the robot body sensor in combination with the viscoelastic model to estimate the contact force without relying on external sensors.

[0041] (2) The present invention can adjust the model parameters in real time by introducing adaptive technology to cope with the dynamic changes of contact conditions such as environmental stiffness and friction characteristics, thereby improving the accuracy and robustness of contact force estimation.

[0042] (3) The present invention can accurately estimate the contact force between the robot end effector and the environment while reducing system costs and improving the safety and flexibility of the interaction between the robot and the environment. It has important research value and broad application prospects. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

[0044] Figure 1 A schematic flow chart of a method for estimating the contact force between a robot end effector and an environment based on a KB viscoelastic model provided by the present invention;

[0045] Figure 2 for Figure 1 The structural diagram of the KB viscoelastic model provided by the present invention is illustrated;

[0046] Figure 3 A structural diagram of the control system between the robot and the environment provided by the present invention;

[0047] Figure 4 Schematic diagram of a two-degree-of-freedom planar robot according to an embodiment of the present invention.

[0048] Figure 5 is a schematic diagram of position tracking of a two-degree-of-freedom robot joint 1 according to an embodiment of the present invention;

[0049] Figure 6 is a schematic diagram of speed tracking of a two-degree-of-freedom robot joint 1 according to an embodiment of the present invention;

[0050] Figure 7 Schematic diagram of actual and estimated contact forces of a two-degree-of-freedom robot according to an embodiment of the present invention. DETAILED DESCRIPTION

[0051] The following will be combined with the attached embodiment of the present invention Figure 1-Figure 7 , the technical solutions in the embodiments of the present invention are clearly and completely described. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0052] The present invention discloses a robot adaptive contact force estimation method based on the KB viscoelastic model. Figure 1 As shown, the following steps are included:

[0053] S1. Build robot and environment models based on robot dynamics equations;

[0054] S2, selecting the KB viscoelastic model based on the robot and environment models to estimate the contact force between the robot end tool and the environment;

[0055] S3, constructing a contact force adaptive estimation scheme based on the estimated value of the contact force between the robot end tool and the environment, and taking the obtained control and estimation laws as the control objectives;

[0056] S4. By proving the asymptotic convergence of the tracking error in the contact force adaptive estimation scheme and the stability of the closed-loop system, the control rate and the adaptive rate for achieving the control target are obtained;

[0057] S5. Based on the control rate and adaptation rate obtained in S4, a stable robot and environment model is obtained, which is used to estimate the contact force between the robot and the environment.

[0058] In this embodiment S1, the robot and environment model is constructed, and the dynamic equation of the n-degree-of-freedom robot with the position coordinate q of its joint is:

[0059]

[0060] Among them, q, Respectively represent the position, velocity and acceleration of the joint, M(q) is the n×n inertia matrix, is the n×1 Coriolis force and centrifugal force vector, G(q) is the n×1 gravity vector, τ is the joint torque control vector of the actuator, F e is an m×1 vector consisting of the contact force and torque applied to the robot end effector, and J(q) is the m×n task space Jacobian matrix.

[0061] In this embodiment Figure 2 In the figure, the structure diagram of the KB viscoelastic model is given. Due to the linearity of the KB viscoelastic model and the linear advantage when using a model-based force controller, the KB model is selected to estimate the contact force between the robot end tool and the environment.

[0062] In S2, when the KB viscoelastic model estimates the contact force between the robot end tool and the environment, its contact force dynamics equation is:

[0063]

[0064] in, is the estimated value of the contact force between the robot end tool and the environment, F is the contact force applied to the robot end effector, x and They are the Cartesian deformation and its derivative of the KB viscoelastic model, and the identification parameters of the γ, α and βK-B viscoelastic model.

[0065] In this embodiment S3, when constructing the contact force adaptive estimation scheme, it is assumed that F e The estimated value of the force between the two for:

[0066]

[0067] The tracking error of the joint trajectory is defined as:

[0068] e(t)=q(t)-q d (t) (4)

[0069] Among them, e(t) is the tracking error of the joint trajectory, q(t) is the actual trajectory of the joint in space, and q d (t) is the second-order differentiable expected trajectory of the joint in space.

[0070] In this embodiment S3, when constructing the contact force adaptive estimation scheme, the control and estimation rules are obtained as control targets to achieve tracking position error e(t) and velocity error Converges to zero while ensuring the estimated contact forces The boundedness of .

[0071] In this embodiment Figure 3 In the figure, the control system structure diagram between the robot and the environment is given. The error between the actual position and velocity and the expected position and velocity is used to obtain the adaptive law. The estimator uses the adaptive law to establish the estimated force and adjust the controller signal to control the robot. The robot control system, the dynamic equation of the n-degree-of-freedom robot with respect to the joint position coordinate q is shown in equation (1), the dynamic equation of the KB viscoelastic model when estimating the contact force between the robot end tool and the environment is shown in equation (2), and the control rate and adaptive rate that meet the control target of the contact force adaptive estimation scheme constructed by S3 are shown in equations (10) and (12).

[0072] In this embodiment S4, the control rate and the adaptive rate used to verify whether the control target of S3 is achieved are:

[0073]

[0074]

[0075] Where τ is the control rate to achieve the control target or the control vector of the actuator joint torque, are the n×1 Coriolis force and centrifugal force vectors, G(q) is the n×1 gravity vector, To achieve the adaptive rate of the control target or the adaptive estimation error of the contact force, q, Respectively represent the position, velocity and acceleration of the joint, M(q) is the n×n inertia matrix, is the first-order derivative of the n×n inertia matrix, J(q) is the m×n task space Jacobian matrix, Γ, K and K f is a constant positive definite matrix, F e The estimated value of F e is an m×1 vector consisting of the contact force and torque applied to the robot end effector, Defined as: e(t) is the tracking error of the joint trajectory, is the first-order derivative of the joint trajectory tracking error with respect to time t, and T is the transpose of the matrix.

[0076] In this embodiment, the specific process of proving the gradual convergence of the tracking error and the stability of the system is as follows:

[0077] choose As a time-varying Lyapunov function, its expression is:

[0078]

[0079] in,

[0080] In formula (7), the derivative of V with respect to time t is obtained, and the expression obtained is:

[0081]

[0082] By taking the derivative of formula (6), we get the following expression:

[0083]

[0084] Substitute equation (8) into equation (7) and Replace with The resulting expression is:

[0085]

[0086] Now consider the following control law (10). Substituting equation (10) into equation (9), the expression obtained is:

[0087]

[0088] Now use the following adaptation law (12), substitute formula (12) into formula (11), and get a new expression, and set the second term equal to zero, and the obtained expression is:

[0089]

[0090] After substituting formula (6) into formula (13), the obtained expression is:

[0091]

[0092] By equating the second term and the last term in equation (14), we obtain the following expression:

[0093]

[0094] therefore, It can be concluded that: e,

[0095] Through Integrating and due to the boundedness of V(∞) and V(0), we can derive: e,

[0096] On the other hand, considering equation (6), and using e and The boundedness of Therefore, using Barballat's lemma, we can get expression (16), which is:

[0097]

[0098] Consider the robot dynamics equation in equation (1):

[0099] F e The boundedness of can be derived from the dynamic equation in equation (2);

[0100] The boundedness of τ is also obvious from equation (10);

[0101] Consider equations (1) and (2) and M -1 The boundedness of (q) shows that The boundedness of .

[0102] therefore, It is also bounded.

[0103] The previously obtained L 2 Using Barbalat's lemma again, we get the expression:

[0104]

[0105] Therefore, both the position and velocity errors of the system converge to zero.

[0106] In this embodiment, in S5, based on the adaptive rate expression of S4 and F e Replace with The obtained stable dynamic equation is expressed as:

[0107]

[0108] in, To achieve the adaptive rate of the control target or the adaptive estimation error of the contact force, γ, α and β are the identification parameters of the KB viscoelastic model. is the estimated value of the contact force, x and are the Cartesian position and velocity of the robot.

[0109] In this embodiment S5, the process of obtaining a stable robot and environment model based on the control rate and adaptive rate obtained in S4 is:

[0110] From the adaptation law (12) we get Then, derive The expression of is used as the estimated value of the contact force, as shown in formula (18):

[0111]

[0112] From the force estimation error equation (3), we can get equation (19):

[0113]

[0114] From formula (19), we can get Substituting it into equation (2), we can get equation (20):

[0115]

[0116] F e Replace with According to the adaptation law (12), we get and With x and is the Cartesian position and velocity of the robot, then The stable dynamics equation can be used to obtain equation (21):

[0117]

[0118] in, To achieve the adaptive rate of the control target or the adaptive estimation error of the contact force, γ, α and β are the identification parameters of the KB viscoelastic model. is the estimated value of the contact force, x and are the Cartesian position and velocity of the robot.

[0119] In this embodiment, in S2, according to the damper and spring parameters, the identification parameters γ, α and β of the KB viscoelastic model in the contact force dynamics equation are defined as: Where k1 and k2 are the spring coefficients and b is the damping coefficient.

[0120] In order to evaluate the effectiveness of the proposed estimation method, we use Figure 4 The 2-DOF planar robot shown has undergone some simulations, as follows:

[0121] The dynamic equation of the robot is shown in formula (1), where M(q), G(q) and J(q) are defined as:

[0122]

[0123]

[0124]

[0125]

[0126] In this embodiment, the robot parameters are set to: l1 = l2 = 1 m, m1 = 2.3 kg, m2 = 0.8 kg and g = 9.81 m / s 2 .

[0127] The parameters of the KB viscoelastic model are: α = 27.2 (N·s / m), β = 190 (N / m) and γ = 0.0345 (s).

[0128] The desired trajectory of the robot end effector is set as: q d (t) = 0.5 [sint cost] T , the initial condition of the robot is: q(0) = [1, 0.75] T (rad), The control parameters are: K = diag (8, 5), K f =0.04I2 and Γ=5I2, where I2 is the 2×2 identity matrix.

[0129] exist Figure 5 In , a schematic diagram of position tracking of joint 1 of a two-degree-of-freedom robot is given; Figure 6 In the figure, a schematic diagram of velocity tracking of joint 1 of a two-degree-of-freedom robot is given; Figure 7 In , a schematic diagram of the actual and estimated contact forces for a two-degree-of-freedom robot is given.

[0130] like Figure 5 and Figure 6 As shown, by providing the estimated contact force for the control force, the position and velocity tracking errors can approach zero more quickly.

[0131] In this embodiment, the construction and application of the KB viscoelastic model are as follows: establishing a KB viscoelastic model that can accurately describe the contact behavior between the robot and the environment, combining elastic properties and viscous properties to achieve an accurate description of the contact force. The key points are: the mathematical expression and parameterization method of the KB viscoelastic model, and the specific application process in contact force estimation.

[0132] In this embodiment, the contact force estimation method based on the robot's internal information is to use the data of the robot's internal joint sensors (such as motor current, joint speed and acceleration, etc.), combined with the dynamic model and the KB viscoelastic model, to derive the contact force estimation method, replacing the traditional force / torque sensor. Its protection point is: the algorithm design that combines the robot dynamics with the KB viscoelastic model, including signal processing, force estimation calculation method and error correction mechanism.

[0133] In this embodiment, the parameter adaptive adjustment mechanism, the key point is to design an adaptive algorithm to adjust the KB viscoelastic model parameters according to real-time environmental changes to ensure the accuracy and robustness of contact force estimation. Its protection points are parameter adjustment rules, update algorithms and application methods in dynamic environments, including model adjustment strategies based on error feedback.

[0134] In other embodiments, the KB viscoelastic model is replaced with a non-Newtonian fluid model similar to a shear thickening fluid (STF), whose dynamic characteristics are feasible for developing a controller that is both impact-resistant and compliant. For a controller designed using STF, it is possible to adapt to sudden changes in contact force, while ensuring the safety of the robot at different contact stages for human-machine interaction. Specifically, when the robot end effector is in close contact with the external environment, a continuous contact force is generated between them, and the robot's movement will not diverge, thereby achieving stable position and velocity tracking, ensuring safety during the contact process.

[0135] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

[0136] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A robot adaptive contact force estimation method based on the KB viscoelastic model, characterized in that: The following steps are involved: S1. Build robot and environment models based on robot dynamics equations; S2, selecting the KB viscoelastic model based on the robot and environment models to estimate the contact force between the robot end tool and the environment; S3, constructing a contact force adaptive estimation scheme based on the estimated value of the contact force between the robot end tool and the environment, and taking the obtained control and estimation laws as the control objectives; S4. By proving the asymptotic convergence of the tracking error in the contact force adaptive estimation scheme and the stability of the closed-loop system, the control rate and the adaptive rate for achieving the control target are obtained; S5. Based on the control rate and adaptation rate obtained in S4, a stable robot and environment model is obtained, which is used to estimate the contact force between the robot and the environment.

2. The robot adaptive contact force estimation method based on the KB viscoelastic model according to claim 1 is characterized in that: In S1, the robot and environment model is constructed, and the dynamic equation of the n-degree-of-freedom robot with the position coordinate q of its joints is: Among them, q, Represent the position, velocity and acceleration of the joint respectively, M(q) is the n×n inertia matrix, is the n×1 Coriolis force and centrifugal force vector, G(q) is the n×1 gravity vector, τ is the joint torque control vector of the actuator, F e is an m×1 vector consisting of the contact force and torque applied to the robot end effector, and J(q) is the m×n task space Jacobian matrix.

3. The resource scheduling method based on annealing genetic algorithm according to claim 2 is characterized in that: In S1, in the dynamic equation of the n-DOF robot with the position coordinate q of the joint, M(q), The definitions of G(q) and J(q) are: Among them, l1, l2, m1, m2 and g are the parameters of the robot, q1 and q2 are the two positions of the joint in space, and are the velocities of the two joints.

4. The robot adaptive contact force estimation method based on KB viscoelastic model according to claim 1 is characterized in that: In S2, when the KB viscoelastic model estimates the contact force between the robot end tool and the environment, its contact force dynamics equation is: in, is the estimated value of the contact force between the robot end tool and the environment, F is the contact force applied to the robot end effector, x and They are the Cartesian deformation and its derivative of the KB viscoelastic model, and the identification parameters of the γ, α and βK-B viscoelastic model.

5. The robot adaptive contact force estimation method based on KB viscoelastic model according to claim 4 is characterized in that: In S2, according to the damper and spring parameters, the identification parameters γ, α and β of the KB viscoelastic model in the contact force dynamics equation are defined as: Where k1 and k2 are the spring coefficients and b is the damping coefficient.

6. A resource scheduling method based on annealing genetic algorithm according to claim 1, characterized in that: In S3, when constructing the adaptive contact force estimation scheme, it is assumed that F e The estimated value of the force between the two for: The tracking error of the joint trajectory is defined as: e(t)=q(t)-q d (t) Among them, e(t) is the tracking error of the joint trajectory, q(t) is the actual trajectory of the joint in space, and q d (t) is the second-order differentiable expected trajectory of the joint in space.

7. The robot adaptive contact force estimation method based on KB viscoelastic model according to claim 6 is characterized in that: In S3, when constructing the contact force adaptive estimation scheme, the control and estimation laws are obtained as control objectives to achieve tracking position error e(t) and velocity error Converges to zero while ensuring the estimated contact forces The boundedness of .

8. The robot adaptive contact force estimation method based on KB viscoelastic model according to claim 1 is characterized in that: In S4, the control rate and adaptive rate used to verify whether the control target of S3 is achieved are: Where τ is the control rate to achieve the control target or the control vector of the actuator joint torque, are the n×1 Coriolis force and centrifugal force vectors, G(q) is the n×1 gravity vector, To achieve the adaptive rate of the control target or the adaptive estimation error of the contact force, q, Respectively represent the position, velocity and acceleration of the joint, M(q) is the n×n inertia matrix, is the first-order derivative of the n×n inertia matrix, J(q) is the m×n task space Jacobian matrix, Γ, K and K f is a constant positive definite matrix, F e The estimated value of F e is an m×1 vector consisting of the contact force and torque applied to the robot end effector, Defined as: e(t) is the tracking error of the joint trajectory, is the first-order derivative of the joint trajectory tracking error with respect to time t, and T is the transpose of the matrix.

9. The robot adaptive contact force estimation method based on KB viscoelastic model according to claim 1 is characterized in that: In S5, based on the adaptive rate expression of S4 and F e Replace with The obtained stable dynamic equation is expressed as: in, To achieve the adaptive rate of the control target or the adaptive estimation error of the contact force, γ, α and β are the identification parameters of the KB viscoelastic model. is the estimated value of the contact force, x and are the Cartesian position and velocity of the robot.

Images