A robot arm collision detection method based on generalized momentum nonlinear disturbance observer

Abstract

The application relates to the technical field of mechanical arm collision detection, and specifically provides a mechanical arm collision detection method based on a generalized momentum nonlinear disturbance observer, which comprises the following steps: according to a dynamic state equation, a nonlinear disturbance observer is established; unknown concentrated disturbance is obtained through the nonlinear disturbance observer; the unknown concentrated disturbance is filtered through a filter; after the filtering, the unknown concentrated disturbance is compared with a time-varying threshold value; if the unknown concentrated disturbance exceeds the range of the time-varying threshold value, it is determined that the mechanical arm collides; otherwise, the mechanical arm does not collide. The detection method provided by the application adopts the nonlinear disturbance observer and the time-varying threshold value, does not depend on sensor equipment and experimental data, is simple and reliable, has fast response speed, has small observation error, and can still realize collision detection when the dynamic model is inaccurate.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of mechanical arm collision detection, and specifically provides a mechanical arm collision detection method based on a generalized momentum nonlinear disturbance observer. BACKGROUND

[0002] Industrial robots are widely used in various fields, such as polishing, handling and welding, etc. However, they have safety hazards in industrial production applications, which 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 operation safety.

[0003] According to whether based on the dynamics model of the mechanical arm, the collision detection method can be divided into two categories: model-free method and model-based method. The model-free method does not depend on the dynamics model of the mechanical arm, so it does not need to identify its dynamics parameters, but this method has the following shortcomings:

[0004] 1. The model-free method needs to collect and learn experimental data extensively, which requires a large amount of data collection and calculation work, and puts significant requirements on the computing power of the controller;

[0005] 2. The model-free collision detection method relies on a series of sensors installed on the robot to collect external force / torque information, including force sensors, tactile surface sensors and cameras, although these sensors can effectively detect collisions, but will increase the cost of industrial production;

[0006] 3. The sensors are easily affected by uncertain working environments such as high temperature and noise, which makes the data collected by the sensors invalid, and the invalid data is easy to cause the system to make a false judgment, so that the system cannot accurately determine whether a collision has occurred;

[0007] 4. Most industrial robots usually lack force sensors, and installing sensor devices will increase the cost of industrial production, so it is not practical.

[0008] The model-based collision detection method of the robot relies on a high-precision dynamics model, and compared with the model-free method, the model-based method has significant advantages in cost and practicability. This method does not need additional sensors to detect external force, but uses disturbance observers and collected joint current, angle, velocity and acceleration data to estimate external disturbance, but this method has the following shortcomings:

[0009] 1. The dynamics model is usually uncertain, which may cause the system to make a false judgment, thinking that the system has collided when there is no collision;

[0010] 2. The model-based robot collision detection method needs to collect joint current and angle in real time, and the collected data inevitably contains a large amount of data noise, which easily affects the sensitivity and accuracy of the collision detection;

[0011] 3. The traditional collision detection method based on the first-order generalized momentum disturbance observer has slow response speed, poor accuracy, and is easily disturbed by external noise.

[0012] After actual testing, the disturbance observer based on generalized momentum has been widely used and good observation results have been obtained, and the advantages are simple and reliable structure, 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 collision detection method, it is necessary to design a more sensitive and efficient robot collision detection method, and it is crucial to develop a high-performance generalized momentum disturbance observer, and for this, the application provides a robot collision detection method based on a generalized momentum nonlinear disturbance observer. SUMMARY

[0013] The application provides a robot collision detection method based on a generalized momentum nonlinear disturbance observer, which adopts a nonlinear disturbance observer and a time-varying threshold, is simple and reliable, has fast response speed, and small observation error.

[0014] The robot collision detection method based on the generalized momentum nonlinear disturbance observer provided by the application comprises the following steps.

[0015] S1: establishing a dynamic state equation of the robot arm;

[0016] S2: establishing a nonlinear disturbance observer according to the dynamic state equation:

[0017]

