A Zero-Force Control Method for Robot Drag Teaching with Friction Adaptive Compensation

Abstract

The present invention discloses a zero-force control method for robot drag teaching with friction force self-adaptive compensation, which includes modeling the joint friction force of the robot using the Coulomb viscous friction model; collecting the torque feedback values when the joints move at a constant speed at different speed levels for fitting calculation of the joint friction force; constructing a starting torque compensation using the trapezoidal bidirectional alternating method; setting joint torque limits and joint speed limits, and combining the friction torque compensation value during robot movement and the constructed starting torque compensation to obtain the output torque for robot drag teaching zero-force control. The present invention performs friction force self-adaptive compensation according to the change of speed during joint movement, optimizes the problem of requiring a large drag torque during startup, and on this basis, limits the torque and speed during joint movement to ensure safety during the movement process.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot control, and particularly relates to a zero-force control method for robot drag teaching with friction self-adaptive compensation. Background Art

[0002] Industrial robots are the core automation equipment in the intelligent manufacturing process, including six-joint robots, four-joint robots, parallel robots, SCARA (Selective Compliance Assembly Robot Arm) robots, and so on. Robot teaching is the key to affecting the working efficiency of robots and is one of the main difficult problems in the research and application of the robot field. Taking the SCARA robot as an example, it is composed of three rotating joints and an up-and-down moving lead screw, and has the characteristics of high precision, high speed, and simple control, and is widely used in various industries such as 3C manufacturing, food, and pharmaceuticals.

[0003] However, the teaching technology of the SCARA robot is the key difficulty in realizing precise machining and efficient control. Drag teaching means that the robot end can be directly dragged by hand to record the teaching trajectory, which has many advantages such as convenient use and simple operation. The prerequisite for realizing drag teaching is to realize the zero-force control of the SCARA robot. Currently, there are two zero-force control schemes. One is the zero-force control method based on position control and sensors, and the other is the zero-force control method under the torque mode. However, position control needs to be used in cooperation with sensors, and the cost is relatively high, while the zero-force control method under the torque mode does not require the use of sensors and is more widely used.

[0004] Currently, realizing zero-force control under the torque mode usually only compensates for the friction and gravity during motion. However, during joint motion, the joint speed is constantly changing rapidly, and the current torque mode compensation method cannot perform self-adaptive real-time compensation for friction. In practical applications, in addition to the need for torque compensation during motion, the static friction before joint startup is also very large, resulting in a large drag force required before startup. Moreover, when performing torque compensation, there is a lack of restriction on joint motion, and the phenomenon of the machine running away may occur. Therefore, it is necessary to propose a zero-force control method for robot drag teaching with friction self-adaptive compensation. Summary of the Invention

[0005] The purpose of the present invention is to provide a zero-force control method for robot drag teaching with friction self-adaptive compensation, which performs friction self-adaptive compensation according to the change of the speed during joint motion, optimizes the problem of the need to apply a large drag torque during startup, and on this basis, restricts the torque and speed during joint motion to ensure the safety during the motion process.

[0006] To achieve the above purpose, the technical solutions adopted by the present invention are as follows:

[0007] A zero-force control method for robot drag teaching with friction adaptive compensation. The zero-force control method for robot drag teaching with friction adaptive compensation includes:

[0008] Step 1: Model the joint friction force of the robot using the Coulomb viscous friction model;

[0009] Step 2: Collect the torque feedback values when the joint moves at a constant speed at different speed levels for fitting calculation of the joint friction force;

[0010] Step 201: Collect the torque feedback values when the joint moves at a constant speed at different speed levels, and filter the torque feedback values using the Kalman filter algorithm;

[0011] Step 202: Take the filtered torque feedback values, calculate the average value of the torque feedback values for each speed level, and perform least squares linear fitting based on the average values of the torque feedback values at different speed levels to obtain the friction torque compensation value during robot movement;

[0012] Step 3: Construct the starting torque compensation using the trapezoidal bidirectional alternating method;

[0013] Step 301: In the trapezoidal bidirectional alternating method, the trapezoidal auxiliary starting torque in one cycle includes a positive constant acceleration torque stage, a positive peak holding stage, a negative constant deceleration torque stage, and a negative peak holding stage;

[0014] Step 302: Calculate the slopes of the positive constant acceleration torque stage and the negative constant deceleration torque stage, and the auxiliary starting torque of the positive peak holding stage and the negative peak holding stage;

[0015] Step 303: Construct a trapezoidal auxiliary starting torque for compensating the starting torque in one cycle;

[0016] Step 4: Set the joint torque limit and the joint speed limit, and combine the friction torque compensation value during robot movement and the constructed starting torque compensation to obtain the output torque of the zero-force control for robot drag teaching.

