A detection method and device of a covariance-based collision detection module

Abstract

The application relates to the field of motor collision detection, and discloses a detection method and device of a collision detection module based on covariance, which comprises steps S1-S6. The application judges the correlation by calculating the feedback current, the feedback acceleration and the rotational inertia through covariance, and then detects whether a collision occurs. Any additional sensor does not need to be added on a servo drive of a motor pair, the cost is well saved, different loads do not need to be re-modeled, dynamics modeling and parameter identification do not need to be integrated, the application has strong universality and small calculation amount, is suitable for being used by a microprocessor with small calculation amount such as an embedded microprocessor, the input of the rotational inertia is increased, so that even if the rotational inertia changes in the motion process, the judgment of the collision is not affected, and the application range of the application can be extended to a multi-axis linkage robot.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of motor collision detection, in particular to a detection method and device of a collision detection module based on covariance. BACKGROUND

[0002] Nowadays, AC permanent magnet servo motors are widely used in mechanical equipment, robots and other fields. In the process of use, obstacles are inevitable to cause collisions. If the occurrence of collision is not detected in time and protection is not given, it will lead to more serious consequences, such as damage to other equipment, and even damage to the drive and motor.

[0003] At present, many methods have been proposed for robot collision detection. Among them, the more common method is to install various sensors on the mechanical arm to detect the occurrence of collision, such as wrist sensors, visual sensors, and sensory skins. Although the installation of sensors can quickly detect collisions, it will also increase the production cost of the robot and the complexity of the system. Considering these problems, some scholars have proposed using sensorless methods to detect collisions. For example, some people propose to compare the torque output of the joint motor of the mechanical arm with the estimated torque output of the established mathematical model, calculate the error between the two, and then set a threshold value for the detection system. If the error exceeds the predetermined threshold value, the robot determines that it is colliding at this time.

[0004] The invention application with the application publication number CN11212333A is to collect the trajectory current during normal operation and save it as a reference current for subsequent motion. If the actual current exceeds a certain threshold value compared with the reference current, it is judged as a collision. This method has low universality, and the actual current needs to be collected again for each different trajectory.

[0005] The invention application with the application publication number CN109940622A is to build a neural network and apply a back propagation algorithm to update the weights and biases of the neural network to obtain an estimated current value. The error value between the joint motor output current of the mechanical arm and the estimated current output by the neural network is compared with the collision detection threshold value for collision determination. This method has large calculation amount and is not suitable for embedded microprocessors.

[0006] The invention application with the application publication number CN113021353A is to use the principle that the joint motor current I of the robot can be decomposed into the motor current component I1 for realizing dynamic motion, the motor current component I2 for overcoming joint friction, and the motor current component I3 caused by collision. When the motor current component I3 caused by collision exceeds the collision threshold value, it is determined that the robot joint connecting rod has collided. This method has large calculation amount and is not suitable for embedded microprocessors, and needs to be re-modeled for different equipment, which has low universality. SUMMARY

[0007] The application aims to solve the above problems and provides a detection method and device of a covariance-based collision detection module, which solves the problems of the prior art, i.e., the need to increase sensors on a servo motor, high use cost, the need to re-model equipment for different robots, the need to re-identify model parameters for different loads, non-compliance with the requirement of universality of servo motors, and large calculation amount, which is not suitable for embedded microprocessors.

[0008] To achieve the above purpose, the application adopts the following technical solutions:

[0009] A detection method of a covariance-based collision detection module, comprising the following steps:

[0010] S1: collecting motor three-phase current data, and obtaining a quadrature-axis current Iq and a direct-axis current Id through Clark transformation and Park transformation;

[0011] Collecting motor encoder values and performing twice differentiation to obtain feedback acceleration a;

[0012] S2: connecting the collision detection module with a motion controller, and issuing a moment of inertia m to the collision detection module through the motion controller, or defaulting the moment of inertia as 1 if no moment of inertia is issued;

[0013] S3: performing low-pass filtering processing on the quadrature-axis current Iq and the feedback acceleration a to obtain required filtered quadrature-axis current Iq2 and filtered feedback acceleration a2;

