A design method for preset time force observer of robot system

Abstract

The present invention discloses a design method for a preset time force observer for a robot system, which relates to the field of robot system control and does not require a force sensor, so as to solve the problems of the existing sensors being expensive and affecting the system dynamic model. The present invention can obtain the joint position of the robot only based on the motor encoder, and use the preset time force observer to estimate the actual force received by the robot, thereby replacing the role of the force sensor to the greatest extent, and does not need to install an expensive and easily damaged force sensor, and can adjust the observation time of the force observer as needed, saving development costs.

Description

Technical Field

[0001] The invention relates to the field of robot system control, and in particular to a design method for a preset time force observer for a robot system. Background Art

[0002] Robot force control is widely used in polishing, assembly, and teaching. Simple position control will cause excessive force due to position error, which may damage parts or robots. When robots move in such restricted motion environments, they often need to be used in conjunction with force control. Under position control, the robot will move strictly according to the pre-set position trajectory. If the robot encounters an obstacle during movement, the robot's position tracking error will increase. At this time, the robot will try hard to "exert force" to track the preset trajectory, which ultimately leads to a huge internal force between the robot and the obstacle. Under force control, the goal is to control the force between the robot and the obstacle. When the robot encounters an obstacle, it will intelligently adjust the preset position trajectory to eliminate the internal force.

[0003] There are two main methods for achieving force control at present, one is impedance control, and the other is force / position hybrid control. Impedance control does not directly control the desired force and position, but achieves the function of force control by controlling the dynamic relationship between force and position. Force / position hybrid control controls force and position separately according to the operation requirements. No matter which method is used, force contact is required. At present, force measurement mostly uses force sensors to measure external force. However, due to the high cost and easy damage of force sensors, some machines cannot be installed.

[0004] Taking into account the impact of convergence time on system performance, some scholars have proposed a fixed-time force observer or a finite-time force observer to replace the force sensor. Since finite-time control needs to consider the influence of initial values, fixed-time control is not convenient for adjusting the convergence time. Therefore, it is very valuable to study a more convenient and time-efficient force observer design method. Summary of the invention

[0005] The technical problem to be solved by the present invention is to provide a design method for a preset time force observer for a robot system, which does not require a force sensor, so as to solve the shortcomings of existing sensors such as high cost and influence on the system dynamic model.

[0006] In order to solve the above technical problems, the technical solution adopted by the present invention is: a design method for a preset time force observer for a robot system, comprising the following steps:

[0007] Step S1: Based on the fact that the robot has three joints driven by independent servo motors, a robot dynamics model is established to identify unknown parameters;

[0008] Step S2: Using a separate P+d controller to control the movement of each joint of the robot;

[0009] Step S3: acquiring the joint position signal of the robot according to the position encoder of the robot;

[0010] Step S4: using impedance control to generate joint target trajectory using external force;

[0011] Step S5: subtract the joint position signal obtained in step S3 and step S4 from the joint target trajectory to calculate the joint position error;

[0012] Step S6: design a preset time differentiator to obtain the velocity signal and acceleration signal of the robot system;

[0013] Step S7: Based on the principles of sliding mode control and disturbance observer, a preset time force observer is designed;

[0014] Step S8: Establishing the Lyapunov equation to prove the given preset time;

[0015] Step S9: Feedback the observed force to the controller design for controlling the robot, and compare the observed external force with the actual external force to verify the accuracy of the force observer.

[0016] A further improvement of the technical solution of the present invention is that the formula of the robot dynamics model in step S1 is:

[0017]

[0018] Where q∈R 3 is the joint position signal; is the joint velocity signal; M(q)∈R 3×3 is the positive definite inertia matrix of the system; is the vector of Coriolis force and centrifugal force; G(q)∈R 3 is the gravitational torque; F∈R 3 is the environmental torque on the robot; τ∈R 3 To control the robot's control torque.

[0019] A further improvement of the technical solution of the present invention is that: the controller of the robot joint in step S2 is

[0020]

