Method, Device, Equipment and Storage Medium for Detecting Collision of Robot Arm

Abstract

The present application discloses a method, device, equipment and storage medium for robotic arm collision detection. Among them, the method includes establishing a robotic arm dynamics model; establishing a sliding mode momentum state observer based on a saturation function according to the robotic arm dynamics model; constructing a sliding mode disturbance observer according to the sliding mode momentum state observer, and estimating an external disturbance torque based on the sliding mode disturbance observer; and judging whether the robotic arm has collided according to the estimated external disturbance torque and a preset collision threshold. It is possible to implement without using additional sensors, only relying on the sensor data of the body, which reduces the cost. While being able to quickly and stably estimate the external disturbance torque, it avoids introducing the chattering phenomenon.

Description

Technical Field

[0001] The present invention relates to the technical field of industrial robots, and particularly relates to a method, device, equipment and storage medium for detecting collisions of a robotic arm. Background Art

[0002] When an industrial robotic arm works in an unknown environment or cooperates with humans, safety is the most fundamental factor to be considered. Especially in some dangerous and harsh working environments, the safety performance of human-machine interaction is particularly important; in human-machine interaction, the industrial robot takes some pre-protection measures to prevent collisions. Then, detecting whether a collision occurs between humans and machines is a crucial technical issue.

[0003] Currently, the technologies for robot collision detection mainly include the following categories:

[0004] 1) Rely on special external sensors for collision detection, such as six-axis force sensors, electronic intelligent skins, etc. This type of detection method cannot avoid the influence of sensor measurement noise. Secondly, it increases the structural complexity and raises the cost of the entire robotic arm. Since collision detection is not limited to a single local area, the cost of the required sensor equipment will account for a large proportion.

[0005] 2) Rely only on the sensors of the robot body for collision detection. The most commonly used method is to detect collisions based on the change of joint motor current. Although it avoids the influence of measurement noise brought by external sensors, this method is prone to misjudgment due to unstable current noise and has a great relationship with the hardware design.

[0006] 3) Rely on the input torque of the controller and the output of the joint angle information of the robotic arm to construct a disturbance observer for collision detection. This type of method relies on the sensors of the robot body and estimation algorithms for collision detection, which has the advantage of low cost, and avoids the influence of measurement noise brought by using external sensors. At the same time, it solves the problem of being prone to misjudgment due to unstable current noise. There are mainly two widely used methods. One is to perform collision detection based on a generalized momentum observer, but its rapidity is insufficient and needs to be enhanced. The other is based on the sliding mode momentum observer estimation method, which combines the advantages of fast convergence of the sliding mode, but introduces the chattering phenomenon. Summary of the Invention

[0007] Embodiments of the present invention provide a method, device, equipment and storage medium for detecting collisions of a robotic arm, which can quickly and stably estimate the external disturbance torque while avoiding introducing the chattering phenomenon.

[0008] In a first aspect, embodiments of the present invention provide a method for detecting collisions of a robotic arm, the method comprising:

[0009] Establish a dynamic model of the robotic arm;

[0010] Based on the dynamic model of the robotic arm, establish a sliding mode momentum state observer based on the saturation function;

[0011] Construct a sliding mode disturbance observer according to the sliding mode momentum state observer, and estimate the external disturbance torque based on this sliding mode disturbance observer;

[0012] Judge whether the robotic arm has collided according to the estimated external disturbance torque and the preset collision threshold.

[0013] In a second aspect, an embodiment of the present invention further provides a robotic arm collision detection device, including,

[0014] A robotic arm dynamic model construction module, configured to establish a dynamic equation of the robotic arm system model according to the Euler-Lagrange formula;

[0015] A sliding mode momentum state observation module, configured to monitor the disturbance observation quantities of each time point of the robotic arm;

[0016] A sliding mode disturbance observation module, configured to estimate the external disturbance torque;

[0017] A collision threshold setting module, configured to set a collision threshold based on the external disturbance torque before collision estimated by the sliding mode disturbance observation module;

[0018] A collision judgment module, configured to judge whether the robotic arm has collided according to the estimated external disturbance torque and the preset collision threshold.