[0017] The following also provides several optional methods, which are not additional limitations to the above overall solution, but only further supplements or optimizations. Without technical or logical contradictions, each optional method can be combined with the above overall solution separately, or multiple optional methods can be combined with each other.

[0018] Preferably, the modeling of the joint friction force of the robot using the Coulomb viscous friction model includes:

[0019]

[0020] where τf is the joint friction force to be solved, f cou is the Coulomb friction coefficient, f vis is the viscous friction coefficient is the motion speed of the current joint, sign function is the direction of the joint motion speed, and its definition is as follows:

[0021]

[0022] In the formula, σ is the threshold for determining whether the speed is 0.

[0023] Preferably, the Kalman filter algorithm is used to filter the torque feedback value, including:

[0024] Establish the discrete state equation and observation equation of the torque feedback value as follows:

[0025] x k = x k-1 + u k-1 + w k-1

[0026] z k = x k + v k

[0027] In the formula, x k is the state of the control system at the k-th moment, x k-1 is the state of the control system at the (k - 1)-th moment, u k-1 is the control quantity of the control system at the (k - 1)-th moment, w k-1 is the process noise at the (k - 1)-th moment during the measurement process, z k is the actual measurement value measured at the k-th moment during the motion control process, v k is the measurement noise at the (k - 1)-th moment during the measurement process;

[0028] Establish the update equation of the Kalman filter as follows:

[0029]

[0030]

[0031] Establish the update equation of the Kalman filter state as:

[0032]

[0033]

[0034]

[0035] In the formula, represents the prior state estimate value at the k-th moment, and respectively represent the posterior state estimate values at the (k - 1)-th and k-th moments, P k-1 and P k respectively represent the posterior estimate covariances at the (k - 1)-th and k-th moments, represents the prior estimate covariance at the k-th moment, K k represents the Kalman gain in the update equation of the Kalman filter, Q is the variance of the process noise, and R is the variance of the measurement noise.

[0036] Preferably, the least squares linear fitting is performed based on the average values of different speed levels to obtain the friction torque compensation value during the movement of the robot, including:

[0037] Calculating the ratio percentage of the average value of the torque feedback values of different speed levels to the maximum torque of the joint motor as the friction torque percentage, and obtaining the fitting relationship between the speed level and the friction torque percentage based on the least squares linear fitting;

[0038] Based on the fitting relationship between the speed level and the friction torque percentage, taking the friction torque percentage as the torque percentage of the joint motor, taking the reverse speed levels with the same speed level value and opposite directions, taking the reverse torque percentages with the same torque percentage value and opposite directions, obtaining the fitting relationship between the bilateral speed levels and the bilateral torque percentages, and taking the torque value corresponding to the torque percentage of the speed level in the fitting relationship between the bilateral speed levels and the bilateral torque percentages as the friction torque compensation value during the movement of the robot.

[0039] Preferably, calculating the slopes of the positive uniform acceleration torque stage and the negative uniform deceleration torque stage, and the auxiliary starting torques of the positive peak holding stage and the negative peak holding stage, including:

[0040] f peak = η·f sta_max

[0041] -f peak = η·(-f sta_max )

[0042]

[0043]

[0044] In the formula, f peak is the auxiliary starting torque of the positive peak holding stage, η is a constant coefficient, f sta_max is the maximum static friction torque of the positive joint, -f peak is the auxiliary starting torque of the negative peak holding stage, -f sta_maxis the maximum static friction torque of the negative joint, k is the slope of the positive uniform acceleration torque stage, -k is the slope of the negative uniform acceleration torque stage, and T a is the time of the positive uniform acceleration torque stage or the negative uniform deceleration torque stage.

[0045] Preferably, the trapezoidal auxiliary starting torque for compensating the starting torque within one cycle of the output includes:

[0046]

[0047] In the formula, τ start is the trapezoidal auxiliary starting torque, k is the slope of the positive uniform acceleration torque stage, t is the current time within one cycle, and T a is the time of the positive uniform acceleration torque stage or the negative uniform deceleration torque stage, f peak is the auxiliary starting torque of the positive peak holding stage, T is the total time of one cycle, and T u is the time of the positive peak holding stage or the negative peak holding stage, -k is the slope of the negative uniform acceleration torque stage, and -f peak is the auxiliary starting torque of the negative peak holding stage.

[0048] Preferably, the setting of the joint torque limit and the joint speed limit, combined with the friction torque compensation value during the movement of the robot and the constructed starting torque compensation, to obtain the output torque of the robot drag teaching zero-force control includes:

[0049] When the robot enters the drag teaching and the speed is 0, the output torque of the robot drag teaching zero-force control is the trapezoidal auxiliary starting torque;

[0050] When the robot enters the drag teaching and the speed is not 0, first perform the joint torque limit judgment. If the joint torque limit is satisfied, obtain the output torque of the robot drag teaching zero-force control according to the joint torque limit. Otherwise, perform the joint speed limit judgment. If the joint speed limit judgment is satisfied, obtain the output torque of the robot drag teaching zero-force control according to the joint speed limit. Otherwise, read the friction torque compensation value obtained in step 202 of the robot joint as the output torque of the robot drag teaching zero-force control.

[0051] Preferably, the joint torque limit and the joint speed limit are set as follows:

[0052] Set the joint torque limit as follows:

[0053]

[0054] In the formula, τ out_tor represents the output torque of the robot drag teaching zero-force control obtained under the joint torque limit, and τ currentis the real-time torque of the current joint, τ max is the positive maximum limit torque of the current joint, -τ max is the negative maximum limit torque of the current joint;

[0055] Set the joint speed limit as follows:

[0056]

[0057] In the formula, τ out_vel represents the output torque of the zero-force control of the robot's drag teaching obtained under the joint speed limit, v current is the real-time speed of the current joint, k is the coefficient of the adjacent maximum limit speed, and 0 ≤ k ≤ 1, -v max is the negative maximum limit speed of the current joint, v max is the positive maximum limit speed of the current joint.

[0058] When the SCARA robot is being dragged, the joint speed is constantly changing, and at this time, the joint friction force is also constantly changing. How to quickly identify and adaptively compensate the joint friction force has become a very important issue. Therefore, the present invention proposes a robot drag teaching zero-force control method with friction force adaptive compensation, which is applied to the drag teaching scenario of the SCARA robot, adaptively compensates for the joint friction force when the SCARA robot is moving, has a simple identification method, small calculation amount, and improves the problem of large static friction force at the start of the zero-force control method in the previous torque mode, optimizes it, and can quickly identify, calculate and compensate when there is no friction model in the system or the load changes due to the replacement of the end effector. It also has good application value in the low-computation scenario of using a low-computation power chip such as an embedded single-chip microcomputer device STM32 as the SCARA robot controller. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 is a flowchart of a robot drag teaching zero-force control method with friction force adaptive compensation according to the present invention;

[0060] Figure 2 is a schematic diagram of the fitting relationship between the speed level and the friction torque percentage according to the present invention;

[0061] Figure 3 is a schematic diagram of the fitting relationship between the bilateral speed level and the bilateral torque percentage according to the present invention;

[0062] Figure 4 is a schematic diagram of the trapezoidal auxiliary starting torque according to the present invention;

[0063] Figure 5 is a flowchart for judging the joint torque limit and the joint speed limit according to the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0064] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0065] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the technical field to which the present invention belongs. The terms used in the description of the present invention in this specification are only for the purpose of describing specific embodiments and are not intended to limit the present invention.

[0066] When performing drag teaching of a SCARA robot, due to various factors, the human hand cannot accurately control the movement speed of the joints, which results in the continuous change of the movement speed of the joints, and the joint friction also changes continuously with the change of the speed. Therefore, it is necessary to adaptively compensate the joint friction according to the different movement speeds of the joints.

[0067] The dynamic model of the SCARA robot is as follows:

[0068]

[0069] In the formula, τ is the joint torque, M(θ) is the manipulator mass matrix, is the action of the inertial force, is the action of the Coriolis force and the centrifugal force, G(θ) is the manipulator gravity, τ f is the joint friction.

[0070] Since the third and fourth joints of the SCARA robot perform up and down displacement and attitude rotation on the basis of the end of the second joint, in practical applications, the trajectory movement of the SCARA robot in the Cartesian space is mainly determined by the first and second joints. For the above reasons, in the zero-force control drag teaching process of the SCARA robot, the first two joints of the SCARA robot are selected for torque compensation to achieve zero-force control, that is, for each of the two selected joints, the zero-force control method of the robot drag teaching with friction adaptive compensation proposed in this embodiment is used for control.

[0071] The first two joints of a SCARA robot are rotational joints, and their joint axes are parallel to the plumb line. During the drag teaching process, the first two axes are not affected by gravity, and the dragging speed is relatively low. The influence of inertial force and Coriolis force can be almost ignored. Therefore, for the zero-force control of a SCARA robot, identifying the joint friction force in different motion states and performing adaptive compensation on it is the most important part.

[0072] As Figure 1 shown, this embodiment provides a zero-force control method for robot drag teaching with friction force adaptive compensation, including the following steps:

