Robot tail end vibration suppression method and device, medium, controller and product

Abstract

The invention provides a robot tail end vibration suppression method and device, a medium, a controller and a product, and the method comprises the steps: respectively collecting original motion signals in a first direction and a second direction through an angular velocity sensor after a robot starts to run; respectively processing the collected original motion signals in the first direction and the second direction to obtain a first vibration frequency in the first direction and a second vibration frequency in the second direction; respectively setting corresponding notch filters according to the obtained first vibration frequency in the first direction and the second vibration frequency in the second direction; a notch filter in the first direction is added to a control instruction of a second motor of the robot, and a notch filter in the second direction is added to a control instruction of a third motor. According to the scheme, mechanical vibration generated by the flexible rigid body when the tail end of the robot moves in multiple directions under the control of the multiple motors can be restrained.

Description

Technical Field

[0001] The present invention relates to the field of control, and in particular to a method, device, storage medium, controller and computer program product for suppressing vibration of a robot terminal. Background Art

[0002] With the development of intelligence and robotics, industrial robots have been widely used in industrial production, assembly and other fields. High speed and high precision have always been the most important technical indicators of industrial robots. However, because there is a flexible deceleration device in the robot servo system, it provides a large driving torque at low speed, but it will cause elastic torsion between the motor and the load, resulting in low-frequency residual jitter at the end of the positioning; at the same time, there are different degrees of flexibility between the robot transmission mechanism and the arm. When the excitation source frequency reaches the robot's resonant frequency, it will cause transient vibrations at the end of the robot's movement. These vibrations will seriously affect the robot's trajectory accuracy and positioning, and even the robot's service life. Summary of the invention

[0003] The main purpose of the present invention is to overcome the defects of the above-mentioned related technologies and to provide a robot terminal vibration suppression method, device, medium, controller and product to solve the problem in the related technology that the flexible deceleration device causes elastic torsion between the motor and the load, resulting in low-frequency residual jitter of the positioning terminal or different degrees of flexibility between the robot transmission mechanism and the arm, causing transient process vibration of the terminal during the robot movement.

[0004] On one hand, the present invention provides a method for suppressing vibration of a robot end, comprising: after the robot starts to run, respectively collecting the original motion signal of the robot in a first direction and the original motion signal in a second direction through an angular velocity sensor; respectively processing the collected original motion signal of the robot in the first direction and the original motion signal in the second direction to obtain a first vibration frequency of the robot end load in the first direction and a second vibration frequency in the second direction; respectively setting corresponding notch filters in the first direction and second directions according to the obtained first vibration frequency of the robot end load in the first direction and the second vibration frequency in the second direction; adding the notch filter in the first direction to the control instruction of the robot's second motor, and adding the notch filter in the second direction to the control instruction of the robot's third motor; the first direction is the rotation direction of the second arm of the robot with the vertical straight line at the connection between the first and second axis joints as the axis center; the second direction is the vibration direction of the end of the second arm of the robot.

[0005] Optionally, the angular velocity sensor is installed at a position at the end of the second arm of the robot close to the third arm.

[0006] Optionally, the collected original motion signals of the robot in the first direction and the collected original motion signals in the second direction are processed respectively to obtain a first vibration frequency of the end load of the robot in the first direction and a second vibration frequency in the second direction, including: filtering the collected original motion signals of the robot in the first direction and the collected original motion signals in the second direction to obtain a vibration signal of the robot in the first direction and a vibration signal in the second direction; performing fast Fourier transform on the filtered vibration signals of the robot in the first direction and the vibration signals in the second direction to obtain a first vibration frequency of the end load of the robot in the first direction and a second vibration frequency in the second direction.

[0007] Optionally, the collected original motion signal in the first direction and the collected original motion signal in the second direction are filtered to obtain the vibration signal in the first direction and the vibration signal in the second direction, including: using a high-pass filter to remove the DC component near 0 Hz in the original motion signal; and / or, using a low-pass filter to remove the high-frequency noise signal in the original motion signal.

[0008] Optionally, it also includes: obtaining corresponding compensation coefficients for compensating phase lag according to the first vibration frequency of the end load of the robot in the first direction and the second vibration frequency in the second direction; adding the corresponding compensation coefficients for compensating phase lag to the control instructions of the second motor of the robot for differential compensation, and adding the corresponding compensation coefficients for compensating phase lag to the control instructions of the third motor of the robot for differential compensation.