[0019] In a third aspect, an embodiment of the present invention further provides a device, including a memory, a processor, and a computer program stored on the memory and executable on the processor; when the processor executes the program, it implements the robotic arm collision detection method provided by any embodiment of the present invention.

[0020] In a fourth aspect, an embodiment of the present invention further provides a computer-readable storage medium, on which a computer program is stored, and when the program is executed by a processor, it implements the robotic arm collision detection method provided by any embodiment of the present invention.

[0021] The technical solution of the embodiment of the present invention can be implemented only by relying on the sensor data of the body without using additional sensors, which reduces the cost. Compared with the existing mechanical arm collision detection method based on the generalized momentum observer, the method of the embodiment of the present invention combines a novel sliding mode momentum observer structure, which has fast convergence while maintaining the estimation stability. The adopted collision threshold setting rule can effectively improve the sensitivity of collision detection and reduce the false alarm rate. Compared with the existing collision detection method of the momentum observer based on the sliding mode technology, the collision detection method provided by the embodiment of the present invention adopts a novel sliding mode momentum observer structure, which includes a saturation function, effectively combines linear and nonlinear, uses high-gain linear correction in the adjacent region, and uses nonlinear variable structure correction in the region far from the equilibrium point, which can not only improve the rapidity but also effectively reduce the chattering phenomenon in the region near the equilibrium point. Description of the Drawings

[0022] Figure 1 It is a flowchart of a mechanical arm collision detection method in an embodiment of the present invention;

[0023] Figure 2 It is a flowchart of establishing a sliding mode momentum state observer based on a saturation function according to the mechanical arm dynamics model in an embodiment of the present invention;

[0024] Figure 3 It is a flowchart of establishing a sliding mode momentum state observer based on a saturation function according to the state equation of the mechanical arm momentum in an embodiment of the present invention;

[0025] Figure 4 It is a flowchart of a collision threshold setting method in an embodiment of the present invention;

[0026] Figure 5 It is a schematic structural diagram of a mechanical arm collision detection device in an embodiment of the present invention;

[0027] Figure 6 It is a principle block diagram of a mechanical arm collision detection method in an embodiment of the present invention;

[0028] Figure 7 It is a schematic diagram of a 2R rigid mechanical arm model in an embodiment of the present invention;

[0029] Figure 8 It is a result analysis diagram of solving the mechanical arm joint threshold in an embodiment of the present invention;

[0030] Figure 9 It is a result comparison and analysis diagram of a collision detection method in an embodiment of the present invention;

[0031] Figure 10 It is a schematic structural diagram inside an industrial robot in an embodiment of the present invention;

[0032] Figure 11 Schematic diagram of the module of the computer-readable storage medium in the embodiments of the present invention. Detailed implementation manners

[0033] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It can be understood that the specific embodiments described herein are only used to explain the present invention, rather than limiting the present invention. In addition, it should be noted that, for the sake of description, only parts related to the present invention rather than all structures are shown in the drawings.

[0034] Before discussing the exemplary embodiments in more detail, it should be mentioned that some exemplary embodiments are described as processes or methods depicted as flowcharts. Although the flowcharts describe the operations (or steps) as sequential processes, many of the operations can be implemented in parallel, concurrently, or simultaneously. In addition, the order of the operations can be rearranged. The process can be terminated when its operations are completed, but it can also have additional steps not included in the drawings. The process can correspond to a method, function, procedure, subroutine, subprogram, etc. In addition, without conflict, the embodiments in the present invention and the features in the embodiments can be combined with each other.

[0035] In addition, in the embodiments of the present invention, words such as "optionally" or "exemplarily" are used to represent examples, illustrations, or explanations. Any embodiment or design solution described as "optionally" or "exemplarily" in the embodiments of the present invention should not be construed as being more preferred or having more advantages than other embodiments or design solutions. Exactly speaking, the use of words such as "optionally" or "exemplarily" is intended to present the relevant concepts in a specific manner.

