Robot position and speed protection method based on zero-force teaching

Abstract

The invention relates to the technical field of industrial robot teaching, in particular to a robot position and speed protection method based on zero-force teaching, which comprises the following steps: (1) establishing a corrected friction compensation model based on a dynamic friction identification model; (2) estimating joint external torque by using a momentum observer according to the robot dynamics identification model and the joint feedback position, speed and torque; deriving a joint reference position and speed according to an admittance control law, and calculating a joint dragging compensation external torque in combination with a PD control law; (3) according to the joint movement range and the impedance position protection width, a joint impedance position protection limiting interval is generated, and then joint impedance position protection reaction force is generated; and (4) the joint dragging starting static friction is inhibited by using the periodic square wave joint dithering moment, and the amplitude and period of the dithering moment are adjustable. The method can effectively solve the problem that the zero-force teaching in the torque mode is difficult to realize the soft limit protection of the dragging position, the dragging speed limitation and the dragging starting static friction suppression.

Description

Technical Field

[0001] The invention relates to the technical field of industrial robot teaching, and in particular to a robot position and speed protection method based on zero-force teaching. Background Art

[0002] At present, the dragging teaching methods of industrial robots are mainly divided into two categories: one is the dragging teaching based on the end six-dimensional force / torque sensor; the other is the zero-force teaching based on dynamic model compensation. The former uses the end force / torque sensor to realize the accurate perception of the external torque, and converts the end external torque into the posture offset through the admittance control law to realize the dragging teaching. This solution has the advantages of labor saving, high dragging accuracy, no need to switch the drive mode, and the end dragging is more in line with the user's dragging habits. However, there are still problems such as the high cost of force sensors, unsuitability for large-scale use, and unstable contact of dragging teaching.

[0003] Zero-force teaching is based on dynamic modeling and identification. By compensating for the model's gravity and friction, the robot joints are kept in a "zero-force" equilibrium state when stationary, and users can drag the robot joints to move more easily. It has the advantages of low hardware cost, simple implementation, and good contact stability.

[0004] However, the zero-force teaching technology requires the driver to work in torque mode, calculate the zero-force drag compensation torque through the model, and send a torque command in each interpolation cycle to achieve zero-force drag. Since the driver works in torque mode, the servo only has the current loop for closed-loop control, and the speed loop and position loop are in open-loop control. Therefore, it is difficult to directly limit the drag position and drag speed through the servo. Existing technologies generally limit the drag position and drag speed by applying reverse torque. Therefore, this technology still has the following shortcomings:

[0005] 1. By applying reverse torque to limit the maximum drag speed, it is difficult to achieve the purpose of constant drag after reaching the maximum drag speed, but it will cause drag jitter problems;

[0006] 2. The method of applying reverse torque is used, which makes the limit of dragging position and dragging speed inaccurate and easy to exceed the limit;

[0007] 3. Due to the existence of static friction in the joints, dragging and starting are difficult.

[0008] Therefore, a new technical solution is urgently needed to solve the above technical problems. Summary of the invention

[0009] The purpose of the present invention is to overcome the shortcomings of the above-mentioned prior art and provide a robot position and speed protection method based on zero-force teaching, so as to solve the technical problem that when the driver is in torque mode, zero-force teaching is difficult to achieve soft limit protection of drag position, drag speed limitation and static friction suppression at drag start in the prior art.

[0010] The above objectives are achieved through the following technical solutions:

[0011] A robot position and speed protection method based on zero-force teaching comprises the following steps:

[0012] Step (1) Based on the dynamic friction identification model, a modified friction compensation model is established as follows:

[0013]

[0014] Where α is the compensation coefficient of Coulomb friction term, β is the compensation coefficient of viscous friction term, is the Coulomb friction term, is the viscous friction term; where:

[0015]

