A torque compensation method for a double-joint flexible robot arm based on resonance control

Abstract

The application belongs to the technical field of flexible robot torque compensation, and discloses a double-joint flexible robot torque compensation method based on resonance control, which comprises the following steps: S1, dynamic modeling of a double-degree-of-freedom series elastic robot; S2, resonance control of the robot joint; and S3, joint decoupling compensation and corresponding gravity compensation. Through dynamic modeling, resonance control of the robot joint is performed based on the model, and finally joint decoupling compensation is performed, so that the problem that the movement of a certain joint of the existing multi-degree-of-freedom robot based on a series elastic driver causes the shaking of the load end of other joints during movement, thereby leading to the deterioration of position tracking effect, is solved, and the accuracy during position tracking is improved. Furthermore, the application can also realize the reduction of the shaking of the elastic robot joint during the positioning process and the overshoot of the actual angle of the connecting rod, overcome the error between the actual angle and the command angle of the connecting rod side caused by gravity, and improve the control precision.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of flexible mechanical arm torque compensation, in particular to a double-joint flexible mechanical arm torque compensation method based on resonance control. BACKGROUND

[0002] On the robot platform, the key factor to achieve dynamic decoupling control, high-precision position and force control is the accurate transmission torque. The traditional industrial robot transmission torque structure usually adopts rigid design, relies on accurate position control, and completes operation according to the established trajectory in the limited environment. Although it can realize high-precision and high-speed transmission torque, it has great safety hidden danger. With the increasing demand for physical contact between robots and external unknown environment, especially in the realization of human-robot interaction and human-robot cooperation, safety needs to be considered first. Therefore, in order to realize good position and force control while ensuring safety, a compliant driver is applied to the robot joint.

[0003] The series elastic driver is one of the most widely used compliant drivers at present, which has the characteristics of series spring or torsional spring between the driving mechanism and the load end as an elastic element, which isolates the load output and the motor inertia. It can not only reduce the damping of the contact between the driver and the environment, but also can indirectly obtain the size of the output force by measuring the displacement value when the stiffness of the elastic element is known. In addition, the energy storage property of the elastic element can meet the safety of human-robot interaction. However, the flexible structure of the series elastic driver makes the control of the structure more complex, especially in the application scene requiring high-speed motion, the response bandwidth of the controller is close to the resonance frequency of the mechanical structure of the robot itself, and the elastic element will vibrate. At this time, the output performance of the series elastic driver is destroyed.

[0004] At present, the control method for suppressing torsional vibration of the elastic element, the resonance control realizes the suppression of torsional vibration of the elastic element while compensating the disturbance caused by the internal friction, parameter uncertainty and other factors of the elastic driver by increasing the feedback of the elastic element torque on the basis of the traditional PD control and cooperating with the disturbance observer. In addition, when the multi-degree-of-freedom robot based on the series elastic driver moves, there is coupling between different joints, which is specifically manifested in that the movement of a joint will cause the shaking of the load end of other joints, resulting in the deterioration of position tracking effect. Therefore, the decoupling compensation algorithm to ensure the accuracy of multi-degree-of-freedom robot position tracking is one of the effective methods to solve the above problems. Based on this, the double-degree-of-freedom series elastic robot joint position tracking and torque compensation are of great significance to improve the accuracy of multi-degree-of-freedom robot position tracking based on the series elastic driver. SUMMARY

[0005] The application intends to provide a torque compensation method for a double-joint flexible robot based on resonance control, which comprises the following steps.

[0006] In order to achieve the above-mentioned purpose, the application provides the following technical scheme.

[0007] A torque compensation method for a double-joint flexible robot based on resonance control, comprising the following steps:

[0008] S1, dynamic modeling of a double-degree-of-freedom series elastic robot;

[0009] S2, resonance control of the robot joint, combining PD control, feedback of the torque of the flexible element and disturbance observer to realize position control of the robot joint and reduce the vibration of the robot link;

[0010] S3, joint decoupling compensation and corresponding gravity compensation, calculating the decoupling compensation input and the gravity compensation input based on the second-order and fourth-order differential forms of the double-joint flexible robot link dynamics equation to reduce the vibration of the robot joint caused by coupling and overcome the error between the actual angle and the command angle of the robot link caused by gravity.

