Collision detection method, system, device, medium and program product for robot

Abstract

The invention provides a collision detection method, system and equipment of a robot, a medium and a program product, and the collision detection method comprises the steps: obtaining the joint torque, the operation angular velocity and the operation angular acceleration of each joint in the robot at the current collection moment, and on the basis of the operation angular speed, the operation angular acceleration and the noise correlation parameters of the operation angular acceleration, a joint torque threshold value of each joint of the robot at the current collection moment is obtained. And obtaining a target collision detection result of the robot according to the joint torque and the joint torque threshold value corresponding to the at least one joint. According to the method, the obtained joint torque threshold value is obtained according to the running state of each joint of the current robot, and the influence of noise on the running angular acceleration can be reduced by introducing the noise correlation parameter of the running angular acceleration, so that the obtained joint torque threshold value is more accurate. And the collision detection result of the robot is more accurate.

Description

Technical Field

[0001] The present disclosure relates to the technical field of robot control, and particularly relates to a collision detection method, system, device, medium and program product for a robot. Background Art

[0002] In order to avoid collision problems of robots during movement, the collision detection method of robots in the prior art is to detect whether the joint torque exceeds a certain fixed threshold. If it exceeds, it is determined that the robot has collided. However, this method is prone to false collision judgments of the robot. For example, when the joint speed crosses zero during the robot's commutation, a mutation will occur, and at this time, the joint torque is likely to exceed the fixed threshold, but in fact, the robot has not collided. Therefore, the collision detection method of the robot still needs to be improved. Summary of the Invention

[0003] The technical problem to be solved by the present disclosure is to overcome the defect that in the prior art, it is easy to have false collision judgments of robots by detecting that the joint torque exceeds a certain fixed threshold, and to provide a collision detection method, system, device, medium and program product for a robot.

[0004] The present disclosure solves the above technical problem through the following technical solutions:

[0005] In a first aspect, there is provided a collision detection method for a robot, characterized in that the collision detection method includes:

[0006] Obtain the joint torque, operating angular velocity, and operating angular acceleration of each joint in the robot at the current acquisition moment;

[0007] Based on the operating angular velocity, the operating angular acceleration, and the noise correlation parameter of the operating angular acceleration, obtain the joint torque threshold of each joint of the robot at the current acquisition moment;

[0008] According to the joint torque and the joint torque threshold corresponding to at least one joint, obtain the target collision detection result of the robot.

[0009] Optionally, the step of obtaining the noise correlation parameter includes:

[0010] Based on a pre-established mapping relationship, obtain the noise correlation parameter corresponding to the operating angular acceleration.

[0011] Optionally, the step of obtaining the joint torque threshold of each joint of the robot at the current acquisition moment based on the operating angular velocity, the operating angular acceleration, and the noise correlation parameter of the operating angular acceleration includes:

[0012] Obtain the limiting parameter of the joint torque threshold; wherein, the limiting parameter is used to limit the range of the joint torque threshold;

[0013] Based on the square term of the running angular velocity, the running angular acceleration, the square term of the noise correlation parameter, and the limiting parameter, calculate the joint torque threshold.

[0014] Optionally, the limiting parameter includes a first limiting parameter and a second limiting parameter;

[0015] The first limiting parameter is used to limit the lower limit of the joint torque threshold; the second limiting parameter is used to limit the upper limit of the joint torque threshold;

[0016] The noise correlation parameter includes the noise standard deviation of the running angular acceleration, the noise gain coefficient of the running angular acceleration, and the noise level estimate of the running angular acceleration;

[0017] Wherein, the noise level estimate is used to represent the estimated value of the noise level of the running angular acceleration;

[0018] The calculation formula for calculating the joint torque threshold based on the square term of the running angular velocity, the running angular acceleration, the square term of the noise correlation parameter, and the limiting parameter is:

[0019]

[0020] Wherein, τ th is the joint torque threshold; τ base is the first limiting parameter; τ max is the second limiting parameter; tanh(*) is the hyperbolic tangent function; m is the amplitude gain coefficient of the joint torque threshold; v is the running angular velocity; k v is the gain coefficient of the running angular velocity; a is the running angular acceleration; k a is the gain coefficient of the running angular acceleration; σ is the noise standard deviation; α is the noise gain coefficient; η is the noise level estimate.

[0021] Optionally, obtaining the running angular velocity and the running angular acceleration includes:

[0022] Obtain the joint angle of the robot through the excitation trajectory of the robot; wherein, the excitation trajectory is obtained based on the Fourier series trajectory;

[0023] Perform a difference calculation on the joint angle to obtain the running angular velocity;

[0024] Perform a difference calculation on the running angular velocity to obtain the running angular acceleration.

[0025] Optionally, obtaining the target collision detection result of the robot according to the joint torque corresponding to at least one of the joints and the joint torque threshold includes:

[0026] In response to the joint torque corresponding to at least one of the joints being greater than or equal to the joint torque threshold and lasting for a preset time interval, it is determined that the robot has collided;

[0027] In response to the joint torque corresponding to each joint being less than the joint torque threshold, it is determined that the robot has not collided.

[0028] In a second aspect, a collision detection system for a robot is provided, characterized in that the collision detection system includes:

[0029] A first acquisition module for acquiring the joint torque, running angular velocity, and running angular acceleration of each joint in the robot at the current acquisition moment;

[0030] A calculation module, based on the running angular velocity, the running angular acceleration, and the noise correlation parameter of the running angular acceleration, obtains the joint torque threshold of each joint of the robot at the current acquisition moment;

[0031] A second acquisition module for obtaining the target collision detection result of the robot according to the joint torque corresponding to at least one of the joints and the joint torque threshold.