[0016] In the formula, is the joint velocity, F c is the Coulomb friction coefficient, F v is the viscous friction coefficient, It is the lower speed threshold of Coulomb friction compensation. When the absolute speed value is less than this value, Coulomb friction compensation will not be performed. The upper speed threshold of Coulomb friction compensation. When the absolute speed value is greater than this value, the Coulomb friction compensation is a constant value.

[0017] By adjusting the Coulomb friction compensation proportional coefficient α, the viscous friction compensation proportional coefficient β, and the Coulomb friction compensation lower speed threshold and Coulomb friction compensation upper speed threshold Adjust the drag feel;

[0018] Step (2) using a momentum observer to estimate the joint external torque according to the robot dynamics identification model and the joint feedback position, velocity and torque;

[0019] The joint reference position and velocity are derived according to the admittance control law.

[0020] Combined with the PD control law, the external torque of joint drag compensation is calculated to achieve closed-loop control of the drag speed;

[0021] Step (3) generating a joint impedance position protection limit interval according to the joint motion range and the joint impedance position protection width, and further generating a joint impedance position protection reaction force by judging whether the robot joint position enters the limit interval;

[0022] Step (4) uses a periodic square wave joint jitter torque to suppress the static friction of the joint drag start, and the amplitude and period of the jitter torque are adjustable.

[0023] Furthermore, in step (1), based on the dynamic friction identification model, a modified friction compensation model is established, including:

[0024] Use Newton-Euler recursion method or Lagrangian dynamics equation method to establish the robot inverse dynamics model;

[0025] The minimum parameter set of the robot is identified through linearization of the dynamic model and weighted least squares method;

[0026] The gravity and friction compensation force is calculated based on the model as follows:

[0027]

[0028] Where q is the joint position, is the gravity term, is the friction term.

[0029] Furthermore, the estimation of the joint external torque based on the momentum observer in step (2) is specifically:

[0030] The momentum observer is designed to estimate the external torque of the joint as follows:

[0031]

[0032] In the formula, is momentum, is the inertia matrix of the kinetic model, is the Coriolis force term, r(k) is the estimated external torque of the joint at time k, r(k-1) is the estimated external torque of the joint at time k-1, p(0) is the initial momentum, which is generally defaulted to zero, and K is the momentum observer gain.

[0033] Furthermore, the joint reference position is derived according to the admittance control law in step (2), specifically:

[0034] Admittance control is used to generate the joint reference position q ref , as follows:

[0035]

[0036] In the formula, is the joint reference acceleration, To estimate the external torque of the joint, the momentum observer is used for estimation. M is the mass matrix. In this embodiment, M is proportional to is the inertia matrix of the dynamic model, D is the damping matrix, which is generally a positive definite diagonal matrix;

[0037] Then the joint reference position q ref Through Obtained by two point calculations.

[0038] Furthermore, the joint drag compensation external torque is calculated in combination with the PD control law in step (2) to achieve closed-loop control of the drag speed, specifically:

[0039] Select PD control law to generate joint drag compensation external torque τ PD , as follows:

[0040]

[0041] In the formula, K P is the proportional term coefficient of the PD control law, K D is the differential term coefficient of the PD control law, It is obtained by integrating the admittance control law, To provide real-time feedback of joint positions and joint velocities;

[0042] Set the absolute value limit V of the maximum drag speed of the joint limt , limit the calculated joint reference velocity

[0043] when When And by adjusting the proportional term coefficient K of the PD control law p , PD control law differential term coefficient K D , to achieve closed-loop control of maximum drag speed.

[0044] Furthermore, the step (3) is specifically as follows:

[0045] Set the joint impedance position protection width q width , and q width >0; According to the robot joint motion range (q min ,q max ), calculate the impedance position protection critical position q dmax =q max -q width ,q dmin =q min +q width , and generate the impedance position protection limit interval (q min ,q dmin ) and (q dmax ,qmax );

[0046] When it is determined that the robot joint position enters the impedance position protection limit interval, the impedance position protection reaction force is generated as follows:

[0047] τ imp =K(q d -q)