[0009] On the other hand, the present invention provides a terminal vibration suppression device for a robot, comprising: a collection unit, which is used to collect the original motion signal of the robot in the first direction and the original motion signal in the second direction through an angular velocity sensor after the robot starts to run; a processing unit, which is used to process the original motion signal of the robot in the first direction and the original motion signal in the second direction collected by the collection unit, respectively, to obtain the first vibration frequency of the terminal load of the robot in the first direction and the second vibration frequency in the second direction; a setting unit, which is used to set the corresponding notch filter in the first direction and the second vibration frequency in the second direction according to the first vibration frequency of the terminal load of the robot in the first direction and the second vibration frequency in the second direction obtained by the processing unit; a suppression unit, which is used to add the notch filter in the first direction to the control instruction of the second motor of the robot, and add the notch filter in the second direction to the control instruction of the third motor of the robot, so as to suppress the terminal vibration of the robot; the first direction is the rotation direction of the second arm of the robot with the vertical straight line at the connection between the first and second axis joints as the axis center; the second direction is the vibration direction of the terminal of the second arm of the robot.

[0010] Optionally, the angular velocity sensor is installed at a position at the end of the second arm of the robot close to the third arm.

[0011] Optionally, the processing unit processes the collected original motion signals of the robot in the first direction and the second direction, respectively, to obtain a first vibration frequency of the end load of the robot in the first direction and a second vibration frequency in the second direction, including: filtering the collected original motion signals of the robot in the first direction and the second direction to obtain a vibration signal of the robot in the first direction and a vibration signal in the second direction; performing fast Fourier transform on the filtered vibration signals of the robot in the first direction and the second direction to obtain a first vibration frequency of the end load of the robot in the first direction and a second vibration frequency in the second direction.

[0012] Optionally, the processing unit performs filtering processing on the collected original motion signal in the first direction and the collected original motion signal in the second direction to obtain the vibration signal in the first direction and the vibration signal in the second direction, including: using a high-pass filter to remove the DC component near 0 Hz in the original motion signal; and / or using a low-pass filter to remove the high-frequency noise signal in the original motion signal.

[0013] Optionally, the processing unit is further used to obtain corresponding compensation coefficients for compensating phase lag according to the first vibration frequency of the end load of the robot in the first direction and the second vibration frequency in the second direction; the suppression unit is further used to add the corresponding compensation coefficients for compensating phase lag to the control instructions of the second motor of the robot for differential compensation, and add the corresponding compensation coefficients for compensating phase lag to the control instructions of the third motor of the robot for differential compensation.

[0014] Another aspect of the present invention provides a storage medium having a computer program stored thereon, wherein the program implements the steps of any of the aforementioned methods when executed by a processor.

[0015] In another aspect, the present invention provides a controller, comprising a processor, a memory, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of any of the aforementioned methods when executing the program.

[0016] In another aspect, the present invention provides a controller, comprising any of the aforementioned terminal vibration control devices.

[0017] In yet another aspect, the present invention provides a computer program product, comprising a computer program, wherein when the computer program is executed by a processor, the steps of any of the aforementioned methods are implemented.

[0018] According to the technical solution of the present invention, it is possible to suppress mechanical vibrations in corresponding directions caused by the flexible rigid body when the robot end moves in multiple directions under the control of multiple motors during the operation of the multi-joint industrial robot.

[0019] According to the technical solution of the present invention, only one sensor is installed at the end of the robot arm to suppress vibration in multiple directions, which reduces the cost and difficulty. The effect of vibration suppression is improved by using a fast Fourier transform (FFT) algorithm module and a notch module with added phase compensation.

[0020] According to the technical solution of the present invention, by installing a sensor for detecting movement in a specified direction at the end connection of the robot arm, not only can vibration be accurately detected, but also vibration of multiple adjacent motors can be detected and suppressed, which greatly reduces design costs and wiring difficulty. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] The drawings described herein are used to provide a further understanding of the present invention and constitute a part of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:

[0022] Figure 11 is a schematic diagram of an embodiment of a robot end restraint method provided by the present invention;

[0023] Figure 2 is a schematic diagram of a multi-joint industrial robot according to the present invention;

[0024] Figure 3 A schematic diagram of a process for processing respectively the collected original motion signal of the robot in the first direction and the collected original motion signal in the second direction to obtain a first vibration frequency of the end load of the robot in the first direction and a second vibration frequency in the second direction according to a specific embodiment of the present invention is shown;

[0025] Figure 4 A flow chart showing how the compensated notch filter of the present invention removes the frequency component in the control instruction that is the same as the vibration frequency;