[0032] Optionally, the first acquisition module is further configured to acquire the noise correlation parameter, specifically including:

[0033] Based on a pre-established mapping relationship, the noise correlation parameter corresponding to the running angular acceleration is acquired.

[0034] Optionally, the calculation module includes;

[0035] An acquisition unit for acquiring a limit parameter of the joint torque threshold; wherein, the limit parameter is used to limit the range of the joint torque threshold;

[0036] A calculation unit, based on the square term of the running angular velocity, the square term of the running angular acceleration and the noise correlation parameter, and the limit parameter, calculates the joint torque threshold.

[0037] Optionally, the limiting parameter includes a first limiting parameter and a second limiting parameter; the first limiting parameter is used to limit the lower limit of the joint torque threshold; the second limiting parameter is used to limit the upper limit of the joint torque threshold; the noise correlation parameter includes the noise standard deviation of the running angular acceleration, the noise gain coefficient of the running angular acceleration, and the noise level estimation of the running angular acceleration; wherein, the noise level estimation is used to represent the estimated value of the noise level of the running angular acceleration;

[0038] The calculation formula for obtaining the joint torque threshold based on the square term of the running angular velocity, the running angular acceleration, the square term of the noise correlation parameter, and the limiting parameter is:

[0039]

[0040] where, τ th is the joint torque threshold; τ base is the first limiting parameter; τ max is the second limiting parameter; tanh(*) is the hyperbolic tangent function; m is the amplitude gain coefficient of the joint torque threshold; v is the running angular velocity; k v is the gain coefficient of the running angular velocity; a is the running angular acceleration; k a is the gain coefficient of the running angular acceleration; σ is the noise standard deviation; α is the noise gain coefficient; η is the noise level estimation.

[0041] Optionally, the first acquisition module is further configured to acquire the running angular velocity and the running angular acceleration, specifically including:

[0042] Obtain the joint angles of the robot through the excitation trajectory of the robot; wherein, the excitation trajectory is obtained based on the Fourier series trajectory;

[0043] Perform a difference calculation on the joint angles to obtain the running angular velocity;

[0044] Perform a difference calculation on the running angular velocity to obtain the running angular acceleration.

[0045] Optionally, the second acquisition module includes:

[0046] The first response unit determines that the robot has collided when the joint torque corresponding to at least one of the joints is greater than or equal to the joint torque threshold and lasts for a preset time interval;

[0047] The second response unit determines that the robot has not collided when the joint torque corresponding to each joint is less than the joint torque threshold.

[0048] In a third aspect, an electronic device is provided, including a memory, a processor, and a computer program stored on the memory and configured to run on the processor. When the processor executes the computer program, the collision detection method of the above-mentioned robot is implemented.

[0049] In a fourth aspect, a computer-readable storage medium is provided, on which a computer program is stored. The computer program, when executed by a processor, is characterized in that the collision detection method of the above-mentioned robot is implemented.

[0050] In a fifth aspect, a computer program product is provided, including a computer program. The computer program, when executed by a processor, is characterized in that the collision detection method of the robot described above is implemented.

[0051] On the basis of conforming to common knowledge in the art, the above preferred conditions can be combined arbitrarily to obtain various preferred examples of the present disclosure.

[0052] The positive and progressive effects of the present disclosure are as follows: By calculating the joint torque threshold based on the running angular velocity, running angular acceleration, and noise correlation parameter of the running angular acceleration at the current acquisition moment of each joint, not only is the obtained joint torque threshold based on the running states of the respective joints of the current robot, avoiding the occurrence of misjudgment of robot collisions caused by sudden speed changes. Moreover, the influence of noise on the running angular acceleration is also considered. By introducing the noise correlation parameter of the running angular acceleration, the influence of noise on the running angular acceleration can be reduced, making the obtained joint torque threshold more accurate. Furthermore, based on the relationship between the joint torque and the joint torque threshold, the collision detection result of the robot is more accurate, thus avoiding the occurrence of misjudgment of robot collisions. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1 It is a flowchart of a collision detection method for a robot provided in Embodiment 1 of the present disclosure;

[0054] Figure 2 It is a relationship diagram of the joint torque threshold, running angular velocity, running angular acceleration, and noise level estimation in a collision detection method for a robot provided in Embodiment 1 of the present disclosure;

[0055] Figure 3 It is a result diagram obtained from simulated collision detection of a collision detection method for a robot provided in Embodiment 1 of the present disclosure;

[0056] Figure 4 It is a flowchart of another collision detection method for a robot provided in Embodiment 1 of the present disclosure;

[0057] Figure 5Schematic diagram of modules of a collision detection system for a robot provided in Embodiment 2 of the present disclosure.

[0058] Figure 6 Schematic diagram of the structure of an electronic device shown in Embodiment 3 of the present disclosure. Detailed implementation manners

[0059] The present disclosure will be further described below by way of embodiments, but the present disclosure is not limited to the scope of the described embodiments.

[0060] In the embodiments of the present disclosure, prefix words such as "first" and "second" are only used to distinguish different described objects, and have no limiting effect on the position, order, priority, quantity or content of the described objects. The use of ordinal numbers and other prefix words for distinguishing described objects in the embodiments of the present disclosure does not constitute a limitation on the described objects. The statements of the described objects refer to the descriptions in the context of the embodiments, and no redundant limitations should be formed due to the use of such prefix words. In addition, in the description of this embodiment, unless otherwise specified, the meaning of "a plurality" is two or more.

[0061] Embodiment 1