[0048] Where K is the impedance position protection stiffness parameter, and q is the joint feedback position;

[0049] When q>q dmax When d =q dmax ;

[0050] When q<q dmin When d =q dmin .

[0051] Furthermore, the step (3) also includes generating an anti-fall joint impedance protection force according to the actual feedback height position of the terminal moving platform and the critical falling height.

[0052] Furthermore, the anti-fall joint impedance protection force is generated according to the actual feedback height position of the terminal moving platform and the critical falling height, specifically, the joint impedance position reaction force is corrected, and the formula is as follows:

[0053] τ imp =K(q d -q)+τ imp_z

[0054] The impedance protection force of the anti-fall joint is:

[0055] τ imp_z =K z (zz limt )

[0056] Where z is the feedback height of the end moving platform, z limt It is the critical height for the end moving platform to prevent falling in height direction.

[0057] Furthermore, the step (4) is specifically as follows:

[0058] When the absolute value of the robot joint speed is less than the speed threshold When , a periodic square wave jitter signal is applied to suppress the static friction of the joint;

[0059] When the absolute value of the robot joint velocity is greater than When , the joint jitter torque is turned off, as shown in the following formula:

[0060]

[0061] Where Amp = γ·F c is the periodic jitter torque amplitude, γ is the proportionality coefficient, F c is the Coulomb friction coefficient, t is the current interpolation time, and T is the jitter torque period.

[0062] Furthermore, in the torque control mode, the robot zero-force teaching gives a real-time torque τ cmd , as follows:

[0063] τ cmd =τ cmp +τ PD +τ imp +τ dither

[0064] In the formula, τ cmp is the gravity and friction compensation torque, τ PD Compensate external torque for PD closed-loop control, τ imp is the impedance position protection compensation torque, τ dither Static friction suppresses the periodic jitter torque.

[0065] The robot position and speed protection method based on zero-force teaching provided by the present invention is applicable to the position and speed protection of serial / parallel robots, and can adjust the position and speed control gain of the PD controller online. It has the advantages of good force control compliance and high contact stability in the torque control mode, and solves the position and speed protection problems in the zero-force teaching torque mode. Compared with the existing zero-force teaching speed limit protection technology, the speed fluctuation is small after the dragging reaches the maximum limit speed, and the dragging process is smoother, realizing speed closed-loop control. Among them, the position impedance protection method is applicable to serial / parallel robots at the same time, filling the gap of zero-force teaching position limit protection of parallel robots; the method of periodic square wave jitter torque to suppress joint static friction is easy to implement and improves the feel of drag start. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] Figure 1 This is a flow chart of a robot position and speed protection method based on zero-force teaching according to the present invention;

[0067] Figure 2 It is a principle block diagram of a robot position and speed protection method based on zero-force teaching according to the present invention;

[0068] Figure 3 The present invention provides a block diagram of the control principle of a momentum observer in a robot position and speed protection method based on zero-force teaching. DETAILED DESCRIPTION

[0069] The present invention is further described in detail below based on the accompanying drawings and embodiments. The described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. 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.

[0070] This scheme provides a robot position and speed protection method based on zero-force teaching. Aiming at the problem that the dragging speed is difficult to limit in the driver torque mode, a PD closed-loop control strategy based on momentum observer external force estimation is proposed to realize speed closed-loop control. Aiming at the problem that the dragging position limit is inaccurate and easy to exceed the limit, the concept of impedance position protection limit interval is introduced, the impedance position protection width is set, the impedance position protection reaction force is generated, and the position soft limit protection is realized. Aiming at the problem that dragging is difficult to start, a periodic square wave joint jitter torque static friction suppression strategy is proposed.

[0071] like Figure 1 and 2 As shown, this solution is applicable to position / speed protection of serial / parallel robots and includes the following steps:

[0072] Step (1) establishes a modified friction compensation model based on the dynamic friction identification model, including:

[0073] Use Newton-Euler recursion method or Lagrangian dynamics equation method to establish the robot inverse dynamics model;