[0036] Figure 1 Flowchart of a robotic arm collision detection method provided by the embodiments of the present invention. This method can be applied to an industrial robot to achieve the detection of robotic arm collisions. This method can be executed by the robotic arm collision detection device provided by the embodiments of the present application, and this device can be implemented in a software and / or hardware manner. In a specific embodiment, this device can be integrated in an industrial robot. The following embodiments will be described by taking this device integrated in an industrial robot as an example. Refer to Figure 1 , the method provided by the embodiments of the present application may specifically include but is not limited to the following steps:

[0037] S101. Establish a dynamic model of the robotic arm.

[0038] Establish the dynamic equation of the robotic arm system model according to the Euler-Lagrange formula. The dynamic equation is as follows,

[0039]

[0040] where, M(q) is a second-order angular derivative term matrix related to the mass, center-of-mass position, and angle of the actual model of the robotic arm; is a first-order angular derivative term matrix related to the mass, center-of-mass position, and angle of the actual model of the robotic arm; g(q) is the gravitational vector of the gravity acting on the robotic arm; τ m is the joint torque; d is the disturbance quantity, that is, the external disturbance torque quantity to be estimated.

[0041] S102. Establish a sliding-mode momentum state observer based on the saturation function according to the dynamic model of the robotic arm.

[0042] Traditional sliding-mode observers, observers based on the super-twisting algorithm, or extended state observers all obtain the disturbance observation quantity in the form of integral of deviation, which will cause the observed disturbance speed to be too slow. On the other hand, it will also cause the gain of the observer to be too large and prone to jitter phenomenon. The sliding-mode momentum state observer constructed in the embodiment of the present invention includes a saturation function, which can not only improve the rapidity but also effectively reduce the chattering phenomenon in the region near the equilibrium point.

[0043] S103. Construct a sliding-mode disturbance observer according to the sliding-mode momentum state observer, and estimate the external disturbance torque based on the sliding-mode disturbance observer.

[0044] In the embodiment of the present invention, a sliding-mode disturbance observer is constructed according to the provided sliding-mode momentum state observer including a saturation function, which can effectively and quickly estimate the external disturbance torque, and avoid the increase of integral gain and the hysteresis caused by integration.

[0045] S104. Judge whether the robotic arm has collided according to the estimated external disturbance torque and the preset collision threshold.

[0046] The external disturbance torque is estimated by using the sliding-mode disturbance observer provided in the embodiment of the present invention, and the external disturbance torque is compared with the preset collision threshold. If the external disturbance torque is within the collision threshold range, it is determined that a collision has occurred.

[0047] The embodiment of the present invention provides a method for detecting the collision of a robotic arm. The method includes: establishing a dynamic model of the robotic arm; establishing a sliding-mode momentum state observer based on the saturation function according to the dynamic model of the robotic arm; constructing a sliding-mode disturbance observer according to the sliding-mode momentum state observer, and estimating the external disturbance torque based on the sliding-mode disturbance observer; judging whether the robotic arm has collided according to the estimated external disturbance torque and the preset collision threshold. Combining the traditional momentum observer with the sliding-mode technology to construct a sliding-mode disturbance observer including a saturation function, which can quickly and effectively estimate the external disturbance torque without additional sensors, and avoid introducing the chattering phenomenon.

[0048] Such as Figure 2As shown, in one example, in the above solution, establishing a sliding mode momentum state observer based on a saturation function according to the manipulator dynamics model may include but is not limited to the following steps:

[0049] S201. The momentum of the manipulator is defined as follows:

[0050]

[0051] Meanwhile, the derivative term matrix M(q) satisfies

[0052] S202. According to the momentum definition of the manipulator and the manipulator dynamics model, the state equation of the manipulator momentum is obtained as follows:

[0053]

[0054] Where is the transpose of the matrix ;

[0055] S203. Based on the state equation of the manipulator momentum, a sliding mode momentum state observer based on a saturation function is established. The sliding mode momentum state observer is as follows:

[0056]

[0057] represents the momentum deviation of the manipulator, sat(·) represents the saturation function, where 0 < μ < 1, 0 < β < 1, k > 0, α > 0, and the saturation function is as follows:

[0058] is the boundary layer thickness.