[0021] In the formula, τ i is the controller of the i-th joint, i = 1, 2, 3, k p is the proportionality coefficient, e i is the error term of the i-th joint position, k dis the damping coefficient, G i is the gravity term of the i-th joint, is the observed force at the i-th joint.

[0022] A further improvement of the technical solution of the present invention is that the actual joint position signal of the robot in step S3 is expressed as q=[q 1 ,q 2 ,q 3 ] T .

[0023] A further improvement of the technical solution of the present invention is that: the step S4 is specifically: converting the external force signal into a joint position signal, and the conversion formula is:

[0024]

[0025] In the formula, From this we can derive the expected joint position q of the system d =[q d1 ,q d2 ,q d3 ] T .

[0026] A further improvement of the technical solution of the present invention is that the joint position error in step S5 is:

[0027]

[0028] Where e=[e 1 ,e 2 ,e 3 ] T .

[0029] A further improvement of the technical solution of the present invention is that the formula for presetting the time differentiator in step S6 is:

[0030]

[0031] Among them, the formula and is an abbreviation. α is a constant, * is an arbitrary variable, k 1 =6, k 2 =4.5, Q i As the input of the preset time differentiator, y 2 is the output speed signal of the preset time differentiator When the speed signal As the input of the preset time differentiator, y 2 is the output speed signal of the differentiator

[0032] A further improvement of the technical solution of the present invention is that the process of designing the preset time force observer in step S7 is:

[0033] Rearrange the system (1) into a feedback system, and let x 1 =q,

[0034]

[0035] in, B=(M) -1 , F x =(M) -1 F, where F x =[F x ,F x ,F x ] T is unknown, and its first-order derivative is bounded, that is, L is a known constant, and by designing w = [w 1 ,w 2 ,w 3 ] T Come to F x Make an estimate, that is,

[0036] The force observer is designed as

[0037]

[0038] Where z = [z 1 ,z 2 ,z 3 ] T is used to estimate x 2 , T 1 ,T 2 > 0, k ≥ max{L}, 0 < a < 1, and

[0039] ε=zx 2 (8)

[0040]

[0041] A further improvement of the technical solution of the present invention is that: the preset time T given by the proof in step S8 1 ,T 2 The process is:

[0042] Derivative (8) and then substitute (6) and (7) into the derived equation to obtain

[0043]

[0044] Based on equations (10) and (9), the force estimation error variable is obtained:

[0045] θ=wF (11)

[0046] The following equations are obtained for equations (8) and (11):

[0047]

[0048] The force observer in (7) is a sliding mode observer. The sliding mode control is divided into the arrival time and the sliding time along the sliding surface. By designing the Lyapunov function V θ Derivation of the arrival time of the force observer

[0049] V θ =||θ|| (13)

[0050] V θ Take the derivative and substitute (12) into it to obtain

[0051]

[0052] Where θ=[|θ 1 |,|θ 2 |,...,|θ n |] T ∈R n , use (|θ 1 |+|θ 2 |+...+|θ n |)≥||θ||, for Further deduction leads to the following inequality:

[0053]

[0054] After T 1 , we get the following formula:

[0055]

[0056] Secondly, by designing the Lyapunov function V ε Derivation of the sliding time of the force observer

[0057] V ε =ε T ε (17)

[0058] By taking the derivative of (17) and combining it with (16), we get the following formula:

[0059]

[0060] The inequality relationship is but The inequality relationship is:

[0061]

[0062] Discuss the above inequality in different cases

[0063] When V ε ≥1, we get because

[0064]

[0065]

[0066] With T 2 >0 and The first part of the preset time meets

[0067] When V ε ≤1, as ||ε||≤1 and 1+ε 2 ≥1, we get because

[0068]

[0069]

[0070] With T 2 >0, the second part of the preset time meets The preset time of the arrival phase of the system is T 2 =T x1 +T x2 ;

[0071] Finally, human and environmental forces

[0072]

[0073] It can be set within the preset time T≤T 1 +T 2 Realized within.