[0014] S4: buffering the moment of inertia m(N), the filtered quadrature-axis current Iq2(N) and the filtered feedback acceleration a2(N) of the last N periods, and performing data extraction on the buffered moment of inertia m(N), filtered quadrature-axis current Iq2(N) and filtered feedback acceleration a2(N) of the last N periods at a fixed interval n to obtain the moment of inertia m(N / n), filtered quadrature-axis current Iq2(N / n) and filtered feedback acceleration a2(N / n);

[0015] S5: performing covariance calculation on the extracted moment of inertia m(N / n), filtered quadrature-axis current Iq2(N / n) and filtered feedback acceleration a2(N / n) in S4 to obtain a required covariance value R;

[0016] S6: comparing the obtained covariance value R with a pre-set covariance threshold t, and determining collision if the covariance value R is less than the covariance threshold t.

[0017] Preferably, the step S1 is to transform the static three-phase current Ia, Ib and Ic into static two-phase current Iα and Iβ by Clark transformation, and then the current Iα and Iβ are transformed into the cross-axis current Iq and the direct-axis current Id by Park transformation.

[0018] Preferably, the formula of the Clark transformation is as follows:

[0019] Iα = Ia;

[0020]

[0021] Preferably, the formula of the Park transformation is as follows:

[0022] Iq = Iβ * cos θ - Iα * sin θ;

[0023] Id = Iα * cos θ + Iβ * sin θ;

[0024] Wherein θ is the angle of rotation.

[0025] Preferably, the formula of the feedback acceleration a is as follows:

[0026] a = d2P / dT2;

[0027] Wherein P is the position of the encoder value, and T is the sampling period.

[0028] Preferably, the formula of the filtered cross-axis current Iq2 and the filtered feedback acceleration a2 is as follows:

[0029] Iq2 = x * Iqact + (1 - x) * Iqref;

[0030] a2 = y * aact + (1 - y) * aref;

[0031] Wherein Iqact and aact are the current period values, Iqref and aref are the last period values, and x and y are the filter coefficients.

[0032] Preferably, the formula of the covariance is as follows:

[0033] X = Iq2 (N / n) * m (N / n);

[0034] Y = a2 (N / n);

[0035] Cov (X, Y) = E{[X - E (X)] * [Y - E (Y)]}.

[0036] A detection device of a collision detection module based on covariance, comprising a motion controller, a servo drive, a three-loop control module, a feedback module and the above-mentioned collision detection module.

[0037] Preferably, the output of the motion controller is electrically connected with the servo driver and the collision detection module respectively, the output of the servo driver is electrically connected with the input of the three-loop control module, the output of the three-loop control module is electrically connected with the feedback module, the output of the feedback module is electrically connected with the input of the three-loop control module and the collision detection module respectively, and the output of the collision detection module is electrically connected with the input of the servo driver.

[0038] Preferably, the three-loop control module comprises a position loop, a speed loop and a current loop.

[0039] The feedback module comprises a motor, a position feedback module, a speed feedback module and a current feedback module.

[0040] The input of the position loop, the speed loop and the current loop is electrically connected with the servo driver, the output of the position loop, the speed loop and the current loop is electrically connected with the input of the motor, the output of the motor is electrically connected with the input of the position feedback module, the speed feedback module and the current feedback module respectively, the output of the position feedback module is electrically connected with the input of the position loop and the collision detection module respectively, the output of the speed feedback module is electrically connected with the input of the speed loop, and the output of the current feedback module is electrically connected with the input of the current loop and the collision detection module respectively.

[0041] The contribution of the present application is that the present application calculates the feedback current, the feedback acceleration and the moment of inertia to determine the correlation, and then detects whether the collision occurs, without adding any additional sensors on the servo driver of the motor pair, thereby saving the cost, and without re-modeling for different loads, without integrating the dynamic modeling and parameter identification, having strong universality and small calculation amount, suitable for embedded microprocessor with small calculation amount, increasing the input of the moment of inertia, so that even if the moment of inertia changes in the motion process, the judgment of the collision is not affected, and the application range can be expanded to multi-axis linkage robot. BRIEF DESCRIPTION OF DRAWINGS

[0042] Figure 1 is the flow chart of the collision detection module of the present application;

[0043] Figure 2 is the schematic diagram of the detection device of the present application;

[0044] Figure 3 is the use schematic diagram of the detection device of the present application; DETAILED DESCRIPTION

[0045] The following examples are further explanation and supplement of the present application, and do not constitute any limitation to the present application.