[0059] The sliding mode momentum state observer provided by the embodiment of the present invention includes a saturation function, effectively combines linearity and non-linearity, adopts high-gain linear correction in the adjacent region, and uses non-linear variable structure correction in the region far from the equilibrium point, which can not only improve the rapidity but also effectively reduce the chattering phenomenon in the region near the equilibrium point.

[0060] As Figure 3 shown, in one example, in the above solution, establishing a sliding mode momentum state observer based on the state equation of the manipulator momentum may include but is not limited to the following steps:

[0061] S301. Obtain a traditional momentum observer based on the state equation of the manipulator momentum,

[0062]

[0063] The traditional momentum observer only relies on the momentum deviation Solve interference by multiples, the main disadvantage is that the observation speed is too slow, only suitable for slow external torque interference. For sudden collision interference signals, they cannot be quickly detected, resulting in low sensitivity of collision detection.

[0064] S302. Use the double power reaching law in sliding mode technology to adjust the deviation to obtain where α is greater than zero and β is less than zero; further simplify the parameter adjustment to obtain

[0065] S303. Use a saturation function to replace the sign function

[0066]

[0067] By replacing the sign function with a saturation function, the chattering phenomenon can be avoided.

[0068] S304. Adjust the gain K to a scaling function and set the variation range to [k, k / μ] to obtain the following:

[0069]

[0070] By increasing the variation range of the gain k, it can change with the error.

[0071] S305. Obtain a sliding mode momentum state observer based on the saturation function.

[0072] Construct a sliding mode disturbance observer according to the sliding mode momentum state observer, and estimate the external disturbance torque based on this sliding mode disturbance observer. The sliding mode disturbance observer is as follows:

[0073]

[0074] It is used for estimating the external disturbance torque.

[0075] Using the above sliding mode disturbance observer can quickly and stably estimate the external disturbance torque, and at the same time avoid the chattering phenomenon in sliding mode technology.

[0076] Furthermore, judge whether the robotic arm has collided according to the estimated external disturbance torque and the preset collision threshold, including: when the estimated disturbance torque is within the preset collision threshold range, it is determined that the robotic arm has collided with the external environment. If a collision is detected, the industrial robot will take some preventive protection measures in time to prevent accidents.

[0077] Such as Figure 4 shown, in an example, the preset collision threshold in the above scheme is set according to the following method:

[0078] S401. Obtain the estimated disturbance information data during the joint movement before the robot arm collides, and estimate the external disturbance torque according to the sliding mode disturbance observer, denoted as d1;

[0079] When no external collision is received, the external disturbance torque estimated by the sliding mode disturbance observer is the total disturbance torque including the friction torque between the joints of the robot arm, measurement noise, and model error. Therefore, the collision threshold is set based on the estimated external disturbance torque.

[0080]

[0081] S402. Perform first-order low-pass filtering on the estimated external disturbance torque d1 to obtain the structure of the first-order low-pass filter as follows:

[0082]

[0083] where, is the total disturbance after filtering, T f is the time constant of the first-order low-pass filter;

[0084] By using the first-order low-pass filter, impurity signals and error signals can be removed to obtain real and effective data. It can distinguish collision signals and measurement noise from the source, which is beneficial to improving the accuracy of threshold calculation.

[0085] S403. Analyze according to the filtered total disturbance to obtain the root mean square value of the total disturbance residual, as shown in the following formula:

[0086]

[0087] n represents the array data, and this value can measure the zero bias degree of the filtered total disturbance;

[0088] In the initialization stage when the robot arm is normally started and operates, the robot arm performs joint movement before collision to ensure that the speed and angular velocity information of the joints are taken into account. Select the estimated disturbance information data before collision to calculate the root mean square value of the total disturbance residual.

[0089] S404. Set the collision threshold as T = σd 1_RMS , where σ is a constant not less than 1.

[0090] The collision threshold setting method provided in the above embodiments of the present invention forms a set of simple experimental measurement and threshold setting rules. The method of setting the collision threshold based on the root mean square value is adopted, which avoids the process of parameter identification for offline data, is simple and easy to implement, can reflect the actual data situation, avoids reducing the detection sensitivity due to too high threshold setting, and avoids too high detection misjudgment rate due to too low threshold setting, providing a theoretical scientific basis for threshold setting.

