Mechanical arm collision detection method based on generalized momentum nonlinear disturbance observer

Abstract

The invention relates to the technical field of mechanical arm collision detection, in particular to a mechanical arm collision detection method based on a generalized momentum nonlinear disturbance observer, which comprises the following steps of: establishing the nonlinear disturbance observer according to a dynamic state equation, obtaining unknown concentrated disturbance through the nonlinear disturbance observer, and determining the collision of the mechanical arm. The method comprises the following steps of: establishing a time-varying threshold value of the mechanical arm, filtering unknown concentrated disturbance through a filter, comparing the unknown concentrated disturbance with the established time-varying threshold value after the unknown concentrated disturbance is processed through the filter, and judging that the mechanical arm is collided if the unknown concentrated disturbance exceeds the range of the time-varying threshold value, otherwise, judging that the mechanical arm is not collided. The detection method provided by the invention adopts the nonlinear disturbance observer and the time-varying threshold, does not depend on sensor equipment and experimental data, is simple and reliable, is high in response speed and small in observation error, and can still realize collision detection when a kinetic model is inaccurate.

Description

Technical Field

[0001] The invention relates to the technical field of robot arm collision detection, and specifically provides a robot arm collision detection method based on a generalized momentum nonlinear disturbance observer. Background Art

[0002] Industrial robotic arms are widely used in various fields, such as polishing, handling, and welding. However, they pose safety risks in industrial production applications and may cause harm to humans or equipment. Therefore, fast and reliable collision detection can ensure the safety of human partners and equipment, thereby reducing potential risks and improving overall operational safety.

[0003] According to whether they are based on the dynamic model of the robot, collision detection methods can be divided into two categories: model-free methods and model-based methods. Model-free methods do not rely on the dynamic model of the robot, so there is no need to identify its dynamic parameters, but this method has the following disadvantages: 1. Model-free methods require extensive collection and learning of experimental data, a large amount of data acquisition and calculation work, and significant requirements on the computing power of the controller; 2. Model-free collision detection methods rely on a series of sensors mounted on the manipulator to collect external force / torque information, including force sensors, tactile surface sensors, and cameras. Although these sensors can effectively detect collisions, they will increase industrial production costs; 3. Sensors are easily affected by uncertain working environments such as high temperature and noise, which may invalidate the data collected by the sensors. Invalid data may easily lead to misjudgment of the system, making it impossible to accurately determine whether a collision has occurred. 4. Most industrial robotic arms usually lack force sensors, and installing sensing equipment will increase industrial production costs and is therefore impractical.

[0004] The model-based robot arm collision detection method relies on a high-precision dynamic model. Compared with the model-free method, the model-based method has significant advantages in cost and practicality. This method does not require additional sensors to detect external forces, but uses a disturbance observer and the collected joint current, angle, velocity and acceleration data to estimate the external disturbance. However, this method has the following disadvantages: 1. Dynamic models are usually uncertain, which may cause the system to misjudge and think that a collision has occurred when there is no collision; 2. The model-based robot arm collision detection method requires real-time acquisition of joint current and angle. The acquired data inevitably contains a large amount of data noise, which can easily affect the sensitivity and accuracy of collision detection. 3. The traditional collision detection method based on first-order generalized momentum disturbance observer has slow response speed, poor accuracy and is easily disturbed by external noise.

[0005] After actual testing, the disturbance observer based on generalized momentum has been widely adopted and has achieved good observation results. Its advantage is that the structure is simple and reliable. However, the low-order linear generalized momentum disturbance observer is easily affected by noise and has a relatively narrow bandwidth. In order to overcome these shortcomings and improve the effectiveness and reliability of the robot arm collision detection method, it is necessary to design a more sensitive and efficient robot arm collision detection method. It is crucial to develop a high-performance generalized momentum disturbance observer. To this end, the present invention proposes a robot collision detection method based on a generalized momentum nonlinear disturbance observer. Compared with the traditional observer, the new nonlinear momentum disturbance observer proposed in the present invention provides better response speed and smaller observation error. Summary of the invention

[0006] In order to solve the above problems, the present invention provides a robot arm collision detection method based on a generalized momentum nonlinear disturbance observer. The detection method adopts a nonlinear disturbance observer and a time-varying threshold, does not rely on sensor equipment and experimental data, is simple and reliable, has a fast response speed, and has a small observation error.

