A robot collision detection method, system and response method

Abstract

The application discloses a robot collision detection method, system and response method, the detection method combines the body structure parameters of the robot, the inertia and the gravity center of each component, and utilizes the virtual work principle to construct the robot dynamics equation; according to the robot dynamics equation and the position, speed and acceleration data of the joint working path point of the robot, the joint driving torque is obtained; the extended state observer based on the generalized momentum is constructed from the robot dynamics equation, and the joint disturbance torque of the robot system is obtained from the extended state observer; the disturbance torque threshold is set, when the obtained joint disturbance torque exceeds the threshold range, it is judged that the robot collides with the outside world. The application does not need to use an additional sensor, only needs to use the robot body sensor, combines the set collision threshold, and can realize the collision detection of the robot, effectively applies to the existing robot, and improves the response speed of the collision detection.

Description

Technical Field

[0001] This invention relates to the field of robotics, and in particular to a robot collision detection method, system, and response method. Background Technology

[0002] Currently, robots are increasingly being used in fields such as automotive manufacturing, food, and new energy 3C products. The operational space of robots is gradually expanding from known environments to complex and ever-changing environments. In these application environments, due to the complexity of the external environment and the robot's difficulty in obtaining unknown environmental information, collisions with the external environment are inevitable. In such cases, the robot may cause significant damage to surrounding objects, humans, or even the robot itself. Therefore, higher requirements have been placed on the safety of robot operations.

[0003] To ensure the safety of robots during operation, they should be able to accurately and quickly detect physical collisions. When a robot collides with its environment, it should be able to quickly detect and respond to protect itself.

[0004] Currently, collision detection methods in the industry are mainly divided into external sensor-based collision detection methods and sensorless collision detection methods. External sensor-based collision detection methods primarily rely on force sensors, allowing robots to directly detect the magnitude of the collision force. However, force sensors are typically expensive, and for existing robots, modifying the mechanical structure to install force sensors is a complex and costly process. Therefore, the most effective collision detection method utilizes the robot's own sensors and dynamic model. The most common method in the industry is based on generalized momentum, but this method is significantly affected by modeling errors and external noise, often requiring a higher threshold to reduce false positives, thus lowering the collision detection sensitivity.

[0005] Furthermore, the response strategies after collision detection are all based on a simple collision response strategy, which cannot be well applied to the motion characteristics of robots in the current material sorting process. Summary of the Invention

[0006] This invention provides a robot collision detection method, system, and response method to solve the technical problems existing in the prior art.

[0007] The technical solution adopted by this invention to solve the technical problems existing in the prior art is as follows:

[0008] A robot collision detection method is proposed, which combines the robot's body structural parameters, the inertia of each component, and the center of gravity to construct the robot's dynamic equations using the principle of virtual work; calculates the joint driving torque based on the robot's dynamic equations and the position, velocity, and acceleration data of the robot's joint working path points; constructs an extended state observer based on generalized momentum from the robot's dynamic equations, and calculates the joint disturbance torque of the robot system from the extended state observer; sets a disturbance torque threshold, and determines that the robot has collided with the outside world when the calculated joint disturbance torque exceeds the threshold range.

[0009] Furthermore, the method includes the following steps:

[0010] Step 1: Construct the robot's dynamic equations as follows:

[0011]

[0012] In the formula:

[0013] θ represents the joint angular displacement;

[0014] Joint angular velocity;

[0015] Joint angular acceleration;

[0016] M(θ) is the inertia matrix;

[0017] The matrix represents the Coriolis force and the centrifugal force.

[0018] G(θ) is the gravity term;

[0019] τ is the joint driving torque;

[0020] τ d The disturbance torque experienced by the joint;

[0021] Step 2, let the generalized momentum of the system be p, and let... Differentiating the generalized momentum of the system with respect to time, we have:

[0022]

[0023] Step 3: Apply the first derivative of the generalized momentum By combining the dynamic equations, an extended state observer based on generalized momentum is constructed, from which the estimated value of the joint disturbance torque is obtained.