[0091] Figure 5 This is a schematic structural diagram of a robotic arm collision detection device provided by an embodiment of the present application. As Figure 5 shown, the device may include: a robotic arm dynamics model construction module, a sliding mode momentum state observer module, a sliding mode disturbance observer module, a collision threshold setting module, and a collision determination module.

[0092] Among them, the robotic arm dynamics model construction module is used to establish the dynamic equation of the robotic arm system model according to the Euler-Lagrange formula;

[0093] The sliding mode momentum state observer module is used to monitor the disturbance observation quantity of each time point of the robotic arm;

[0094] The sliding mode disturbance observer module is used to estimate the external disturbance torque;

[0095] The collision threshold setting module is used to set the collision threshold based on the external disturbance torque before collision estimated by the sliding mode disturbance observer module;

[0096] The collision determination module is used to determine whether the robotic arm has collided according to the estimated external disturbance torque and the preset collision threshold.

[0097] The above-mentioned robotic arm collision detection device can execute Figure 1 the provided robotic arm collision detection method, and has the corresponding devices and beneficial effects in this method.

[0098] Figure 6 This is a principle block diagram of collision detection using the above-mentioned robotic arm collision detection method provided by an embodiment of the present invention.

[0099] As Figure 6 shown, this is a principle block diagram of a robotic arm collision detection method provided by an embodiment of the present invention. The entire detection system consists of a controller, an actuator, a robotic arm dynamics model, a sliding mode momentum state observer, a sliding mode disturbance observer, a first-order low-pass filter, threshold generation and collision detection. Assume that when the working robotic arm is collided by the outside world, using the torque signal output, joint angle and angular velocity output of the controller, combined with the sliding mode momentum state observer, the momentum estimated value of the robotic arm can be calculated in real time, and the momentum estimated value is subtracted from the actual momentum value of the robotic arm to obtain a momentum deviation value. This momentum deviation value contains information about the external collision disturbance torque quantity. According to the momentum deviation value, a sliding mode disturbance observer is designed to adjust the parameter gain, and the external disturbance torque is estimated through the sliding mode disturbance observer. Then, the estimated external disturbance torque is compared with the collision threshold set according to the threshold setting rule provided by the embodiment of the present invention. If the external disturbance torque is within the collision threshold range, it is determined that a collision has been detected.

[0100] The method for setting the collision threshold is as follows. When no external collision is received, the momentum estimation value of the robotic arm is calculated in real time by using the torque signal output, joint angle, and angular velocity output of the controller, in combination with a sliding mode momentum state observer. Then, the momentum estimation value is subtracted from the actual momentum value of the robotic arm to obtain a momentum deviation value, which contains information about the disturbance torque of the external collision. Based on the momentum deviation value, a sliding mode disturbance observer is designed to adjust the parameter gain. The external disturbance torque is estimated by the sliding mode disturbance observer, and the filtered external disturbance torque value is obtained through first-order low-pass filtering of the external disturbance torque. This filtered value is applied to threshold generation and collision detection, and then it can be determined whether the robotic arm has collided according to the threshold setting rule.

[0101] Exemplarily, in the embodiment of the present invention, a 2R rigid robotic arm model is taken as an example to illustrate the robotic arm collision detection method provided by the embodiment of the present invention, as follows:

[0102] Step 1) Establish the dynamic model equation of the robotic arm. Taking a 2R rigid robotic arm model as an example, as Figure 7 shown, the specific established equation is as follows:

[0103]

[0104] Among them,

[0105]

[0106]

[0107]

[0108] G1 = (m1r c1 + m2r1)gcosq1 + m2gr c2 cos(q1 + q2),

[0109] G2 = m2gr c2 cos(q1 + q2)