[0011] Further, in S1, the motor side dynamics equation is:

[0012]

[0013] In the formula, τ m , J m , θ and q are the total torque of the motor, the rotational inertia of the motor side, the angle of the motor side and the angle of the load side, respectively; K f is the stiffness of the flexible element.

[0014] The directions of the positive rotation of the two joints on the load side are opposite, and the dynamics equation is:

[0015]

[0016] Wherein,

[0017]

[0018]

[0019]

[0020] wherein, J1 = m2l1d2, G1 = m1gd1 + m2gl1, G2 = m2gd2; the first term on the right side is the inertia term, the second term is the Coriolis force and centrifugal force term, and the third term is the gravity term. After the dynamic parameter identification, the dynamic parameters of the motor and load side of the two-degree-of-freedom series elastic robot are selected as shown in the following table:

[0021] Parameter Value J m1 ]]> 0.1814 [kgm2 2 ]]]> J m2 ]]> 0.4383 [kgm 2 ]]]> K f1 ]]> 126.0507 [Nm / rad] K f2 ]]> 63.0253 [Nm / rad] ​ 0.8067

[001] J1 0.1921 [CD AT J2] 0.1432 [G1] 19.3451 [G2] 6.6429

[0022] Further, in S2, the robot joint of the series elastic driver is equivalent to a "motor-spring-load" double-mass system, the resonance control is used to realize the vibration suppression of the double-mass system, and the feedback of the flexible element torque is introduced at the input end of the controlled double-mass system:

[0023]

[0024] wherein, τ reac = K(θ - q) is the flexible element torque, K r is the gain of the flexible element torque feedback; the resonance frequency Let For the two-degree-of-freedom flexible robot, J ai is the equivalent load inertia at the i-th joint;

[0025] The Coriolis force and centrifugal force terms and the gravity term are ignored in the load side dynamics equation, and is the equivalent inertia of the load side of one joint and two joints, J a1 and J a2 change with q2, K p , K v , and K r also change with q2, and the values of K p , K v , and K r at the current q2 are obtained by using the values of J0, J1, and J2 in the above table and the current q2 value of the robot.

[0026] For the two-degree-of-freedom flexible robot affected by gravity, in order to eliminate the static error caused by gravity, the gravity compensation is added, and q cmd = 0 is set, then the motor side angle For the ideal model, the output of the disturbance observer is the torque K f (θ - q) = G, and at the input end:

[0027]

[0028] G c is the gravity compensation transfer function matrix to be derived, and after arrangement, we get

[0029]

[0030] Further, in S3, the method of joint decoupling compensation and corresponding gravity compensation comprises the following steps:

[0031] A1, divide the total input of the system into input for realizing motion control and input for realizing decoupling;

[0032] A2, analyze the second-order differential state equation of the double-joint flexible robot with respect to the load-side angle and the torsion angle;

[0033] A3, further split the input for realizing motion control, and calculate the input component of each joint affected by another joint;

[0034] A4, differentiate the second-order differential state equation of step A2 again to obtain a fourth-order differential state equation, and set the sum of the coupling-related terms in the fourth-order differential state equation to 0 to obtain the decoupling compensation input of each joint of the double-joint flexible robot;

[0035] A5, for the double-joint flexible robot in the vertical state, after obtaining the decoupling compensation input, add the gravity compensation to eliminate the load-side angle static error caused by gravity.

[0036] Further, in A1, take the reference acceleration as the total input of the robot joint system, To realize the control of each joint, the control input u 11ref , u 22ref is added with a compensation term u 1Cref , u 2Cref to offset the disturbance, so that the total input u 1ref of the first joint = u 11ref + u 1Cref , and the total input u 2ref of the second joint = u 22ref + u 2Cref .

[0037] Further, in A2, the obtained second-order differential state equation is:

[0038]

[0039] In the formula, δ1 = θ1 - q1, δ2 = θ2 - q2.