[0046] A detection method of a collision detection module based on covariance, comprising the following steps:

[0047] S1: collecting motor three-phase current data, and obtaining a quadrature axis current Iq and a direct axis current Id through Clark transformation and Park transformation;

[0048] Collecting motor encoder values, and obtaining feedback acceleration a through twice differentiation;

[0049] S2: connecting the collision detection module with a motion controller, and issuing a moment of inertia m to the collision detection module through the motion controller, and defaulting the moment of inertia as 1 if no issuance;

[0050] S3: performing low-pass filtering processing on the quadrature axis current Iq and the feedback acceleration a to obtain required filtered quadrature axis current Iq2 and filtered feedback acceleration a2, and inputting the filtered quadrature axis current Iq2 and the filtered feedback acceleration a2 into the collision detection module;

[0051] S4: buffering the moment of inertia m(N), the filtered quadrature axis current Iq2(N) and the filtered feedback acceleration a2(N) of the last N periods, and performing data extraction on the buffered moment of inertia m(N), the filtered quadrature axis current Iq2(N) and the filtered feedback acceleration a2(N) of the N periods at a fixed interval n to obtain the moment of inertia m(N / n), the filtered quadrature axis current Iq2(N / n) and the filtered feedback acceleration a2(N / n);

[0052] S5: performing covariance calculation on the extracted moment of inertia m(N / n), the filtered quadrature axis current Iq2(N / n) and the filtered feedback acceleration a2(N / n) in S4 to obtain required covariance value R;

[0053] S6: comparing the obtained covariance value R with a pre-set covariance threshold t, and determining collision if the covariance value R is less than the covariance threshold t.

[0054] The present application detects and determines whether collision occurs through the collision detection module of the motor, specifically, as follows: Figure 1As shown, first, data of three-phase current is collected, then stationary three-phase current la, lb and lc is converted into stationary two-phase current Ia and Iβ through Clark transformation, and then stationary two-phase current Ia and Iβ is converted to obtain cross-axis current Iq and direct-axis current Id through Park transformation, at the same time, encoder value of the motor is collected and twice differential is performed to obtain feedback acceleration a, while collecting data, moment of inertia is sent to the collision detection module through the motion controller, and low-pass filtering processing is performed on cross-axis current Iq and feedback acceleration a to obtain filtered cross-axis current Iq2 and filtered feedback acceleration a2, moment of inertia m(N), filtered cross-axis current Iq2(N) and filtered feedback acceleration a2(N) of the last N periods are buffered, and the buffered moment of inertia m(N), filtered cross-axis current Iq2(N) and filtered feedback acceleration a2(N) of the above N periods are extracted at a fixed interval n to obtain moment of inertia m(N / n), filtered cross-axis current Iq2(N / n) and filtered feedback acceleration a2(N / n), wherein n is a value less than or equal to N, covariance calculation is performed on the extracted moment of inertia m(N / n), filtered cross-axis current Iq2(N / n) and filtered feedback acceleration a2(N / n) in S4 to obtain the required covariance value R, and the covariance value R is compared with the covariance threshold t to determine whether a collision occurs, specifically, when the covariance value R is less than the covariance threshold t, it is determined that a collision occurs, and when the covariance value R is greater than the covariance threshold t, it is determined that no collision occurs.