[0110] Among them, m1 and m2 are the masses of joint 1 and joint 2 respectively, r1 and r2 are the lengths of joint 1 and joint 2 respectively, r c1 , r c2 are the distances from the centers of mass of joint 1 and joint 2 to the joints respectively, q1 and q2 are the angles of joint 1 and joint 2 respectively, and g is the acceleration due to gravity. Using the specific robotic arm model information, the dynamic model of the robotic arm is established. Here, m1 = m2 = 0.55 kg; rc1 = rc2 = 0.18 m, r1 = r2 = 0.36 m.

[0111] Step 2) Establish a sliding mode momentum state observer based on the saturation function according to the manipulator dynamics model.

[0112]

[0113] Among them, represents the momentum deviation of the manipulator, sat(·) represents the saturation function, where 0 < μ < 1, 0 < β < 1, k > 0, α > 0. Here, the parameters can be adjusted. Let μ = 0.6, β = 0.5, k = 25, α = 20. The saturation function is as follows:

[0114] is the boundary layer thickness, and the boundary layer thickness can be set to 0.005.

[0115] Step 3) Construct a sliding mode disturbance observer according to the sliding mode momentum state observer, and estimate the external disturbance torque based on this sliding mode disturbance observer, as follows

[0116]

[0117] Step 4) Set the collision threshold according to the sliding mode disturbance observer. The setting steps are as follows:

[0118] First, when not affected by external collisions, the disturbance torque estimated by the sliding mode disturbance observer should include the total disturbance torque of the friction torque between the manipulator joints, measurement noise, and model error. Therefore, the collision threshold should be set based on this total disturbance torque. Assume that the estimated total disturbance torque d1 is as follows:

[0119]

[0120] Then, the estimated total disturbance d1 is passed through a first-order low-pass filter. The structure of the first-order low-pass filter is as follows:

[0121]

[0122] Among them, is the total disturbance after filtering, T f is the time constant of the first-order low-pass filter, which is related to the cut-off frequency in specific implementation. Its discrete form is as follows:

[0123]

[0124] Among them, h is the integration step size, is the time constant. Here, the cut-off frequency can be set to ω c = 100Hz. The main purpose is to filter out the high-frequency noise in the measurement process, but not filter out the collision signal.

[0125] Secondly, based on the total interference after filtering for analysis. During the initialization stage when the robotic arm is normally turned on and operating, let the robotic arm perform joint movements before collision, ensuring that the speed and angular acceleration information of the joints are taken into account. Select the estimated interference information data before collision to calculate the root mean square value of the total interference residual, that is, as follows:

[0126]

[0127] n represents the array data, and this value can measure the zero-bias degree of the total interference after filtering.

[0128] In the simulation provided by the embodiments of the present invention, a robotic arm model is built according to the model parameters of a 2R robotic arm. In order to measure the RMS value of the joints, a model of joint friction torque is added as follows:

[0129]

[0130] Among them, F c is the Coulomb friction coefficient, which is 2.8×10 -3 Nm.s / rad, and F v is the viscous friction coefficient, which is 1.1×10 - 3 Nm.s / rad. At the same time, Gaussian white noise with a signal-to-noise ratio of 50 is added on the basis of the friction torque model to further illustrate the simulation effect of the embodiments of the present invention.

[0131] First, let the angles of joints 1 and 2 of the robotic arm operate according to the commands sin(0.1t) and 0.1cos(0.1t) respectively. In this way, when calculating the RMS value using the data, the changes in joint angles and angular velocities can be taken into account.

[0132] As Figure 8 shown, it is the result analysis diagram for solving the joint threshold of the embodiments of the present invention. Before collision, the joints perform the specified working movements. The original interference torque signal has white noise, and it can be seen that it is related to the speed of joint movement; the interference estimated by the new sliding mode momentum observer almost coincides with the interference after filtering, indicating that the new sliding mode momentum observer has a built-in filtering function. According to the size of the filtered interference, the RMS value is calculated, and the maximum value of the RMS value is 0.0039. In order to make the threshold envelope the entire interference value, the threshold is set here as:

[0133] T = σd 1_RMS where σ = 3.8,

[0134] Figure 8 shows the upper and lower limits of this threshold, which can ensure both sensitivity and accuracy.

