Robot force compliance control method based on variable speed impedance control

Abstract

The present invention discloses a robot force compliance control method based on variable speed impedance control, comprising the following steps: S1, establishing a dynamic model of contact between the robot and the environment, and analyzing the steady-state error of the contact force. S2, constructing a variable speed impedance controller based on the model. S3, designing a reference trajectory based on task requirements, combining the dynamic model with feedforward control, calculating the output torque, and verifying the proposed control algorithm. The beneficial effects of the present invention are: online adjusting the movement speed of the robot according to the contact force error during the contact process between the robot and the environment, adjusting the contact situation between the robot and the environment by changing the movement speed, thereby improving the contact force control accuracy and improving the compliance of the contact.

Description

Technical Field

[0001] The present invention relates to the technical field of robot automation control, and in particular to a robot force compliance control method based on variable speed impedance control. Background Art

[0002] Robots are increasingly used in today's production and everyday life. When robots perform tasks involving forces acting on the environment, such as grinding and assembly, simple position control can cause excessive forces due to position errors, failing to achieve the desired compliant contact, potentially damaging parts or the robot's end-of-line tooling. To achieve compliant contact when robots operate in these constrained environments, it's necessary to incorporate contact force into the control system as a control element, modify the controller structure, improve system response characteristics, and enhance force control accuracy.

[0003] Traditional force control strategies include PID force control, force / position hybrid control, impedance control and other methods. Among them, PID force control adjusts the control output according to the actual feedback information of the controlled object to achieve the control effect, but the control accuracy is generally not high, it is prone to oscillation, and is sensitive to noise. Force / position hybrid control divides the control space into subspaces of force control and position control, and performs independent control, with force control in the normal direction and position control in the tangential direction. Impedance control establishes a dynamic relationship between the end force of the robot and its position deviation - the "spring-mass-damper" model, and controls the displacement of the robot joint to achieve the purpose of controlling the end force of the robot. However, the impedance model parameters need to be adapted according to different environments and tasks, and its universality is poor.

[0004] At present, in general, traditional force control strategies have poor adaptability to environmental changes. When the environmental position, stiffness, etc. change, the compliant control of force is not effective. Summary of the Invention

[0005] In view of this, the present invention provides a robot force compliance control method based on variable speed impedance control, which can adapt to the environment. When the environmental position, stiffness, etc. change, the overshoot of the contact force can be reduced, and the steady-state error between the contact force and the expected force can be reduced, thereby realizing force compliance control.

[0006] To achieve the above purpose, the technical solution of the present invention is as follows:

[0007] A robot force compliance control method based on variable speed impedance control comprises the following steps:

[0008] S1: Establish a dynamic model of the contact between the robot and the environment and analyze the steady-state error of the contact force. The process is as follows:

[0009] The impedance control model based on position control is:

[0010]

[0011] In formula (1), m d 、b d and k d are the inertia parameter, damping parameter, and stiffness parameter of the impedance model respectively; x, x, and x are the displacement, velocity, and acceleration of the current robot motion respectively; x r 、 and are the desired motion trajectory, velocity and acceleration of the robot respectively; e f =f r -f e , e f is the force tracking error, f r is the expected force, f e is the contact force between the robot and the environment;

[0012] The dynamic model of the environment is expressed as:

[0013] f e =k e (xx e ) (2)

[0014] In formula (2), k e is the environmental stiffness. e for the location of the environment;

[0015] When the contact between the robot and the environment reaches a steady state, the steady-state error of the contact force is expressed as:

[0016]

[0017] S2: Construct a variable speed impedance controller based on the model;

[0018] The variable speed impedance controller model is established as follows:

[0019]

[0020] in, Through the contact force error e f and the first-order derivative of the contact force error For online adjustment, the form is:

[0021]

[0022] In formula (5), α and λ are constants;

[0023] S3: Design a reference trajectory based on the mission requirements, combine it with the dynamic model feedforward control, calculate the output torque, and verify the proposed control algorithm.

[0024] Furthermore, in step S1, since the accurate environment stiffness and position information cannot be accurately obtained, the contact force between the robot and the environment cannot reach the expected value, and there is a steady-state error. Therefore, in the steady-state error formula (3) of the contact force, let k d =0, then we can get e ss =0; therefore, by selecting a suitable m d and b d Theoretically, the contact force error between the robot and the environment can be made to approach 0.