[0026] Figure 5 It is a structural block diagram of an embodiment of the terminal vibration suppression device of the robot provided by the present invention. DETAILED DESCRIPTION

[0027] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the specific embodiments of the present invention and the corresponding drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0028] It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0029] Vibration suppression methods in related technologies include optimizing connection devices, active suppression and passive suppression. Optimizing connection devices has great limitations and is not easy to implement. Active suppression suppresses vibrations through controller parameters, such as feedback control through observation or sensors, but its vibration observation is difficult, and the use of sensors increases costs and design difficulties. Passive vibration suppression is based on the vibration frequency to eliminate the relevant frequencies in the control signal, but the vibration frequency is not easy to identify, and it is easy to fail to suppress vibrations well due to delays. Most of the vibration suppression methods in related technologies are one of them, and they are all vibration suppression in a single joint motion direction. The present invention proposes a multi-joint industrial robot terminal vibration suppression method that combines active suppression and passive suppression. The problem of industrial robot vibration being difficult to suppress is solved efficiently and at low cost.

[0030] The invention provides a method for suppressing vibration of a robot terminal. The robot is a multi-joint robot. Figure 2 Schematic diagram of the structure of a multi-joint industrial robot according to the present invention.

[0031] like Figure 2 As shown, the robot includes a base A, a first arm B1, a second arm B2, a third arm B3, a joint connection G1, a joint connection G2, a joint connection G3, an angular velocity sensor S, and a robot end load P. The robot is placed on the ground through the base A, the first arm B1 is connected to the base A, the second arm B2 is connected to the first arm B1, and the third arm B3 is connected to the second arm B2. A first motor is provided in the first arm of the robot, and the first motor controls the first arm; a second motor is provided in the second arm of the robot, and the second motor controls the second arm; a third motor is provided in the third arm of the robot, and the third motor controls the third arm. Specifically, the first arm B1 is connected to the base A through the joint G1, and rotates around the C1 axis under the control of the first motor; the second arm B2 is connected to the first arm B1 through the joint G2, and rotates around the C2 axis under the control of the second motor; the third arm B3 is connected to the second arm through the joint G3, and moves up and down in the D direction along the C3 axis under the control of the third motor; P is the robot end load, which is installed at the end of the third arm B3 through a clamp. Vibration suppression mainly refers to the load here having no abnormal vibration during movement. D and w1 are the two movement directions of the robot end load, that is, the vibration direction; the first direction w1 is the second arm B2 in a straight line in the vertical direction of the joint G2 between the first and second arms (that is, Figure 2The middle dotted line C2) is the rotation direction of the axis center; the second direction w2 is the rotation direction of the end of the second arm B2 with the straight line perpendicular to the second arm (i.e., perpendicular to the paper) in the horizontal direction of the first and second arm joint connection G2 as the axis center; the second direction w2 is the vibration direction of the end of the second arm B2 of the robot, that is, the vibration direction of the second arm B2 caused by the end moving in the vertical direction (D direction) and the elastic connection device; S is a two-axis angular velocity sensor, which is installed at the end of the second arm of the robot near the third arm, and can detect the angular velocity of the first direction w1 and the second direction w2. The angular velocity sensor can be specifically a multi-axis acceleration, angular velocity, and vibration sensor.

[0032] Figure 1 It is a method schematic diagram of an embodiment of the robot end suppression method provided by the present invention.

[0033] like Figure 1 As shown, according to one embodiment of the present invention, the robot end suppression method includes at least step S110, step S120, step S130 and step S140.

[0034] Step S110: After the robot starts to run, an original running signal of the robot in a first direction and an original motion signal in a second direction are collected by an angular velocity sensor.

[0035] The first direction w1 is the rotation direction of the second arm B2 of the robot with the vertical straight line at the connection between the first and second arm joints as the axis center; the second direction w2 is the vibration direction of the end of the second arm of the robot, specifically, the rotation direction generated by the end of the second arm B2 of the robot with the straight line in the horizontal direction of the connection between the first and second arm joints G2 perpendicular to the second arm (i.e., perpendicular to the paper plane) as the axis center, that is, the second direction w2 is the vibration direction of the second arm caused by the movement of the end of the second arm B2 along the D direction and the elastic connection device.

[0036] Specifically, after the robot receives the position instruction from the host computer and starts running, the angular velocity sensor collects the original motion signal of the robot in the first direction w1 and the original motion signal of the robot in the second direction w2. The angular velocity sensor can be installed at the end of the second arm of the robot near the third arm.