[0073] Step 1: Model the joint friction force of the robot using the Coulomb-viscous friction model.

[0074] There are many common friction force models. For example, the Stribeck friction model and the LuGre friction model are relatively complex friction force models. Although they have higher accuracy, a large amount of calculation is required for parameter identification, increasing the load of the system. During the process of manually pulling the end of a SCARA robot, the Coulomb friction force (the friction force is proportional to the normal pressure acting on the friction surface and has nothing to do with the external contact area) and the viscous friction force are the main friction forces, and other friction forces can be ignored. Therefore, this embodiment uses the Coulomb-viscous friction force model as the friction force model of the SCARA robot. The Coulomb-viscous friction force model is expressed as follows:

[0075]

[0076] In the formula, τ f is the joint friction force to be solved; f cou is the Coulomb friction coefficient. The magnitude of the Coulomb friction force is determined by the Coulomb friction coefficient, but its direction is related to the direction of the joint movement speed; f vis is the viscous friction coefficient. It can be obtained from the formula that the viscous friction force is proportional to the magnitude of the joint movement speed; is the current joint movement speed; the sign function is the direction of the joint movement speed, and its definition is as follows:

[0077]

[0078] In the formula, considering the error influence, σ is set as the threshold for determining whether the speed is 0. When , it is considered that the joint is not moving. σ can be determined according to specific mechanical parameters and operating conditions in combination with the calculation accuracy. For example, σ = 0.001 - 0.01.

[0079] The Coulomb viscous friction model is suitable for SCARA robots and is relatively simple computationally, making it suitable for application scenarios in SCARA robot controllers with low computing power. Moreover, it includes the main part of the error, has a very small error compared to the true friction, and the error friction can be ignored in actual drag teaching application scenarios, having great applicability and practicality.

[0080] Step 2: Collect the torque feedback values when the joint moves at a constant speed at different speed levels for the fitting calculation of joint friction.

[0081] Step 201: Collect the torque feedback values when the joint moves at a constant speed at different speed levels, and use the Kalman filter algorithm to filter the torque feedback values.

[0082] After selecting the friction model, the parameter identification process begins. From physical knowledge, when the joint moves at a constant speed, the joint torque and the friction torque values are equal. Therefore, in the friction parameter identification, it is set as a single joint rotating at a constant speed, the motion range of each joint is [-π / 4, π / 4], and other variables are controlled to remain unchanged, and it moves at a constant speed according to different speed levels.

[0083] The torque feedback value can be obtained through the motor driver or estimated through the current value, and there is no limitation in this embodiment. Since the Coulomb viscous friction model has symmetry, during data collection, the two-way data can be combined and calculated with unified signs, reducing the acquisition working time by half, that is, only the torque feedback values at positive speeds need to be collected, and the torque feedback values at negative speeds are obtained symmetrically from the torque feedback values at positive speeds.

[0084] Since the actually obtained torque feedback values will fluctuate, and in the presence of interference, there may be sudden invalid values. If directly used for calculation, it will reduce the calculation accuracy. Therefore, the Kalman filter algorithm is used to filter the torque feedback value data. The discrete state equation and observation equation of the torque feedback value are as follows:

[0085] x k =x k-1 +u k-1 +w k-1

[0086] z k =x k +v k

[0087] In the formula, x k is the state of the control system at the k-th moment, x k-1 is the state of the control system at the (k - 1)-th moment, u k-1 is the control quantity of the control system at the (k - 1)-th moment, w k-1is the process noise at the (k - 1)-th moment during the measurement, z k is the actual measurement value measured at the k-th moment during the motion control process, v k represents the measurement noise at the (k - 1)-th moment during the measurement process.

[0088] The update equation of the Kalman filter is established as follows:

[0089]

[0090]

[0091] The update equation for the state of the Kalman filter is:

[0092]

[0093]

[0094]

[0095] In the formula, represents the prior state estimate value at the k-th moment, and represent the posterior state estimate values at the (k - 1)-th and k-th moments, P k-1 and P k represent the posterior estimate covariances at the (k - 1)-th and k-th moments, represents the prior estimate covariance at the k-th moment, K k represents the Kalman gain in the update equation of the Kalman filter, Q is the variance of the process noise w, and R is the variance of the measurement noise v.

[0096] When using the Kalman filtering algorithm, only the first few data are used in each iteration, and the computational complexity is very small, which is suitable for application scenarios with low computing power. The values of Q and R in the formula can be set according to different mechanical structure compositions. The Kalman filtering algorithm in this embodiment is a conventional algorithm. Suppose the number of sampling speed levels is M, and the number of data points sampled for each speed level is N, then the sampled data is t i (j) (i = 1, 2,..., M; j = 1, 2,..., N), where i represents the speed level, that is, the percentage of the current speed to the maximum speed. Then it can be obtained that M < 100. Each time, only the torque feedback value of one speed level is taken for filtering processing, and the filtered torque feedback value is t_k i (j).