[0007] The present invention provides a robot arm collision detection method based on a generalized momentum nonlinear disturbance observer, comprising: S1: Establish the dynamic state equation of the robot arm; S2: Based on the dynamic state equation, a nonlinear disturbance observer is established: ; in, represents the first state variable of the disturbance observer, which is the observed generalized momentum of the manipulator, The second state variable of the disturbance observer is the observed concentrated disturbance applied to the manipulator. represents the first gain parameter, represents the second gain parameter, represents the third gain parameter, represents the observed unknown concentrated disturbance, represents the estimated generalized momentum of the manipulator, represents the total torque of the robot arm, , represents the input torque of the robot arm, represents the Coriolis force of the robot arm, represents the gravity of the robot arm, represents the friction torque of the robot arm, represents the dynamic model error compensated by Gaussian process regression, represents the extended state variable of the nonlinear disturbance observer, represents the actual generalized momentum of the manipulator, represents the robot arm dynamics model error, represents the external disturbance torque, Indicates time, represents the first coefficient, represents the second coefficient, represents the third coefficient, and , , ; Through the nonlinear disturbance observer, the unknown concentrated disturbance is obtained ; S3: Identify the uncertainty of the robot arm dynamics model as follows: ; in, represents the uncertainty of the dynamics model, represents the uncertainty of the manipulator's moment of inertia, represents the uncertainty of the friction torque of the robot arm, is the uncertainty of the manipulator's inertial parameters, , , and represents the uncertainty in the friction parameters.

[0008] S4: Establish a time-varying threshold based on the uncertainty of the kinetic model: ; in, represents the upper limit of the time-varying threshold, represents the lower limit of the time-varying threshold, represents the first gain coefficient of the time-varying threshold, The second gain coefficient representing the time-varying threshold, represents the uncertainty of the inertial parameters of the robot arm, yes The estimated value of represents the uncertainty of the friction torque, yes The estimated value of represents the first bias parameter of the time-varying threshold, represents the second bias parameter of the time-varying threshold, represents the regression matrix of the robot arm dynamics model without friction terms; S5: Determine whether the robot arm collides based on the time-varying threshold: ; when , the robot arm collides; when , the robot arm did not collide.

[0009] Preferably, the kinetic state equation is: ; in, represents the first state variable of the dynamic model, represents the generalized momentum of the manipulator, represents the second state variable of the kinetic model, represents the third state variable of the dynamic model, represents the output of the kinetic model, , represents the real concentrated disturbance, represents the external disturbance torque, represents the error of the dynamic model.

[0010] Preferably, S2 further includes: filtering the unknown concentrated disturbance Perform filtering.

[0011] Preferably, the expression of the filter is: ; in, represents the damping of the filter, represents the bandwidth of the filter, represents the high-speed filter bandwidth, represents the low-speed filter bandwidth, Represents the gain coefficient of the filter.

[0012] Compared with the prior art, the present invention can achieve the following beneficial effects: The present invention proposes a robot arm collision detection method based on a generalized momentum nonlinear disturbance observer. The nonlinear disturbance observer obtains the external disturbance received by the robot arm, and sets a time-varying threshold to determine whether the external disturbance received is caused by a collision. Compared with the traditional first-order and second-order linear disturbance observers, the nonlinear disturbance observer can accurately estimate the external disturbance within a limited time, so that the control system of the robot arm can quickly restore performance, thereby improving the response speed and stability of the control system. In order to meet the nonlinear characteristics of the method of the present invention, based on the dynamic model and regression matrix of the robot arm, the present invention proposes a new time-varying threshold, which can effectively accommodate the uncertainty of the model, and will not cause collision misjudgment when the dynamic model is inaccurate. The collision detection algorithm of the present invention does not rely on external sensor equipment, but only requires the robot arm joint motor encoder to read the real-time joint position and real-time joint motor current of the robot arm, and identifies the disturbance based on the real-time joint position and real-time joint motor current to determine whether the disturbance is caused by a collision, effectively reducing the cost of the control system and making the collision judgment simpler and more accurate. BRIEF DESCRIPTION OF THE DRAWINGS

[0013] Figure 1 is a flowchart of robot arm collision detection provided according to an embodiment of the present invention. DETAILED DESCRIPTION