[0055] As shown in Figure 2 and Figure 3 As shown, Figure 2 and Figure 3 are specific use examples of the device, as can be seen from the figure, the motion controller first sends position and control word to the servo drive, at the same time, the motion controller sends a moment of inertia to the collision detection module (if not sent, the default moment of inertia is 1), the servo drive controls the position loop, speed loop and current loop in the three-loop control module after receiving the position and control word from the motion controller, the position loop, speed loop and current loop output corresponding three-phase voltage after calculation, and then control the motor. Through motor position sampling, three-phase current sampling obtains position feedback, speed feedback and current feedback, wherein the speed feedback module directly acts on the speed loop to control the speed, the position feedback module and the current feedback module act on the corresponding position loop and current loop respectively, and at the same time, the feedback signals are transmitted to the collision detection module, the collision detection module performs covariance calculation processing after receiving the position feedback (twice differential to obtain feedback acceleration), current feedback (obtained after filtering processing through Clark and Park transformation) and moment of inertia sent by the motion controller (processing process as shown in Figure 1The covariance value is obtained by calculating the covariance of the feedback current, the feedback acceleration and the moment of inertia, and then the covariance value is compared with a covariance threshold value to determine whether the motor has collided, and the determined result is transmitted to the servo drive for protection and logic processing.

[0056] The present application determines the correlation by calculating the covariance of the feedback current, the feedback acceleration and the moment of inertia, and then detects whether the collision has occurred, without adding any additional sensors to the servo drive of the motor pair, thereby saving the cost, and without re-modeling for different loads, without integrating the dynamic modeling and parameter identification, having strong universality and small calculation amount, suitable for embedded microprocessors with small calculation amount, increasing the input of the moment of inertia, so that even if the moment of inertia changes during the movement, it does not affect the judgment of the collision, and the application range can be extended to multi-axis linkage robots.

[0057] Further, in the step S1, the three-phase current Ia, Ib and Ic is converted into two-phase current Iα and Iβ by Clark transformation, and the current Iα and Iβ is converted into the quadrature axis current Iq and the direct axis current Id by Park transformation.

[0058] Further, the formula of the Clark transformation is as follows:

[0059] Iα=Ia;

[0060]

[0061] Further, the formula of the Park transformation is as follows:

[0062] Iq=Iβ*cosθ-Iα*sinθ;

[0063] Id=Iα*cosθ+Iβ*sinθ;

[0064] Wherein, θ is the angle of rotation.

[0065] θ is the angle of rotation, also known as the electrical angle, and the electrical angle is equal to the mechanical angle multiplied by the number of pole pairs, and the space electrical angle is the electrical angle.

[0066] Further, the formula of the feedback acceleration a is as follows:

[0067] a=d^2P / dT^2;

[0068] Wherein, P is the position of the encoder value, and T is the sampling period.

[0069] Further, the formula of the filtered quadrature axis current Iq2 and the filtered feedback acceleration a2 is as follows:

[0070] Iq2=x*Iqact+(1-x)*Iqref;

[0071] a2=y*aact+(1-y)*aref;

[0072] Wherein Iqact and aact are current cycle values, Iqref and aref are last cycle values, and x and y are filter coefficients.

[0073] Further, the formula of the covariance is as follows:

[0074] X=Iq2(N / n)*m(N / n);

[0075] Y=a2(N / n);

[0076] Cov(X,Y)=E{[X-E(X)]*[Y-E(Y)]}.

[0077] A detection device of a collision detection module based on covariance, comprising a motion controller, a servo drive, a three-loop control module, a feedback module and the collision detection module.

[0078] Further, the output end of the motion controller is electrically connected with the servo drive and the collision detection module respectively, the output end of the servo drive is electrically connected with the input end of the three-loop control module, the output end of the three-loop control module is electrically connected with the feedback module, the output end of the feedback module is electrically connected with the input end of the three-loop control module and the collision detection module respectively, and the output end of the collision detection module is electrically connected with the input end of the servo drive.

[0079] Further, the three-loop control module comprises a position loop, a speed loop and a current loop.

[0080] The feedback module comprises a motor, a position feedback module, a speed feedback module and a current feedback module.

[0081] The input end of the position loop, the speed loop and the current loop is electrically connected with the servo drive, the output end of the position loop, the speed loop and the current loop is electrically connected with the input end of the motor, the output end of the motor is electrically connected with the input end of the position feedback module, the speed feedback module and the current feedback module respectively, the output end of the position feedback module is electrically connected with the input end of the position loop and the collision detection module respectively, the output end of the speed feedback module is electrically connected with the input end of the speed loop, and the output end of the current feedback module is electrically connected with the input end of the current loop and the collision detection module respectively.

[0082] Although the present application has been disclosed by the above embodiments, the scope of the present application is not limited to the above, and modifications, replacements, etc. of the above components will fall within the scope of the claims of the present application without departing from the concept of the present application.

Claims

1. A collision detection method based on a covariance-based collision detection module, characterized in that, Includes the following steps: S1: Collect the three-phase current data of the motor, and obtain the quadrature-axis current Iq and direct-axis current Id through Clark transformation and Park transformation; Collect the motor encoder value and perform a second derivative to obtain the feedback acceleration a; S2: Connect the collision detection module to the motion controller, and send the moment of inertia m to the collision detection module through the motion controller. If no m is sent, the moment of inertia is defaulted to 1. S3: The filter processor performs low-pass filtering on the quadrature-axis current Iq and the feedback acceleration a to obtain the required filtered quadrature-axis current Iq2 and filtered feedback acceleration a2, and inputs the filtered quadrature-axis current Iq2 and filtered feedback acceleration a2 into the collision detection module; S4: The collision detection module automatically caches the rotational inertia m(N), filtered cross-axis current Iq2(N), and filtered feedback acceleration a2(N) of the most recent N cycles, and extracts the rotational inertia m(N), filtered cross-axis current Iq2(N), and filtered feedback acceleration a2(N) of the cached N cycles at fixed intervals n to obtain the rotational inertia m(N / n), filtered cross-axis current Iq2(N / n), and filtered feedback acceleration a2(N / n); S5: Calculate the covariance of the moment of inertia m (N / n), filtered quadrature-axis current Iq2 (N / n), and filtered feedback acceleration a2 (N / n) extracted in S4 to obtain the required covariance value R; the formula for the covariance is as follows: X = Iq²(N / n) * m(N / n); Y = a²(N / n); Cov(X,Y)=E{[XE(X)]*[YE(Y)]}; S6: Compare the obtained covariance value R with the preset covariance threshold t. If the covariance value R is less than the covariance threshold t, then it is determined to be a collision.

2. The detection method of the collision detection module based on covariance according to claim 1, characterized in that: In step S1, the stationary three-phase currents Ia, Ib and Ic are transformed into stationary two-phase currents Iα and Iβ through Clark transformation. The currents Iα and Iβ are then transformed by Park transformation to obtain quadrature-axis current Iq and direct-axis current Id.

3. The detection method of the collision detection module based on covariance according to claim 2, characterized in that, The formula for the Clark transform is as follows: Iα = Ia; 4. The detection method of the collision detection module based on covariance according to claim 1, characterized in that, The formula for the Park transform is as follows: Iq = Iβ*cosθ - Iα*sinθ; Id = Iα*cosθ + Iβ*sinθ; Where θ is the angle of rotation.

5. The detection method of the collision detection module based on covariance according to claim 1, characterized in that, The formula for calculating the feedback acceleration 'a' is as follows: a = d^2P / dT^2; Where P is the position of the encoder value and T is the sampling period.

6. The detection method of the collision detection module based on covariance according to claim 1, characterized in that, The calculation formulas for the filtered quadrature-axis current Iq2 and the filtered feedback acceleration a2 are as follows: Iq2 = x * Iqact + (1 - x) * Iqref; a2 = y * aact + (1 - y) * aref; Where Iqact and aact are the values ​​of the current cycle, Iqref and aref are the values ​​of the previous cycle, and x and y are the filter coefficients.

7. A detection device for a collision detection module based on covariance, characterized in that: The detection method includes a motion controller, a servo drive, a three-loop control module, a feedback module, and a collision detection module based on covariance as described in any one of claims 1-6.

8. The detection device for a collision detection module based on covariance according to claim 7, characterized in that: The output terminal of the motion controller is electrically connected to the servo drive and the collision detection module, respectively. The output terminal of the servo drive is electrically connected to the input terminal of the three-loop control module. The output terminal of the three-loop control module is electrically connected to the feedback module. The output terminal of the feedback module is electrically connected to the input terminals of the three-loop control module and the collision detection module, respectively. The output terminal of the collision detection module is electrically connected to the input terminal of the servo drive.

9. The detection device for a collision detection module based on covariance according to claim 8, characterized in that: The three-loop control module includes a position loop, a speed loop, and a current loop; The feedback module includes a motor, a position feedback module, a speed feedback module, and a current feedback module; The input terminals of the position loop, velocity loop, and current loop are all electrically connected to the servo drive. The output terminals of the position loop, velocity loop, and current loop are all electrically connected to the input terminal of the motor. The output terminal of the motor is electrically connected to the input terminals of the position feedback module, velocity feedback module, and current feedback module, respectively. The output terminal of the position feedback module is electrically connected to the input terminals of the position loop and the collision detection module, respectively. The output terminal of the velocity feedback module is electrically connected to the input terminal of the velocity loop. The output terminal of the current feedback module is electrically connected to the input terminals of the current loop and the collision detection module, respectively.

Images