[0097] Step 202: Take the filtered torque feedback value, calculate the average value of the torque feedback values for each speed level, and perform least squares linear fitting based on the average values of different speed levels to obtain the friction torque compensation value during the robot motion.

[0098] After obtaining the filtered torque feedback value, since only one data is required for each speed level during the fitting calculation, the average value of the filtered torque feedback value is calculated to obtain the average value \(t_{ka}\) of the torque feedback value for each speed level. i :

[0099]

[0100] In the formula, \(t_{ka}\) i is the average value of the torque feedback value for speed level \(i\), and \(t_k\) i (j) is the \(j\)-th torque feedback value of speed level \(i\) after filtering.

[0101] Based on the average values of different speed levels, a linear fitting of the least squares method is performed to obtain the fitting relationship between the speed level (i.e., speed percentage) and the torque feedback value. To correspond to the speed level, the torque feedback value is converted into the friction torque percentage, that is, the ratio percentage of the current friction torque (i.e., the average value of the torque feedback value) to the maximum torque of the motor. The obtained fitting relationship is as Figure 2 shown, Figure 2 where the circles are discrete values and the straight line is the fitted relationship line.

[0102] Based on the fitting relationship between the speed percentage and the friction torque percentage, the reverse speed percentage with the same speed percentage value and opposite direction is taken, and the reverse friction torque percentage with the same friction torque percentage value and opposite direction is taken to obtain the fitting relationship between the bilateral speed level and the bilateral friction torque percentage. Since the motor needs to provide torque to eliminate the influence of the friction torque, the torque percentage that the motor should output can be calculated, that is, the ratio percentage of the output torque (the same as the friction torque) to the maximum torque, that is, taking the friction torque percentage as the torque percentage of the joint motor. The result is as Figure 3 shown, and the torque value corresponding to the torque percentage of the speed level in the fitting relationship between the bilateral speed level and the bilateral torque percentage is used as the friction torque compensation value during the movement of the robot.

[0103] Step 3: To address the problem of large static friction during startup, a trapezoidal bidirectional alternating method is used to construct the starting torque compensation.

[0104] After obtaining the friction torque compensation value during the movement, the movement torque during the drag teaching can be compensated, but the static friction torque during startup needs to be processed. The joint static friction torque in the static state is related to the drag torque, and they are equal in magnitude and opposite in direction. Assuming that the drag torque applied by the human hand is \(f\) hand , the joint friction torque of the SCARA robot is \(f\) sta , and the maximum joint static friction torque is \(f\) sta_max, then the relationship between them is: if the joint is in a static state, then f hand <f sta_max and f hand = -f sta ; if the joint is about to start moving, then f hand =f sta_max . If no torque is applied before startup, then when the human hand drags, a drag torque greater than or equal to the maximum static friction torque needs to be applied to make the joint of the SCARA robot start to move. At this time, the startup drag torques in the positive and negative directions that need to be applied are f sta_max and -f sta_max .

[0105] If the motor applies a unidirectional auxiliary startup torque, and its magnitude is f sta_max , then the startup drag torque in the same direction that needs to be applied at this time is 0, and the startup drag torque in the different direction becomes twice the original value in magnitude. And according to the direction of the auxiliary startup torque, the direction of the drag torque is also different. The positive direction is 2f sta_max , and the negative direction is -2f sta_max .

[0106] For this situation, this embodiment proposes a trapezoidal bidirectional alternating auxiliary startup torque, that is, as time changes, the motor applies auxiliary startup torques in different directions on the joint, and the change mode of the auxiliary startup torque is a periodic trapezoidal change, which is simply called the trapezoidal auxiliary startup torque.

[0107] Step 301, as Figure 4 shown, let the change period of the trapezoidal auxiliary startup torque be T, that is, the time for one positive and negative alternation. There are four stages in a change period, namely the positive uniform acceleration torque stage, the positive peak holding stage, the negative uniform deceleration torque stage, and the negative peak holding stage, where the peak holding stage duration is T u , (that is, the duration of the positive peak holding stage is T u , and the duration of the negative peak holding stage is also T u ) The positive uniform acceleration and negative uniform deceleration stage durations are T a (that is, the duration of the positive uniform acceleration torque stage is T a , and the duration of the negative uniform acceleration torque stage is also T a ) Their relationship is as follows:

[0108] T = 2(T u + 2T a )