[0014] Hereinafter, embodiments of the present invention will be described with reference to the accompanying drawings. In the following description, the same modules are represented by the same reference numerals. In the case of the same reference numerals, their names and functions are also the same. Therefore, the detailed description thereof will not be repeated.

[0015] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and do not constitute a limitation of the present invention.

[0016] like Figure 1 As shown, the present invention provides a robot arm collision detection method based on a generalized momentum nonlinear disturbance observer, comprising: S1: The dynamic model of the robot arm is crucial to the control of the robot arm. When designing a control algorithm, the more accurate the dynamic model is, the more effective the control algorithm is. The expression of the dynamic model of the robot arm established in the embodiment of the present invention is: ; in, Represents the real-time joint output torque of the robot arm, represents the external disturbance torque, represents the inertia matrix of the estimated dynamic model, represents the estimated Coriolis and centrifugal force matrices, represents the estimated gravity matrix, represents the estimated friction matrix, represents the error of the dynamics model, Represents the joint angle of the robot arm, represents the robot joint speed, Represents the robot joint acceleration.

[0017] Simplifying the dynamic model of the robot arm can make the analysis and calculation more convenient and facilitate the design of the controller. Therefore, the embodiment of the present invention linearizes the dynamic model of the robot arm, and the process is as follows: The dynamic parameters of the robot include the six components of the inertia tensor, the center of mass position of the connecting rod, the mass of the connecting rod, and the inertia of the joint motor. In order to more effectively estimate and represent the dynamic parameters of the robot, the dynamic parameters are recombined into a minimum parameter set and jointly represented in the form of a regression matrix: ; in, represents the regression matrix, represents the actual standard dynamic parameters, denotes the estimated standard dynamic parameters, Indicates the number of standard dynamic parameters.

[0018] Typically, the actual standard dynamic parameters Including dynamic parameters of all motion types, there are: ; ; in, represents the six components of the inertia tensor, Indicates connecting rod The centroid position of Indicates connecting rod Quality, Indicates the inertia of the joint motor.

[0019] The Gaussian process regression model is used to train the dynamics model, and the trained Gaussian process regression model is used to compensate for the error of the dynamics model, thereby further reducing the error of the mechanical arm dynamics model. Represents any joint of the robot arm. , the input of the Gaussian process regression model is , the output of the Gaussian process regression model is ,in, Indicates joints The joint angular velocity, Indicates joints The joint angular acceleration, Indicates joints The friction torque, It represents the identification error generated in the process of least squares method to identify the dynamic parameters. By using the collected data set, a Gaussian process regression model is trained for each joint of the robot arm, and the dynamic model error is compensated.

[0020] make represents the input dataset of the training set in the Gaussian process regression model, Represents the output dataset of the training set in the Gaussian process regression model. There are three parameters in the Gaussian process regression model. , and Need to be optimized, In order to improve the prediction performance of the Gaussian process regression model, it is necessary to select appropriate hyperparameters. The embodiment of the present invention uses the radial basis function as the kernel function, and the optimization of the hyperparameters can be expressed as follows: ; in, represents the maximum likelihood probability, represents the radial basis kernel function, represents the variance, represents the identity matrix, Represents the number of samples in the training set.

[0021] The error of the dynamic model compensated by the Gaussian process regression model is Represented as: The dynamic model of the robot arm can be transformed into: .

[0022] Since the dynamic model of the robot arm satisfies: ,use Represents the generalized momentum of the robot, then the optimization expression of the hyperparameters can be transformed into: ; Joint angular acceleration of the robot arm The noise is contained in , which can be removed after linearization. The dynamic model of the manipulator based on generalized momentum can be expressed as follows after linearization: .

[0023] In order to avoid calculating the inertia matrix, Coriolis and centrifugal force matrices, and gravity matrix of the manipulator, a dynamic linearization method is used to calculate the torque of the manipulator. The identified nonlinear friction model can be reconstructed and used to replace the linear friction model in the regression matrix.

[0024] The generalized momentum, inertia, and gravity moments of the robot can be calculated as follows: ; in, represents the regression matrix corresponding to the generalized momentum, represents the regression matrix corresponding to the moment of inertia after excluding the influence of gravity, represents the regression matrix corresponding to gravity, where gravity depends only on the joint position of the manipulator and is not affected by the joint velocity and joint acceleration. represents the regression matrix corresponding to the friction torque, which is only related to the joint velocity. represents the kinetic basis parameters.