[0074] Due to the adoption of the above technical solution, the technical progress achieved by the present invention is:

[0075] The present invention provides a design method for a preset time force observer for a robot system, which replaces the force sensor. The joint position of the robot can be obtained only based on the motor encoder, and the preset time force observer is used to estimate the actual force applied to the robot, thereby replacing the role of the force sensor to the greatest extent. There is no need to install expensive and easily damaged force sensors, and the observation time of the force observer can be adjusted as needed, saving development costs. BRIEF DESCRIPTION OF THE DRAWINGS

[0076] Figure 1 A robot control system based on a preset time force observer;

[0077] Figure 2 This is a diagram of the simple structure of a three-link tandem robot;

[0078] Figure 3 This is the experimental effect diagram of the preset time force observer;

[0079] Figure 4 It is the joint trajectory tracking diagram. DETAILED DESCRIPTION

[0080] The present invention is further described in detail below in conjunction with embodiments:

[0081] The present invention is further described below in conjunction with the embodiments and drawings. The specific embodiment selects a three-degree-of-freedom serial robot (such as Figure 2 as shown) as the object of action.

[0082] The following describes in detail the principles and implementation methods of a design method for a preset time-force observer for a robot system of the present invention so that those skilled in the art can clearly understand a design method for a preset time-force observer for a robot system.

[0083] like Figure 1 As shown, a design method for a preset time force observer for a robot system includes the following steps:

[0084] Step S1: Figure 2 The robot has three joints driven by three servo motors. Based on the fact that the robot has three joints driven by independent servo motors, a robot dynamics model is established to identify unknown parameters.

[0085] The formula of the robot dynamics model is:

[0086]

[0087] Where q∈R 3 is the joint position signal; is the joint velocity signal; M(q)∈R 3×3 is the positive definite inertia matrix of the system; is the vector of Coriolis force and centrifugal force; G(q)∈R 3 is the gravitational torque; F∈R 3 is the environmental torque on the robot; τ∈R 3 To control the robot's control torque.

[0088] Step S2: Using a separate P+d controller to control the movement of each joint of the robot;

[0089] The controller of the robot joint is

[0090]

[0091] In the formula, τ i is the controller of the i-th joint, i = 1, 2, 3, k p is the proportionality coefficient, e i is the error term of the i-th joint position, k d is the damping coefficient, G i is the gravity term of the i-th joint, is the observed force at the i-th joint.

[0092] Step S3: According to the position encoder of the robot, the joint position signal of the robot is obtained. The actual joint position signal of the robot is expressed as q=[q 1 ,q 2 ,q 3 ] T ;

[0093] Step S4: using impedance control to generate joint target trajectory using external force;

[0094] The specific process is: convert the external force signal into the joint position signal, the conversion formula is:

[0095]

[0096] In the formula, From this we can derive the expected joint position q of the system d =[q d1 ,q d2 ,q d3 ] T .

[0097] Step S5: Subtract the joint position signal obtained in step S3 and step S4 from the joint target trajectory to calculate the joint position error. The joint position error is:

[0098] e=qq d (4)

[0099] Where e=[e 1 ,e 2 ,e 3 ] T .

[0100] Step S6: Design a preset time differentiator to obtain the velocity signal and acceleration signal of the robot system; the formula of the preset time differentiator is:

[0101]

[0102] Among them, the formula and is an abbreviation. α is a constant, * is an arbitrary variable, k 1 =6, k 2 =4.5, Q i As the input of the preset time differentiator, y 2 is the output speed signal of the preset time differentiator When the speed signal As the input of the preset time differentiator, y 2 is the output speed signal of the differentiator

[0103] Step S7: Based on the principles of sliding mode control and disturbance observer, a preset time force observer is designed; the process of designing the preset time force observer is as follows:

[0104] Rearrange the system (1) into a feedback system, and let x 1 =q,

[0105]