[0074] The minimum parameter set of the robot is identified through linearization of the dynamic model and weighted least squares method;

[0075] The gravity and friction compensation force is calculated based on the model as follows:

[0076]

[0077] Where q is the joint position, is the gravity term, is the friction term;

[0078] Then we have:

[0079]

[0080] Where α is the compensation coefficient of Coulomb friction term, β is the compensation coefficient of viscous friction term, is the Coulomb friction term, is the viscous friction term; where:

[0081]

[0082] In the formula, is the joint velocity, Fc is the Coulomb friction coefficient, F v is the viscous friction coefficient, It is the lower speed threshold of Coulomb friction compensation. When the absolute speed value is less than this value, Coulomb friction compensation will not be performed. The upper speed threshold of Coulomb friction compensation. When the absolute speed value is greater than this value, the Coulomb friction compensation is a constant value.

[0083] By adjusting the Coulomb friction compensation proportional coefficient α, the viscous friction compensation proportional coefficient β, and the Coulomb friction compensation lower speed threshold and Coulomb friction compensation upper speed threshold Adjust the drag feel;

[0084] Step (2) estimates the joint external torque based on the momentum observer according to the joint feedback position, velocity and torque in the robot dynamics identification model. In this embodiment, a PD closed-loop control framework is introduced in the zero-force teaching mode to achieve drag speed limitation.

[0085] like Figure 3 As shown, specifically including:

[0086] Step (2-1) Design a momentum observer to estimate the external torque of the joint, as follows:

[0087]

[0088] In the formula, is momentum, is the inertia matrix of the kinetic model, is the Coriolis force term, r(k) is the estimated external torque of the joint at time k, r(k-1) is the estimated external torque of the joint at time k-1, p(0) is the initial momentum, which is generally defaulted to zero, and K is the momentum observer gain.

[0089] Step (2-2) derives the joint reference position and velocity according to the admittance control law. Specifically, the joint reference position q is generated by adopting the admittance control ref , as follows:

[0090]

[0091] In the formula, is the joint reference acceleration, Estimate the external torque for the joint using momentum observer, M is the mass matrix, and D is the damping matrix;

[0092] Then the joint reference position q ref Through Obtained by two point calculations.

[0093] Step (2-3) combines the PD control law to calculate the joint drag compensation external torque to achieve closed-loop control of the drag speed, specifically:

[0094] Select PD control law to generate joint drag compensation external torque τ PD , as follows:

[0095]

[0096] In the formula, K P is the proportional term coefficient of the PD control law, K D is the differential term coefficient of the PD control law, It is obtained by integrating the admittance control law, To provide real-time feedback of joint positions and joint velocities;

[0097] Set the absolute value limit V of the maximum drag speed of the joint limt , limit the calculated joint reference velocity

[0098] when When And by adjusting the proportional term coefficient K of the PD control law p , PD control law differential term coefficient K D , to achieve closed-loop control of maximum drag speed.

[0099] Step (3) generates a joint impedance position protection limit interval according to the joint motion range and the joint impedance position protection width, and further generates a joint impedance position protection reaction force by judging whether the robot joint position enters the limit interval; specifically,

[0100] Set the joint impedance position protection width q width , and q width >0; According to the robot joint motion range (q min ,q max ), calculate the impedance position protection critical position q dmax =q max -q width ,q dmin =q min +q width , and generate the impedance position protection limit interval (q min ,q dmin ) and (q dmax ,q max );

[0101] When it is determined that the robot joint position enters the impedance position protection limit interval, the impedance position protection reaction force is generated as follows:

[0102] τ imp =K(qd -q)

[0103] Where K is the impedance position protection stiffness parameter, and q is the joint feedback position;

[0104] When q>q dmax When d =q dmax ;

[0105] When q<q dmin When d =q dmin .