[0040] Furthermore, in A3, the method for calculating the input components of each joint affected by another joint is as follows: control input u 11ref with u 22ref Each is affected by the torsion angle δ of another joint, where u 11ref =u 11Sref +u 11Dref u 22ref =u 22Sref +u 22Dref ;u 11Sref u 22Sref wei is the input calculated through the state of this joint, u 11Dref u 22Dref The input is calculated based on the state of another joint; since θ1 = q1 + δ1, θ2 = q2 + δ2, θ1 is affected by δ2, and θ2 is affected by δ1, q is ignored. cmd And q, let Substituting into the resonance control expression, we can calculate u. 11Dref with u 22Dref .

[0041] Furthermore, in A4, the fourth differential equation of state is obtained as follows:

[0042]

[0043] In the formula, the third and fourth terms on the right side of the equation are the coupling-related terms to be compensated. Let the sum of the coupling-related terms be 0, and then... Substituting the relevant equations, we get:

[0044]

[0045] In the formula,

[0046] Furthermore, in A5, the method to eliminate the static error of the load-side angle caused by gravity is as follows: set q cmd =0, motor side angle For the ideal model, the output of the disturbance observer is torque K. f (θ-q)=G, at the input:

[0047]

[0048] In the formula, G c The gravity compensation transfer function matrix to be derived is set to 2×2, u c The input is a 2×1 decoupling compensation input. The total input of the two-DOF flexible robot joint system with the added disturbance observer is:

[0049]

[0050] The beneficial effects of the technical solutions are:

[0051] 1. The double-joint flexible robot torque compensation method based on resonance control provided by the application can accurately describe the kinematics and dynamics characteristics of the robot by modeling the dynamics of the double-degree-of-freedom series elastic robot, thereby providing an accurate model basis for subsequent compensation control.

[0052] 2. The double-joint flexible robot torque compensation method based on resonance control provided by the application can realize position control of the robot joint and reduce the vibration of the robot connecting rod by comprehensively using technical means such as PD control, feedback of flexible element torque, and disturbance observer, and can effectively suppress the vibration problem caused by the controller response bandwidth approaching the resonance frequency, thereby improving the output performance of the robot.

[0053] 3. The double-joint flexible robot torque compensation method based on resonance control provided by the application can calculate the decoupling compensation input and gravity compensation input based on the second-order and fourth-order differential forms of the connecting rod side dynamics equation of the double-joint flexible robot, thereby reducing the jitter of the robot joint caused by coupling and overcoming the error between the actual angle and the command angle of the connecting rod side caused by gravity, and the robot can more accurately realize the motion of the specified position and attitude through decoupling compensation and gravity compensation.

[0054] In summary, the double-joint flexible robot torque compensation method based on resonance control provided by the application solves the problem that the movement of a certain joint of the existing multi-degree-of-freedom robot based on a series elastic driver causes the shaking of the load end of other joints during movement, thereby improving the accuracy of position tracking. In addition, it can also reduce the jitter of the elastic robot joint during positioning and the overshoot of the actual angle of the connecting rod, and overcome the error between the actual angle and the command angle of the connecting rod side caused by gravity, thereby improving the control accuracy. It brings substantial improvement to the precise operation in the field of robot application and the safety of human-computer interaction. BRIEF DESCRIPTION OF DRAWINGS

[0055] Figure 1 In step S1 of the embodiment provided by the application, the double-degree-of-freedom series elastic robot structure diagram is obtained.

[0056] Figure 2 In step S2 of the embodiment provided by the application, J a1 and J a2 Trend graph changing with q2;

[0057] Figure 3 In step S2 of the embodiment provided by the application, the resonance control block diagram obtained is as follows:

[0058] Figure 4The MATLAB simulation obtained under the resonance control of the gravity compensation in step S2 of the embodiment provided by the present application Figure 1 ;

[0059] Figure 5 The MATLAB simulation obtained under the resonance control of the gravity compensation in step S2 of the embodiment provided by the present application Figure 2 ;

[0060] Figure 6 The total input block diagram of the dual-freedom flexible robot joint system with the disturbance observer added in step S3 of the embodiment provided by the present application