[0025] Furthermore, in step S2, due to the contact force f e It is measured by the force sensor, contains noise signals, and is obtained by directly derivatizing the force signal. is inaccurate, in order to get accurate Depend on Alternative Because r Usually a constant, then You can get:

[0026]

[0027] Usually within a short, defined period of time e is also a constant, so:

[0028]

[0029] at this time, It can be expressed as:

[0030]

[0031] In formula (8), β = -k e λ is a constant;

[0032] To facilitate actual programming, the above second-order variable speed impedance control can be equivalent to:

[0033]

[0034] Furthermore, the variable speed impedance controller system includes three modules: reference trajectory, adaptive variable speed controller and environment. The adaptive variable speed impedance controller calculates the adjustment amount of the robot's motion speed online through contact force error and motion speed, and dynamically adjusts the robot's terminal motion speed through α and β gain factors to achieve smooth contact between the robot and the environment.

[0035] Furthermore, in step S3, a simulation model is established using Matlab Simulink software to verify the variable speed impedance control method under the following two environmental changes, and compare the force tracking effect with the traditional impedance control method:

[0036] (1) Sudden change of environmental stiffness

[0037] Set the environment stiffness to:

[0038]

[0039] Expected force f r =30N,x e =1m,m d =1,b d =30; Because the environment position can often only obtain theoretical parameters, it is impossible to obtain precise parameters, such as pits, bumps, stiffness, etc., here we take the expected trajectory x r =1.01m; at 2 and 4 seconds, the environmental stiffness suddenly changes to 4000N / m and 5000N / m respectively; when α=-5, β=200, the overshoot caused by the change in environmental stiffness can be quickly reduced to the minimum, and there is basically no oscillation, achieving an ideal compliant contact effect;

[0040] (2) Sudden changes in environmental location

[0041] The location where you set up your environment is as follows:

[0042]

[0043] That is, between 2 and 3 seconds, the environmental position has a radial change of 0.01sin(2π(t-2))m; the environmental stiffness k e =6000N / m,x e =1m,x r =1.01m,m d =1,b d =30; when α=-50, β=300, the contact force overshoot caused by environmental position changes can be significantly reduced, achieving an ideal compliant contact effect.

[0044] Compared with the prior art, the present invention has the following beneficial effects:

[0045] 1. The robot force compliance control method based on variable speed impedance control provided by the present invention automatically adapts to environmental changes, limits the overshoot of the contact force between the robot and the environment, and the steady-state error between the environmental contact force and the expected force basically converges to zero, thereby improving the compliance of the contact between the robot and the environment. It has the technical advantages of being simple, easy to use and easy to implement.

[0046] 2. The robot force compliance control method based on variable speed impedance control provided by the present invention is adopted to adjust the robot's movement speed online according to the contact force error during the contact process between the robot and the environment. The contact situation between the robot and the environment is adjusted by changing the movement speed, thereby improving the contact force control accuracy and improving the compliance of the contact. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 It is the structural block diagram of the adaptive variable speed impedance controller system;

[0048] Figure 2 Simulation model built for Matlab Simulink;

[0049] Figure 3 This is the simulation result diagram under the sudden change of environmental stiffness;

[0050] Figure 4 This is the simulation result diagram under sudden change of environmental position. DETAILED DESCRIPTION

[0051] The present invention will be further described below with reference to the embodiments and accompanying drawings.

[0052] A robot force compliance control method based on variable speed impedance control is proposed. The main steps are summarized as follows:

[0053] Step S1: Establish a dynamic model of the contact between the robot and the environment and analyze the steady-state error of the contact force.

[0054] Step S2: construct a variable speed impedance controller according to the model.

[0055] In step S3, a reference trajectory is designed according to the task requirements, and the output torque is calculated in combination with the feedforward control of the dynamic model to verify the proposed control algorithm.

[0056] The above method can automatically adapt to changes in the environment and achieve compliant control of the contact force between the robot and the environment. The detailed implementation process is as follows:

[0057] The impedance control model based on position control is:

[0058]