[0109] Step 302. In the design of the trapezoidal bidirectional alternating auxiliary starting torque, the torque in the peak holding stage can be represented by a constant coefficient η ∈ (0, 1) to represent its relationship with the maximum static friction torque. Generally speaking, the value of η should be set close to 1. Otherwise, there will still be a situation where the starting drag torque needs to be increased. Let the slope of the positive uniform acceleration torque stage be k, then the slope of the negative uniform deceleration torque stage is -k. Then the slope k and the torque f in the peak holding stage can be calculated as follows: peak as follows:

[0110] f peak = η·f sta_max

[0111]

[0112] Then the auxiliary starting torque in the positive peak holding stage is f peak , and the auxiliary starting torque in the negative peak holding stage is -f peak .

[0113] Step 303. Construct a trapezoidal auxiliary starting torque that compensates the starting torque within one period.

[0114] Let the trapezoidal auxiliary starting torque output by the joint motor be τ start , then the expression of the trapezoidal auxiliary starting torque that needs to be provided after the SCARA robot enters the drag teaching and is in the static period (taking the first period as an example) is:

[0115]

[0116] In the above expression formula of the trapezoidal auxiliary starting torque τ start , the time of t is from 0 to T. The time period 0 to T can be understood as the time of the first period. When it needs to be extended to each period, 0 is understood as the starting moment of the period for which the trapezoidal auxiliary starting torque needs to be calculated, and T is understood as the ending moment of the period for which the trapezoidal auxiliary starting torque needs to be calculated.

[0117] The provided trapezoidal auxiliary starting torque τ start can switch the direction of the auxiliary starting torque within a very short cycle duration. During actual operation, even if the direction of the drag force applied by the human hand is opposite to the direction of the auxiliary force, it can still switch to the other direction within a very short time so that the auxiliary torque and the human hand drag torque are in the same direction, and this time is almost negligible for the perception of the human hand. When selecting the constant coefficient η, a value close to 1 above 0.9 can be selected so that the auxiliary starting torque can offset the static friction as much as possible, so that the human hand can move the joints of the SCARA robot with almost zero drag torque.

[0118] Step 4: Limit the calculated joint friction compensation to ensure the safety of joint movement. Therefore, set joint torque limits and joint speed limits, and combine the friction torque compensation value during robot movement and the constructed starting torque compensation to obtain the output torque of the robot's drag teaching zero-force control.

[0119] Since zero-force control is in torque control mode, after obtaining the calculated friction compensation values at different joint movement speeds, it is necessary to limit them to ensure the safety of the movement process and prevent phenomena such as running away and losing control. Let the forward maximum limit speed of the current joint be v max , and the forward maximum limit torque be τ max .

[0120] It should be noted that in this embodiment, torque and moment should be understood as equivalent interchangeable concepts. For joints, it is usually called moment, while in the motor field, moment is generally called torque. Therefore, both moment and torque are described in this embodiment.

[0121] As Figure 5 shown, first, limit the torque. When the joint is not started and the speed is 0, the trapezoidal auxiliary starting torque τ start designed in the previous step can be used; when the joint speed is not 0, if the joint torque exceeds the maximum torque, the torque should be limited to the maximum torque. If the current torque is less than the minimum torque, the torque should be limited to the minimum torque, as shown in the following formula.

[0122]

[0123] In the formula, τ out_tor represents the torque that the current joint motor needs to output under the calculated torque limit, τ current is the real-time torque of the current joint, and -τ max is the negative maximum limit torque of the current joint.

[0124] Secondly, limit the joint speed. When the joint speed is very close to the maximum limit speed, the torque supply should be stopped to prevent the joint from exceeding the limit speed. If the joint speed continues to increase until it exceeds the maximum limit speed at this time, a reverse torque should be provided to decelerate. To decelerate quickly, the deceleration torque magnitude can be set to the maximum limit torque until the joint speed is lower than the maximum limit speed, as shown in the following formula.

[0125]

[0126] In the formula, τ out_vel represents the torque that the current joint motor needs to output under the calculated speed limit; v currentis the real-time speed of the current joint; k is the coefficient of the adjacent maximum limit speed, and its value is limited to 0 ≤ k ≤ 1, which can be set according to requirements in actual use, -v max is the negative maximum limit speed of the current joint.

[0127] Therefore, by combining the joint torque limit and the joint speed limit, the output torque of the zero-force control for robot teaching by dragging can be obtained as follows:

[0128] When the robot enters the teaching by dragging and the speed is 0, the output torque of the zero-force control for robot teaching by dragging is the trapezoidal auxiliary starting torque.