[0061] Figure 7 The simulation obtained under the resonance control of the decoupling compensation and the gravity compensation in step S3 of the embodiment provided by the present application Figure 1 ;

[0062] Figure 8 The simulation obtained under the resonance control of the decoupling compensation and the gravity compensation in step S3 of the embodiment provided by the present application Figure 2 . DETAILED DESCRIPTION

[0063] The present application will be further described in detail below in combination with the drawings and embodiments:

[0064] A torque compensation method for a dual-joint flexible robot based on resonance control, comprising the following steps:

[0065] S1, dynamically modeling a dual-freedom series elastic robot; ignoring disturbance terms such as friction and external force, the structure block diagram of the dual-freedom series elastic robot is shown in Parameter , and the motor-side dynamic equation is as follows:

[0066]

[0067] In the formula, τ m , J m , θ and q are the total torque of the motor, the rotational inertia of the motor side, the angle of the motor side and the angle of the load side; K f is the stiffness of the flexible element;

[0068] The dynamics equation of the load side is as follows because the directions of the two joints are opposite:

[0069]

[0070] Wherein,

[0071]

[0072]

[0073]

[0074] wherein, J1 = m2l1d2, G1 = m1gd1 + m2gl1, G2 = m2gd2; the first term on the right side is called the inertia term, the second term is called the Coriolis force and centrifugal force term, and the third term is the gravity term. After the identification of the kinetic parameters, the following table shows the kinetic parameters of the motor and the load side of the double-degree-of-freedom series elastic robot:

[0075] Value 126.0507 [Nm / rad] J m1 ]]> 0.1814 [kgm2 2 ]]]> J m2 ]]> 0.4383 [kgm 2 ]]]> K f1 ]]> 63.0253 [Nm / rad] K f2 ]]> Figure 2 ​ 0.8067 ​ 0.1921

[0077] J2 0.1432 [G1] 19.3451 [G2] 6.6429

[0076] S2, resonance control of the robot joint is performed, and the specific steps are as follows:

[0077] For a robot joint based on a series elastic driver, it is equivalent to a "motor-spring-load" double-mass system, and resonance control is used to realize vibration suppression of the double-mass system. The principle is to introduce the feedback of the flexible element torque at the input end of the controlled double-mass system:

[0078]

[0079] wherein, τ reac = K(θ - q) is the flexible element torque, K r is the gain of the flexible element torque feedback; resonance control of the robot joint requires determination of the values of K p , K v , and K r ; the resonance frequency is introduced, and At this time, the transfer function of the double-mass system will have four identical negative real roots;

[0080] For a double-degree-of-freedom flexible robot, J ai is the equivalent load inertia at the i-th joint; the Coriolis force and centrifugal force terms and the gravity term are ignored for the load side dynamics equation, and is the equivalent inertia of the load side of a joint and two joints, and it is easy to know that J a1 and J a2 change with q2, and the specific values of J0, J1, and J2 in the above table are substituted and q2 ∈ [0, π] is taken, it is easy to know that J a1 ∈ [0.4058, 0.6635], J a2 ∈ [0.0488, 0.1432], and the trend of change with q2 is shown in Figure 3 Therefore, the gains K p , K v , and K rJ0, J1, J2 in the table above and the current q2 value of the robot are substituted into the equation to obtain K under the current q2 p , K v , K r ;

[0081] For the two-degree-of-freedom flexible robot affected by gravity, gravity compensation should also be added to eliminate the static error caused by gravity. Assuming q cmd = 0, it is easy to know that the motor-side angle For the ideal model, the output of the disturbance observer is the torque K f (θ-q) = G, so at the input end, there is:

[0082]

[0083] In the formula, G c is the gravity compensation transfer function matrix to be derived, and the above formula is finally derived as:

[0084]

[0085] The resonance control block diagram is shown in Figure 4 .

[0086] In the embodiment, MATLAB software is used for simulation verification. In the simulation, the parameters in the above table are substituted, and the step input is set The simulation results obtained under the resonance control combined with gravity compensation are shown in Figure 5 and Figure 6 The actual angles of the two joints on the load side have undergone a process of damped oscillation and are finally stabilized at the target angle.