[0135] In addition, the embodiments of the present invention have carried out simulation verification for the specifically received disturbance torque.

[0136] In order to verify the effect of the collision detection method provided by the embodiments of the present invention, in the simulation, on the basis of the conditions for calculating the RMS simulation described above, that is, adding a friction torque model and Gaussian white noise with a signal-to-noise ratio of 50, an external disturbance torque of 6 N·m was additionally applied to the robotic arm joint 1 within the time period of 5 s ≤ t ≤ 6 s. At the same time, the traditional generalized momentum observer method and the momentum observer method based on the traditional sliding mode were compared. At the same time, it was also necessary to verify whether the collision detection signal of the embodiments of the present invention was accurate. The comparison results of the above information are as Figure 9 shown.

[0137] From Figure 9 it can be seen that compared with the other two external torque estimation methods, the robotic arm collision detection method provided by the present invention has better rapidity while ensuring stability, and at the same time overcomes the chattering defect in the sliding mode technology, and can complete the estimation of the magnitude of the external torque. In addition, for the collision detection method in the present invention, through the solution of RMS, from the simulation Figure 9 it can be seen that the collision detection can quickly identify the collision signal, and there is no misjudgment even when not collided in the first few seconds but affected by the friction torque and noise.

[0138] Figure 10 FIG. is a schematic structural diagram of an industrial robot provided by an embodiment of the present application. As Figure 10 shown, the industrial robot includes a controller, a memory, an input device, and an output device; the number of controllers in the industrial robot can be one or more, Figure 10 and one controller is taken as an example here; the controller, memory, input device, and output device in the industrial robot can be connected through a bus or other means, Figure 10 and taking the connection through the bus as an example here.

[0139] The memory, as a computer-readable storage medium, can be used to store software programs, computer-executable programs, and modules, such as Figure 1 the program instructions / modules corresponding to the robotic arm collision detection method in the embodiment. The controller executes various functional applications and data processing in the industrial robot by running the software programs, instructions, and modules stored in the memory, that is, implements the above-mentioned robotic arm collision detection method.

[0140] The memory may mainly include a program storage area and a data storage area. Among them, the program storage area may store an operating system and application programs required for at least one function; the data storage area may store data created according to the use of the terminal, etc. In addition, the memory may include a high-speed random access memory, and may also include a non-volatile memory, such as at least one magnetic disk storage device, a flash memory device, or other non-volatile solid-state storage devices. In some instances, the memory may further include a memory remotely set relative to the controller, and these remote memories may be connected to the terminal / server through a network. Examples of the above network include but are not limited to the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0141] The input device can be used to receive input digital or character information, and generate key signal inputs related to user settings and function control of the industrial robot. The output device may include a display device such as a display screen.

[0142] As Figure 11 shown, an embodiment of the present application further provides a computer-readable storage medium and a computer controller. The computer-executable instructions, when executed by the computer controller, are used to execute a method for detecting collisions of a robotic arm. The method includes Figure 1 the steps shown.

[0143] From the above description of the embodiments, those skilled in the art can clearly understand that the present application can be implemented by means of software and necessary general hardware. Of course, it can also be implemented by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present application, in essence, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as a floppy disk, a read-only memory (ROM), a random access memory (RAM), a flash memory (FLASH), a hard disk, or an optical disc of a computer, etc., including several instructions to enable the industrial robot to implement the methods or functions described in various embodiments of the present application.

[0144] It should be noted that the modules included in the above robotic arm collision detection device are only divided according to functional logic, but are not limited to the above division method. As long as the corresponding functions can be realized, it is not used to limit the protection scope of the present application.

[0145] Note that the above is only the preferred embodiment of the present application and the technical principles applied. Those skilled in the art will understand that the present application is not limited to the specific embodiments described herein, and various obvious changes, re-adjustments and substitutions can be made by those skilled in the art without departing from the protection scope of the present application. Therefore, although the present application has been described in more detail through the above embodiments, the present application is not limited to the above embodiments. Without departing from the concept of the present application, more other equivalent embodiments can be included, and the scope of the present application is determined by the scope of the appended claims.

Claims