[0037] Step S120, respectively processing the collected original motion signal of the robot in the first direction and the original motion signal in the second direction to obtain a first vibration frequency ω of the end load of the robot in the first direction 01 and a second vibration frequency ω in the second direction 02 .

[0038] Figure 3 The figure shows that the collected original motion signal of the robot in the first direction and the original motion signal in the second direction are processed respectively according to a specific embodiment of the present invention to obtain the first vibration frequency ω of the end load of the robot in the first direction. 01 and a second vibration frequency ω in the second direction 02 Schematic diagram of the process.

[0039] like Figure 3 As shown, in a specific implementation, step S120 includes step S121 and step S122.

[0040] Step S121, filtering the collected original motion signal of the robot in the first direction and the collected original motion signal in the second direction to obtain a vibration signal of the robot in the first direction and a vibration signal in the second direction.

[0041] In a specific embodiment, the DC component and high-frequency noise signal near 0 Hz in the original motion signal of the robot in the first direction and the original motion signal in the second direction are removed to obtain vibration signals in two directions. In a specific embodiment, a high-pass filter is used to remove the DC component near 0 Hz in the original motion signal. The DC component near 0 Hz in the original signal has a low frequency, and the DC component near 0 Hz in the original motion signal can be removed using a high-pass filter. In a specific embodiment, a low-pass filter is used to remove the high-frequency noise signal in the original motion signal.

[0042] Step S122, performing fast Fourier transform on the vibration signal of the robot in the first direction and the vibration signal in the second direction after filtering, to obtain a first vibration frequency ω of the end load of the robot in the first direction 01 and a second vibration frequency ω in the second direction 02 .

[0043] Specifically, the two vibration signals obtained after processing are subjected to fast Fourier transform (FFT) to obtain the vibration frequency ω of the robot end load in the w1 and w2 directions. 01 With ω 02 .

[0044] Step S130, based on the obtained first vibration frequency ω of the end load of the robot in the first direction 01 and a second vibration frequency ω in the second direction 02 , respectively set the corresponding notch filter in the first direction and the notch filter in the second direction.

[0045] The second motor in the second arm B1 of the robot controls the movement of the robot end in the first direction w1, and the third motor in the third arm B3 of the robot controls the vibration of the robot end in the direction D, that is, the second direction w2. According to the vibration frequency ω in the directions w1 and w2 01 With ω 02 Design the corresponding notch filters respectively and compensate for the phase lag.

[0046] Below is ω 01 Take the design as an example:

[0047] According to the Bode diagram of the system, the vibration frequency ω is obtained 01 The two frequency points ω where the amplitude attenuation is 3db H01 With ω L01 , then the transfer function of the notch filter in the first direction is:

[0048]

[0049] Similarly, the vibration frequency ω 02 The two frequency points ω where the amplitude attenuation is 3db H02 With ω L02 , then the transfer function of the notch filter in the second direction is:

[0050]

[0051] In the formula, s is a complex variable; k 1 , k 2 is the filter width and depth; ω 01 is the vibration frequency in the w1 direction measured by the robot end sensor; ω 02 is the vibration frequency in the w2 direction measured by the robot end sensor; where the notch width k 1 =8k 2 ;

[0052] Filter Depth

[0053] Then the filter formula in the first direction can be expanded as:

[0054]

[0055] Similarly, the filter formula in the second direction is expanded as follows:

[0056]

[0057] Preferably, because the differential link has a phase advance effect, a differential link is added to compensate for the phase lag, that is, a differential link is added to the filter design as compensation. Specifically, according to the first vibration frequency ω of the end load of the robot in the first direction 01 and a second vibration frequency ω in the second direction 02 , and the corresponding compensation coefficients for compensating the phase lag are obtained respectively. Among them, the differential link works by setting the compensation coefficient K, so let the speed given ω ref , position given θ ref , the position after adding the differential link filter is given as θ * ref , then the position before and after filtering is given by:

[0058]

[0059] Where s is a complex variable; θ ref is the position given; θ * ref To add the position given after the filtering link; ω ref is the speed setting; K is the compensation coefficient of the filter differential link.

[0060] When the robot is in a steady state, the position error before and after the filter should be 0, and s = 0, so equation (3) and equation (4) should be equal. Combining equation (2) yields:

[0061]

[0062] It can be further obtained that the compensation coefficient K of the phase lag of the notch filter in the first direction is:

[0063]

[0064] Similarly, the compensation coefficient K of the notch filter compensating the phase lag in the second direction can be obtained as:

[0065]

[0066] Step S140, adding a notch filter in the first direction to the control instruction of the second motor of the robot, and adding a notch filter in the second direction to the control instruction of the third motor of the robot, so as to suppress the terminal vibration of the robot.

[0067] Specifically, the designed forward channel notch filter is used to eliminate the robot vibration. The notch filter is placed after the position command to remove the frequency component with the same frequency as the vibration frequency in the position command, and a differential compensation link is added to solve the problems of phase lag and response time. The process of removing the frequency component with the same frequency as the vibration frequency in the control command by the filter can be referred to Figure 4 As shown in the figure. By removing the frequency component with the same frequency as the vibration in the control command, the end vibration of the multi-joint robot will be significantly suppressed.

[0068] Preferably, a corresponding compensation coefficient for compensating phase lag may be added to the control instruction of the second motor of the robot for differential compensation, and a corresponding compensation coefficient for compensating phase lag may be added to the control instruction of the third motor of the robot for differential compensation. That is, the set compensation coefficient K of the differential compensation link obtained above is added for differential compensation, which can be referred to Figure 4 As shown in the figure, the vibration in motion is eliminated more quickly due to the addition of phase compensation.

[0069] The present invention also provides a robot terminal vibration suppression device. The robot is a multi-joint robot. Figure 2 Schematic diagram of a multi-joint industrial robot according to the present invention.

[0070] like Figure 2 As shown, the robot includes a base A, a first arm B1, a second arm B2, a third arm B3, a joint connection G1, a joint connection G2, a joint connection G3, an angular velocity sensor S and a robot end load P. The robot is placed on the ground through the base A, the first arm B1 is connected to the base A, the second arm B2 is connected to the first arm B1, and the third arm B3 is connected to the second arm B2. A first motor is arranged in the first arm of the robot, and the first motor controls the first arm; a second motor is arranged in the second arm of the robot, and the second motor controls the second arm; a third motor is arranged in the third arm of the robot, and the third motor controls the third arm. Specifically, the first arm B1 is connected to the base A through the joint G1, and rotates around the C1 axis under the control of the first motor; the second arm B2 is connected to the first arm B1 through the joint G2, and rotates around the C2 axis under the control of the second motor; the third arm B3 is connected to the second arm through the joint G3, and moves up and down in the D direction along the C3 axis under the control of the third motor; P is the robot end load, and vibration suppression mainly refers to the load here having no abnormal vibration during movement. D and w1 are two movement directions of the robot end load, that is, the vibration direction; the first direction w1 is the straight line from the second arm B2 to the vertical direction of the joint G2 between the first and second arms (that is, Figure 2The middle dotted line C2) is the rotation direction of the axis center; the second direction w2 is the rotation direction of the end of the second arm B2 with the straight line perpendicular to the second arm (i.e., perpendicular to the paper plane) in the horizontal direction of the first and second arm joint connection G2 as the axis center; the second direction w2 is the vibration direction of the end of the second arm B2 of the robot, that is, the vibration direction of the second arm B2 caused by the end moving in the vertical direction (D direction) and the elastic connection device; S is a two-axis angular velocity sensor, which is installed at the end of the second arm of the robot near the third arm, and can detect the angular velocity of the first direction w1 and the second direction w2. The angular velocity sensor can be specifically a multi-axis acceleration, angular velocity, and vibration sensor.

[0071] Figure 5 FIG. 1 is a structural block diagram of an embodiment of a terminal vibration suppression device of a robot provided by the present invention. Figure 5 As shown, the terminal vibration suppression device 100 includes: a collection unit 110 , a processing unit 120 , a setting unit 130 and a suppression unit 140 .

[0072] The collecting unit 110 is used to collect the original motion signal of the robot in the first direction and the original motion signal of the robot in the second direction respectively through an angular velocity sensor after the robot starts to run.

[0073] The first direction w1 is the rotation direction of the second arm B2 of the robot with the vertical straight line at the connection between the first and second arm joints as the axis center; the second direction w2 is the vibration direction of the end of the second arm of the robot, specifically, the rotation direction generated by the end of the second arm B2 of the robot with the straight line upward from the horizontal direction of the connection between the first and second joints G2 and perpendicular to the second arm (i.e., perpendicular to the paper plane) as the axis center; that is, the second direction w2 is the vibration direction of the second arm caused by the movement of the end of the second arm B2 along the D direction and the elastic connection device.