[0087] S3, decoupling compensation is derived from resonance control, and corresponding gravity compensation is derived based on resonance control with decoupling compensation. The effect of decoupling compensation is compared under the premise of no static error. The specific steps are as follows:

[0088] A1, the total input of the system is divided into two parts: the input for realizing motion control and the input for realizing decoupling. The reference acceleration is taken as the total input of the robot joint system, to realize the control of the two joints respectively, their control inputs u 11ref , u 22ref are added with compensation terms u 1Cref , u 2Cref to offset the disturbance, so the total input u 1ref of the first joint = u 11ref + u 1Cref , and the total input u 2ref of the second joint = u 22ref + u 2Cref ;

[0089] A2, analyze the second-order differential state equation of the dual-joint flexible robot arm with respect to the load-side angle and the torsion angle:

[0090] Considering the dual-degree-of-freedom flexible robot joint system with disturbance observer, there are the following state equation groups:

[0091]

[0092] In the formula, δ1 = θ1 - q1, δ2 = θ2 - q2;

[0093] A3, control input u 11ref and u 22ref are affected by the torsion angle δ of the other joint, so they can be further split, where u 11ref = u 11Sref + u 11Dref , u 22ref = u 22Sref + u 22Dref ; u 11Sref , u 22Sref are inputs calculated through the state of the joint, u 11Dref , u 22Dref are inputs calculated through the state of the other joint; since θ1 = q1 + δ1, θ2 = q2 + δ2, θ1 is affected by δ2 and θ2 is affected by δ1 according to the state equation group, ignoring q cmd and q , and let 11Dref u 22Dref ;

[0094] A4, differentiate the second-order differential state equation of step A2 again to obtain the fourth-order differential state equation:

[0095]

[0096] In the formula, the third and fourth terms on the right side of the equation are the coupling-related terms to be compensated; let the sum of the coupling-related terms be 0 and substitute the equation related to in the state equation group, and finally obtain:

[0097]

[0098] In the formula,

[0099] A5: Make the gravity G(q) have no effect on the system output q, assuming q cmd = 0, it can be known that the motor-side angle For the ideal model, the output of the disturbance observer is torque K f (θ-q) = G, then at the input end, there are:

[0100]

[0101] In the formula, G c is the gravity compensation transfer function matrix to be derived, set to 2x2, u c is a 2x1 decoupling compensation input, so the total input of the two-degree-of-freedom flexible robot joint system with the disturbance observer is The block diagram is shown in Figure 7 .

[0102] Finally, the above compensation method is simulated and verified in this embodiment. In the simulation, the parameters in the above table are substituted, and the step input is set The simulation results obtained under the resonant control combined with decoupling compensation and gravity compensation are shown in Figure 8 and ​ The actual angle of the load side of the two joints no longer attenuates and oscillates, and finally stabilizes at the target angle; wherein the maximum overshoot of the actual angle of the load side of the first joint is reduced from 30.85% to 6.1%, and the actual angle of the load side of the first joint no longer overshoots.

[0103] The above is only an embodiment of the present application, and well-known specific technical solutions or characteristics in the scheme are not described in detail. It should be noted that for those skilled in the art, without departing from the technical solutions of the present application, a number of modifications and improvements can be made, which should also be considered as the protection scope of the present application, and these will not affect the effect and practicality of the present application. The protection scope of the present application should be subject to the content of its claims, and the specific implementation mode and the like in the specification can be used to explain the content of the claims.

Claims

1. A method for torque compensation of a dual-joint flexible robot arm based on resonance control, characterized by, The method comprises the following steps: S1, performing dynamic modeling on the double-joint flexible manipulator; S2, performing resonance control on the joints of the double-joint flexible manipulator, combining PD control, feedback of torque of the flexible element and disturbance observer to realize position control on the joints of the double-joint flexible manipulator and reduce vibration of the connecting rod of the double-joint flexible manipulator; S3, performing joint decoupling compensation and corresponding gravity compensation, calculating decoupling compensation input and gravity compensation input based on second-order and fourth-order differential forms of the dynamic equation of the connecting rod of the double-joint flexible manipulator to realize reduction of shaking of the joints of the double-joint flexible manipulator caused by coupling and overcome error between the actual angle and the command angle of the connecting rod caused by gravity.