[0018] wherein, represents a first state variable of the disturbance observer, and is the observed generalized momentum of the robot arm, represents a second state variable of the disturbance observer, and is the observed concentrated disturbance applied to the robot arm, represents a first gain parameter, represents a second gain parameter, represents a third gain parameter, represents an observed unknown concentrated disturbance, represents an estimated generalized momentum of the robot arm, ​denotes the lumped moment of the robot arm, , denotes the input moment of the robot arm, denotes the Coriolis force of the robot arm, denotes the gravity force of the robot arm, denotes the friction moment of the robot arm, denotes the dynamics model error compensated by Gaussian process regression, denotes the extended state variable of the nonlinear disturbance observer, denotes the actual generalized momentum of the robot arm, denotes the dynamics model error of the robot arm, denotes the external disturbance moment, denotes time, denotes a first coefficient, denotes a second coefficient, denotes a third coefficient, and , , ;

[0019] unknown lumped disturbance is obtained by the nonlinear disturbance observer ;

[0020] S3: Identify the uncertainty of the robot arm dynamics model as follows:

[0021] ;

[0022] wherein, denotes the uncertainty of the dynamics model, denotes the uncertainty of the inertia moment of the robot arm, denotes the uncertainty of the friction moment of the robot arm, is the uncertainty of the inertia parameter of the robot arm, , , and denotes the uncertainty of the friction parameter.

[0023] S4: Establish a time-varying threshold according to the uncertainty of the dynamics model:

[0024] ;

[0025] wherein, denotes the upper limit of the time-varying threshold, denotes the lower limit of the time-varying threshold, denotes a first gain coefficient of the time-varying threshold, denotes a second gain coefficient of the time-varying threshold, denotes the uncertainty of the inertia parameter of the robot arm, is an estimate of an uncertainty of the friction torque, is an estimate of a first bias parameter of the time-varying threshold, a second bias parameter of the time-varying threshold, denotes a regression matrix of the robot dynamics model without the friction term;

[0026] S5: judging whether the robot collides or not according to the time-varying threshold:

[0027] ;

[0028] when , the robot collides;

[0029] when , the robot does not collide.

[0030] Preferably, the dynamics state equation is:

[0031] ;

[0032] wherein, denotes a first state variable of the dynamics model, denotes a generalized momentum of the robot, denotes a second state variable of the dynamics model, denotes a third state variable of the dynamics model, denotes an output of the dynamics model, , denotes a real lumped disturbance, denotes an external disturbance torque, denotes a dynamics model error.

[0033] Preferably, S2 further comprises: filtering the unknown lumped disturbance by a filter.

[0034] Preferably, the expression of the filter is:

[0035] ;

[0036] wherein, denotes a damping of the filter, denotes a bandwidth of the filter, denotes a high-speed filter bandwidth, denotes a low-speed filter bandwidth, denotes a gain coefficient of the filter.

[0037] Compared with the prior art, the present application can achieve the following beneficial effects:

[0038] The application provides a mechanical arm collision detection method based on a generalized momentum nonlinear disturbance observer. An external disturbance received by the mechanical arm is obtained through the nonlinear disturbance observer, and a time-varying threshold is set to determine whether the received external disturbance is from a collision. Compared with a traditional first-order and second-order linear disturbance observer, the nonlinear disturbance observer can accurately estimate the external disturbance in a limited time, so that the control system of the mechanical arm can quickly restore performance, thereby improving the response speed and stability of the control system. To meet the nonlinear characteristics of the method of the application, based on the dynamics model and regression matrix of the mechanical arm, the application provides a brand-new time-varying threshold, which can effectively contain the uncertainty of the model, and collision misjudgment will not occur in the case of inaccurate dynamics model. The collision detection algorithm of the application does not rely on external sensor equipment, only needs the mechanical arm joint motor encoder to read the real-time joint position and real-time joint motor current of the mechanical arm, and identifies the disturbance and determines whether the disturbance is from a collision according to the real-time joint position and real-time joint motor current, thereby effectively reducing the cost of the control system and making the collision judgment more simple and accurate. BRIEF DESCRIPTION OF DRAWINGS

[0039] Figure 1 is a flowchart of the mechanical arm collision detection provided according to an embodiment of the application. DETAILED DESCRIPTION

[0040] In the following description, the same reference numerals are used to denote the same components throughout the drawings. 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.

[0041] In order to make the purpose, technical solutions and advantages of the application more clear, the application will be further described in detail below with reference to the drawings and specific embodiments. It should be understood that the specific embodiments described herein are only used to explain the application and do not constitute a limitation on the application.