[0106] This embodiment aims at the problem of joint drop caused by inaccurate joint gravity compensation of the parallel robot during dragging. In step (3), the anti-drop joint impedance protection force is generated according to the actual feedback height position of the terminal moving platform and the critical drop height. Specifically, the reaction force of the joint impedance position is corrected. The formula is as follows:

[0107] τ imp =K(q d -q)+τ imp_z

[0108] The impedance protection force of the anti-fall joint is:

[0109] τ imp_z =K z (zz limt )

[0110] Where z is the feedback height of the end moving platform, z limt It is the critical height for the end moving platform to prevent falling in height direction.

[0111] Step (4) uses a periodic square wave joint jitter torque to suppress the static friction of the joint drag start, and the amplitude and period of the jitter torque are adjustable.

[0112] Specifically, when the absolute value of the robot joint speed is less than the speed threshold When , a periodic square wave jitter signal is applied to suppress the static friction of the joint;

[0113] When the absolute value of the robot joint velocity is greater than When , the joint jitter torque is turned off, as shown in the following formula:

[0114]

[0115] Where Amp = γ·F c is the periodic jitter torque amplitude, γ is the proportionality coefficient, F c is the Coulomb friction coefficient, t is the current interpolation time, and T is the jitter torque period.

[0116] In the torque control mode, the robot is taught with zero force and given torque τ in real time. cmd , as follows:

[0117] τ cmd =τ cmp +τ PD +τ imp +τ dither

[0118] In the formula, τ cmp is the gravity and friction compensation torque, τ PD Compensate external torque for PD closed-loop control, τ imp is the impedance position protection compensation torque, τ dither Static friction suppresses the periodic jitter torque.

[0119] The above description is only for illustrating the implementation mode of the present invention and is not intended to limit the present invention. For those skilled in the art, any modification, equivalent substitution, improvement, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A robot position and speed protection method based on zero-force teaching, characterized in that: The steps include: Step (1) Based on the dynamic friction identification model, a modified friction compensation model is established as follows: Where α is the compensation coefficient of Coulomb friction term, β is the compensation coefficient of viscous friction term, is the Coulomb friction term, is the viscous friction term; where: In the formula, is the joint velocity, F c is the Coulomb friction coefficient, F v is the viscous friction coefficient, It is the lower speed threshold of Coulomb friction compensation. When the absolute speed value is less than this value, Coulomb friction compensation will not be performed. The upper speed threshold of Coulomb friction compensation. When the absolute speed value is greater than this value, the Coulomb friction compensation is a constant value. By adjusting the Coulomb friction compensation proportional coefficient α, the viscous friction compensation proportional coefficient β, and the Coulomb friction compensation lower speed threshold and Coulomb friction compensation upper speed threshold Adjust the drag feel; Step (2) using a momentum observer to estimate the joint external torque according to the robot dynamics identification model and the joint feedback position, velocity and torque; The joint reference position and velocity are derived according to the admittance control law. Combined with the PD control law, the external torque of joint drag compensation is calculated to achieve closed-loop control of the drag speed; Step (3) generating a joint impedance position protection limit interval according to the joint motion range and the joint impedance position protection width, and further generating a joint impedance position protection reaction force by judging whether the robot joint position enters the limit interval; Step (4) uses a periodic square wave joint jitter torque to suppress the static friction of the joint drag start, and the amplitude and period of the jitter torque are adjustable.

2. A robot position and speed protection method based on zero-force teaching according to claim 1, characterized in that: In the step (1), a modified friction compensation model is established based on the dynamic friction identification model, including: Use Newton-Euler recursion method or Lagrangian dynamics equation method to establish the robot inverse dynamics model; The minimum parameter set of the robot is identified through linearization of the dynamic model and weighted least squares method; The gravity and friction compensation force is calculated based on the model as follows: Where q is the joint position, is the gravity term, is the friction term.