[0062] In order to avoid misjudgment of robot collisions, Figure 1 Flowchart of a collision detection method for a robot provided in Embodiment 1 of the present disclosure. Figure 1 Flowchart of a collision detection method for a robot provided in Embodiment 1 of the present disclosure. The collision detection method for the robot includes the following steps:

[0063] Step 101: Obtain the joint torque, running angular velocity, and running angular acceleration of each joint in the robot at the current acquisition moment.

[0064] Preferably, the robot is a collaborative robot.

[0065] The following describes one method for obtaining the joint torque of each joint of the robot at the current acquisition moment.

[0066] S1: Obtain the joint current of each joint of the robot at the current acquisition moment.

[0067] S2: Identify the minimum dynamic parameters obtained by using the excitation trajectory of the robot and combining the least squares method.

[0068] The least dynamic parameters of the collaborative robot are identified by combining the least squares method. The method is simple and effective and easy to implement. The step-by-step iterative least squares method incorporating reasonable prior values can also adjust the joint dynamic parameters with a decreasing step size in each iteration using different excitation trajectories, avoiding the disadvantages of insufficient data volume and motion range of a single excitation trajectory and fixed environmental temperature, improving the identification accuracy, and making the subsequent collision detection more accurate.

[0069] S3: Input the running angular velocity, running angular acceleration, joint current, and least dynamic parameters corresponding to each joint into the dynamic model to obtain the joint torque of each joint when the robot runs normally.

[0070] The following explains the least dynamic parameters calculated using the excitation trajectory of the robot above.

[0071] (1) Construct the set of least dynamic parameters to be identified for the collaborative robot:

[0072] The traditional dynamic inertia parameter set is:

[0073] X j =[XX j ,XY j ,XZ j ,YY j ,YZ j ,ZZ j ,mX j ,mY j ,mZ j ,m j

[0074] Among them, (XX j ,…,ZZ j ) is the element of the inertia matrix j J j J j of the link j with respect to the origin O j of the coordinate system j, (mX j ,mY j ,mZ j ) is the mass moment of the link j with respect to the centroid of mass, and m

[0075] If the j joint is a rotational joint, the inertia parameters YY j , mZ j , m j in the inertia parameter set of the link j can be eliminated by the method of recombining XX j and the inertia parameters of the link j. The recombined robot inertia parameters are:

[0076] XXR j =XX j -YY​j

[0077]

[0078] XYR j-1 = XY j-1 + d j Sα j mZ j + d j r j Sα j m j

[0079] XZR j-1 = XZ j-1 + d j Cα j mZ j + d j r j Cα j m j

[0080]

[0081] mXR j-1 = mX j-1 + d j m j

[0082] mYR j-1 = mY j-1 - Sα j mZ j - r j Sα j m j

[0083] mZR j-1 = mZ j-1 - Cα j mZ j - r j Cα j m j

[0084] mR j-1 = m j-1 + m j

[0085] where (XXR j , …, ZZR j-1 ) are the elements of the inertia matrix j J j of the reconfigured connecting rod j with respect to the origin O of the coordinate system j j (mXR j-1 , …, mR j-1)is the mass moment of the reorganized connecting rod j with respect to the centroid, m j is the mass of the connecting rod j, r j , d j , α j respectively represent the joint distance, link length, and link twist angle in the D-H parameters. r j , d j , α j , θ j represent the joint distance, link length, link twist angle, and joint rotation angle in the D-H parameters, S represents the sin function, and C represents the cos function.

[0086] In summary, the set of minimum dynamic parameters to be identified for the collaborative robot.

[0087] (2) The process of identifying the above set of minimum dynamic parameters to be identified for the collaborative robot based on the established dynamic model of the collaborative robot to obtain the set of minimum dynamic parameters of the collaborative robot is as follows:

[0088] Establish a dynamic model of the collaborative robot. Among them, the collaborative robot is a type of robot.

[0089] According to the Newton-Euler formula, regarding the collaborative robot as a link model, the joint torque f of the collaborative robot n and the spatial inertia I n , the spatial acceleration a n , the spatial velocity v n are related as follows:

[0090] f n = I n a n + v n × I n v n

[0091] I n a n The calculation formula of

[0092]

[0093] where is the linear acceleration; its calculation formula is: is the spatial rotation angle, ω n is the spatial angular velocity, m n is the link mass, S(*) = diag(*) diagonal matrix; is the inertia matrix of the link; is the spatial angular acceleration.

[0094] v n × I n vn The calculation formula is as follows:

[0095]

[0096] where c n represents the centroid position in space.

[0097] Substitute I n a n and v n ×I n v n into f n = I n a n + v n ×I n v n , we can get:

[0098]

[0099] Regarding the mass matrix m n c n in the above formula as the parameter to be identified in parameter identification, we can get:

[0100]

[0101] where A n is a 6×10 matrix, Φ n is a vector with 10 inertia parameters, and can be obtained through the construction process of the set of minimum dynamic parameters to be identified of the collaborative robot in (1). L(*) is a 3*6 matrix.

[0102] When performing parameter identification calculation using the excitation trajectory, the data points measured on the trajectory can be combined into a matrix A and a vector f, that is:

[0103]

[0104] where P is the number of data points.

[0105] For a serial robot with n links, define f ij as the spatial force generated by only moving link j on joint i, and f ii is the spatial force generated by only moving the link where joint i is located on the joint, then there is: i f ii = i A i Φ i , and the total spatial force of joint i is Each spatial force of joint i i f ij can be obtained through jf jj The spatial force transformation matrix is obtained as follows:

[0106]

[0107] where represents the coordinate transformation of the force vector from the j coordinate system to the i coordinate system, i p j is the translation transformation matrix from the j coordinate system to the i coordinate system, and S(*) is a skew diagonal matrix.

[0108] From the above formulas, we can obtain:

[0109]

[0110] where i A j is the rotation transformation matrix from the j coordinate system to the i coordinate system.

[0111] Usually, for a linkage system, only the torque τ i about the joint rotation axis z i can be measured. Each spatial force i f i must be projected onto the joint rotation axis. Therefore, the above formula can be simplified to:

[0112] τ = Kφ;

[0113]

[0114] Φ i = (m i m i r xi m i r yi m i r zi I oxxi I oxyi I oxzi I oyyi I oyzi I ozzi F c F v ) T

[0115] where τ is an nP×1 vector, K is an nP×10n matrix, which is the linearized matrix corresponding to the minimum dynamic parameter set of the dynamic model, and Φ i is the minimum dynamic parameter set of this collaborative robot.

[0116] In the prior art, generally, some parameters that cannot be identified or linearly combined and identifiable are set to zero to obtain the values of other parameters. This may cause the obtained parameters to lose their physical meanings. For example, problems such as negative mass and negative moment of inertia may occur. If they are used in robot control, it may lead to system instability. However, the above is identified in a step-by-step iterative manner. The unidentifiable dynamic parameters in the current step are replaced with the parameter values of the previous step to ensure the stability of the solution and gradually converge the dynamic parameters to the optimal values. Compared with the methods in the prior art, it has higher accuracy and faster convergence speed.

[0117] Step 102: Based on the running angular velocity, running angular acceleration, and noise correlation parameter of the running angular acceleration, obtain the joint torque threshold of each joint of the robot at the current acquisition moment.

[0118] Step 103: According to the joint torque and the joint torque threshold corresponding to at least one joint, obtain the target collision detection result of the robot.

[0119] In this embodiment, calculating the joint torque threshold based on the running angular velocity, running angular acceleration, and noise correlation parameter of the running angular acceleration of each joint at the current acquisition moment not only makes the obtained joint torque threshold obtained according to the running states of each joint of the current robot to avoid the phenomenon of misjudging robot collision caused by sudden speed change. Moreover, it also takes into account the influence of noise on the running angular acceleration. By introducing the noise correlation parameter of the running angular acceleration, the influence of noise on the running angular acceleration can be reduced, so that the obtained joint torque threshold is more accurate. Furthermore, according to the relationship between the joint torque and the joint torque threshold, the collision detection result of the robot is more accurate, thus avoiding the phenomenon of misjudging robot collision. In addition, applying this joint torque threshold enhances the universality and robustness of the collision detection algorithm and avoids the situation of detecting a collision at the motor commutation point. There is no need to add another sensor or flexible device to the joint, which can reduce costs and has good application value in fields such as service-type human-robot collaboration robots.

[0120] In one embodiment, obtaining the noise correlation parameter of the running angular acceleration includes: based on a pre-established mapping relationship, obtaining the noise correlation parameter corresponding to the running angular acceleration.

[0121] It should be noted that the pre-established mapping relationship is established by relevant personnel after a large number of experiments. Therefore, this mapping relationship can accurately represent the corresponding relationship between the running angular acceleration and the noise correlation parameter.

[0122] In this embodiment, the pre-established mapping relationship is equivalent to a clear index, which can directly associate the noise correlation parameter corresponding to the running angular acceleration, so that the corresponding noise correlation parameter can be obtained without complex calculations based on the running angular acceleration during the acquisition process, thereby greatly saving the search time.

[0123] In one embodiment, the steps of obtaining the joint torque threshold of each joint of the robot at the current acquisition moment based on the running angular velocity, the running angular acceleration, and the noise correlation parameter of the running angular acceleration include:

[0124] Step 102-1: Obtain the limit parameter of the joint torque threshold; wherein, the limit parameter is used to limit the range of the joint torque threshold.

[0125] During the movement of the joint, if the joint torque exceeds its physical limit, problems such as motor overheating and gear damage may occur. Therefore, the limit parameter can ensure that the joint torque is always within a safe range

[0126] Step 102-2: Calculate the joint torque threshold based on the square term of the running angular velocity, the running angular acceleration, the square term of the noise correlation parameter, and the limit parameter.

[0127] In this embodiment, by setting the limit parameter of the joint torque threshold, the joint motor can be effectively prevented from being damaged due to overload. In addition, the square term of the running angular velocity, the running angular acceleration, and the square term of the noise correlation parameter will grow faster than the linear term, making the obtained joint torque threshold more sensitive to parameter fluctuations, and thus making the subsequent collision detection of the robot more accurate.

[0128] In one embodiment, the limit parameter includes a first limit parameter and a second limit parameter; the first limit parameter is used to limit the lower limit of the joint torque threshold; the second limit parameter is used to limit the upper limit of the joint torque threshold; the noise correlation parameter includes the noise standard deviation of the running angular acceleration, the noise gain coefficient of the running angular acceleration, and the noise level estimate of the running angular acceleration; wherein, the noise level estimate is used to represent the estimated value of the noise level of the running angular acceleration;

[0129] The calculation formula for obtaining the joint torque threshold based on the square term of the running angular velocity, the running angular acceleration, the square term of the noise correlation parameter, and the limit parameter is:

[0130]

[0131] where, τ th is the joint torque threshold; τ base is the first limit parameter; τ maxis the second limit parameter; tanh(*) is the hyperbolic tangent function; m is the amplitude gain coefficient of the joint torque threshold; v is the operating angular velocity; k v is the gain coefficient of the operating angular velocity; a is the operating angular acceleration; k a is the gain coefficient of the operating angular acceleration; σ is the noise standard deviation; α is the noise gain coefficient; η is the noise level estimate.

[0132] To avoid misjudgment of the error spikes during speed commutation. When the speed commutes, the joint speed crosses zero and the error mutates. Therefore, a larger threshold change needs to be added near zero, that is, a function expression with a larger growth rate near zero and a smaller growth rate at other positions is required, and the growth rate of the mutation threshold can be determined according to the set parameters. Therefore, the 1 - tanh() function form is selected.

[0133] However, only the speed term cannot accurately judge the motion state of the robot. Therefore, an acceleration signal with a faster response speed is introduced for comprehensive decision-making. But due to the obvious acceleration noise, which affects the judgment result, a noise term needs to be further added to optimize the result.

[0134] To achieve the envelope of the obtained joint torque threshold for the actual observed value, a limit parameter is introduced.

[0135] In addition, the expression (a - α·σ) / (1 + η) reflects the relationship between the operating angular acceleration a and the noise level estimate η. During the signal process:

[0136] 1. Separation of signal and noise: In many signal processing problems, the goal is to extract useful signals from noise. The operating angular acceleration a can be regarded as part of the signal, while η represents the noise level estimate of the operating angular acceleration a. By adjusting the noise level, the threshold of signal processing can be changed, so that the extracted operating angular acceleration a is more accurate.

[0137] 2. Adjustment of noise tolerance: When the noise level estimate η increases, the denominator 1 + η becomes larger, which causes the value of the entire expression to decrease. This can be interpreted as that as the noise level estimate increases, the system's tolerance for noise increases, so the influence of the operating angular acceleration a on the final result decreases relatively. This conforms to the actual situation because in a high-noise environment, by reducing the sensitivity to the signal, wrong decisions can be avoided.

[0138] 3. In control theory, this expression can be used to model the system's response to disturbances. In this case, an increase in the noise level estimate η can be regarded as an increase in the system's tolerance for external disturbances, making the system more robust.

[0139] Specifically, the following is how to understand the influence of the noise level estimate η on the expression:

[0140] When the noise level estimate η increases, 1 + η becomes larger, so the value of the whole fraction decreases. This means that when the noise level rises, the sensitivity of the system to the operating angular acceleration a decreases, thereby increasing the noise tolerance.

[0141] When the operating angular acceleration a increases, if the noise level estimate η remains unchanged, the value of the numerator increases, which increases the value of the whole expression, unless the increase in α·σ offsets the increase in a.

[0142] In this expression, the roles of the noise gain coefficient α and the noise standard deviation σ are to adjust the influence of noise on the final result. An increase in the noise gain coefficient α enhances the influence of noise, while an increase in the noise standard deviation σ indicates an increase in the intensity of the noise itself.

[0143] Therefore, this expression is reasonable, and it reflects how to influence the system's processing of the operating angular acceleration a by adjusting the noise level estimate η in the presence of noise.

[0144] Figure 2 It is a relationship diagram of the joint torque threshold with the operating angular velocity, operating angular acceleration, and noise level estimate in a collision detection method for a robot provided in Embodiment 1 of the present disclosure. Figure 2 The three axes in it respectively represent the operating angular velocity v, the operating angular acceleration a, and the noise level estimate η. This diagram can intuitively show the relationship among the operating angular velocity v, the operating angular acceleration a, and the noise level estimate η.

[0145] In this embodiment, the joint torque threshold obtained through this calculation formula can not only avoid the problem of sudden change in model error caused by zero-crossing of the speed value, resulting in an overly large fixed threshold setting and low collision detection sensitivity; but also add an acceleration term to make a comprehensive decision on the motion state of the robot, avoiding misjudgment of collisions caused by a single speed term.

[0146] In one embodiment, obtaining the operating angular velocity and the operating angular acceleration includes:

[0147] Step 101-1: Obtain the joint angles of the robot through the excitation trajectory of the robot; wherein, the excitation trajectory is obtained based on the Fourier series trajectory.

[0148] The following specifically describes how to obtain the above-mentioned excitation trajectory of the robot based on the Fourier series trajectory:

[0149] Adopt the Fourier series trajectory to obtain the excitation trajectory with the minimum condition number of the dynamic matrix of the robot.

[0150] Specifically, in order to increase the order of the dynamic matrix K of the robot, it is necessary to matrix K at different timesi (where \(i = 1, 2, \cdots, s\), and \(s\) is the number of sampling points) are combined into a large \(K\) matrix:

[0151] \(K=[K_1\ K_2\cdots K\) s T

[0152] The torque vectors \(\tau\) at corresponding moments i are combined into a long \(\tau\) vector:

[0153] \(\tau=[\tau_1\ \tau_2\cdots\tau\) s T

[0154] The sampling time points are distributed within a motion cycle of the robot. To improve the stability of parameter identification, it is required that the condition number of matrix \(K\) be as small as possible. At the same time, the input identification trajectory should be sufficiently representative and contain as much characteristic information of the robot as possible. A trajectory that changes with time \(t\) using a finite Fourier series is adopted:

[0155]

[0156] where, \(a\) j , \(b\) j are Fourier coefficients, \(\theta_0\) is the reference number of the finite Fourier series trajectory, \(j\) represents the number of Fourier terms, \(N\) represents the maximum value of the number of Fourier terms, preferably \(N\) takes 3 - 5, and the fundamental frequency \(\omega\) avoids the resonance frequency of the robot. At the same time, considering the limitations of the robot on joint angles, angular velocities, and angular accelerations, it is related to the condition number of the dynamic matrix \(K\) of the robot:

[0157]

[0158] Taking this as the optimization condition, an excitation trajectory with the minimum condition number of \(K\) is obtained, where \(\lambda_1\) and \(\lambda_2\) are adjustment parameters.

[0159] Step 101 - 2: Perform a difference calculation on the joint angles to obtain the running angular velocity.

[0160] Step 101 - 3: Perform a difference calculation on the running angular velocity to obtain the running angular acceleration.

[0161] ​​In this embodiment, in view of the problem that traditional identification methods can only identify linear dynamic models with low identification accuracy, Fourier series excitation trajectories are used, and commutation points are removed from the operation data. Multiple excitation trajectories can minimize the influence of cumulative errors, making the obtained dynamic model more in line with the actual model and making the identification result more reliable. Thus, the joint angles obtained according to the excitation trajectories are more accurate, and further, the operation angular velocity and operation angular acceleration obtained by subsequent differential calculations are more accurate. In addition, based on the step-by-step iterative least squares method incorporating reasonable prior values, different excitation trajectories are used to adjust the joint dynamic parameters at a decreasing step size in each iteration, avoiding the disadvantages such as insufficient data volume and motion range of a single excitation trajectory and fixed environmental temperature, improving the identification accuracy, and making the collision detection more accurate.

[0162] In one embodiment, obtaining the target collision detection result of the robot according to the joint torque and the joint torque threshold corresponding to at least one joint includes:

[0163] In response to the joint torque corresponding to at least one joint being greater than or equal to the joint torque threshold and lasting for a preset time interval, it is determined that the robot has collided.

[0164] In response to the joint torque corresponding to each joint being less than the joint torque threshold, it is determined that the robot has not collided.

[0165] Figure 3 This is a result graph obtained by simulating the collision detection of a collision detection method for a robot provided in Embodiment 1 of the present disclosure. As Figure 3 shown, the six axes in the figure represent the six joints corresponding to the robot. The orange line is a fluctuating curve formed by the joint torque thresholds corresponding to each acquisition moment, and the blue line is a fluctuating curve formed by the actual joint torques obtained at each acquisition moment. At the same acquisition moment, if the joint torque on any of the blue lines on the six axes is higher than or equal to the joint torque threshold on the orange line and lasts for a preset time interval, a collision occurs. For example, near the sampling moments of 1000 points, 2000 points, and 3000 points, since the joint torques of the six axes simultaneously exceed the joint torque threshold and last for a preset time interval, it can be determined that the collaborative robot has collided at this time. At the same acquisition moment, if the joint torques on the blue lines on the six axes are not higher than the joint torque threshold on the orange line, no collision occurs. For example, when the sampling moment is 1500 points, the joint torques of the six axes do not exceed the joint torque threshold, so it can be determined that the collaborative robot has not collided at this time.

[0166] In one embodiment, the optimization method for the above dynamic model includes:

[0167] Step 201, obtain the torque coefficient and transmission efficiency of the robot joint.

[0168] Step 202: Input the running angular velocity, running angular acceleration, joint current, torque coefficient, and transmission efficiency into a filtering algorithm to obtain the filtered joint torque.

[0169] Step 203: Based on the joint torque and the filtered joint torque, correct the minimum dynamic parameter set of the robot.

[0170] The following describes one implementation method for correcting the minimum dynamic parameter set of the robot:

[0171] Step 203-1: Calculate the torque difference between the joint torque and the filtered joint torque. The formula is as follows:

[0172]

[0173] Where, represents the torque difference, τ represents the joint torque; represents the filtered joint torque; K is the dynamic matrix; Φ is the initially input minimum dynamic parameter set; is the minimum dynamic parameter error set; is the corrected minimum dynamic parameter set.

[0174] Step 203-2: Update the initially input minimum dynamic parameter set according to the torque error. The update formula is as follows:

[0175]

[0176] In this embodiment, continuously fine-tuning the dynamic parameters during the operation of the robot according to the torque difference between the joint torque and the filtered joint torque can make the joint torque output by the kinematic model closer to the actual torque, thereby making the subsequent collision detection of the robot more accurate.

[0177] In one embodiment, Figure 4 is a flowchart of another collision detection method for the robot provided in Embodiment 1 of the present disclosure. In combination with Figure 3 further illustrate the collision detection of the robot.

[0178] S1: Establish the dynamic model and excitation trajectory of the robot.

[0179] S2: Identify the minimum dynamic parameter set based on the excitation trajectory.

[0180] S3: Obtain the motion information at the current acquisition moment.

[0181] When a cobot performs collision detection, it needs to continuously collect the motion information of each joint of the robot at the current acquisition moment. Taking the acquisition of the motion information of six joints of the cobot as an example: When the cobot performs collision detection, it needs to collect the joint angles and current information of the six joints at the current acquisition moment, and obtain the running angular velocity and running angular acceleration through time difference. Therefore, the motion information includes: joint angles, current information, running angular velocity, and running angular acceleration.

[0182] S4: Input the motion information into the filtering algorithm to obtain the filtered joint torque; input the motion information into the dynamic model to obtain the joint torque.

[0183] According to the motion information during the operation of the cobot, input it into the filtering algorithm to obtain the filtered joint torque; input the motion information and the minimum dynamic parameter set into the dynamic model to obtain the joint torques of each joint of the cobot during normal operation at the current acquisition moment.

[0184] S5: Calculate the torque difference between the filtered joint torque and the joint torque.

[0185] S6: Update the dynamic model based on the torque difference.

[0186] S7: Calculate the joint torque threshold.

[0187] S8: Determine whether the joint torque is greater than or equal to the joint torque threshold and lasts for a preset time interval. If so, the robot has collided; if not, return to S6.

[0188] Embodiment 2

[0189] Corresponding to the foregoing embodiment of the collision detection method for a robot, the present disclosure also provides an embodiment of a collision detection system for a robot. Figure 5 The module schematic diagram of a collision detection system for a robot provided in Embodiment 2 of the present disclosure. The collision detection system 50 of the robot includes:

[0190] The first acquisition module 51 is configured to acquire the joint torque, running angular velocity, and running angular acceleration of each joint in the robot at the current acquisition moment;

[0191] The calculation module 52 is configured to obtain the joint torque threshold of each joint of the robot at the current acquisition moment based on the running angular velocity, running angular acceleration, and noise correlation parameter of the running angular acceleration;

[0192] The second acquisition module 53 is configured to obtain the target collision detection result of the robot according to the joint torque and the joint torque threshold corresponding to at least one joint.

[0193] In one embodiment, the first acquisition module 51 is further configured to acquire the noise correlation parameter, specifically including:

[0194] Based on a pre-established mapping relationship, obtain a noise correlation parameter corresponding to the running angular acceleration.

[0195] In one embodiment, the calculation module 52 includes:

[0196] An acquisition unit for acquiring a limit parameter of the joint torque threshold; wherein, the limit parameter is used to limit the range of the joint torque threshold;

[0197] A calculation unit, based on the square term of the running angular velocity, the running angular acceleration, the square term of the noise correlation parameter, and the limit parameter, calculates the joint torque threshold.

[0198] In one embodiment, the limit parameter includes a first limit parameter and a second limit parameter; the first limit parameter is used to limit the lower limit of the joint torque threshold; the second limit parameter is used to limit the upper limit of the joint torque threshold; the noise correlation parameter includes the noise standard deviation of the running angular acceleration, the noise gain coefficient of the running angular acceleration, and the noise level estimate of the running angular acceleration; wherein, the noise level estimate is used to represent the estimated value of the noise level of the running angular acceleration;

[0199] Based on the square term of the running angular velocity, the running angular acceleration, the square term of the noise correlation parameter, and the limit parameter, the calculation formula for obtaining the joint torque threshold is:

[0200]

[0201] Wherein, τ th is the joint torque threshold; τ base is the first limit parameter; τ max is the second limit parameter; tanh(*) is the hyperbolic tangent function; m is the amplitude gain coefficient of the joint torque threshold; v is the running angular velocity; k v is the gain coefficient of the running angular velocity; a is the running angular acceleration; k a is the gain coefficient of the running angular acceleration; σ is the noise standard deviation; α is the noise gain coefficient; η is the noise level estimate.

[0202] In one embodiment, the first acquisition module 51 is further configured to acquire the running angular velocity and the running angular acceleration, specifically including:

[0203] Obtain the joint angle of the robot through the excitation trajectory of the robot; wherein, the excitation trajectory is obtained based on the Fourier series trajectory;

[0204] Perform a difference calculation on the joint angle to obtain the running angular velocity;

[0205] Perform a difference calculation on the running angular velocity to obtain the running angular acceleration.

[0206] In one embodiment, the second acquisition module 53 includes:

[0207] A first response unit that determines that the robot has collided if the joint torque corresponding to at least one joint is greater than or equal to a joint torque threshold and continues for a preset time interval;

[0208] A second response unit that determines that the robot has not collided if the joint torque corresponding to each joint is less than the joint torque threshold.

[0209] For the system embodiments, since they basically correspond to the method embodiments, the relevant parts can be referred to the partial descriptions of the method embodiments. The system embodiments described above are only illustrative. The units described as separate components may or may not be physically separated. The components as units may or may not be physical units, that is, they may be located in one place or distributed to multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the present disclosure solution.

[0210] Embodiment 3

[0211] Figure 6 The structural schematic diagram of an electronic device shown in an exemplary embodiment of the present disclosure. The electronic device includes a memory, a processor, and a computer program stored in the memory and configured to run on the processor. When the processor executes the computer program, it implements the collision detection method of the robot in any of the above embodiments.

[0212] Figure 6 The shown electronic device 60 is only an example and should not impose any limitation on the functions and usage scope of the embodiments of the present disclosure.

[0213] As Figure 6 shown, the electronic device 60 can be presented in the form of a general computing device, for example, it can be a server device. The components of the electronic device 60 may include, but are not limited to: at least one of the above processors 61, at least one of the above memories 62, and a bus 63 connecting different system components (including the memory 62 and the processor 61).

[0214] The bus 63 includes a data bus, an address bus, and a control bus.

[0215] The memory 62 may include volatile memory, such as a random access memory (RAM) 621 and / or a cache memory 622, and may further include a read-only memory (ROM) 623.

[0216] The memory 62 may also include a program tool 625 (or utility) having a set (at least one) of program modules 624. Such program modules 624 include, but are not limited to: an operating system, one or more application programs, other program modules, and program data. Each or some combination of these examples may include the implementation of a network environment.

[0217] The processor 61 executes various functional applications and data processing by running computer programs stored in the memory 62, such as the collision detection method of the robot provided in any of the above embodiments.

[0218] The electronic device 60 may also communicate with one or more external devices 64 (such as a keyboard, a pointing device, etc.). Such communication may be performed through the input / output (I / O) interface 65. And, the electronic device 60 may also communicate with one or more networks (such as a local area network (LAN), a wide area network (WAN), and / or a public network, such as the Internet) through the network adapter 66. As shown in the figure, the network adapter 66 communicates with other modules of the electronic device 60 through the bus 63. It should be understood that, although not shown in the figure, other hardware and / or software modules may be used in combination with the electronic device 60, including but not limited to: microcode, device drivers, redundant processors, external disk drive arrays, RAID (disk array) systems, tape drives, and data backup storage systems, etc.

[0219] It should be noted that, although several units / modules or sub-units / modules of the electronic device are mentioned in the above detailed description, this division is merely exemplary and not mandatory. In fact, according to the embodiments of the present disclosure, the features and functions of two or more of the above-described units / modules may be embodied in one unit / module. Conversely, the features and functions of one unit / module described above may be further divided and embodied by multiple units / modules.

[0220] Embodiment 4

[0221] The embodiments of the present disclosure also provide a computer-readable storage medium, on which a computer program is stored. When the program is executed by a processor, it implements the collision detection method of the robot provided in any of the above embodiments.

[0222] Among them, the more specific readable storage medium may include, but is not limited to: a portable disk, a hard disk, a random access memory, a read-only memory, an erasable programmable read-only memory, an optical storage device, a magnetic storage device, or any suitable combination of the above.

[0223] Embodiment 5

[0224] An embodiment of the present disclosure also provides a computer program product, including a computer program which, when executed by a processor, implements the collision detection method of the robot in any one of the above.

[0225] Among them, the program code for executing the computer program product of the present disclosure can be written in any combination of one or more programming languages, and the program code can be executed entirely on the user device, partially on the user device, executed as an independent software package, partially on the user device and partially on a remote device, or entirely on a remote device.

[0226] Although the specific embodiments of the present disclosure have been described above, those skilled in the art should understand that this is only an example, and the protection scope of the present disclosure is defined by the appended claims. Without departing from the principle and essence of the present disclosure, those skilled in the art can make various changes or modifications to these embodiments, but these changes and modifications all fall within the protection scope of the present disclosure.

Claims

1. A collision detection method for a robot, characterized in that, The collision detection method includes: Obtaining the joint torque, operating angular velocity, and operating angular acceleration of each joint in the robot at the current acquisition moment; Based on the operating angular velocity, the operating angular acceleration, and the noise correlation parameter of the operating angular acceleration, obtaining the joint torque threshold of each joint of the robot at the current acquisition moment; According to the joint torque corresponding to at least one of the joints and the joint torque threshold, obtaining the target collision detection result of the robot.

2. The collision detection method according to claim 1, wherein The step of obtaining the noise correlation parameter includes: Based on a pre-established mapping relationship, obtaining the noise correlation parameter corresponding to the operating angular acceleration.

3. The collision detection method according to claim 1, characterized in that The step of obtaining the joint torque threshold of each joint of the robot at the current acquisition moment based on the operating angular velocity, the operating angular acceleration, and the noise correlation parameter of the operating angular acceleration includes: Obtaining a limit parameter for the joint torque threshold; wherein, the limit parameter is used to limit the range of the joint torque threshold; Based on the square term of the operating angular velocity, the operating angular acceleration, the square term of the noise correlation parameter, and the limit parameter, calculating to obtain the joint torque threshold.

4. The collision detection method according to claim 3, characterized in that The limit parameter includes a first limit parameter and a second limit parameter; The first limit parameter is used to limit the lower limit of the joint torque threshold; the second limit parameter is used to limit the upper limit of the joint torque threshold; The noise correlation parameter includes the noise standard deviation of the operating angular acceleration, the noise gain coefficient of the operating angular acceleration, and the noise level estimate of the operating angular acceleration; Wherein, the noise level estimate is used to represent the estimated value of the noise level of the operating angular acceleration; The calculation formula for calculating the joint torque threshold based on the square term of the operating angular velocity, the operating angular acceleration, the square term of the noise correlation parameter, and the limit parameter is: where τ th is the joint torque threshold; τ base is the first limit parameter; τ max is the second limit parameter; tanh(*) is the hyperbolic tangent function; m is the amplitude gain coefficient of the joint torque threshold; v is the running angular velocity; k v is the gain coefficient of the running angular velocity; a is the running angular acceleration; k a is the gain coefficient of the running angular acceleration; σ is the noise standard deviation; α is the noise gain coefficient; η is the noise level estimation.

5. The collision detection method according to any one of claims 1-4, characterized in that, Obtaining the operating angular velocity and the operating angular acceleration includes: Obtaining the joint angle of the robot through the excitation trajectory of the robot; wherein, the excitation trajectory is obtained based on the Fourier series trajectory; Performing a difference calculation on the joint angle to obtain the operating angular velocity; Performing a difference calculation on the operating angular velocity to obtain the operating angular acceleration.

6. The collision detection method according to any one of claims 1-4, characterized in that The obtaining the target collision detection result of the robot according to the joint torque corresponding to at least one of the joints and the joint torque threshold includes: In response to the joint torque corresponding to at least one of the joints being greater than or equal to the joint torque threshold and lasting for a preset time interval, determining that the robot has collided; In response to the joint torque corresponding to each joint being less than the joint torque threshold, determining that the robot has not collided.

7. A collision detection system for a robot, characterized in that, The collision detection system includes: A first acquisition module, configured to acquire the joint torque, operating angular velocity, and operating angular acceleration of each joint in the robot at the current acquisition moment; A calculation module, based on the running angular velocity, the running angular acceleration, and the noise correlation parameter of the running angular acceleration, obtains the joint torque threshold of each joint of the robot at the current acquisition moment; A second acquisition module is configured to obtain a target collision detection result of the robot according to the joint torque corresponding to at least one of the joints and the joint torque threshold.

8. An electronic device, comprising a memory, a processor, and a computer program stored on the memory and configured to run on the processor, characterized in that, When the processor executes the computer program, the collision detection method of the robot according to any one of claims 1-6 is implemented.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, the collision detection method of the robot according to any one of claims 1-6 is implemented.

10. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, the collision detection method of the robot according to any one of claims 1-6 is implemented.

Images