[0025] Then the dynamic state equation of the robot arm can be expressed as: ; in, represents the first state variable of the dynamic model, represents the generalized momentum of the manipulator, represents the second state variable of the kinetic model, represents the third state variable of the dynamic model, represents the output of the kinetic model, , represents the real concentrated disturbance.

[0026] S2: Based on the dynamic state equation of the above manipulator, a third-order finite-time nonlinear extended state disturbance observer is established. The expression of the disturbance observer is as follows: ; in, represents the first state variable of the disturbance observer, which is the observed generalized momentum of the manipulator, The second state variable of the disturbance observer is the observed concentrated disturbance applied to the manipulator. represents the first gain parameter, represents the second gain parameter, Indicates the third gain parameter, the first gain parameter , the second gain parameter and the third gain parameter is a fixed value, which is determined based on past experience. represents the observed unknown concentrated disturbance, represents the estimated generalized momentum of the manipulator, represents the total torque of the robot arm, , represents the input torque of the robot arm, represents the Coriolis force of the robot arm, represents the gravity of the robot arm, represents the friction torque of the robot arm, represents the extended state variable of the nonlinear disturbance observer, represents the actual generalized momentum of the manipulator, represents the robot arm dynamics model error, represents the external disturbance torque, Indicates time, represents the first coefficient, represents the second coefficient, represents the third coefficient, the first coefficient , the second coefficient and the third coefficient is related to the stability of the disturbance observer and satisfies , , .

[0027] ,in, represents the symbolic function, Figure 1 The inverse operation of "integration" in mathematics is the differentiation.

[0028] By inputting the signal of the robot arm into the above disturbance observer, the unknown concentrated disturbance can be obtained. ,The signals of the robot include the robot joint speed, joint position, and joint current.

[0029] Due to the high complexity of friction modeling, friction errors are difficult to identify, and it is difficult to obtain an accurate model for static friction modeling. When the speed passes through zero, a torque peak problem will occur. These peaks may cause false alarms in the control system. The embodiment of the present invention uses a bandpass filter with a time-varying bandwidth to reduce the impact of inaccurate friction. Through this filter, the unknown concentrated disturbance output by the disturbance observer is Filtering is performed to eliminate the peaks at the initial moment and the low-speed moment to avoid misjudgment. At the same time, the output of the nonlinear disturbance observer may have relatively large oscillations, which can be reduced by filtering. The expression of the filter is as follows: ; in, represents the damping of the filter, represents the bandwidth of the filter, represents the high-speed filter bandwidth, represents the low-speed filter bandwidth, It represents the gain coefficient of the filter and is the adjustment factor of the filter.

[0030] When the joint velocity is relatively high, i.e. and When the joint speed is relatively low, the influence of static friction is relatively small, so the bandwidth of the filter is set relatively large to improve the response speed and accuracy of the filter; when the joint speed is relatively low, that is, and When the error caused by friction is relatively large, in order to reduce the impact of output jumps in the disturbance observer, the bandwidth of the filter is set relatively small to reduce the response speed of the filter.

[0031] S3: In the field of robotic arm technology, the accuracy of actual force estimation and collision detection directly affects the safety of the robotic arm system and the performance of the robotic arm. The actual force estimation and collision detection errors may be caused by measurement noise, modeling errors and friction. Model errors, measurement noise and other uncertainties are divided into two parts: robotic arm inertia uncertainty and friction torque uncertainty, as shown below: ; in, represents the uncertainty of the dynamic model, that is, the deviation between the output of the disturbance observer and the actual torque, represents the uncertainty of the manipulator's moment of inertia, represents the uncertainty of the friction torque of the robot arm, is the uncertainty of the manipulator's inertial parameters, , , and represents the uncertainty in the friction parameters.

[0032] S4: After identifying the uncertainty of the dynamic model, the embodiment of the present invention sets a time-varying threshold, which can adapt to the nonlinear characteristics of the method of the embodiment of the present invention to reduce the uncertainty of the model. The time-varying threshold is set as follows: ; in, represents the upper limit of the time-varying threshold, represents the lower limit of the time-varying threshold, represents the first gain coefficient of the time-varying threshold, The second gain coefficient represents the time-varying threshold. The first gain coefficient and the second gain coefficient are fixed values, which are determined based on accumulated experience. represents the uncertainty of the inertial parameters of the manipulator, yes The estimated value of represents the uncertainty of the friction torque, yes The estimated value of represents the first bias parameter of the time-varying threshold, The second bias parameter represents the time-varying threshold. The smaller the first bias parameter and the second bias parameter are, the more sensitive the time-varying threshold response is, and the possibility of misjudgment will increase. Therefore, the values ​​of the first bias parameter and the second bias parameter should be designed according to the actual situation. Represents the regression matrix of the robot arm dynamics model without friction terms.