[0106] in, B=(M) -1 , F x =(M) -1 F, where F x =[F x ,F x ,F x ] T is unknown, and its first-order derivative is bounded, that is, L is a known constant, and by designing w = [w 1 ,w 2 ,w 3 ] T Come to F x Make an estimate, that is,

[0107] The force observer is designed as

[0108]

[0109] Where z = [z 1 ,z 2 ,z 3 ] T is used to estimate x 2 , T 1 ,T2 > 0, k ≥ max{L}, 0 < a < 1, and

[0110] ε=zx 2 (8)

[0111]

[0112] Step S8: Establishing the Lyapunov equation to prove the given preset time;

[0113] Prove that the given preset time T 1 ,T 2 The process is:

[0114] First, to prove that the given time T 1 ,T 2 , we need some vector derivatives. Derivative (8) and substitute (6) and (7) into the derived equations to obtain

[0115]

[0116] Based on equations (10) and (9), the force estimation error variable is obtained:

[0117] θ=wF (11)

[0118] The following equations are obtained for equations (8) and (11):

[0119]

[0120] The force observer in (7) is a sliding mode observer. The sliding mode control is divided into the arrival time and the sliding time along the sliding surface. By designing the Lyapunov function V θ Derivation of the arrival time of the force observer

[0121] V θ =||θ|| (13)

[0122] V θ Take the derivative and substitute (12) into it to obtain

[0123]

[0124] Where θ=[|θ 1 |,|θ 2 |,…,|θ n |] T ∈R n , use (|θ 1 |+|θ 2 |+...+|θ n |)≥||θ||, for Further deduction leads to the following inequality:

[0125]

[0126] After T 1 , we get the following formula:

[0127]

[0128] Secondly, by designing the Lyapunov function V ε Derivation of the sliding time of the force observer

[0129] V ε =ε T ε (17)

[0130] By taking the derivative of (17) and combining it with (16), we get the following formula:

[0131]

[0132] The inequality relationship is but The inequality relationship is:

[0133]

[0134] Discuss the above inequality in different cases

[0135] When V ε ≥1, we get because

[0136]

[0137] With T 2 >0 and The first part of the preset time meets

[0138] When V ε ≤1, as ||ε||≤1 and 1+ε 2 ≥1, we get because

[0139]

[0140] With T 2 >0, the second part of the preset time meets The preset time of the arrival phase of the system is T 2 =T x1 +T x2 ;

[0141] Finally, human and environmental forces

[0142]

[0143] It can be set within the preset time T≤T 1 +T 2 Realized within.

[0144] Step S9: Feedback the observed force to the controller design for controlling the robot, and compare the observed external force with the actual external force to verify the accuracy of the force observer.

[0145] By combining Figure 1 , Figure 3 , Figure 4 Verify the accuracy of the designed observer.

[0146] The force observer provided by the present invention is applied to Figure 2 The serial robot shown in Figure 1. Take the first joint as an example. Figure 3 It means q 1 Force observations, where the convergence of the observer can be clearly seen, Figure 4 Indicates q 1 The joint position tracking diagram. It can be seen that the observer has accurate observation capability and does not affect the trajectory tracking effect. Therefore, the design method of the preset time force observer of the robot system proposed in this paper is feasible.

Claims

1. A design method for a preset time force observer for a robot system, characterized in that: The steps include: Step S1: Based on the fact that the robot has three joints driven by independent servo motors, a robot dynamics model is established to identify unknown parameters; Step S2: Using a separate P+d controller to control the movement of each joint of the robot; Step S3: acquiring the joint position signal of the robot according to the position encoder of the robot; Step S4: using impedance control to generate joint target trajectory using external force; Step S5: subtract the joint position signal obtained in step S3 and step S4 from the joint target trajectory to calculate the joint position error; Step S6: design a preset time differentiator to obtain the velocity signal and acceleration signal of the robot system; the formula of the preset time differentiator in step S6 is: Among them, the formula and is an abbreviation. α is a constant, * is an arbitrary variable, k1=6, k2=4.5, Q i As the input of the preset time differentiator, y2 is the output speed signal of the preset time differentiator When the speed signal As the input of the preset time differentiator, y2 is the output speed signal of the differentiator Step S7: Based on the principles of sliding mode control and disturbance observer, a preset time force observer is designed; Step S8: Establishing the Lyapunov equation to prove the given preset time; Step S9: Feedback the observed force to the controller design for controlling the robot, and compare the observed external force with the actual external force to verify the accuracy of the force observer.