[0129] When the robot enters the teaching by dragging and the speed is not 0, first, the joint torque limit is judged. If the joint torque limit is satisfied, the output torque of the zero-force control for robot teaching by dragging is obtained according to the joint torque limit. Otherwise, the joint speed limit is judged. If the joint speed limit judgment is satisfied, the output torque of the zero-force control for robot teaching by dragging is obtained according to the joint speed limit. Otherwise, the friction torque compensation value obtained in step 202 of the robot joint is read as the output torque of the zero-force control for robot teaching by dragging.

[0130] The present invention designs a zero-force control method for robot teaching by dragging with friction self-adaptive compensation in combination with the mechanical structure of a SCARA robot. It performs self-adaptive friction compensation according to the speed of the joint during dragging and adopts the method of trapezoidal bidirectional alternating torque compensation to solve the problem of large static friction during startup. On this basis, the torque and speed of the joint are limited to ensure safety during operation.

[0131] The present invention first uses the Coulomb viscous friction model to identify the friction force. The error between the friction force obtained by this model and the real friction force is very small and the calculation is simple. When performing parameter identification, only simple round-trip uniform motion is required to collect the required data, and through the one-dimensional Kalman filter algorithm and the least square method for linear fitting, the friction force calculation model can be quickly obtained, which is simple and convenient. If the friction force changes due to the change of the end load after replacing the end fixture, it can also be quickly identified and compensated. The entire identification and calculation process has a small amount of computation and a fast response, and has good applicability in low-computing power usage scenarios.

[0132] The technical features of the above embodiments can be combined arbitrarily. For the sake of brevity of description, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, it should be considered as the scope recorded in this specification.

[0133] The above-described embodiments merely represent several implementation manners of the present invention. The description thereof is relatively specific and detailed, but it should not be construed as a limitation to the scope of the invention. It should be noted that for those of ordinary skill in the art, without departing from the concept of the present invention, several modifications and improvements can still be made, and these all fall within the protection scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the appended claims.

Claims

1. A zero-force control method for robot drag teaching with friction force self-adaptive compensation, characterized in that, The zero-force control method for robot drag teaching with friction force adaptive compensation includes: Step 1: Model the joint friction force of the robot using the Coulomb viscous friction model; Step 2: Collect the torque feedback values when the joint moves at a constant speed at different speed levels for fitting calculation of the joint friction force; Step 201: Collect the torque feedback values when the joint moves at a constant speed at different speed levels, and filter the torque feedback values using the Kalman filter algorithm; Step 202: Take the filtered torque feedback values, calculate the average value of the torque feedback values for each speed level, and perform least squares linear fitting based on the average values of the torque feedback values at different speed levels to obtain the friction torque compensation value during robot movement; Step 3: Construct the starting torque compensation using the trapezoidal bidirectional alternating method; Step 301: In the trapezoidal bidirectional alternating method, the trapezoidal auxiliary starting torque in one cycle includes a positive constant acceleration torque stage, a positive peak holding stage, a negative constant deceleration torque stage, and a negative peak holding stage; Step 302: Calculate the slopes of the positive constant acceleration torque stage and the negative constant deceleration torque stage, and the auxiliary starting torque of the positive peak holding stage and the negative peak holding stage; Step 303: Construct a trapezoidal auxiliary starting torque for compensating the starting torque in one cycle; Step 4: Set the joint torque limit and joint speed limit, and combine the friction torque compensation value during robot movement and the constructed starting torque compensation to obtain the output torque of the robot drag teaching zero-force control.

2. The zero-force control method for robot drag teaching with friction force self-adaptive compensation according to claim 1, wherein The modeling of the joint friction force of the robot using the Coulomb viscous friction model includes: where τ f is the joint friction force to be solved, f cou is the Coulomb friction coefficient, f vis is the viscous friction coefficient, is the motion speed of the current joint, and the sign function is the direction of the joint motion speed, which is defined as follows: where σ is the threshold for determining whether the speed is 0.

3. The friction force self-adaptive compensation robot drag teaching zero-force control method according to claim 1, characterized in that The filtering of the torque feedback values using the Kalman filter algorithm includes: Establish the following discrete state equation and observation equation for the torque feedback values: x k = x k-1 + u k-1 + w k-1 z k = x k + v k where x k is the state of the control system at the k-th moment, x k-1 is the state of the control system at the (k - 1)-th moment, u k-1 is the control quantity of the control system at the (k - 1)-th moment, w k-1 is the process noise at the (k - 1)-th moment during the measurement process, z k is the actual measured value measured at the k-th moment during the motion control process, v k is the measurement noise at the (k - 1)-th moment during the measurement process; Establish the following update equation for the Kalman filter: Establish the update equation for the state of the Kalman filter as: In the formula, represents the prior state estimate value at the k-th moment, and respectively represent the posterior state estimate values at the (k - 1)-th moment and the k-th moment, P k-1 and P k respectively represent the posterior estimate covariance at the (k - 1)-th moment and the k-th moment, represents the prior estimate covariance at the k-th moment, K k represents the Kalman gain in the update equation of the Kalman filter, Q is the variance of the process noise, and R is the variance of the measurement noise.