[0074] Specifically, after the robot receives the position instruction from the host computer and starts running, the acquisition unit 110 respectively acquires the original motion signal of the robot in the first direction w1 and the original motion signal of the robot in the second direction w2 through the angular velocity sensor. The angular velocity sensor can be installed at the end of the second arm of the robot near the third arm.

[0075] The processing unit 120 is used to process the original motion signal of the robot in the first direction and the original motion signal in the second direction collected by the collection unit, respectively, to obtain a first vibration frequency of the end load of the robot in the first direction and a second vibration frequency in the second direction.

[0076] In a specific embodiment, the processing unit 120 includes: filtering the collected original motion signal of the robot in the first direction and the original motion signal in the second direction to obtain the vibration signal of the robot in the first direction and the vibration signal in the second direction; performing fast Fourier transform on the filtered vibration signal of the robot in the first direction and the vibration signal in the second direction to obtain the first vibration frequency of the end load of the robot in the first direction and the second vibration frequency in the second direction.

[0077] In a specific embodiment, the DC component and high-frequency noise signal near 0 Hz in the original motion signal of the robot in the first direction and the original motion signal in the second direction are removed to obtain vibration signals in two directions. In a specific embodiment, a high-pass filter is used to remove the DC component near 0 Hz in the original motion signal. The DC component near 0 Hz in the original signal has a low frequency, and the DC component near 0 Hz in the original motion signal can be removed using a high-pass filter. In a specific embodiment, a low-pass filter is used to remove the high-frequency noise signal in the original motion signal.

[0078] By performing fast Fourier transform (FFT) on the two vibration signals obtained after processing, the vibration frequency ω of the robot end load in the w1 and w2 directions can be obtained. 01 With ω 02 .

[0079] A setting unit 130 is configured to obtain a first vibration frequency ω of the end load of the robot in the first direction obtained by the processing unit. 01 and a second vibration frequency ω in the second direction 02 , respectively set the corresponding notch filter in the first direction and the notch filter in the second direction.

[0080] The second motor in the second arm B1 of the robot controls the movement of the robot end in the first direction w1, and the third motor in the third arm B3 of the robot controls the vibration of the robot end in the direction D, that is, the second direction w2. According to the vibration frequency ω in the directions w1 and w2 01 With ω 02 Design the corresponding notch filters respectively and compensate for the phase lag.

[0081] Below is ω 01 Take the design as an example:

[0082] According to the Bode diagram of the system, the vibration frequency ω is obtained 01 The two frequency points ω where the amplitude attenuation is 3db H01 With ωL01 , then the transfer function of the notch filter in the first direction is:

[0083]

[0084] Similarly, the vibration frequency ω 02 The two frequency points ω where the amplitude attenuation is 3db H02 With ω L02 , then the transfer function of the notch filter in the second direction is:

[0085]

[0086] In the formula, s is a complex variable; k 1 , k 2 is the filter width and depth; ω 01 is the vibration frequency in the w1 direction measured by the robot end sensor; ω 02 is the vibration frequency in the w2 direction measured by the robot end sensor; where the notch width k 1 =8k 2 ;

[0087] Filter Depth

[0088] Then the filter formula in the first direction can be expanded as:

[0089]

[0090] Similarly, the filter formula in the second direction is expanded as follows:

[0091]

[0092] Preferably, because the differential link has a phase advance effect, a differential link is added to compensate for the phase lag, that is, a differential link is added to the filter design as compensation. Specifically, according to the first vibration frequency ω of the end load of the robot in the first direction 01 and a second vibration frequency ω in the second direction 02 , and the corresponding compensation coefficients for compensating the phase lag are obtained respectively. Among them, the differential link works by setting the compensation coefficient K, so let the speed given ω ref , position given θ ref , the position after adding the differential link filter is given as θ * ref , then the position before and after filtering is given by:

[0093]

[0094] Where s is a complex variable; θref is the position given; θ * ref To add the position given after the filtering link; ω ref is the speed setting; K is the compensation coefficient of the filter differential link.

[0095] When the robot is in a steady state, the position error before and after the filter should be 0, and s = 0, so equation (3) and equation (4) should be equal. Combining equation (2) yields:

[0096]

[0097] It can be further obtained that the compensation coefficient K of the phase lag of the notch filter in the first direction is:

[0098]

[0099] Similarly, the compensation coefficient K of the notch filter compensating the phase lag in the second direction can be obtained as:

[0100]

[0101] The suppression unit 140 is used to add a notch filter in the first direction to the control instruction of the second motor of the robot, and add a notch filter in the second direction to the control instruction of the third motor of the robot, so as to suppress the terminal vibration of the robot.