[0042] As shown in Figure 1 , the application provides a mechanical arm collision detection method based on a generalized momentum nonlinear disturbance observer, comprising:

[0043] S1: The dynamics model of the mechanical arm is crucial to the control of the mechanical arm. When designing a control algorithm, the more accurate the dynamics model is, the more effective the obtained control algorithm is. The expression of the dynamics model of the mechanical arm established in the embodiment of the application is:

[0044] ;

[0045] wherein, represents the real-time joint output torque of the mechanical arm, Indicates the external disturbance torque. The inertia matrix represents the estimated dynamic model. This represents the estimated Coriolis force and centrifugal force matrices. This represents the estimated gravity matrix. This represents the estimated friction matrix. Indicates the error of the dynamic model. Indicates the joint angle of the robotic arm. Indicates the joint speed of the robotic arm. This indicates the acceleration of the robotic arm joints.

[0046] Simplifying the dynamic model of the robotic arm makes analysis and calculation more convenient and facilitates controller design. Therefore, this embodiment of the invention linearizes the dynamic model of the robotic arm as follows:

[0047] The dynamic parameters of the robotic arm include the six components of the inertia tensor, the position of the link's center of mass, the link's mass, and the inertia of the joint motor. To more effectively estimate and represent the dynamic parameters of the robotic arm, these parameters are recombine into a minimal parameter set and jointly represented as a regression matrix.

[0048] ;

[0049] in, Represents the regression matrix. This represents the actual standard dynamic parameters. This represents the estimated standard dynamic parameters. This indicates the number of standard dynamic parameters.

[0050] Typically, the actual standard dynamic parameters Including dynamic parameters for all motion types, we have:

[0051] ;

[0052] ;

[0053] in, The six components of the inertia tensor. Indicates the link The position of the center of mass, Indicates the link quality This indicates the inertia of the joint motor.

[0054] A Gaussian process regression model is used to train the dynamic model, and the trained Gaussian process regression model is used to compensate for the error in the dynamic model, further reducing the error in the robotic arm's dynamic model. This invention embodiment uses... represents any one joint of the robot arm, for the joint , the input of the Gaussian process regression model is , the output of the Gaussian process regression model is , wherein, represents the joint angular velocity of the joint , represents the joint angular acceleration of the joint , represents the friction torque of the joint , represents the identification error generated in the process of identifying the dynamic parameters using the least square method. 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.

[0055] Let represent the input data set of the training set in the Gaussian process regression model, represent the output data set of the training set in the Gaussian process regression model. In the Gaussian process regression model, three parameters , and need to be optimized, , in order to improve the prediction performance of the Gaussian process regression model, the appropriate hyperparameters need to be selected. The embodiment of the application adopts a radial basis function as a kernel function, and the optimization of the hyperparameters can be expressed as follows:

[0056] ;

[0057] , wherein, represents the maximum likelihood probability, represents the radial basis kernel function, represents the variance, represents the unit matrix, represents the number of samples in the training set.

[0058] The dynamic model error compensated by the Gaussian process regression model is represented by , then the dynamic model of the robot arm can be transformed as:

[0059] .

[0060] Since the dynamic model of the robot arm satisfies: , the generalized momentum of the robot arm is represented by , then the optimization expression of the hyperparameters can be transformed as:

[0061] ;

[0062] The joint angular acceleration of the robot arm The noise contained in the middle can be removed after linearization, and the dynamics model of the generalized momentum-based robot arm can be expressed as:

[0063] .

[0064] To avoid calculating the inertia matrix, the Coriolis force and centrifugal force matrix, and the gravity matrix of the robot arm, a dynamics linearization method is used to calculate the torque of the robot arm. The identified nonlinear friction model can be reconstructed and used to replace the linear friction model in the regression matrix.

[0065] The generalized momentum, inertia, and gravity torque of the robot can be calculated as follows:

[0066] ;

[0067] wherein, represents the regression matrix corresponding to the generalized momentum, represents the regression matrix corresponding to the inertia force matrix excluding the influence of gravity, represents the regression matrix corresponding to the gravity, which only depends on the joint position of the robot arm 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 dynamics base parameter.

[0068] The dynamics state equation of the robot arm can be expressed as:

[0069] ;

[0070] wherein, represents the first state variable of the dynamics model, represents the generalized momentum of the robot arm, represents the second state variable of the dynamics model, represents the third state variable of the dynamics model, represents the output of the dynamics model, represents the real concentrated disturbance.

[0071] S2: According to the dynamics state equation of the robot arm described above, a third-order finite-time nonlinear extended state disturbance observer is established, and the expression of the disturbance observer is as follows:

[0072] ;

[0073] wherein, represents the first state variable of the disturbance observer, which is the observed generalized momentum of the robot arm, represents the second state variable of the disturbance observer, is the observed concentrated disturbance applied to the robot arm, represents the first gain parameter, represents the second gain parameter, represents the third gain parameter, the first gain parameter , the second gain parameter and the third gain parameter is a fixed value, which is valued according to previous experience, represents the observed unknown concentrated disturbance, represents the estimated generalized momentum of the robot arm, represents the concentrated 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 robot arm, represents the robot arm dynamics model error, represents the external disturbance torque, represents 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 , , .

[0074] wherein, represents the sign function, Figure 1 the "integral" in the above formula is the inverse operation of derivation in mathematics.

[0075] The signal input of the robot arm is input into the above disturbance observer, so that the unknown concentrated disturbance can be obtained, and the signal of the robot arm includes the joint speed, the joint position and the joint current of the robot arm.

[0076] Due to the strong complexity of friction modeling, the friction error is difficult to identify, and it is difficult to obtain an accurate model when the static friction modeling is difficult to obtain an accurate model, and when the speed is zero, the torque peak value problem occurs, which may cause false alarm of the control system, and the embodiment of the present application adopts a band-pass filter with time-varying bandwidth to reduce the influence of inaccurate friction, and through the filter, the unknown concentrated disturbance​ The filtering process eliminates the peak values at the initial moment and low speed moment, avoiding false judgments. At the same time, the output of the nonlinear disturbance observer may have a relatively large shock, and the filtering process can reduce the shock. The expression of the filter is as follows:

[0077] ;

[0078] wherein, denotes the damping of the filter, denotes the bandwidth of the filter, denotes the high-speed filter bandwidth, denotes the low-speed filter bandwidth, denotes the gain coefficient of the filter, which is the adjustment factor of the filter.

[0079] When the joint speed is relatively high, i.e. and , the influence of static friction is relatively small, therefore, the bandwidth of the filter is set relatively large to improve the corresponding speed and accuracy of the filter; when the joint speed is relatively low, i.e. and , the error caused by friction is relatively large, in order to reduce the influence of the output jump in the disturbance observer, the bandwidth of the filter is set relatively small to reduce the response speed of the filter.

[0080] S3: In the field of robotic arms, the accuracy of actual force estimation and collision detection directly affects the safety and performance of the robotic arm system, and the actual force estimation and collision detection error can be caused by measurement noise, modeling error and friction force. Model error, measurement noise and other uncertainties are divided into two parts: robotic arm inertia uncertainty and friction torque uncertainty, as shown below:

[0081] ;

[0082] wherein, denotes the uncertainty of the dynamic model, i.e. the deviation between the output of the disturbance observer and the actual torque, denotes the uncertainty of the inertia moment of the robotic arm, denotes the uncertainty of the friction moment of the robotic arm, is the uncertainty of the inertia parameters of the robotic arm, , , and denote the uncertainty of the friction parameters.

[0083] S4: After identifying the uncertainty of the dynamic model, the embodiment of the present application sets a time-varying threshold, which can adapt to the nonlinear characteristics of the method of the embodiment of the present application, to reduce the uncertainty of the model. The time-varying threshold is set as follows:

[0084]

[0085] wherein, 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, represents the second gain coefficient of the time-varying threshold, the first gain coefficient and the second gain coefficient are fixed values, which are valued according to experience accumulation, represents the uncertainty of the inertia parameter of the robot arm, is an estimated value of represents the uncertainty of the friction torque, is an estimated value of represents the first bias parameter of the time-varying threshold, represents the second bias parameter of the time-varying threshold, the smaller the first bias parameter and the second bias parameter, the more sensitive the response of the time-varying threshold, and at the same time, the possibility of misjudgment is increased, therefore, the first bias parameter and the second bias parameter should be valued according to actual situation, represents the regression matrix of the robot arm dynamic model without friction term.