[0059] Among them, m d 、b d and k d are the inertia parameter, damping parameter, and stiffness parameter of the impedance model respectively. x, x, and x are the displacement, velocity, and acceleration of the current robot motion respectively. r 、 and are the desired trajectory, velocity, and acceleration of the robot, respectively.f =f r -f e , e f is the force tracking error, f r is the expected force, f e is the contact force between the robot and the environment.

[0060] The dynamic model of the environment is expressed as:

[0061] f e =k e (xx e ) (2)

[0062] Among them, k e is the environmental stiffness. e The location of the environment.

[0063] When the contact between the robot and the environment reaches a steady state, the steady-state error of the contact force is:

[0064]

[0065] Because the accurate environment stiffness and position information cannot be accurately obtained, the contact force between the robot and the environment cannot reach the expected value, and there is a steady-state error. Here, let k d =0, then we get e ss = 0. So by choosing a suitable m d and b d Theoretically, the contact force error between the robot and the environment can be made to approach 0.

[0066] When environmental parameters such as environmental stiffness and environmental position change, it will cause force error e ss It is not always 0, and may even be larger. In order to improve the force tracking effect and enhance the contact compliance, a variable speed impedance control method is proposed.

[0067] The variable speed impedance controller model is as follows:

[0068]

[0069] Where Δx(t) is calculated by the contact force error e f , and the first-order derivative of the contact force error e f For online adjustment, the form is:

[0070]

[0071] α and λ are constants.

[0072] In practical applications, due to the contact force f e It is measured by the force sensor and contains noise signals, so it is directly derived by taking the derivative of the force signal. is inaccurate. In order to get accurate One possible approach is to Alternative Because r Usually a constant, then You can get:

[0073]

[0074] Usually within a short, defined period of time e is also a constant, so:

[0075]

[0076] This method is suitable for situations where the sampling frequency is high and the end-effector velocity measurement is accurate.

[0077] Δx(t) can be expressed as:

[0078]

[0079] Where β = -k e λ is a constant.

[0080] To facilitate practical programming, the above-mentioned second-order variable speed impedance control can be equivalent to:

[0081]

[0082] By implementing the above algorithmic process, the robot's joints can automatically adapt to environmental changes, limiting overshoot in the contact force between the robot and the environment. The steady-state error between the environmental contact force and the desired force essentially converges to zero. In this embodiment, it is assumed that the robot can accurately control its position, i.e., the position information output by the variable speed impedance controller can be accurately executed by the robot. Therefore, no virtual modeling of the robot is performed. In this embodiment, the robot force compliance control method can be applied to industrial robots, service robots, special-purpose robots, and the like.

[0083] Please refer to the attached Figure 1 As shown, this embodiment also specifically provides a method for adaptive variable speed impedance control of a robot. The adaptive variable speed impedance controller system mainly includes three modules: a reference trajectory, an adaptive variable speed controller, and an environment. The adaptive variable speed impedance controller calculates the adjustment amount of the robot's motion speed online based on the contact force error and motion speed. Using the α and β gain factors, it dynamically adjusts the motion speed of the robot's end point to achieve smooth contact between the robot and the environment.

[0084] For example Figure 2As shown in the figure, a simulation model was built using Matlab Simulink. The variable speed impedance control method was verified under the following two environmental conditions and the force tracking effect was compared with the traditional impedance control method:

[0085] a. Sudden change in environmental stiffness;

[0086] Set the environment stiffness to:

[0087]

[0088] Expected force f r =30N,x e =1m,m d =1,b d = 30. Because the environment position can only obtain theoretical parameters, it is impossible to obtain precise parameters such as pits, bumps, stiffness, etc., here we take the expected trajectory x r =1.01m. At 2 and 4 seconds, the environmental stiffness suddenly changes to 4000N / m and 5000N / m respectively. The simulation results are as follows Figure 3 As shown. c is the traditional impedance control force response curve, F v is the variable speed impedance control force response curve, F r When α = -5 and β = 200, the overshoot caused by the change in environmental stiffness can be quickly reduced to a minimum, with almost no oscillation, achieving an ideal compliant contact effect.

[0089] b. Sudden changes in environmental location;

[0090] The location where you set up your environment is as follows:

[0091]