[0033] S5: After setting the time-varying threshold, the robot arm can be judged whether it has collided based on the time-varying threshold. When no collision occurs, the unknown concentrated disturbance output by the disturbance observer Keep within the upper and lower limits of the time-varying threshold. Once the unknown concentrated disturbance exceeds the time-varying threshold, it is considered that a collision has occurred. The specific judgment method is as follows: ; when , the robot arm collides; when , the robot arm did not collide.

[0034] Although the embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and cannot be understood as limiting the present invention. Those skilled in the art may change, modify, replace and modify the above embodiments within the scope of the present invention.

[0035] The above specific implementations of the present invention do not constitute a limitation on the protection scope of the present invention. Any other corresponding changes and modifications made based on the technical concept of the present invention should be included in the protection scope of the claims of the present invention.

Claims

1. A robot arm collision detection method based on generalized momentum nonlinear disturbance observer, It is characterized in that include: S1: Establish the dynamic state equation of the robot arm; S2: Based on the dynamic state equation, a nonlinear disturbance observer is established: ； in, represents the first state variable of the disturbance observer, which is the observed generalized momentum of the manipulator, The second state variable of the disturbance observer is the observed concentrated disturbance applied to the manipulator. represents the first gain parameter, represents the second gain parameter, represents the third gain parameter, represents the observed unknown concentrated disturbance, represents the estimated generalized momentum of the manipulator, represents the total torque of the robot arm, , represents the input torque of the robot arm, represents the Coriolis force of the robot arm, represents the gravity of the robot arm, represents the friction torque of the robot arm, represents the dynamic model error compensated by Gaussian process regression, represents the extended state variable of the nonlinear disturbance observer, represents the actual generalized momentum of the manipulator, represents the robot arm dynamics model error, represents the external disturbance torque, Indicates time, represents the first coefficient, represents the second coefficient, represents the third coefficient, and , , ; Through the nonlinear disturbance observer, the unknown concentrated disturbance is obtained ; S3: Identify the uncertainty of the robot arm dynamics model as follows: ； in, represents the uncertainty of the dynamics model, represents the uncertainty of the manipulator's moment of inertia, represents the uncertainty of the friction torque of the robot arm, is the uncertainty of the manipulator's inertial parameters, , , and represents the uncertainty in the friction parameters. S4: Establish a time-varying threshold based on the uncertainty of the kinetic model: ； in, represents the upper limit of the time-varying threshold, represents the lower limit of the time-varying threshold, represents the first gain coefficient of the time-varying threshold, The second gain coefficient representing the time-varying threshold, represents the uncertainty of the inertial parameters of the robot arm, yes The estimated value of represents the uncertainty of the friction torque, yes The estimated value of represents the first bias parameter of the time-varying threshold, represents the second bias parameter of the time-varying threshold, represents the regression matrix of the robot arm dynamics model without friction terms; S5: judging whether the robot arm collides based on the time-varying threshold: ； when , the robot arm collides; when , the robot arm did not collide.

2. The robot arm collision detection method based on generalized momentum nonlinear disturbance observer as claimed in claim 1, It is characterized in that The kinetic state equation is: ； in, represents the first state variable of the dynamic model, represents the generalized momentum of the manipulator, represents the second state variable of the kinetic model, represents the third state variable of the dynamic model, represents the output of the kinetic model, , represents the real concentrated disturbance, represents the external disturbance torque, represents the error of the dynamic model.

3. The robot arm collision detection method based on generalized momentum nonlinear disturbance observer as claimed in claim 1, It is characterized in that The S2 further includes: filtering the unknown concentrated disturbance Perform filtering.

4. The robot arm collision detection method based on generalized momentum nonlinear disturbance observer as claimed in claim 3, It is characterized in that The expression of the filter is: ； in, represents the damping of the filter, represents the bandwidth of the filter, represents the high-speed filter bandwidth, represents the low-speed filter bandwidth, represents the gain coefficient of the filter.

Images