[0102] Specifically, the suppression unit 140 uses the designed forward channel notch filter to eliminate the robot vibration. The notch filter is placed after the position command to remove the frequency component with the same frequency as the vibration frequency in the position command, and a differential compensation link is added to solve the problem of phase lag and response time. The process of removing the frequency component with the same frequency as the vibration frequency in the control command by the filter can be referred to Figure 4 As shown in the figure. By removing the frequency component with the same frequency as the vibration in the control command, the end vibration of the multi-joint robot will be significantly suppressed.

[0103] Preferably, the suppression unit 140 can also add a corresponding compensation coefficient for compensating phase lag in the control instruction of the second motor of the robot to perform differential compensation, and add a corresponding compensation coefficient for compensating phase lag in the control instruction of the third motor of the robot to perform differential compensation. That is, adding the set compensation coefficient K of the differential compensation link obtained above to perform differential compensation, can refer to Figure 4 As shown in the figure, the vibration in motion is eliminated more quickly due to the addition of phase compensation.

[0104] The present invention also provides a storage medium corresponding to the robot terminal vibration suppression method, on which a computer program is stored, and when the computer program is executed by a processor, the steps of any of the aforementioned methods are implemented.

[0105] The present invention also provides a controller corresponding to the robot terminal vibration suppression method, including a processor, a memory, and a computer program stored in the memory and executable on the processor, and the processor implements the steps of any of the aforementioned methods when executing the computer program.

[0106] The present invention also provides a controller corresponding to the robot terminal vibration suppression device, including any of the aforementioned robot terminal vibration suppression devices.

[0107] The present invention also provides a computer program product corresponding to the robot terminal vibration suppression method, including a computer program, and when the computer program is executed by a processor, the steps of any of the aforementioned methods are implemented.

[0108] Accordingly, the solution provided by the present invention, by installing a sensor for detecting movement in a specified direction at the end connection of the end position of the robot's mechanical arm, can not only accurately detect vibrations, but also detect and suppress vibrations of multiple adjacent motors, thereby greatly reducing design costs and wiring difficulty.

[0109] According to the technical solution of the present invention, the vibration signal is processed by online fast Fourier transform (FFT) and a compensated notch filter is added to process the current control signal, thereby reducing the system delay, solving the response speed and phase lag problems in the traditional notch filter, and improving the vibration suppression effect of the system.

[0110] According to the technical solution of the present invention, when a multi-joint robot terminal controlled by multiple motors moves in multiple axes, only one angular velocity sensor (or vibration sensor, acceleration sensor) needs to be installed near the joint to suppress the vibration of the robot terminal in two directions of movement.

[0111] According to the technical solution of the present invention, through the online fast Fourier transform (FFT) algorithm and the notch filter after phase compensation, not only the jitter of the end of the multi-joint robot is suppressed, but also the phase lag is avoided and the response speed is accelerated.

[0112] The functions described herein may be implemented in hardware, software executed by a processor, firmware, or any combination thereof. If implemented in software executed by a processor, the functions may be stored as one or more instructions or codes on a computer-readable medium or transmitted via a computer-readable medium. Other examples and implementations are within the scope and spirit of the present invention and the appended claims. For example, due to the nature of software, the functions described above may be implemented using software executed by a processor, hardware, firmware, hard wiring, or a combination of any of these. In addition, each functional unit may be integrated into a processing unit, each unit may exist physically separately, or two or more units may be integrated into one unit.

[0113] In the several embodiments provided in this application, it should be understood that the disclosed technical content can be implemented in other ways. Among them, the device embodiments described above are only schematic. For example, the division of the units can be a logical function division. There may be other division methods in actual implementation. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of units or modules, which can be electrical or other forms.

[0114] The units described as separate components may or may not be physically separated, and the components of the control device may or may not be physical units, that is, they may be located in one place or distributed in multiple units. Some or all of the units may be selected according to actual needs to achieve the purpose of the present embodiment.

[0115] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the relevant technology or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions to enable a computer device (which can be a personal computer, a server or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), mobile hard disk, disk or optical disk and other media that can store program codes.

[0116] The above description is only an embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent substitution, improvement, etc. made within the spirit and principle of the present invention shall be included in the scope of the claims of the present invention.

Claims