3. A robot position and speed protection method based on zero-force teaching according to claim 1, characterized in that: The estimation of the joint external torque based on the momentum observer in step (2) is specifically: The momentum observer is designed to estimate the external torque of the joint as follows: In the formula, is momentum, is the inertia matrix of the kinetic model, is the Coriolis force term, r(k) is the estimated external torque of the joint at time k, r(k-1) is the estimated external torque of the joint at time k-1, p(0) is the initial momentum, which is generally defaulted to zero, and K is the momentum observer gain.

4. A robot position and speed protection method based on zero-force teaching according to claim 3, characterized in that: The joint reference position is derived according to the admittance control law described in step (2), specifically: Admittance control is used to generate the joint reference position q ref , as follows: In the formula, is the joint reference acceleration, Estimate the external torque for the joint using momentum observer, M is the mass matrix, and D is the damping matrix; Then the joint reference position q ref Through Obtained by two point calculations.

5. A robot position and speed protection method based on zero-force teaching according to claim 4, characterized in that: The method described in step (2) is to calculate the joint drag compensation external torque in combination with the PD control law to achieve closed-loop control of the drag speed, specifically: Select PD control law to generate joint drag compensation external torque τ PD , as follows: In the formula, K P is the proportional term coefficient of the PD control law, K D is the differential term coefficient of the PD control law, q ref , It is obtained by integrating the admittance control law, q, To provide real-time feedback of joint positions and joint velocities; Set the absolute value limit V of the maximum drag speed of the joint limt , limit the calculated joint reference velocity when When And by adjusting the proportional term coefficient K of the PD control law p , PD control law differential term coefficient K D , to achieve closed-loop control of maximum drag speed.

6. A robot position and speed protection method based on zero-force teaching according to claim 1, characterized in that: The step (3) is specifically: Set the joint impedance position protection width q width , and q width >0; According to the robot joint motion range (q min ,q max ), calculate the impedance position protection critical position q dmax =q max -q width ,q dmin =q min +q width , and generate the impedance position protection limit interval (q min ,q dmin ) and (q dmax ,q max ); When it is determined that the robot joint position enters the impedance position protection limit interval, the impedance position protection reaction force is generated as follows: τ imp =K(q d -q) Where K is the impedance position protection stiffness parameter, and q is the joint feedback position; When q>q dmax When d =q dmax ; When q<q dmin When d =q dmin .

7. A robot position and speed protection method based on zero-force teaching according to claim 1, characterized in that: The step (3) also includes generating an anti-fall joint impedance protection force according to the actual feedback height position of the terminal moving platform and the critical falling height.

8. A robot position and speed protection method based on zero-force teaching according to claim 7, characterized in that: According to the actual feedback height position of the terminal moving platform and the critical drop height, the anti-fall joint impedance protection force is generated, specifically, the joint impedance position reaction force is corrected, and the formula is as follows: t imp =K(q d -q)+τ imp_z The impedance protection force of the anti-fall joint is: τ imp_z =K z (z limt ) Where z is the feedback height of the end moving platform, z limt It is the critical height for the end moving platform to prevent falling in height direction.

9. A robot position and speed protection method based on zero-force teaching according to claim 1, characterized in that: The step (4) is specifically: When the absolute value of the robot joint speed is less than the speed threshold When , a periodic square wave jitter signal is applied to suppress the static friction of the joint; When the absolute value of the robot joint velocity is greater than When , the joint jitter torque is turned off, as shown in the following formula: Where Amp = γ·F c is the periodic jitter torque amplitude, γ is the proportionality coefficient, F c is the Coulomb friction coefficient, t is the current interpolation time, and T is the jitter torque period.

10. A robot position and speed protection method based on zero-force teaching according to claim 9, characterized in that: In torque control mode, the robot is taught with zero force and given torque τ in real time cmd , as follows: t cmd =t cmp +t PD +t imp +t dither In the formula, τ cmp is the gravity and friction compensation torque, τ PD Compensate external torque for PD closed-loop control, τ imp is the impedance position protection compensation torque, τ dither Static friction suppresses the periodic jitter torque.

Images