An industrial robot collision detection method

Abstract

An industrial robot collision detection method solves the problem of error accumulation in torque-level dynamic identification caused by inaccurate identification of joint torque constants in robot collision detection, and belongs to the field of robot collision detection. The present invention includes: S1. Input the nonlinear friction vector coefficient α of the industrial robot joint and the dynamic base parameter set χ at the current level WLS and the collision detection threshold δ; i = H χ , where i represents the joint current of the industrial robot, #imgabs0# is the base parameter set set at the current level, #imgabs1# is the corresponding regression matrix and is full rank; the dynamic base parameter set χ WLS is the estimated value of χ; S2. Given the joint command signal at time k: q d , #imgabs2# and #imgabs3# collect the joint current i at time k k , and calculate the regression matrix #imgabs4# corresponding to the dynamic base parameter set χ WLS at time k; S3. Calculate the current residual r i,k at time k, and determine whether r i,k > δ is satisfied. If so, it is determined that the industrial robot has collided. If not, k = k + 1, and go to S2.

Description

Technical Field

[0001] The present invention relates to an industrial robot collision detection method based on current residuals, belonging to the field of robot collision detection. Background Technique

[0002] Current robot collision detection methods can be divided into two main types: model-free collision detection and model-based collision detection. Model-free methods achieve collision detection by using external sensors, such as vision sensors, force / torque sensors (mounted on the robot's wrist or base), tactile sensors, and so on. However, this method is limited by the inherent limitations of sensors such as vision blind spots, limited collision detection range, and manufacturing costs. Another model-free method involves collecting the collision data of the robot and using machine learning techniques to train the collision detection data. For example, Sharkawy et al. proposed a human-robot collision detection method using a multi-layer neural network. They used data related to the robot joint position error, joint velocity, and joint torque for offline training, enabling them to predict the external collision torque and determine whether a collision exists. In addition, Parker et al. introduced two learning-based detection methods aimed at using support vector machines and convolutional neural networks (CNNs) to address soft collisions and hard collisions respectively. The collision information, which ignores the consideration of joint friction terms, is used as one of the inputs for learning and preventing unmodeled collisions. However, this method requires a large amount of training data and brings a large real-time computational burden, which needs to be further refined in practical applications.

[0003] The classic model-based approach is the torque residual, which compares the measured torque with the predicted torque calculated through the identified torque-level dynamics or the robot control law. Another classic model-based approach is the generalized momentum observer, which utilizes the change in the robot's generalized momentum to estimate external collisions. The main advantage of this method is that it eliminates the acceleration signal and the mass matrix inversion, and has been integrated into the controllers of KUKA iiwa and FRANKA EMika, highlighting its practical applicability. However, in the case of industrial robots lacking joint torque sensors, the inherent first-order low-pass filtering characteristic of the generalized momentum observer poses a challenge to achieving the best collision detection results. For example, it may amplify modeling errors, such as errors in friction modeling during velocity reversal. In addition, in the field of model-based collision detection, techniques such as energy observers, joint velocity observers, nonlinear disturbance observers, and admittance control have also been utilized. However, when the robot is stationary, the energy observer cannot identify external collisions, and the velocity observer, nonlinear disturbance observer, and admittance control techniques require complex matrix operations, including the inversion or differentiation of the mass matrix. Most importantly, the above model-based collision detection methods have a common limitation: the accurate torque-level dynamic model can only be measured through joint torque sensors. But for industrial robots without joint torque sensors, only their joint currents can be measured. And the method of estimating the measured joint torque by assuming a linear relationship between the joint current and the joint torque constant usually has an uncertainty of 10%. Summary of the Invention

[0004] Aiming at the problem of error accumulation in torque-level dynamic identification caused by inaccurate identification of joint torque constants in robot collision detection, the present invention provides an industrial robot collision detection method without joint torque sensors that enhances detection performance.

[0005] An industrial robot collision detection method of the present invention includes:[[]]

[0006] S1. Input the nonlinear friction vector coefficient α of the industrial robot joints, the dynamic base parameter set χ at the current level WLS and the collision detection threshold δ;

[0007] i = Hχ, where i represents the joint current of the industrial robot, is the base parameter set set at the current level, is the corresponding regression matrix and is full rank; the dynamic base parameter set χ WLS is the estimated value of χ;

[0008] S2. Given the joint command signals at time k: q d , and Collect the joint current i at time k k , and then combine with qd , and the non - linear friction vector coefficient α of the joint, calculate the dynamic base parameter set χ at time k WLS The corresponding regression matrix

[0009] S3. Calculate the current residual r at time k i,k , Judge whether r satisfies i,k > δ. If so, it is determined that the industrial robot has collided. If not, k = k + 1, and go to S2.

[0010] Preferably, the method for obtaining the non - linear friction vector coefficient α of the joint is as follows:

[0011] For the j - th joint, establish an optimization problem:

[0012]

[0013] s.t. 0 < α j < 1

[0014] Optimization variable The initial value of the non - linear friction vector coefficient αj of the j - th joint is 1, is the friction force model vector at the current level of the current residual, τ f,j represents the joint friction torque vector of the j - th joint, is the diagonal torque constant matrix, Kj represents the j - th element of K, is the j - th row of is in matrix form, represents the estimated joint friction current, n represents the number of degrees of freedom of the robot, and N represents the number of groups of joint currents collected;

[0015] Solve the optimization problem to obtain the non - linear friction coefficient α of the j - th joint j , and the non - linear friction coefficients α of each joint j constitute the vector α.

[0016] Preferably, the estimated joint friction current is:

[0017]

[0018] where χ WLS,i represents the inertial part in χ WLS , represents the weighted regression matrix.

[0019] Preferably, the joint friction torque vector τ of the j-th joint f,j is:

[0020]

[0021] where F c,j , F v,j , B j are the linear friction coefficients of the j-th joint, and q j is the coordinate of the j-th joint.

[0022] Preferably, the kinetic base parameter set χ is obtained by estimating χ through the weighted least squares (WLS) method WLS .

[0023] Preferably,

[0024]

[0025] where the covariance matrix is Σ is updated through , the current residual vector is the data weight vector, W e each column in is W; is the stacked current vector, is the stacked regression matrix, N represents the number of groups of joint currents collected, n represents the number of degrees of freedom of the robot, and r represents the dimension of the elements in χ; the initial value of the variance matrix Σ is the initial current residual vector, χ OLS is the estimated value of χ obtained by ordinary least squares.

[0026] Preferably, when the current residual vector is known, the data weight vector W and its matrix form W e are updated as:

[0027]

[0028] where the function Ψ(·) outputs a vector with the same dimension as the input vector, and its function is: if one of the elements in exceeds the collision detection threshold δ, then the element at the corresponding position in W is set to 0, otherwise it is set to 1; the initial value of W is a vector with all elements equal to 1.

[0029] Advantages of the present invention: The present invention proposes a method that directly uses the measured joint current for robot dynamics modeling and identifies dynamic parameters to avoid identifying joint torque constants and estimating the measured joint torque. Then, external collisions are monitored through a significant change between the measured current and the predicted current calculated by inverse dynamics based on the identified current level. The present invention proposes a novel collision detection method based on current residuals for robots without joint torque sensors. Compared with the model-based method using torque-level dynamics, the proposed method is based on current-level dynamics and performs modeling and identification by using the measured joint current. This method avoids the error accumulation of torque-level dynamic identification caused by inaccurate identification of joint torque constants, and ultimately enhances the collision detection performance of robots without joint torque sensors. Brief Description of the Drawings

[0030] Figure 1 It is a schematic flow chart of the method of the present invention;

[0031] Figure 2 It is the result of the balloon collision experiment, where (a) is the current residual of joint 2, (b) is the current residual of joint 3, (c) is the end Z-axis linear velocity, and (d) is the end Z-axis force;

[0032] Figure 3 It is the balloon collision process. Detailed Embodiment

[0033] 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.

[0034] It should be noted that, without conflict, the embodiments in the present invention and the features in the embodiments can be combined with each other.

[0035] The following further illustrates the present invention with reference to the accompanying drawings and specific embodiments, but it is not a limitation of the present invention.

[0036] The industrial robot collision detection method of this embodiment directly uses the measured joint current for robot dynamics modeling and identifies dynamic parameters to avoid identifying joint torque constants and estimating the measured joint torque, and is mainly described from the following two steps:

[0037] The first step is to derive the relationship between the output signal of the collision detection algorithm and the dynamic parameters at the torque level, and the second step is the manipulator collision detection method based on current residuals.

[0038] Relationship between the output signal of the collision detection algorithm and the dynamic parameters at the torque level:

[0039] The dynamic equation of a rigid robot considering motor dynamics can be written as:

[0040]

[0041] where, is the generalized coordinate of the robot, n is the degree of freedom of the robot, is the inertia matrix of the robot, Ia is the equivalent moment of inertia at the motor end, is the matrix of centrifugal and Coriolis forces of the link, is the gravity vector of the link, is the vector of joint friction torques, is the vector of joint torques.

[0042] The non - linear friction model is beneficial to improving the dynamic identification accuracy. Therefore, for the j - th joint, the friction model selected in this embodiment is:

[0043]

[0044] where, F c,j , F v,j , B j are the linear friction coefficients of joint j, and α j is the non - linear friction coefficient of joint j.

[0045] After deleting the unidentifiable parameters, the above dynamic equation can be written in the following linear form:

[0046]

[0047] In the above formula, is the basic parameter set, where p is the number of elements. The corresponding full - rank regression matrix is i.e., the regression matrix of the basic parameter set .

[0048] Based on the determined dynamic parameters at the torque level A common and effective method used in industry for robot collision detection is to observe the torque residual, that is, the difference between the measured joint torque and the predicted torque. The torque residual can be expressed as follows:

[0049]

[0050] In the above formula, is the torque residual calculated at the k - th sampling, τ kis the torque measured at the k-th sampling. During online calculation, the actual joint acceleration needs to be calculated through two interpolations based on the actual joint position. Large noise will affect the accuracy of the collision signal. Therefore, the commanded joint signal, i.e., q d 、 and are used to calculate the regression matrix. For simplicity, in the following text, will be denoted as Y.

[0051] Another classic robot collision detection method is the generalized momentum observer method, where the change in the robot's generalized momentum is attributed to an external collision. The output of the momentum observer can be expressed as follows:

[0052]

[0053] In the above equation, is the output signal of the momentum observer at the k-th sampling, is the diagonal constant gain matrix. p k is the generalized momentum of the robot at the k-th sampling, which can be calculated by , and p0 is the initial momentum of the robot. r m，k-1 is the output signal of the momentum observer at the (k - 1)-th sampling. T is the sampling period. p k and include matrices and vectors, i.e., M at the k-th sampling k , C k , g k and τ f,k . The calculation of these quantities depends on the dynamic parameters at the torque level

[0054] For the vast majority of industrial robots, there are usually no joint torque sensors, and only the motor current can be measured. To estimate the joint torque of the robot, the linear relationship between the motor current and the joint torque τ can be assumed as τ = K i , where the diagonal torque constant matrix is Substituting the above linear relationship into Equation (1) and dividing both sides of Equation (1) by K, the robot dynamics equation can be obtained as follows, i.e., for the j-th joint:

[0055]

[0056] In the equation, M j , I a,j , C j are the j-th rows of M, I a , C respectively, g j , K jThen they are respectively the j-th element of g and the j-th element of K.

[0057] Since (6) is obtained by dividing both sides of (1) by K, (6) can also be written in a linear form similar to (3), that is j The motor current of the j-th joint is i . The j-th row of Y is Y j . j . After that, the above n linear equations are combined into a matrix form as follows. The above equation can be written in the following linear form:

[0058]

[0059] Due to the structural zero elements, the above regressor is rank-deficient. The linearly dependent columns in the regressor can be removed by QR decomposition. Then, Equation (7) can be rewritten as follows:

[0060] i = Hχ (8)

[0061] In the formula, are the base parameters set at the current level, is the corresponding regression matrix, full rank. It is worth explaining that H is also a function of α. The element numbers in χ are r.

[0062] After collecting N groups of joint data, the measured current and the regressor can be stacked as follows:

[0063]

[0064] In the formula, the stacked current vector is The stacked regression matrix

[0065] Directly use the measured joint current in the above relationship to identify the nonlinear friction vector coefficient α of the industrial robot joint and the set of dynamic base parameters χ at the current level WLS ;

[0066] The industrial robot collision detection method of this embodiment includes:

[0067] Step 1, input the nonlinear friction vector coefficient α of the industrial robot joint, the set of dynamic base parameters χ at the current level WLS and the collision detection threshold δ;

[0068] Step 2, given the joint command signal at time k: q d , and Collect the joint current i at time k k , and then combine it with q d , and the non - linear friction vector coefficient α of the joint, calculate the dynamic base parameter set χ at time k WLS The corresponding regression matrix

[0069] Step 3: Calculate the current residual r at time k i,k , Judge whether it satisfies r i,k > δ. If so, it is determined that the industrial robot has a collision. If not, k = k + 1, and go to Step 2

[0070] In this embodiment, by using the difference between the measured joint current and the predicted joint current calculated from the current - level dynamic parameters, the inverse dynamics at the current level can be identified using the measured joint current instead of the estimated joint torque. The method of this embodiment reduces the accumulation of torque - level dynamic identification errors caused by inaccurate joint torque constants, and finally improves the collision detection performance of robots without joint torque sensors. A large number of experimental results verify the correctness and effectiveness of this method

[0071] In this embodiment, the identification method of the dynamic base parameter set χ at the current level WLS is as follows

[0072] Generally, the parameter set χ at the current level can be identified by ordinary least squares (OLS), that is However, the OLS method is sensitive to the singularity of the regression matrix, and the variance of its identification result is not optimal. To improve the identification accuracy of χ, a double - loop iterative method can be used to eliminate outliers and adapt to the non - linear friction model. In the inner loop, outliers can be removed according to the current residual, and then the parameter set χ can be estimated by the weighted least squares (WLS) method. In the outer loop, the non - linear friction parameter α can be identified by non - linear optimization techniques

[0073] Outlier data caused by sampling noise or dynamic model uncertainty will affect the performance of linear regression and non - linear fitting. The data weighting matrix can be used to reduce the adverse effects of outliers

[0074]

[0075] Among them, the data weight vector is Its matrix form is Each column is W. The role of the mathematical operator ⊙ is to multiply the corresponding elements of two matrices or two vectors and are respectively and the versions after data weighting. Then, considering the above data weights, the basic parameter set at the current level can be solved by the WLS method as follows

[0076]

[0077] where χ WLS is the WLS solution of the dynamic parameter at the current level. The covariance matrix is which can be updated by The current residual vector can be calculated as The initial value of the covariance matrix can be calculated by where is the initial current residual vector,

[0078] When the current residual vector is known, the data weighting vector W and its matrix form W e can be updated to:

[0079]

[0080] where the function Ψ(·) can return an output vector with the same dimension as the input vector. If one of the elements in exceeds the set threshold δ, the corresponding element in W is set to 0, otherwise it is set to 1. The initial value of W is a vector with all elements equal to 1.

[0081] In this embodiment, a method for identifying the non - linear friction vector coefficient α of the industrial robot joint:

[0082] To identify the non - linear friction parameters, the joint friction current should be estimated first. Usually, the current of the joint moving at a constant speed can be measured. This method is suitable for independent joints but not for simultaneous movement of multiple joints. Assuming that the joint friction force is an odd function of the joint speed, it can be estimated by subtracting the currents measured from the forward and reverse symmetric excitation trajectories and taking half of it. However, obviously (2) does not satisfy this assumption. Therefore, in the preferred embodiment, the current corresponding to the inertial parameters is subtracted from the measured current. Then, the remaining current corresponds to the friction component. In addition, data weighting is also used to make the estimated friction fit (2) better:

[0083]

[0084] where the estimated friction current is χ WLS The inertial part in can be represented by χ WLS,i

[0085] Then, the non - linear friction parameter α can be calculated by non - linear optimization techniques. For example, for the j - th joint, the optimization problem can be expressed as:

[0086] ​

[0087] θ j is the optimization variable α j The initial value of the current is 1, is the friction model vector at the current level of the current residual, is the j-th row of is the matrix form of. Solving the optimization problem, the nonlinear friction coefficient α of the j-th joint is obtained j , and the nonlinear friction coefficients α of each joint j constitute the vector α.

[0088] As described above, most industrial robots are not equipped with joint torque sensors. Therefore, only the joint current can be measured. Then, the joint torque must be estimated by the product of the joint torque constant and the measured joint current. However, whether the joint torque constant is provided by the manufacturer or calibrated by researchers, they will necessarily contain certain errors. This will reduce the estimation accuracy of the joint torque. In addition, it will increase the cumulative identification error of the torque-level dynamic parameters, thereby reducing the collision detection performance of the model-based algorithm using torque-level dynamics. To solve this problem, this embodiment proposes a new method for collision detection of industrial robots based on current residuals. As the name implies, the current residual can be calculated by the difference between the measured joint current and the predicted current. Since the set of dynamic parameters χ WLS at the current level is identified, the predicted joint current can be calculated through it. Since the measured joint torque is not required when calculating the current residual, the above problems can be avoided and the collision detection performance can be improved.

[0089] When χ WLS is known, the current residual can be calculated by the following formula:

[0090]

[0091] In the above formula, is the current residual calculated at the k-th sampling, and i k is the current measured at the k-th sampling. is the regression matrix with respect to χ WLS at the k-th sampling. Due to an external collision, a spike will appear in the current residual. Once the peak value exceeds the set threshold, it means that a collision has occurred.

[0092] The process of the balloon collision experiment of the current residual method is as follows.

[0093] Set the desired linear velocity of the end effector of the robotic arm along the Z-axis to -0.7 m / s. Similarly, set the first 0.3 seconds after the robotic arm starts to be in a sleep state, that is, the collision detection function is not triggered. Then, turn on the collision detection function after 0.3 seconds. After online calculation, the current residuals of joint 2 and joint 3 are as shown in Figure 2 (a) and (b). In addition, set the current residual detection thresholds of joint 2 and joint 3 to 1.05 A and 1.02 A respectively. It can be found from the figure that at about 0.656 seconds, the collision is successfully detected. The contact force generated between the end effector of the robotic arm and the balloon causes the current residuals of these two joints to gradually increase. When the residual exceeds the set threshold, it is regarded as a collision occurring. After detecting the collision, the robotic arm immediately enters an emergency stop state, and then returns to the initial pose at a linear velocity of 0.1 m / s. The velocity curve of the end effector of the robotic arm in the / -axis direction is as shown in Figure 2 (c). As shown in Figure 2 and Figure 3 , during the separation process of the end effector of the robotic arm and the balloon, since it will still be in contact with the balloon, the current residual is still large and will oscillate. When the end effector of the robotic arm is completely separated from the balloon, the current residual tends to be stable and approaches 0 A. The maximum collision contact force generated in the Z-axis direction can be measured by the six-axis force sensor at the end of the robotic arm to be approximately -25 N, as shown in Figure 2 (d). Similarly, this six-axis force sensor is only used for measuring and quantitatively analyzing the external collision force, and the collision detection is realized by the current residual method.

[0094] Although the present invention has been described herein with reference to specific embodiments, it should be understood that these embodiments are merely examples of the principles and applications of the present invention. Therefore, it should be understood that many modifications can be made to the exemplary embodiments, and other arrangements can be designed, as long as they do not deviate from the spirit and scope of the present invention as defined by the appended claims. It should be understood that the different dependent claims and the features described herein can be combined in a manner different from that described in the original claims. It should also be understood that the features described in connection with a single embodiment can be used in other described embodiments.

Claims

1. A collision detection method for an industrial robot, characterized in that: The method comprises: S1, input the nonlinear friction vector coefficient α of the industrial robot joint, the dynamic basis parameter set χ at the current level WLS and collision detection threshold δ; i=Hχ, i represents the joint current of the industrial robot, is the basic parameter set at the current level, is the corresponding regression matrix, and is full rank; the dynamic basis parameter set χ WLS is the estimated value of χ; S2, given the joint command signal at time k: q d , and Collect joint current i at time k k , combined with q d , and the nonlinear friction vector coefficient α of the joint, calculate the dynamic basis parameter set χ at time k WLS The corresponding regression matrix S3. Calculate the current residual r at time k i，k , Determine whether r is satisfied i，k >δ, if yes, it is determined that the industrial robot has collided, if not, k=k+1, and go to S2.

2. The industrial robot collision detection method according to claim 1, characterized in that: The method for obtaining the nonlinear friction vector coefficient α of the joint: For the jth joint, establish the optimization problem: s.t.0<α j <1 Optimization variables Nonlinear friction vector coefficient α of the jth joint j The initial value of is 1. is the friction model vector of the current residual at the current level, τ f,j represents the joint friction torque vector of the j-th joint, is the diagonal torque constant matrix, K j represents the j-th element of K, yes The jth row of yes In matrix form, represents the estimated joint friction current, n represents the number of robot degrees of freedom, and N represents the number of sets of joint currents collected; Solve the optimization problem and obtain the nonlinear friction coefficient α of the jth joint j , the nonlinear friction coefficient α of each joint j Constitute the vector α.

3. The industrial robot collision detection method according to claim 1, characterized in that: Estimation of joint friction current for: Among them, χ WLS，i Represents χ WLS The inertial part of represents the weighted regression matrix.

4. The industrial robot collision detection method according to claim 2, characterized in that: The joint friction torque vector τ of the jth joint f，j for: Among them, F c，j 、F v，j , B j is the linear friction coefficient of the jth joint, q j is the coordinate of the jth joint.

5. The industrial robot collision detection method according to claim 1, characterized in that: The weighted least squares (WLS) method is used to estimate the kinetic basis parameter set χ WLS .

6. The industrial robot collision detection method according to claim 5, characterized in that: The covariance matrix is pass Update ∑, current residual vector is the data weight vector, W e Each column in is W; is the stacked current vector, is the stacked regression matrix, N represents the number of joint currents collected, n represents the number of robot degrees of freedom, and r represents the dimension of the elements in χ; the initial value of the variance matrix ∑ is the initial current residual vector, χ OLS is the estimated value of χ obtained by least squares.

7. The industrial robot collision detection method according to claim 5, characterized in that: When the current residual vector When known, the data weight vector W and its matrix form W e Updated to: The function Ψ(·) outputs a vector with the same dimension as the input vector, and its function is: If an element in exceeds the collision detection threshold δ, the element at the corresponding position in W is set to 0, otherwise it is set to 1; the initial value of W is a vector with all elements set to 1.

8. A computer-readable storage device storing a computer program, characterized in that: When the computer program is executed, the industrial robot collision detection method according to any one of claims 1 to 7 is implemented.

9. An industrial robot collision detection device, comprising a storage device, a processor, and a computer program stored in the storage device and executable on the processor, characterized in that: The processor executes the computer program to implement the industrial robot collision detection method according to any one of claims 1 to 7.

Images