1. A method for suppressing vibration of a robot terminal, characterized in that: The robot is a multi-joint robot, comprising: a base, a first arm, a second arm and a third arm; the first arm is connected to the base, the second arm is connected to the first arm, the third arm is connected to the second arm, and the load of the robot is installed at the end of the third arm; the method comprises: After the robot starts to run, an original motion signal of the robot in a first direction and an original motion signal in a second direction are respectively collected by an angular velocity sensor; The collected original motion signals of the robot in the first direction and the collected original motion signals in the second direction are processed respectively to obtain a first vibration frequency of the end load of the robot in the first direction and a second vibration frequency in the second direction; According to the obtained first vibration frequency of the end load of the robot in the first direction and the second vibration frequency in the second direction, respectively set the corresponding notch filter in the first direction and the notch filter in the second direction; Adding a notch filter in the first direction to a control instruction of a second motor of the robot, and adding a notch filter in the second direction to a control instruction of a third motor of the robot; The first direction is the rotation direction of the second arm of the robot with the vertical straight line at the connection between the first and second arm joints as the axis center; the second direction is the vibration direction of the end of the second arm of the robot; the second motor controls the second arm, and the third motor controls the third arm.

2. The method according to claim 1, characterized in that The angular velocity sensor is installed at a position at the end of the second arm of the robot close to the third arm.

3. The method according to claim 1, characterized in that The collected original motion signals of the robot in the first direction and the collected original motion signals in the second direction are processed respectively to obtain a first vibration frequency of the end load of the robot in the first direction and a second vibration frequency in the second direction, including: Performing filtering processing on the collected original motion signal of the robot in the first direction and the collected original motion signal in the second direction to obtain a vibration signal of the robot in the first direction and a vibration signal in the second direction; The vibration signal of the robot in the first direction and the vibration signal in the second direction after filtering are subjected to fast Fourier transform to obtain a first vibration frequency of the end load of the robot in the first direction and a second vibration frequency in the second direction.

4. The method according to claim 3, characterized in that The collected original motion signal in the first direction and the collected original motion signal in the second direction are filtered to obtain a vibration signal in the first direction and a vibration signal in the second direction, including: Using a high-pass filter to remove the DC component near 0 Hz in the original motion signal; and / or, A low-pass filter is used to remove high-frequency noise signals from the original motion signal.

5. The method according to any one of claims 1 to 4, characterized in that: Also includes: Obtain corresponding compensation coefficients for compensating phase lag according to the obtained first vibration frequency of the end load of the robot in the first direction and the obtained second vibration frequency in the second direction; A corresponding compensation coefficient for compensating phase lag is added to the control instruction of the second motor of the robot for differential compensation, and a corresponding compensation coefficient for compensating phase lag is added to the control instruction of the third motor of the robot for differential compensation.

6. A terminal vibration suppression device for a robot, characterized in that: The robot is a multi-joint robot, comprising: a first arm, a second arm and a third arm; the device comprises: a collection unit, configured to collect, after the robot starts to run, an original motion signal of the robot in a first direction and an original motion signal in a second direction respectively through an angular velocity sensor; a processing unit, configured to process the original motion signal of the robot in the first direction and the original motion signal in the second direction collected by the collection unit, respectively, to obtain a first vibration frequency of the end load of the robot in the first direction and a second vibration frequency in the second direction; A setting unit, configured to respectively set a corresponding notch filter in the first direction and a notch filter in the second direction according to a first vibration frequency of the end load of the robot in the first direction and a second vibration frequency in the second direction obtained by the processing unit; a suppression unit, configured to add a notch filter in the first direction to a control instruction of a second motor of the robot, and to add a notch filter in the second direction to a control instruction of a third motor of the robot, so as to suppress the terminal vibration of the robot; The first direction is the rotation direction of the second arm B2 of the robot with the vertical straight line at the connection between the first and second arm joints as the axis center; the second direction is the vibration direction of the end of the second arm of the robot; the second motor controls the second arm, and the third motor controls the third arm.

7. The device according to claim 6, characterized in that The angular velocity sensor is installed at a position at the end of the second arm of the robot close to the third arm.

8. A storage medium, characterized in that: A computer program is stored thereon, and when the program is executed by a processor, the steps of any method described in claims 1-5 are implemented.

9. A controller, characterized in that: The method comprises a processor, a memory and a computer program stored in the memory and executable on the processor. When the processor executes the program, the steps of the method described in any one of claims 1 to 5 are implemented, including the terminal vibration suppression device described in any one of claims 6 to 7.

10. A computer program product, characterized in that The invention comprises a computer program, which implements the steps of any one of the methods of claims 1 to 5 when executed by a processor.

Images