[0086] S5: After setting the time-varying threshold, whether the robot arm collides can be judged according to the time-varying threshold, when no collision occurs, the unknown concentrated disturbance output by the disturbance observer is kept 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 the collision occurs. The specific judgment method is as follows:

[0087]

[0088] When , the robot arm collides;

[0089] When , the robot arm does not collide.

[0090] Although the embodiments of the present application have been shown and described above, it can be understood that the above embodiments are exemplary and cannot be understood as limiting the present application. Those skilled in the art can make changes, modifications, replacements and variations to the above embodiments within the scope of the present application.

[0091] ​​​​The above detailed description of the application is not intended to limit the scope of the application. Various other changes and modifications of the application can be made by those skilled in the art without departing from the scope of the application.

Claims

1. A collision detection method for a robotic arm based on a generalized momentum nonlinear perturbation observer, characterized in that, include: S1: Establish the dynamic state equations of the robotic arm; S2: Based on the aforementioned dynamic state equations, establish a nonlinear disturbance observer: ； in, The first state variable representing the perturbation observer is the observed generalized momentum of the robotic arm. The second state variable representing the perturbation observer is the observed concentrated perturbation applied to the robotic arm. Indicates the first gain parameter. This represents the second gain parameter. This represents the third gain parameter. This represents the observed, unknown concentrated disturbance. This represents the estimated generalized momentum of the robotic arm. This indicates the lumped torque of the robotic arm. , This indicates the input torque of the robotic arm. The Coriolis force represents the force on the robotic arm. This indicates the weight of the robotic arm. This represents the frictional torque of the robotic arm. This represents the error in the dynamic model compensated by Gaussian process regression. This represents the extended state variables of the nonlinear perturbation observer. This represents the actual generalized momentum of the robotic arm. This indicates the error in the robotic arm's dynamics model. Indicates the external disturbance torque. Indicates time, Indicates the first coefficient. Indicates the second coefficient. Denotes the third coefficient, and , , ; The unknown concentrated disturbance is obtained through the nonlinear disturbance observer. ; S3: Identify the uncertainties in the robotic arm dynamics model as follows: ； in, This represents the uncertainty of the dynamic model. This represents the uncertainty of the robotic arm's inertial torque. This indicates the uncertainty of the frictional torque of the robotic arm. It is the uncertainty of the robotic arm's inertial parameters. , , and This indicates the uncertainty of friction parameters; S4: Based on the uncertainty of the dynamic model, establish a time-varying threshold: ； in, This represents the upper limit of the time-varying threshold. This represents the lower limit of the time-varying threshold. The first gain coefficient represents the time-varying threshold. The second gain coefficient represents the time-varying threshold. This represents the uncertainty in the inertial parameters of the robotic arm. yes The estimated value, This represents the uncertainty of the frictional torque. yes The estimated value, The first bias parameter represents the time-varying threshold. The second bias parameter represents the time-varying threshold. Represents the regression matrix of the robotic arm dynamics model that does not include friction terms; S5: Based on the time-varying threshold, determine whether the robotic arm has collided. ； when The robotic arm collided; when The robotic arm did not collide.

2. The robotic arm collision detection method based on a generalized momentum nonlinear perturbation observer as described in claim 1, characterized in that, The dynamic state equation is: ； in, Represents the first state variable of the dynamic model. The generalized momentum of the robotic arm. This represents the second state variable in the dynamic model. This represents the third state variable in the dynamic model. This represents the output of the dynamic model. This indicates a real concentrated disturbance. Indicates the external disturbance torque. This represents the error in the dynamic model.

3. The robotic arm collision detection method based on a generalized momentum nonlinear perturbation observer as described in claim 1, characterized in that, The S2 further includes: filtering the unknown concentrated disturbance through a filter. Perform filtering.

4. The robotic arm collision detection method based on a generalized momentum nonlinear perturbation observer as described in claim 3, characterized in that, The expression for the filter is: ； in, Indicates the damping of the filter. Indicates the bandwidth of the filter. Indicates the bandwidth of the high-speed filter. Indicates the bandwidth of the low-speed filter. This represents the gain coefficient of the filter.

Images