[0024] Step four, set the threshold for the joint disturbance torque, when the obtained... When the threshold range is exceeded, it is determined that the robot has collided with the outside world.

[0025] Furthermore, step three includes the following sub-steps:

[0026] Step A1, the first derivative of the generalized momentum Substituting this into the robot's dynamics equations, we get:

[0027]

[0028] Step A2, construct the following extended state observer based on generalized momentum:

[0029]

[0030] in:

[0031] The estimated generalized momentum of the system;

[0032] K p1 =diag[k p11 ,k p12 ,k p13 ], K p2 =[k p21 ,k p22 ,k p23 And there is k p1i >0, k p2i >0, i=1,2;

[0033] In the formula:

[0034] The derivative of the estimated momentum value;

[0035] z p2 The estimated external collision torque;

[0036] This is the derivative of the estimated external collision torque;

[0037] K p1 for The diagonal matrix of the observer gain;

[0038] K p2 for The diagonal matrix of the observer gain;

[0039] e p The error is the generalized momentum of the system.

[0040] α p1 for The power term of the observer's nonlinear function fal;

[0041] α p2 for The power term of the observer's nonlinear function fal;

[0042] β p1 This represents the piecewise error of the corresponding fal function, which is the same as the sampling period;

[0043] β p2 This represents the piecewise error of the corresponding fal function, which is the same as the sampling period;

[0044] k p11 for The values ​​of the first row of the main diagonal of the observer gain matrix;

[0045] k p12 for The values ​​of the elements in the second row of the main diagonal of the observer gain matrix;

[0046] k p13 for The values ​​of the elements in the third row of the main diagonal of the observer gain matrix;

[0047] k p21 for The values ​​of the first row of the main diagonal of the observer gain matrix;

[0048] k p22 for The values ​​of the elements in the second row of the main diagonal of the observer gain matrix;

[0049] k p23 for The values ​​of the elements in the third row of the main diagonal of the observer gain matrix;

[0050] Step A3: Obtain the estimated value of the joint disturbance torque using the following formula.

[0051]

[0052] Furthermore, before constructing the robot's dynamic equations, the following steps are also included: using cubic or quintic polynomials to plan the robot's trajectory, and obtaining the position, velocity, and acceleration data of a series of points on the working path of the robot joints.

[0053] Furthermore, when setting the threshold for collision detection, 10% of the maximum input torque of the joint is selected as the threshold for the joint disturbance torque.

[0054] The present invention also provides a robot collision detection system, including a memory and a processor, wherein the memory is used to store a computer program; and the processor is used to execute the computer program and, when executing the computer program, implement the robot collision detection method steps as described above.

[0055] The present invention also provides a robot collision response method, which uses the above-mentioned robot collision detection method to detect whether a collision occurs during the operation of the robot. When a collision is detected between the robot and the outside world, a segmented response strategy is adopted to deal with the collision in combination with the robot's running trajectory.

[0056] Furthermore, the robot is a robot used for sorting operations. The robot's sorting work path includes a grasping stage, a transfer stage, and a placement stage. The stage the robot is in is determined based on the feedforward increment of the robot's end position.

[0057] When a collision between the robot and the outside world is detected, the robot is in the grasping or placing phase. The robot control system enters the compliant control mode and adjusts the robot's posture according to the obtained joint disturbance torque.

[0058] When a collision between the robot and the external environment is detected, the robot is in the transfer phase, causing the robot control system to enter zero-force control mode.

[0059] Furthermore, the method for determining the stage of the robot based on the feedforward increment of the robot's end-effector position includes the following steps:

[0060] Based on the position feedforward after robot trajectory planning, the increment in the z-direction is determined. When the absolute value of the increment in the z-direction is greater than 0, it is determined that the robot end effector is in the grasping or placement stage of the sorting process. When the absolute value of the increment in the z-direction is 0, it is determined that the robot end effector is in the transfer stage.

[0061] The advantages and positive effects of this invention are:

[0062] (1) The extended state observer based on generalized momentum proposed in this invention does not require the use of additional sensors. It only requires the use of the robot body sensor and the setting of a collision threshold to realize the collision detection of the robot, which is effectively applicable to existing robots.

[0063] (2) This invention combines the advantages of extended state observer and generalized momentum, and performs order reduction processing on extended state observer to improve the response speed of collision detection.

[0064] (3) This invention conducts an engineering analysis of the robot sorting and grasping process and proposes a collision detection response strategy based on the feedforward segmented response of the operation space position, which can be better applied to robot sorting applications. Attached Figure Description

[0065] Figure 1 This is a flowchart of a robot collision detection and response method for sorting operations according to the present invention.

[0066] Figure 2 This is a diagram showing the effect of collision detection method on joint disturbance torque under step disturbance.

[0067] Figure 3 This is a diagram showing the effect of a collision detection method on detecting joint disturbance torque when subjected to a sinusoidal wave disturbance.

[0068] Figure 4 This is a schematic diagram of the robot's sorting process.

[0069] In the diagram: τ d The disturbance torque experienced by the joint; P1 to P6 are points on the working path of the robot sorting, where P1 to P3 is the grasping stage, P3 to P4 is the transfer stage, and P4 to P6 is the placement stage. Detailed Implementation

[0070] The present invention will now be described in detail with reference to the accompanying drawings and embodiments. It should be understood that the preferred embodiments described herein are for illustration and explanation only and are not intended to limit the present invention.

[0071] In the description of this invention, the terms "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," and "bottom," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are used only for the convenience of describing the invention and do not require the invention to be constructed and operated in a specific orientation; therefore, they should not be construed as limitations on the invention. The terms "connected" and "linked" used in this invention should be interpreted broadly. For example, they can refer to a fixed connection or a detachable connection; they can refer to a direct connection or an indirect connection through intermediate components. Those skilled in the art can understand the specific meaning of the above terms according to the specific circumstances.

[0072] Please see Figures 1 to 4 A robot collision detection method is proposed, which combines the robot's body structure parameters, the inertia of each component and the center of gravity, and constructs the robot dynamic equation using the principle of virtual work; calculates the joint driving torque based on the robot dynamic equation and the position, velocity and acceleration data of the robot joint working path points; constructs an extended state observer based on generalized momentum from the robot dynamic equation, and calculates the joint disturbance torque of the robot system from the extended state observer; sets a disturbance torque threshold, and determines that the robot has collided with the outside world when the calculated joint disturbance torque exceeds the threshold range.

[0073] The joints here can be either intermediate joints or end joints of the robot.

[0074] Preferably, the method may include the following steps:

[0075] Step one, the robot dynamics equations can be constructed as follows:

[0076]

[0077] In the formula:

[0078] θ represents the joint angular displacement;

[0079] Joint angular velocity;

[0080] Joint angular acceleration;

[0081] M(θ) is the inertia matrix;

[0082] The matrix represents the Coriolis force and the centrifugal force.

[0083] G(θ) is the gravity term;

[0084] τ is the joint driving torque;

[0085] τ d The disturbance torque experienced by the joint;

[0086] Step two, let the generalized momentum of the system be p, and let... Differentiating the generalized momentum of the system with respect to time, we have:

[0087]

[0088] Step three, the first derivative of the generalized momentum can be obtained. By combining the dynamic equations, an extended state observer based on generalized momentum is constructed, from which the estimated value of the joint disturbance torque is obtained.

[0089] Step four: A threshold for the joint disturbance torque can be set. When the obtained value... When the threshold range is exceeded, it is determined that the robot has collided with the outside world.

[0090] Preferably, step three may include the following sub-steps:

[0091] Step A1 allows us to obtain the first derivative of the generalized momentum. Substituting this into the robot's dynamics equations, we get:

[0092]

[0093] Step A2 allows for the construction of the following extended state observer based on generalized momentum:

[0094]

[0095] in:

[0096] The estimated generalized momentum of the system;

[0097] K p1 =diag[k p11 ,k p12 ,k p13 ], K p2 =[k p21 ,k p22 ,k p23 And there is k p1i >0, k p2i >0, i=1,2;

[0098] In the formula:

[0099] The derivative of the estimated momentum value;

[0100] z p2 The estimated external collision torque;

[0101] This is the derivative of the estimated external collision torque;

[0102] K p1 for The diagonal matrix of the observer gain;

[0103] K p2 for The diagonal matrix of the observer gain;

[0104] e p The error is the generalized momentum of the system.

[0105] α p1 for The power term of the observer's nonlinear function fal;

[0106] α p2 for The power term of the observer's nonlinear function fal;

[0107] β p1 The piecewise error of the corresponding fal function can generally be set to be the same as the sampling period;

[0108] β p2 The piecewise error of the corresponding fal function can generally be set to be the same as the sampling period;

[0109] k p11 for The values ​​of the first row of the main diagonal of the observer gain matrix;

[0110] k p12 for The values ​​of the elements in the second row of the main diagonal of the observer gain matrix;

[0111] k p13 for The values ​​of the elements in the third row of the main diagonal of the observer gain matrix;

[0112] k p21 for The values ​​of the first row of the main diagonal of the observer gain matrix;

[0113] k p22 for The values ​​of the elements in the second row of the main diagonal of the observer gain matrix;

[0114] k p23 for The values ​​of the elements in the third row of the main diagonal of the observer gain matrix;

[0115] Step A3, set The estimated value of the joint disturbance moment can be obtained by the following formula.

[0116] If generalized momentum is not used, the extended state observer can typically be constructed using the following method:

[0117] The following robot dynamics equations are:

[0118]

[0119] Transformed into:

[0120]

[0121] Based on this, the following extended state observer is constructed:

[0122]

[0123] in:

[0124] Indicates the estimated joint angle;

[0125] K a1 =diag[k a11 ,k a12 ,k a13 ];

[0126] K a2 =[k a21 ,k a22 ,k a23 ];

[0127] K a3 =diag[k a31 ,k a32 ,k a33 ];

[0128] And k a1j >0, k a2j >0 and k a3j >0, j = 1, 2, 3;

[0129] In the formula:

[0130] This is the derivative of the robot's joint displacement;

[0131] This is the derivative of the robot's joint velocity;

[0132] This is the derivative of the estimated external collision torque;

[0133] z a2 These are the observed values ​​of the robot's joint velocities;

[0134] z a3 The observed values ​​of the external collision torque on the robot;

[0135] K a1 for The diagonal matrix of the observer gain;

[0136] K a2 for The diagonal matrix of the observer gain;

[0137] K a3 for The diagonal matrix of the observer gain;

[0138] e a This is the estimated joint rotation angle error;

[0139] α a1 for The power term of the observer's nonlinear function fal;

[0140] α a2 for The power term of the observer's nonlinear function fal;

[0141] α a3 for The power term of the observer's nonlinear function fal;

[0142] β a1 This is the piecewise error of the corresponding fal function, which is generally the same as the sampling period;

[0143] β a2 This is the piecewise error of the corresponding fal function, which is generally the same as the sampling period;

[0144] β a3 This is the piecewise error of the corresponding fal function, which is generally the same as the sampling period;

[0145] k a11 for The values ​​of the first row of the main diagonal of the observer gain matrix;

[0146] k a12 for The values ​​of the elements in the second row of the main diagonal of the observer gain matrix;

[0147] k a13 for The values ​​of the elements in the third row of the main diagonal of the observer gain matrix;

[0148] k a21 for The values ​​of the first row of the main diagonal of the observer gain matrix;

[0149] k a22 for The values ​​of the elements in the second row of the main diagonal of the observer gain matrix;

[0150] k a23 for The values ​​of the elements in the third row of the main diagonal of the observer gain matrix;

[0151] k a31 for The values ​​of the first row of the main diagonal of the observer gain matrix;

[0152] k a32 for The values ​​of the elements in the second row of the main diagonal of the observer gain matrix;

[0153] k a33 for The values ​​of the elements in the third row of the main diagonal of the observer gain matrix;

[0154] set up The estimated value of the joint disturbance moment can be obtained by the following formula.

[0155]

[0156] As can be seen from the above formula, if the robot collision detection method of the present invention is not used, it is necessary to solve the inverse matrix of the inertia matrix M(θ) to obtain the collision torque. This is a very complex and time-consuming task, which cannot meet the requirements of the robot for collision detection sensitivity during sorting and grasping, and the detection speed is low.

[0157] Preferably, before constructing the robot dynamics equations, the following steps may be included: robot trajectory planning can be performed using cubic or quintic polynomials to obtain position, velocity, and acceleration data of a series of points on the working path of the robot joints.

[0158] Preferably, when setting the threshold for collision detection, 10% of the maximum input torque of the joint can be selected as the threshold for the joint disturbance torque.

[0159] The present invention also provides a robot collision detection system, including a memory and a processor, wherein the memory is used to store a computer program; and the processor is used to execute the computer program and, when executing the computer program, implement the robot collision detection method steps as described above.

[0160] The present invention also provides a robot collision response method, which uses the above-mentioned robot collision detection method to detect whether a collision occurs during the operation of the robot. When a collision is detected between the robot and the outside world, a segmented response strategy is adopted to deal with the collision in combination with the robot's running trajectory.

[0161] Preferably, the robot can be a robot used for sorting operations, and the robot sorting work path includes a grasping stage, a transfer stage, and a placement stage; the stage in which the robot is located can be determined based on the feedforward increment of the robot's end position.

[0162] When a collision between the robot and the outside world is detected, the robot is in the grasping or placing phase. The robot control system can be put into a compliant control mode, and the robot's posture can be adjusted according to the obtained joint disturbance torque.

[0163] When a collision between the robot and its surroundings is detected, the robot is in a transfer phase, which allows the robot control system to enter a zero-force control mode.

[0164] Preferably, the method for determining the stage of a robot based on the feedforward increment of the robot's end-effector position may include the following steps:

[0165] Based on the position feedforward after robot trajectory planning, the increment in the z-direction can be determined. When the absolute value of the increment in the z-direction is greater than 0, it can be determined that the robot end effector is in the grasping or placement stage of the sorting process. When the absolute value of the increment in the z-direction is 0, it can be determined that the robot end effector is in the transfer stage.

[0166] The working principle of the present invention will be further explained below with reference to preferred embodiments:

[0167] like Figure 1 As shown, taking a robot used for sorting operations as an example, this paper presents a robot collision detection and response method for collision detection and response at its end-effector joint. Based on the robot's grasping path, trajectory planning is performed using third, fourth, and fifth-order polynomials to obtain the position, velocity, and acceleration information of a series of points on the robot's end-effector grasping path. This prepares for subsequent position feedforward and dynamic model torque calculations.

[0168] Based on the robot's structural parameters, the inertia of each component, and its center of gravity, the robot's dynamic equations are constructed using the principle of virtual work as follows:

[0169]

[0170] Where θ is the joint angular displacement. The joint angular velocity, Let θ be the joint angular acceleration. M(θ) is the inertia matrix. Here are the Coriolis force and centrifugal force matrices, G(θ) is the gravity term, and τ is the joint driving torque. d The disturbance torque experienced by the joint can be considered as the collision torque experienced by the system.

[0171] The driving torque τ of the joint can be obtained from the robot's dynamic equations and the position, velocity, and acceleration information of a series of points on the grasping path.

[0172] To meet the collision detection sensitivity requirements of robots during sorting and grasping, generalized momentum is introduced to improve detection speed. The basic principle of the generalized momentum observer is based on Newton's second law: force equals mass multiplied by acceleration. For a robot, its mass is fixed, so the force acting on the robot can be calculated by measuring its acceleration. In the absence of a collision, the robot's acceleration should be stable. However, in the event of a collision, the robot's acceleration will change suddenly, and this change in acceleration can be detected to determine if a collision has occurred. To implement the generalized momentum observer, the robot's acceleration needs to be measured. An accelerometer can be used to measure the robot's acceleration in three directions. By superimposing the accelerations in the three directions, the total acceleration of the robot can be obtained. Then, multiplying the robot's mass by the total acceleration gives the force acting on the robot. When implementing the generalized momentum observer, attention must be paid to obtaining the driving torque. Driving torque refers to the torque used to generate power in the robot. In robots, driving torque is usually generated by motors. Therefore, when implementing a generalized momentum observer, the driving torque can be obtained by measuring the motor current. The motor current is proportional to the driving torque. Thus, by measuring the motor current, the driving torque acting on the robot can be obtained.

[0173] The generalized momentum of a system can be defined as:

[0174]

[0175] The first derivative of the generalized momentum of the system with respect to time can be expressed as:

[0176]

[0177] To facilitate the construction of the observer, the above equation is modified as follows:

[0178]

[0179] Substituting the above equation into the robot's dynamics equations, we get:

[0180]

[0181] Therefore, the following extended state observer based on generalized momentum can be constructed:

[0182]

[0183] In the formula, K represents the estimated generalized momentum of the system. p1 =diag[k p11 ,k p12 ,kp13 ], K p2 =[k p21 ,k p22 ,k p23 And there is k p1i >0, k p2i >0 and i=1,2.

[0184] In the formula:

[0185] The derivative of the estimated momentum value;

[0186] z p2 The estimated external collision torque;

[0187] This is the derivative of the estimated external collision torque;

[0188] K p1 for The diagonal matrix of the observer gain;

[0189] K p2 for The diagonal matrix of the observer gain;

[0190] e p The error is the generalized momentum of the system.

[0191] α p1 for The power term of the observer's nonlinear function fal;

[0192] α p2 for The power term of the observer's nonlinear function fal;

[0193] β p1 The piecewise error of the corresponding fal function can generally be set to be the same as the sampling period;

[0194] β p2 The piecewise error of the corresponding fal function can generally be set to be the same as the sampling period;

[0195] k p11 for The values ​​of the first row of the main diagonal of the observer gain matrix;

[0196] k p12 for The values ​​of the elements in the second row of the main diagonal of the observer gain matrix;

[0197] k p13 for The values ​​of the elements in the third row of the main diagonal of the observer gain matrix;

[0198] k p21 for The values ​​of the first row of the main diagonal of the observer gain matrix;

[0199] k p22 for The values ​​of the elements in the second row of the main diagonal of the observer gain matrix;

[0200] k p23 for The values ​​of the elements in the third row of the main diagonal of the observer gain matrix;

[0201] The Fal function is a special nonlinear structure function and a core component of the extended state observer in an active disturbance rejection controller. Fal function filters, based on the Fal function, offer good noise filtering performance.

[0202] The diag function represents a diagonal matrix, which is a square matrix whose elements are all zero except for the main diagonal.

[0203] Based on experiments, the possible value for K is: p1 = diag[-100,-100,-100], K p2 = [-40000, -40000, -40000].

[0204] The estimated value of the joint disturbance torque can be obtained as follows:

[0205]

[0206] This is an estimate of the joint disturbance torque.

[0207] The experiment involved applying a step perturbation and using an extended state observer to estimate the joint perturbation torque. The experimental results are as follows: Figure 2 As shown.

[0208] The experimental results of estimating the joint disturbance torque using an extended state observer after applying a sinusoidal perturbation are as follows: Figure 3 As shown.

[0209] from Figure 2 and Figure 3 As can be seen, the robot collision detection method proposed in this invention can detect both step disturbance signals and sinusoidal disturbance signals using the generalized momentum-based extended state observer, with a detection delay of about 0.01s and high sensitivity.

[0210] The observer's error can be defined as:

[0211]

[0212] in,

[0213] In the formula:

[0214] e is a matrix composed of dynamic observation error and torque observation error;

[0215] e τ This is the error due to the external collision torque.

[0216] The derivative of the estimated system generalized momentum error;

[0217] To estimate the derivative of the external collision moment error;

[0218] The stability of the proposed algorithm can be proven using Lyapunov functions:

[0219]

[0220] Clearly, V is a positive definite scalar function. Taking the first derivative of the above equation with respect to time, we get:

[0221]

[0222] In the formula:

[0223] e pm The estimated momentum error is given by m = 1, 2, 3;

[0224] e τm The estimated external disturbance torque values ​​are given by m = 1, 2, 3;

[0225] k p1m This represents the value of the m-th element in the main diagonal of the diagonal matrix corresponding to the observer gain; m = 1, 2, 3;

[0226] k p2m The value of the m-th element in the main diagonal of the diagonal matrix corresponding to the observer gain; m = 1, 2, 3.

[0227] It can be seen that the collision detection algorithm proposed in this invention is stable.

[0228] Set a collision detection threshold. When the robot collides with a person or the surrounding environment, 10% of the maximum input torque of the joint is generally selected as the collision detection threshold. When the disturbance torque detected by the collision detection algorithm exceeds the threshold range, it is judged that the robot has collided with the outside world.

[0229] Common paths for robot sorting and grasping include: Figure 4 As shown, P1 to P3 is the grasping stage, P3 to P4 is the transfer stage, and P4 to P6 is the placement stage. The stage of the robot is determined based on the position feedforward increment.

[0230] After a collision is detected, the robot feeds forward the position θ after trajectory planning and determines the increment in the z direction. When the absolute value of the increment in the z direction is greater than 0, it is determined that the robot end effector is in the grasping and releasing stage of the sorting process. When the absolute value of the increment in the z direction is 0, it is determined that the robot end effector is in the transfer stage. Different collision response methods are adopted according to different stages.

[0231] Based on the characteristics of the robot's sorting and grasping process, when a collision occurs during the grasping and releasing phase, the robot's collision detection response enters a compliant control mode because there is a difference between the grasped object and the set trajectory. The robot's posture is adjusted based on the external force obtained from the collision detection.

[0232] If a collision occurs during the transfer phase, which is highly likely to involve a collision with staff or the surrounding environment, the robot's collision detection response will enter zero-force control mode to ensure the safety of both staff and the robot.

[0233] The embodiments described above are only used to illustrate the technical ideas and features of the present invention. Their purpose is to enable those skilled in the art to understand the content of the present invention and implement it accordingly. The patent scope of the present invention should not be limited by these embodiments. That is, any equivalent changes or modifications made in accordance with the spirit disclosed in the present invention still fall within the patent scope of the present invention.

Claims

1. A robot collision detection method, characterized in that, Combining the robot's structural parameters, the inertia of each component, and the center of gravity, the robot's dynamic equations are constructed using the principle of virtual work. Based on the robot's dynamic equations and the position, velocity, and acceleration data of the robot's joint working path points, the joint driving torques are calculated. An extended state observer based on generalized momentum is constructed from the robot's dynamic equations, and the joint disturbance torques of the robot system are obtained from the extended state observer. A threshold for the disturbance torques is set, and when the obtained joint disturbance torques exceed the threshold range, it is determined that the robot has collided with the outside world. The method includes the following steps: Step 1: Construct the robot's dynamic equations as follows: ； In the formula: This refers to the joint angular displacement; Joint angular velocity; Joint angular acceleration; The inertia matrix; The matrix represents the Coriolis force and the centrifugal force. This is the term related to gravity. This refers to the joint driving torque; The disturbance torque experienced by the joint; Step 2, let the generalized momentum of the system be... ,set up Differentiating the generalized momentum of the system with respect to time, we have: ； Step 3: Apply the first derivative of the generalized momentum By combining the dynamic equations, an extended state observer based on generalized momentum is constructed, from which the estimated value of the joint disturbance torque is obtained. ; Step four, set the threshold for the joint disturbance torque, when the obtained... When the threshold range is exceeded, it is determined that the robot has collided with the outside world; Step three includes the following sub-steps: Step A1, the first derivative of the generalized momentum Substituting this into the robot's dynamics equations, we get: ； Step A2, construct the following extended state observer based on generalized momentum: ； in: , The estimated generalized momentum of the system; K p1 = diag[ k p11 , k p12 , k p13 ], K p2 = [ k p21 , k p22 , k p23 And there are k p1i > 0, k p2i > 0, i = 1, 2; In the formula: The derivative of the estimated momentum value; The estimated external collision torque; This is the derivative of the estimated external collision torque; for The diagonal matrix of the observer gain; for The diagonal matrix of the observer gain; The error is the generalized momentum of the system. for Observer nonlinear function fal The power term; for Observer nonlinear function fal The power term; This represents the piecewise error of the corresponding fal function, which is the same as the sampling period; This represents the piecewise error of the corresponding fal function, which is the same as the sampling period; k p11 for The values ​​of the first row of the main diagonal of the observer gain matrix; k p12 for The values ​​of the elements in the second row of the main diagonal of the observer gain matrix; k p13 for The values ​​of the elements in the third row of the main diagonal of the observer gain matrix; k p21 for The values ​​of the first row of the main diagonal of the observer gain matrix; k p22 for The values ​​of the elements in the second row of the main diagonal of the observer gain matrix; k p23 for The values ​​of the elements in the third row of the main diagonal of the observer gain matrix; Step A3: Obtain the estimated value of the joint disturbance torque using the following formula. : 。 2. The robot collision detection method according to claim 1, characterized in that, Before constructing the robot's dynamic equations, the following steps are also included: using cubic or quintic polynomials to plan the robot's trajectory and obtain the position, velocity, and acceleration data of a series of points on the working path of the robot joints.

3. The robot collision detection method according to claim 1, characterized in that, When setting the threshold for collision detection, select 10% of the maximum input torque of the joint as the threshold for the joint disturbance torque.

4. A robot collision detection system, comprising a memory and a processor, characterized in that, The memory is used to store a computer program; the processor is used to execute the computer program and, when executing the computer program, implement the steps of the robot collision detection method as described in any one of claims 1 to 3.

5. A robot collision response method, characterized in that, The robot collision detection method according to any one of claims 1 to 3 is used to detect whether a collision occurs during the operation of the robot. When a collision is detected between the robot and the outside world, a segmented response strategy is adopted to deal with the collision in combination with the robot's running trajectory.

6. The robot collision response method according to claim 5, characterized in that, The robot is used for sorting operations. The robot's sorting work path includes a grasping stage, a transfer stage, and a placement stage. The stage the robot is in is determined by the feedforward increment of the robot's end position. When a collision between the robot and the outside world is detected, the robot is in the grasping or placing phase. The robot control system enters the compliant control mode and adjusts the robot's posture according to the obtained joint disturbance torque. When a collision between the robot and the external environment is detected, the robot is in the transfer phase, causing the robot control system to enter zero-force control mode.

7. The robot collision response method according to claim 6, characterized in that, The method for determining the robot's current stage based on the robot's end-effector position feedforward increment includes the following steps: Based on the position feedforward after robot trajectory planning, determine Directional increment, when When the absolute value of the directional increment is greater than 0, it is determined that the robot's end effector is in the grasping or placing stage of the sorting process. When the absolute value of the directional increment is 0, it is determined that the robot's end effector is in the transfer phase.

Images