2. The method according to claim 1, wherein, In S1, in the process of dynamic modeling, the motor-side dynamic equation is: ； wherein , , are the total motor torque, the motor-side moment of inertia, the motor-side angle and the load-side angle, respectively; is the stiffness of the flexible element; The two joints of the load side rotate in opposite directions, and the dynamic equation is: The first term on the right side of the equation is the inertial term, the second term is the Coriolis and centrifugal force terms, and the third term is the gravitational term. In S2, the double-joint flexible manipulator with a series elastic driver is equivalent to a "motor-spring-load" double-mass system, resonance control is used to realize vibration suppression of the double-mass system, and feedback of torque of the flexible element is introduced at the input end of the controlled double-mass system: ； ； ； wherein = 0.8067, = 0.1921, = 0.1432, = 19.3451, = 6.6429.

3. The method according to claim 2, wherein, In S3, the method of joint decoupling compensation and corresponding gravity compensation comprises the following steps: ； wherein is the flexible element torque, is the gain of the flexible element torque feedback; resonance frequency , , let , for a dual-joint flexible manipulator, is the equivalent load inertia at the th joint; The Coriolis force and centrifugal force terms and the gravity term are neglected in the load side dynamics equation, and the inertia matrix , 、 is the equivalent inertia of the one-joint and two-joint load sides, and vary with , 、 、 also vary with , the values of 、 、 and the current of the two-joint flexible manipulator are used to obtain the values of , 、 、 under the current For the double-joint flexible robot arm affected by gravity, in order to eliminate the static error caused by gravity, gravity compensation is added, and the setting Then the motor side angle ; For the ideal model, the output of the disturbance observer is torque At the input, there is: ； wherein is the gravity compensation transfer function matrix to be derived, and after rearrangement, we have 。 4. The method according to claim 3, wherein, A1, dividing the total input of the system into input for realizing motion control and input for realizing decoupling; A2, analyzing the second-order differential state equation of the double-joint flexible manipulator with respect to the load-side angle, the motor-side angle and the difference between the load-side angle and the motor-side angle; A3, further splitting the input for realizing motion control to calculate the input component of each joint affected by another joint; A4, differentiating the second-order differential state equation of step A2 again to obtain a fourth-order differential state equation, and setting the sum of the coupling-related terms in the fourth-order differential state equation to 0 to obtain the decoupling compensation input of each joint of the double-joint flexible manipulator; A5, for the double-joint flexible manipulator in the vertical state, adding gravity compensation to the decoupling compensation input to eliminate the static error of the load-side angle caused by gravity. In A2, the obtained second-order differential state equation is:

5. The method of claim 4, wherein, In A1, the reference acceleration is the total input of the double-joint flexible robot arm joint system, , ; to achieve the respective control of the two joints, their control inputs 、 are added with compensation terms 、 to offset the interference, so the total input of the 1st joint = + , and the total input of the 2nd joint = + .

6. The method of claim 4, wherein, In A4, the obtained fourth-order differential state equation is: ； In the formula, , , , , , .

7. The method according to claim 6, wherein, In A3, the method of calculating the input components of each joint affected by another joint is: = control input = control input = control input = control input = control input = control input = control input = control input = control input = control input = control input = control input = control input = control input = control input = control input = control input = control input = control input = control input = control input = control input = control input = control input 8. The method according to claim 7, wherein, ​ ； In the formula, the third and fourth terms on the right side of the equal sign are coupling-related terms to be compensated, the sum of the coupling-related terms is set to 0, and the equations related to the state equation group are substituted to obtain: , ​ ； In the formulae, , , .

9. The method of claim 4, wherein, In A5, the method to eliminate the load side angle static error caused by gravity is to set , the motor side angle ; for the ideal model, the output of the disturbance observer is torque , at the input end ； wherein is the gravity compensation transfer function matrix to be derived, set to 2x2, is the 2x1 decoupled compensation input, the total input to the dual-joint flexible manipulator joint system with disturbance observer is .

Images