2. A design method for a preset time force observer for a robot system according to claim 1, characterized in that: The formula of the robot dynamics model in step S1 is: Where q∈R 3 is the joint position signal; is the joint velocity signal; M (q)∈R 3×3 is the positive definite inertia matrix of the system; is the vector of Coriolis force and centrifugal force; G(q)∈R 3 is the gravitational torque; F∈R 3 is the environmental torque on the robot; τ∈R 3 To control the robot's control torque.

3. A design method for a preset time force observer for a robot system according to claim 2, characterized in that: The controller of the robot joint in step S2 is In the formula, τ i is the controller of the i-th joint, i = 1, 2, 3, k p is the proportionality coefficient, e i is the error term of the i-th joint position, k d is the damping coefficient, G i is the gravity term of the i-th joint, is the observed force at the i-th joint.

4. A design method for a preset time force observer for a robot system according to claim 3, characterized in that: The actual joint position signal of the robot in step S3 is expressed as q=[q1,q2,q3] T .

5. A design method for a preset time force observer for a robot system according to claim 4, characterized in that: The step S4 is specifically: converting the external force signal into a joint position signal, and the conversion formula is: In the formula, From this we can derive the expected joint position q of the system d =[q d1 ,q d2 ,q d3 ] T .

6. A design method for a preset time force observer for a robot system according to claim 5, characterized in that: The joint position error in step S5 is: e=q-q d (4) Where e=[e1,e2,e3] T .

7. A design method for a preset time force observer for a robot system according to claim 1, characterized in that: The process of designing the preset time force observer in step S7 is as follows: Rearrange the system (1) into a feedback system, let x1 = q, in, B=(M) -1 , F x =(M) -1 F, where F x =[F x ,F x ,F x ] T is unknown, and its first-order derivative is bounded, that is, L is a known constant, and by designing w = [w1, w2, w3] T Come to F x Make an estimate, that is, The force observer is designed as Where z = [z1, z2, z3] T is used to estimate x2, T1, T2>0, k≥max{L}, 0<a<1, and ε=z-x2 (8) 8. A design method for a preset time force observer for a robot system according to claim 7, characterized in that: The process of proving the given preset times T1, T2 in step S8 is: Derivative (8) and then substitute (6) and (7) into the derived equation to obtain Based on equations (10) and (9), the force estimation error variable is obtained: θ=wF (11) The following equations are obtained for equations (8) and (11): The force observer in (7) is a sliding mode observer, and the sliding mode control is divided into the arrival time and the sliding time along the sliding surface; By designing the Lyapunov function V θ Derivation of the arrival time of the force observer V θ =||θ|| (13) V θ Take the derivative and then substitute (12) into it to obtain Among them, θ=[|θ1|,|θ2|,…,|θ n |] T ∈R n , using (|θ1|+|θ2|+...+|θ n |)≥||θ||, for Further deduction leads to the following inequality: After T1, the following formula is obtained: Secondly, by designing the Lyapunov function V ε Derivation of the sliding time of the force observer V ε =e T e (17) By taking the derivative of (17) and combining it with (16), we get the following formula: The inequality relationship is but The inequality relationship is: Discuss the above inequality in different cases When V ε ≥1, we get because As T2>0 and The first part of the preset time meets When V ε ≤1, as ||ε||≤1 and 1+ε 2 ≥1, we get because As T2>0, the second part of the preset time meets The preset time of the arrival phase of the system is T2 = T x1 +T x2 ; Finally, human and environmental forces It can be realized within a preset time T≤T1+T2.

Images