4. The zero-force control method for robot drag teaching with friction force self-adaptive compensation according to claim 1, characterized in that, The least squares linear fitting based on the average values at different speed levels to obtain the friction torque compensation value during robot movement includes: Calculate the ratio percentage of the average value of the torque feedback values at different speed levels to the maximum torque of the joint motor as the friction torque percentage, and obtain the fitting relationship between the speed level and the friction torque percentage based on least squares linear fitting; Based on the fitting relationship between the speed level and the friction torque percentage, take the friction torque percentage as the torque percentage of the joint motor, take the reverse speed levels with the same speed level value and opposite directions, take the reverse torque percentages with the same torque percentage value and opposite directions, obtain the fitting relationship between the bilateral speed levels and the bilateral torque percentages, and take the torque value corresponding to the torque percentage at the speed level in the fitting relationship between the bilateral speed levels and the bilateral torque percentages as the friction torque compensation value during robot movement.

5. The zero-force control method for robot drag teaching with friction force adaptive compensation according to claim 1, characterized in that, The calculation of the slopes of the positive constant acceleration torque stage and the negative constant deceleration torque stage, and the auxiliary starting torque of the positive peak holding stage and the negative peak holding stage includes: f peak = η·f sta_max -f peak = η·(-f sta_max ) where f peak is the auxiliary starting torque in the positive peak holding stage, η is a constant coefficient, f sta_max is the maximum static friction torque of the positive joint, -f peak is the auxiliary starting torque in the negative peak holding stage, -f sta_max is the maximum static friction torque of the negative joint, k is the slope in the positive constant acceleration torque stage, -k is the slope in the negative constant acceleration torque stage, T a is the time in the positive constant acceleration torque stage or the negative constant deceleration torque stage.

6. The zero-force control method for robot drag teaching with friction force adaptive compensation according to claim 1, characterized in that The output of the trapezoidal auxiliary starting torque for compensating the starting torque in one cycle includes: where τ start is the trapezoidal auxiliary starting torque, k is the slope in the positive constant acceleration torque phase, t is the current time within a period, T a is the time in the positive constant acceleration torque phase or the negative constant deceleration torque phase, f peak is the auxiliary starting torque in the positive peak holding phase, T is the total time of a period, T u is the time in the positive peak holding phase or the negative peak holding phase, -k is the slope in the negative constant acceleration torque phase, -f peak is the auxiliary starting torque in the negative peak holding phase.

7. The zero-force control method for robot drag teaching with friction force self-adaptive compensation according to claim 1, characterized in that Set the joint torque limit and joint speed limit, and combine the friction torque compensation value during the robot's movement and the constructed starting torque compensation to obtain the output torque of the robot's drag teaching zero-force control, including: When the robot enters drag teaching and the speed is 0, the output torque of the robot's drag teaching zero-force control is the trapezoidal auxiliary starting torque; When the robot enters drag teaching and the speed is not 0, first perform the joint torque limit judgment. If the joint torque limit is satisfied, obtain the output torque of the robot's drag teaching zero-force control according to the joint torque limit. Otherwise, perform the joint speed limit judgment. If the joint speed limit judgment is satisfied, obtain the output torque of the robot's drag teaching zero-force control according to the joint speed limit. Otherwise, read the friction torque compensation value obtained in step 202 as the output torque of the robot's drag teaching zero-force control.

8. The zero-force control method for robot drag teaching with friction force self-adaptive compensation according to claim 1 or 7, characterized in that The joint torque limit and joint speed limit are set as follows: Set the joint torque limit as follows: where τ out_tor represents the output torque of the zero-force control for robot drag teaching obtained under the joint torque limit, τ current is the real-time torque of the current joint, τ max is the maximum positive limit torque of the current joint, and -τ max is the maximum negative limit torque of the current joint; Set the joint speed limit as follows: where τ out_vel represents the output torque of the zero-force control for robot drag teaching obtained under the joint speed limit, v current is the real-time speed of the current joint, k is the coefficient of the near maximum limit speed, and 0 ≤ k ≤ 1, -v max is the negative maximum limit speed of the current joint, v max is the positive maximum limit speed of the current joint.

Images