1. A method for detecting collisions of a robotic arm, characterized in that, The method includes: Establishing a dynamic model of the robotic arm; Establishing a sliding mode momentum state observer based on a saturation function according to the dynamic model of the robotic arm; Constructing a sliding mode disturbance observer according to the sliding mode momentum state observer, and estimating the external disturbance torque based on the sliding mode disturbance observer; Judging whether the robotic arm collides according to the estimated external disturbance torque and a preset collision threshold; The establishment of the dynamic model of the robotic arm is specifically as follows: Establishing the dynamic equation of the robotic arm system model according to the Euler-Lagrange formula. The dynamic equation is as follows. Among them, \(M(q)\) is a matrix of second-order angular derivative terms related to the mass, center of mass position, and angle of the actual model of the robotic arm; is a matrix of first-order angular derivative terms related to the mass, center of mass position, and angle of the actual model of the robotic arm; \(g(q)\) is the gravitational vector of the gravity acting on the robotic arm; \(\tau\) m is the joint torque; \(d\) is the disturbance quantity, that is, the external disturbance torque quantity that needs to be estimated; Establishing a sliding mode momentum state observer based on a saturation function according to the dynamic model of the robotic arm, including: 1) The momentum of the robotic arm is defined as follows: Meanwhile, its derivative term matrix M(q) satisfies 2) According to the definition of the momentum of the robotic arm and the dynamic model of the robotic arm, obtaining the state equation of the momentum of the robotic arm as follows: Among them, is the transpose of the matrix . 3) Establishing a sliding mode momentum state observer based on a saturation function based on the state equation of the momentum of the robotic arm. The sliding mode momentum state observer is as follows: represents the momentum deviation of the robotic arm, sat(·) represents the saturation function, where 0 < μ < 1, 0 < β < 1, k > 0, α > 0, and the saturation function is as follows: is the boundary layer thickness.

2. The robotic arm collision detection method according to claim 1, wherein Based on the state equation of the momentum of the robotic arm, establishing a sliding mode momentum state observer based on a saturation function, as follows: 1) Obtaining a traditional momentum observer based on the state equation of the momentum of the robotic arm. 2) The double power reaching law in sliding mode technology is adopted to adjust the deviation and obtain where α is greater than zero and β is less than zero; further simplify the parameter adjustment to obtain 3) Using a saturation function to replace the sign function. 4) Adjusting the gain K to a scaling function and setting the variation range to [k, k / μ], obtaining the following: 5) Further obtaining a sliding mode momentum state observer based on a saturation function.

3. The robotic arm collision detection method according to claim 2, characterized in that Constructing a sliding mode disturbance observer according to the sliding mode momentum state observer, and estimating the external disturbance torque based on the sliding mode disturbance observer. The sliding mode disturbance observer is as follows: It is used for the estimation of the external disturbance torque.

4. The robotic arm collision detection method according to claim 1, wherein, Judging whether the robotic arm collides according to the estimated external disturbance torque and a preset collision threshold, including: When the estimated disturbance torque is within the preset collision threshold range, it is determined that the robotic arm has collided with the external environment. A collision has occurred.

5. The robotic arm collision detection method according to claim 4, wherein The preset collision threshold is set according to the following method: As follows: 1) Obtaining the estimated disturbance information data during the joint movement before the collision of the robotic arm, and estimating the external disturbance torque according to the sliding mode disturbance observer, denoted as d1; 2) Performing first-order low-pass filtering on the estimated external disturbance torque d1 to obtain the structure of the first-order low-pass filter, as follows: 3) Analyzing according to the total disturbance after filtering to obtain the root mean square value of the total disturbance residual, as follows: Among them, is the total interference after filtering, T f is the time constant of the first-order low-pass filter; n represents the array data, and this value can measure the zero bias degree of the total disturbance after filtering; When the program is executed by the processor, it implements the robotic arm collision detection method as described in any one of claims 1-5. 4) Set the collision threshold to T = σd 1_RMS , where σ is a constant not less than 1.

6. A device, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, When the program is executed by the processor, it implements the robotic arm collision detection method as described in any one of claims 1-5.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that, ​

Images