[0092] That is, between 2 and 3 seconds, the environment position has a radial change of 0.01sin(2π(t-2))m. The environment stiffness k e =6000N / m,x e =1m,x r =1.01m,m d =1,b d =30. The simulation results are as follows Figure 4 When α = -50 and β = 300, the contact force overshoot caused by environmental position changes can be significantly reduced, achieving an ideal compliant contact effect.

[0093] The simulation verifies that the variable speed impedance control method of this embodiment has good adaptability to environmental changes, can significantly reduce contact force errors, and improves the compliance of the robot's contact with the environment.

[0094] Finally, it should be noted that the above description is only a preferred embodiment of the present invention. Under the guidance of the present invention, ordinary technicians in this field can make various similar expressions without violating the purpose and claims of the present invention. Such changes fall within the scope of protection of the present invention.

Claims

1. A robot force compliance control method based on variable speed impedance control, characterized in that: The following steps are involved: S1: Establish a dynamic model of the contact between the robot and the environment and analyze the steady-state error of the contact force. The process is as follows: The impedance control model based on position control is: In formula (1), m d 、b d and k d are the inertia parameter, damping parameter and stiffness parameter of the impedance model respectively; x, and are the displacement, velocity and acceleration of the current robot motion respectively; x r 、 and are the desired motion trajectory, velocity and acceleration of the robot respectively; e f =f r -f e , e f is the force tracking error, f r is the expected force, f e is the contact force between the robot and the environment; The dynamic model of the environment is expressed as: f e =k e (x-x e ) (2) In formula (2), k e is the environmental stiffness, x e for the location of the environment; When the contact between the robot and the environment reaches a steady state, the steady-state error of the contact force is expressed as: S2: Construct a variable speed impedance controller based on the model; The variable speed impedance controller model is established as follows: in, Through the contact force error e f and the first-order derivative of the contact force error For online adjustment, the form is: In formula (5), α and λ are constants; S3: Design a reference trajectory based on the mission requirements, combine it with the dynamic model feedforward control, calculate the output torque, and verify the proposed control algorithm.

2. The robot force compliance control method based on variable speed impedance control according to claim 1, characterized in that: In step S1, in the steady-state error formula (3) of the contact force, let k d =0, then we can get e ss =0; therefore, by selecting a suitable m d and b d The contact force error between the robot and the environment can be made to approach 0.

3. The robot force compliance control method based on variable speed impedance control according to claim 1, characterized in that: In step S2, due to the contact force f e It is measured by the force sensor, contains noise signals, and is obtained by directly derivatizing the force signal. is inaccurate, in order to get accurate Depend on Alternative f r is a constant, then get: In a short period of time x e is also a constant, so: at this time, Expressed as: In formula (8), β = -k e λ is a constant; To facilitate actual programming, variable speed impedance control is equivalent to:

4. The robot force compliance control method based on variable speed impedance control according to claim 3, characterized in that: The variable speed impedance controller system includes three modules: reference trajectory, adaptive variable speed controller and environment. The adaptive variable speed impedance controller calculates the adjustment amount of the robot's motion speed online through contact force error and motion speed, and dynamically adjusts the robot's end motion speed through α and β gain factors to achieve smooth contact between the robot and the environment.

5. The robot force compliance control method based on variable speed impedance control according to claim 1, characterized in that: In step S3, a simulation model is established using Matlab Simulink software to verify the variable speed impedance control method under the following two environmental changes, and compare the force tracking effect with the traditional impedance control method: (1) Sudden change of environmental stiffness Set the environment stiffness to: Expected force f r =30N,x e =1m,m d =1,b d =30; take the expected trajectory x r =1.01m; at 2 and 4 seconds, the environmental stiffness suddenly changes to 4000N / m and 5000N / m respectively; when α=-5, β=200, the overshoot caused by the change in environmental stiffness can be quickly reduced to the minimum, and there is basically no oscillation, achieving an ideal compliant contact effect; (2) Sudden changes in environmental location The location where you set up your environment is as follows: That is, between 2 and 3 seconds, the environmental position has a radial change of 0.01sin(2π(t-2))m; the environmental stiffness k e =6000N / m,x e =1m,x r =1.01m,m d =1,b d =30; when α=-50, β=300, the contact force overshoot caused by environmental position changes can be significantly reduced, achieving an ideal compliant contact effect.

Images