JP2011161562A - Wireless transmission device, vibration suppression control device of robot using the same and robot control device - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To improve a vibration suppression effect using a sensor and to reduce power consumption in a sensor substrate, when a wireless acceleration sensor is mounted in a robot. <P>SOLUTION: This wireless transmission device includes a signal analysis device obtaining vibration parameters (frequencies, amplitude and phases) of main components of a sensor signal, and a signal generator 81 for generating an approximate waveform of the sensor signal from the parameters, and performs wireless transmission of the parameters only when the parameters are changed. Speed of each axis motor is determined from the obtained approximate waveform, motor speed based on robot operations is obtained by simulation and a difference of motor speed is made as vibration components of the motor speed. The vibration components of the motor speed are fed back to a position-speed control loop of the motor to suppress vibration generated in a robot arm. <P>COPYRIGHT: (C)2011,JPO&INPIT

Description

  The present invention relates to a robot control device, for example, and more specifically, a wireless transmission device that transmits a vibration signal generated by attaching an acceleration sensor to the tip of a robot arm to a robot controller and a robot that suppresses the vibration of the arm tip by applying the same. The present invention relates to a vibration suppression control device.

A clean robot used in the semiconductor field requires high-speed operation of the robot in order to improve productivity. When performing high-speed operation, there is a problem that vibration of the mechanism of the arm is excited and residual vibration is generated in which the arm vibrates in a direction perpendicular to the operation direction even after the robot stops operating.
When a clean robot transports a wafer and stores it in a cassette, if the residual vibration occurs, the wafer and the cassette will collide and cause damage to the wafer. Therefore, it is necessary to wait for the residual vibration to settle down. This is one of the causes that hinder improvement.
As one of the robot arm vibration suppression methods, an acceleration sensor is attached to the end of the arm, and a compensation signal is generated using the acceleration signal generated by vibration and some robot parameters, and the compensation obtained from the torque command of each axis motor A method of suppressing vibration by feeding back a signal is disclosed (Japanese Patent Laid-Open No. 10-100085).
Therefore, in the conventional method, the acceleration sensor signal is transmitted to the robot controller as time series data, and is collectively processed with reference to various robot parameters inside the robot controller.

On the other hand, when an acceleration sensor is attached to a robot, it is desired to use it without wiring (Patent Documents 5 and 6). Because with wiring
(1) When the turning operation is performed, the operation range is limited.
(2) Torsion or pulling force is applied to the wiring attached to the sensor by the operation of the robot, and disconnection or wiring contact failure occurs.
(3) There is a restriction on the mounting position of the acceleration sensor.
(4) It takes time to route the wiring inside the robot.
This is because of such reasons.

FIG. 14 shows a block diagram of the sensor device of Patent Document 5. In the present invention, as described above, a sensor device that transmits acceleration information at the tip of a robot arm as time-series data is shown, and a method for saving power by turning on / off the power is disclosed.
FIG. 15 is a block diagram of the wireless output sensor disclosed in Patent Document 6. As shown in the figure, in the conventional invention, an acceleration sensor unit that detects a detection target, a wireless transmission unit that wirelessly transmits sensor data related to detection by the acceleration sensor unit, and a process that is performed by the wireless transmission unit There is disclosed a technique of a wireless output sensor comprising: a transmission antenna that transmits sensor data; and a battery mounting section that mounts a battery serving as a power source for each of the acceleration sensor section and the wireless transmission section.

In the wireless output sensor of FIG. 15, a vibration waveform is generated in the AC amplifier unit 184 according to the magnitude of the output signal of the triaxial acceleration sensor 183, and the signal is detected by the demodulator 185 and the output unit 186 as to whether the detection target is movable. It has been converted to a signal to be detected. A technique is disclosed in which this movable signal is converted into an AC signal by an oscillating unit 133, processed by an amplifier unit 134 and a filter 135, and then wirelessly output via an antenna 24.

As shown in FIG. 14 and FIG. 15, when the acceleration sensor is wireless, at present, the measurement data is transmitted wirelessly, and power consumed by the sensor board is supplied by a battery provided inside the board. Realistic. In Patent Documents 5 and 6, it is proposed to extend the battery life by using an auxiliary power source of solar charging type or vibration charging type.
An example of the power consumption of the electronic components used for the sensor substrate is shown in FIG. It can be seen that most of the power consumed by the sensor board is consumed wirelessly.

Further, as shown in Non-Patent Document 1, a large number of acceleration sensors are arranged in a building structure, and signals from each sensor are wirelessly transmitted to a central master station, and a large number of signals transmitted to the master station are also obtained. A structural health monitoring system that detects the occurrence of disasters such as earthquakes has been developed.
In this system, the signal of each sensor is processed so that the amount of data transferred in the signal processing circuit provided on the sensor board is minimized, and the transmission amount transmitted wirelessly from the sensor board to the master station is reduced. A technique for reducing consumption of a battery provided on a sensor substrate is disclosed.

  Patent Document 1 discloses a technology of a road surface state estimating device that calculates a vibration level calculation value from an acceleration sensor signal provided on a tire side and transmits the calculated value to the vehicle body side by radio. FIG. 12 is a block diagram showing the configuration of the road surface state estimation system. The signal of the acceleration sensor mounted on the tire is frequency-analyzed by the frequency analysis means, and the vibration level calculation value is calculated and wirelessly transmitted to a road surface state estimating device provided on the vehicle body side. The road surface state estimation device determines the road surface state in which the vehicle is traveling using the transmitted vibration level calculation value.

In Patent Document 2, Patent Document 3 and Patent Document 4, an acceleration sensor is installed in the fixed part of the bearing, and the vibration acceleration generated when the bearing rotates is FFT processed and compared with the frequency component of the rotation speed signal. Thus, an abnormality diagnosis apparatus for mechanical equipment that detects the presence / absence of an abnormality of a rotating part and an abnormal part is disclosed.
FIG. 13 shows a block diagram of a signal processing unit of the abnormality diagnosis apparatus. The signal of the acceleration sensor provided in the detection unit is frequency analyzed by the diagnostic spectrum calculation unit of the signal processing unit, and the result is compared with the result of frequency analysis of the rotation information by the rotation analysis unit to determine whether there is a bearing abnormality. Has been.

  As described above, the wireless transmission device using the conventional acceleration sensor transmits the data obtained from the acceleration sensor in time series and performs all of the state estimation of the measurement object and the abnormality detection processing by the controller, or the sensor side After the data is processed using frequency analysis or other means, the amount of transmitted information is reduced, and the controller estimates the state of the object to be measured and detects anomalies and reduces the power consumed by wireless transmission. Has been done.

JP2007-55284 (Bridgestone) JP2007-263609 (NSK) JP2007-285875 (Nippon Seiko) JP2009-20090 (NSK) JP2008-186336 (Yaskawa Electric) JP2008-171403 (Koyo Electronics Industry)

“Development of Structural Health Monitoring System Using Smart Sensors and Wireless Networks” Proceedings of the Japan Earthquake Engineering Society Vol.7, No.6, 2007, p.17-30

In the invention of Patent Document 6, only information on whether or not the object is movable is detected. The robot operation status can be detected even if the wireless communication is configured to send one-shot for 0.5 seconds (see paragraph number (0045)), and even if the sample rate is less than 2 [sample / s], there is no problem and the battery life It is considered that a certain period can be obtained. However, in the case of arm vibration suppression control, a time-series signal of the acceleration signal is required. Since the vibration frequency of the arm is about 10 [Hz] and time series data of about 300 [sample / s] that is 10 to 30 times that is necessary, the amount of transmission information becomes large, and the wireless sensor module of FIG. In the case of electronic components, the battery life is about several days.
The same is true of the invention of the sensor device disclosed in Patent Document 5, and the short battery life is a problem, which is one of the reasons why the wireless acceleration sensor cannot be applied to vibration suppression control.

In the case of a conventional structural health monitoring system, sensor data is processed independently by each sensor, converted into a damage index that is a cumulative value of the number of zero cross points of acceleration and the maximum value and absolute amplitude value on the sensor board, and is wireless. It is transmitted to the master station by communication.
In the case of a conventional road surface state estimating device, most of the sensor signal processing is performed on the tire side. Although the acceleration sensor is affected by gravity as the tire rotates, it is not necessary to correct the acceleration change (± 1 [g]) due to the rotation because the acceleration of the vibration generated as the vehicle travels is about 40 [g]. .
In addition, in conventional machine equipment abnormality diagnosis devices, sensor signal processing is performed independently,
No consideration is given to the acceleration generated when the machine tool operates. In general, in machine tools, the frequency of acceleration due to the operation of the machine and the acceleration of abnormal vibration are different, and the acceleration due to the operation of the machine is handled as noise and removed using means such as frequency analysis. That is, sensor signal processing is not performed in combination with controller parameters.

  In vibration suppression control of a robot using an acceleration sensor, it is necessary to calculate a compensation signal to be added to the motor torque command by processing the acceleration signal in combination with various parameters of the robot that change depending on the robot's posture and operating state. . Therefore, the data processing in each sensor must be within a range that can be corrected in combination with the robot parameters on the robot controller side.

In addition, because the frequency bands of vibration and motion acceleration overlap in a robot and the magnitudes of vibration and motion acceleration are similar, the acceleration generated when the robot moves can be processed as noise. First, the method of correcting the motion acceleration calculated from the posture and motion state from the sensor signal (Japanese Patent Laid-Open No. 2005-316937) becomes effective, and the combination of the conventional wireless transmission device technologies increases the effect of vibration suppression. I can't.
In order to perform such signal processing by the robot controller, it is necessary to transmit sensor signals in time series and perform all processing on the robot controller side as shown in Patent Document 5.

When high sampling speed of the sensor signal is required, such as robot vibration suppression control, it is necessary to transmit the sensor signal at all times, and the wireless circuit is constantly transmitting, so use a commercially available dry cell. If this is the case, the battery power will be consumed in several days, and the battery needs to be replaced or charged frequently.
Patent Documents 5 and 6 describe extending the battery life of the auxiliary power supply. However, in the case of the power consumption shown in FIG. 16, in the case of solar charging, it is necessary to increase the area of the solar cell panel. It becomes difficult. Also in the case of vibration charging, since the generated electric power per volume of the piezoelectric element currently obtained is small, it is necessary to increase the volume in order to obtain an auxiliary effect as a power source, and it is difficult to attach it to the tip of the robot arm.
When the acceleration sensor is used without wiring and applied to applications that require real-time performance such as vibration suppression control, not only the improvement of the robot control performance is achieved, but also low power consumption of the sensor board is achieved at the same time. There is a problem that it cannot be put into practical use unless it can be devised.

  As can be seen from FIG. 16, the power consumption of each component of the wireless sensor module is considerably larger than that of the other components when the wireless circuit always performs a transmission operation. In addition, the power consumption of the components of the signal processing circuit including the sensor has been greatly reduced over the past few years. Therefore, if the power consumption of the wireless circuit can be reduced by reducing the transmission information amount of the measurement data, it becomes one of the effective methods for reducing the power consumption of the sensor substrate.

Further, in applications that require real-time properties such as vibration suppression control, there is a problem that a phase delay occurs in the acceleration signal due to a transmission delay time of wireless transmission, and improvement in control performance is restricted.
As wireless circuits, digital wireless devices such as Bluetooth, whose performance and price are becoming lower, can be considered, but when using commercially available wireless circuits, transmission delays of about 5 [ms] occur due to sampling delays, etc. Therefore, in the case of a vibration waveform of 10 [Hz], the phase delay is about 18 °, which causes deterioration of the control performance of the vibration suppression control.

  The present invention has been made in view of such problems, and when performing vibration suppression control of a robot using a wireless acceleration sensor, the effect of vibration suppression is improved and the power consumption of the wireless sensor module is improved. An object of the present invention is to provide a robot control apparatus that reduces the above-described problem.

In order to solve the above problem, the present invention is configured as follows.
In the wireless transmission device that transmits a sensor signal to a controller using a pair of wireless circuits, the present invention obtains parameters including frequency, amplitude, and phase of vibrations that are main components by frequency analysis of the sensor signal, A radio transmission apparatus comprising: a signal analysis device that transmits the parameter from the radio circuit when the parameter changes; and a signal generator that generates an approximate waveform of the sensor signal using the parameter of the received vibration. It is what.

  According to the present invention, the wireless transmission device according to claim 1, the Jacobian inverse conversion means for obtaining the motor speed of each axis of the robot from the transmitted biaxial sensor signals, and each axis motor is operated from the position command of the robot. It has a means to obtain the speed that occurs at times, subtract the speed that occurs when it operates from the motor speed obtained from the sensor to obtain the vibration component of the motor speed, the motor speed in the speed command of the motor position-speed control loop The robot control device is characterized in that the vibration component is compensated to suppress the vibration generated in the arm.

Further, the present invention includes a wireless transmission device according to claim 1 and means for obtaining a frequency component of a motor speed generated when the robot operates from a result of frequency analysis of a position command generated by the motion unit,
Subtract the frequency component of the speed generated when operating from the frequency component of the sensor speed signal to obtain the frequency component of the vibration component of the motor speed, and approximate the vibration of the motor speed from the obtained vibration frequency, amplitude, and phase parameters A robot control apparatus is characterized in that a waveform is generated and compensated as a vibration component in a motor position-speed control loop to suppress vibration generated in an arm.

  The present invention also provides a sensor signal from a motor torque command using a notch filter capable of changing a center frequency and a Q value according to the frequency and amplitude of vibration of a main component of a sensor signal transmitted with the signal analysis device according to claim 1. The robot control apparatus is characterized by removing the vibration frequency component of the main component.

  In the robot controller, the vibration frequency of each axis motor constituting the arm is calculated and transmitted to the wireless sensor module in the robot controller. In the wireless sensor module, the amplitude of the frequency component of the two vibration frequencies of the Y-axis sensor signal is calculated. And the phase is calculated and transmitted to the robot controller. The robot controller uses the transmitted amplitude, phase and vibration frequency obtained by the calculation, and posture information of the robot to calculate the S-axis vibration amplitude and the L-axis vibration amplitude. The vibration amplitude of the S-axis motor and the vibration amplitude of the L-axis motor are calculated by the generator, and the signal generator generates the S-axis vibration waveform from the vibration amplitude of the S-axis motor, the phase of the Y-axis signal at the S-axis vibration frequency, and the S-axis vibration frequency. Is generated from the vibration amplitude of the L-axis motor, the phase of the Y-axis signal at the L-axis vibration frequency, and the L-axis vibration frequency. To synthesize L vibration waveform by, it is an robot controller and performs vibration suppression control using the two synthetic waveform.

  In addition, the present invention performs transmission of amplitude information based on phase information based on amplitude and phase information obtained by frequency analysis of the sensor signal, and the robot controller arrives at the arrival time of the transmitted amplitude information. 6. The robot according to claim 5, further comprising: a signal generator that calculates a phase obtained by frequency analysis based on the frequency and synthesizes a vibration waveform from the amplitude, the calculated phase, and a vibration frequency inside the controller. This is a control device.

  The present invention also includes a phase compensator that incorporates the offline measurement result of the transmission delay time corresponding to each frequency, at the S-axis vibration frequency ωS and the L-axis vibration frequency ωL obtained by the vibration frequency calculator. 7. A robot controller according to claim 5, wherein a signal generator is configured to input a transmission delay time to the signal generator and compensate for the transmission delay time generated by the wireless sensor module and the two wireless circuits. To do.

According to the present invention, it is only necessary to transmit the parameter only when the vibration parameter changes, and the number of wireless transmissions can be reduced, so that the power consumption of the sensor device can be reduced.
Further, according to the present invention, the compensation signal of each axis motor can be calculated using the approximate waveform of the sensor signal of the signal generator, and the compensation signal can be fed back to the control loop to increase the vibration suppression effect. Moreover, since the parameter is transmitted only when the vibration parameter changes, the power consumption of the sensor device can be reduced.
Further, according to the present invention, the frequency component of the vibration of the motor speed can be obtained using the transmitted vibration parameter and the result of the frequency analysis of the position command. Using this, a time waveform of vibration of the motor speed can be obtained. This can be fed back to the control loop as a compensation signal to increase the vibration suppression effect. Moreover, since the parameter is transmitted only when the vibration parameter changes, the power consumption of the sensor device can be reduced.
Further, according to the present invention, a notch filter that can change the center frequency and the Q value according to the frequency and amplitude of the transmitted vibration parameter can be inserted into the torque command, and the vibration frequency component of the robot arm can be removed from the torque command. The vibration excitation of the robot arm can be prevented. In addition, since the parameter is transmitted only when the vibration parameter of the machine changes, the power consumption of the sensor device can be reduced.
Further, according to the present invention, the vibration waveform generated in each motor shaft can be generated from the S-axis resonance frequency component, the L-axis resonance frequency component of the Y-axis sensor signal, and the signal information in the controller. The amount of information transmitted between controllers can be reduced, and the power consumption of the wireless sensor module can be reduced.
According to the present invention, the phase information obtained by analyzing the vibration frequency component can be transmitted as time information for transmitting the amplitude obtained from the vibration frequency component, so that transmission of phase data can be omitted and the amount of transmission information can be reduced. Is possible. The power consumption of the wireless sensor module can be reduced.
In addition, according to the present invention, it is possible to compensate for the transmission delay time generated by wireless transmission between the wireless sensor module and the controller, so that it is possible to generate a compensation signal in phase with the vibration acceleration generated by the operation of the robot, and vibration suppression control The control performance can be improved.

1 is a block diagram of a wireless transmission device showing a first embodiment of the present invention. The block diagram which shows the internal structure of the signal generator of this invention The block diagram of the vibration suppression control apparatus which shows 2nd Example of this invention. Expressions of elements of Jacobian matrix for explaining Jacobian inverse transformation of the second embodiment The block diagram of the vibration suppression control apparatus which shows 3rd Example of this invention. Block diagram for explaining the transfer function of the third embodiment The block diagram of the vibration suppression control apparatus which shows 4th Example of this invention. Configuration diagram explaining operation of robot arm Configuration diagram explaining the inside of the robot controller The block diagram of the vibration suppression apparatus which shows 5th Example of this invention. Configuration diagram for explaining the fifth embodiment The block diagram for demonstrating the conventional road surface state estimation apparatus Configuration diagram for explaining a conventional machine equipment abnormality diagnosis device Block diagram for explaining a conventional sensor device Block diagram for explaining a conventional wireless output sensor The figure which shows the power consumption of the component of the wireless sensor module

Hereinafter, embodiments of the present invention will be described with reference to the drawings.
8 and 9 are diagrams for explaining the robot used in the embodiment of the present invention. FIG. 8 is a diagram schematically showing an outline of a robot system including a robot. As shown in FIG. 8, the robot system 1 includes a horizontal articulated robot 2 and a robot controller 20. The robot 2 includes a base 3, a first arm 4, a second arm 5, a fork 6, an S-axis motor 7, an L-axis motor 8, and a wireless sensor module 15. The base 3 is formed in a columnar shape and configured to be movable up and down. The first arm 4 is attached to the base 3 via a bearing or the like so as to be capable of turning in a horizontal plane, and performs a turning operation by a rotational movement of an S-axis motor 7 attached to the first arm base side. Further, the second arm 5 is attached to the distal end side of the first arm 4 so as to be able to turn in a horizontal plane, and the turning operation is performed by the rotational motion of the L-axis motor 8 attached to the base side of the second arm. In many of the clean robots described here, the motor of each axis is not configured to directly drive the center of rotation of the arm. A reduction gear is inserted in the motor, and the reduced output is transmitted via a belt or the like. Thus, the arm is swung by applying the rotation direction of the arm. In this embodiment, the S-axis motor 7 and the L-axis motor 8 including a mechanism such as a speed reducer and a belt are used as an example, and a motor with a reduction ratio of 1 directly drives the rotation shaft of the arm.

A fork 6 is attached to the distal end side of the second arm 5 so as to be rotatable in a horizontal plane, and the fork 6 is formed so that a work W to be transported can be placed thereon. The second arm 5 and the fork 6 are configured to turn while maintaining a predetermined speed ratio (rotation ratio) by driving the L-axis motor 8, thereby moving the fork 6 in the radial direction of the base 3. It is configured to be able to.
The wireless sensor module 15 includes a two-axis acceleration sensor 151, a signal analysis device 152, a wireless circuit 71, and a battery 154. The two-axis acceleration sensor 151 measures acceleration in a horizontal plane, and the signal analysis device 152 performs frequency analysis on the data. Thus, the measurement parameter is wirelessly transmitted from the wireless circuit 71 to the robot controller 20. The battery 154 is a power source for each component of the wireless sensor module 15.

The operation of the robot arm 2 is controlled by the robot controller 20. FIG. 9 is a block diagram showing an outline of functions of the robot controller 20. The robot controller 20 includes a robot multi-axis control arithmetic unit 201 and a robot multi-axis amplifier 202.
As a configuration example of the robot controller 20, the robot multi-axis control arithmetic unit 201 can be configured using a personal computer, and the robot multi-axis amplifier 202 can be configured using a plurality of motor controllers capable of controlling the torque of each axis motor of the robot. . In this case, the motion unit and servo unit, which are the main functions of the robot multi-axis control arithmetic unit 201, are constituted by software executed in the personal computer, and are two tasks having different execution cycles.

The robot operation program is created in advance by the user and stored in the nonvolatile memory on the robot multi-axis control arithmetic unit 201. When operating the robot, the robot operation program is read and decoded by the motion unit to calculate the position command of each axis motor. In the servo unit, the position command calculated by the motion unit and each axis motor position obtained from the encoder attached to the motor are used to constitute a position-speed control system to calculate the torque command of each axis motor.
The robot multi-axis amplifier 202 supplies a current corresponding to the torque command of each axis motor calculated by the servo unit to each axis motor, controls the motor position and motor speed, and generates the robot posture generated by the operation program. And getting the behavior.
Here, a vibration is detected using a two-axis acceleration sensor (X-axis sensor, Y-axis sensor) attached to the base of the fork 6 of the robot 2 as vibration detection means at the arm tip, and the wireless sensor module 15 and the robot controller 20 are detected. Consider a case where a measurement signal is wirelessly transmitted between the internal wireless circuit 153 and the wireless circuit 70 and the measurement signal is taken into the robot controller 20.

  Hereinafter, embodiments of the present invention will be described with reference to the drawings.

FIG. 1 shows a block diagram of the signal transmission apparatus of the present embodiment. In the figure, the robot arm 2 includes a two-axis acceleration sensor 151 that measures the X-axis direction component and the Y-axis direction component of the horizontal acceleration, a signal analysis device 152 that performs frequency analysis thereof, and the analysis result wirelessly to the robot controller 20. A wireless sensor module 15 having a wireless circuit 71 for transmitting data and a battery 154 serving as a power source for these is mounted. The robot controller 20 is equipped with a wireless module 700 including a wireless circuit 70 and a signal generator 81.
The signal generator 81 is composed of the blocks shown in FIG. The signal generator 81 receives a frequency ω, an amplitude V, and a phase θ, which are vibration parameters received by the wireless circuit. These parameters are latched in the register of the signal generator 81 when reception is completed. In addition, there is a timer inside the signal generator 81, which latches t0 when the data is latched. The sine wave calculator uses the time t indicated by the timer.
A = V ・ SIN (ω ・ (t−t0) + θ) (1 set)
And the acceleration approximate waveform A is output.

A signal generated by the biaxial acceleration sensor 151 when the robot arm 2 moves in a horizontal plane is subjected to frequency analysis by a CPU (not shown) inside the signal analysis device 152, and vibration parameters (frequency ω, Amplitude V, phase θ) are determined.
As a means of frequency analysis, when performing vibration suppression control of the robot arm, real-time characteristics are required, and therefore it is assumed that wavelet transform is performed. The obtained vibration parameters of the principal component of each axis are wirelessly transmitted between the wireless circuit 71 and the wireless circuit 70 and are taken into the robot controller 20.
The vibration parameters taken into the controller are set inside the signal generator 81, and an approximate acceleration waveform A consisting only of the main components of the acceleration sensor signal is generated inside the robot controller 20.
In the first embodiment, the wireless transmission is considered to be performed only when the vibration parameter element calculated by the signal analysis device 152 changes more than a certain value.
When the main component signal is a sine wave having a constant frequency, amplitude, and phase, once the vibration parameter is transmitted between the radio circuit 71 and the radio circuit 70, the calculation of the acceleration approximate waveform A is performed thereafter. This is performed inside the signal generator 81, and the number of wireless transmissions can be greatly reduced.
Although the case where the transmission signal is only the main component of each axis sensor is considered in FIG. 1, it is also possible to configure so that there are a plurality of components, and the signal generator 81 has a plurality of sine wave calculators. The acceleration approximate waveform A is obtained by the sum. With this configuration, the approximate acceleration waveform A can be brought close to the signal of the acceleration sensor 151.

FIG. 3 is a diagram illustrating the configuration of the second embodiment. The case where the wireless sensor module 15 and the wireless module 700 shown in the first embodiment are provided in the robot system 1 and the vibration suppression control device is configured to suppress the vibration generated in the robot arm 2 is shown.
The wireless module 700 is mounted on a servo unit inside the robot controller 20. In the signal analyzer 152, the vibration accelerations of the X-axis acceleration sensor and the Y-axis acceleration sensor are subjected to frequency analysis, and the vibration parameters of the main components of the respective axes are obtained. Here, it is assumed that the principal component frequency ωx in the X-axis direction is different from the vibration frequency ωy of the principal component in the Y-axis direction, and the obtained vibration parameters are converted from acceleration to velocity in the signal analysis device 152 and processed. To do.
Here, only the control block diagram of the S-axis motor is shown. The control block for the L-axis motor is configured similarly.
The signal generator 82 includes the two signal generators 81 described in the first embodiment. The two signal generators 81 are set with vibration parameters in the X-axis direction and the Y-axis direction received by the wireless circuit 70, and respective approximate speed waveforms are generated inside the robot controller 20.
The two speed approximate waveforms are converted into the speeds of the S-axis motor and the L-axis motor by Jacobian inverse transformation. The Jacobian inverse transformation is performed by multiplying the Jacobian inverse matrix 90. In the case of the horizontal articulated robot of FIG. 8, the Jacobian inverse matrix is a 2 × 2 matrix shown by (Equation 2) of FIG.
Each element depends on the joint angles θs and θl of the two motors and changes as the robot posture changes.
The motor control system uses the position command given from the motion unit and the encoder position attached to the motor, and the position loop has P control and the speed loop has PI control. It consists of a simulation unit that sequentially calculates the motion of the robot's rigid body model using control parameters. The simulation unit obtains the velocity of the rigid model by sequentially substituting the inertia that changes with the posture of the robot.
Here, only the principal components ωx and ωy of the X-axis velocity and the Y-axis velocity are considered, so that the velocity obtained by the simulation also has only two frequency components, so that the center with the change of the principal component frequencies ωx and ωy. A pair of bandpass filters 300 and 301 whose frequencies change so as to coincide with ωx and ωy are included.
The result of subtracting the S-axis motor speed obtained by the simulation from the S-axis motor speed obtained from the sensor is used as a vibration component to compensate the speed command for the main loop.
Since the simulation is performed by giving the S-axis position command when the robot operates to a rigid model having inertia equivalent to the robot, the motion speed obtained by the simulation approximates the motion of the robot that does not include vibration components. Represents the speed generated by the movement of the robot. Since the speed measured by the sensor includes the speed component generated by the motion as well as the component generated by the vibration, the speed component generated by the vibration can be obtained by removing the component generated by the motion of the robot obtained by the simulation. . The vibration of the robot arm is suppressed by compensating the speed component of the vibration to the speed command of the main loop.

  In this way, the sensor signal is frequency-analyzed to obtain an approximate velocity waveform from the vibration parameters (amplitude, frequency, phase) of the principal component, and the compensation signal is calculated based on the approximate waveform. The effect can be improved and the power consumption in the sensor substrate can be reduced because the parameter is wirelessly transmitted only when the vibration parameter changes.

FIG. 5 is a diagram illustrating the configuration of the third embodiment. It is assumed that the wireless module 700 mounts the robot controller 20 on the motion unit. The case where the vibration component of the motor speed is calculated from the parameters of the vibration transmitted by radio, the rigid body model of the robot, the transfer function obtained from its control system, and the frequency analysis result of the position command is shown.
In this embodiment, the wavelet transformer 900 for analyzing the frequency of the position command calculated in the motion unit is created, and the result of the frequency analysis is created so as to give the signal intensity and phase to the two parameters of the robot operation time and frequency. A conversion table 901, a transfer function 902 of the operation speed with respect to the position command from the output of the conversion table and the frequency of the vibration parameter, a frequency converted Jacobian inverse conversion 903 that gives a frequency component of the speed of the S-axis motor from the transmitted vibration parameter, A signal generator 81 that generates a time waveform from the frequency component of the vibration component is a constituent element.

In the motion unit, position commands for the S-axis motor and the L-axis motor are generated. The frequency is analyzed by the wavelet transform 900, and the amplitude and phase of the frequency component of the position command with respect to the robot operation time can be calculated. Based on this, a conversion table 901 is created that gives the amplitude and phase of the position command with respect to the robot operating time and the vibration parameter frequency ωx (or ωy).
For a rigid model, given a vibration parameter frequency ωx (or ωy), a transfer function is calculated. FIG. 6 shows a block diagram of the S-axis motor when the position command is given and the rigid body model operates. The transfer function of the S-axis motor speed Vs with respect to the position command θref is as shown in the following equation.

Vs = {(Kp ・ (Kv ・ S ² + Ki ・ S)) / (Js ・ S ³ + Kv ・ S ² + (Kp ・ Kv + Ki) ・ S + Kp ・ Ki)} ・ θref
... (3 formulas)

When a signal is received by the wireless circuit 70, the vibration frequency ωx (ωy) of the principal component in the X-axis direction and the Y-axis direction is received. Also, the robot operating time t1 is obtained from the received time. The amplitude component and the phase component of the position command are obtained by the conversion table 901 obtained above from the set of ωx and t1.
The frequency component of the speed of the S-axis motor can be calculated when a position command is given by multiplying the complex amplitude obtained from the obtained amplitude and phase by a transfer function. This obtains a velocity component not including the vibration of the robot when a position command is given.

On the other hand, the frequency component of the S-axis motor speed of the sensor is obtained by multiplying the transmitted vibration parameter by a matrix obtained by FFT-transforming the Jacobian inverse matrix.
The frequency component of the S-axis motor speed obtained from the command is compensated for the frequency component of the S-axis motor speed of the sensor to obtain the frequency component of the vibration of the S-axis motor speed.
The time waveform of the vibration component of the motor speed is obtained by the signal generator 81 from the parameters (frequency, amplitude, phase) of the vibration frequency component.
The time waveform of this vibration is compensated with the speed command of the position-speed control system 100, and the effect of suppressing the vibration generated in the robot arm 2 is obtained.
The position command for calculating Jinv (ω) can be the respective motor positions obtained from the S-axis encoder and the L-axis encoder. Further, it may be a calculated value obtained by combining the position command and the encoder position. Moreover, it is good also as a calculated value obtained from the calculation formula containing a torque command.

FIG. 7 is a diagram showing the configuration of the fourth embodiment. A notch filter 50 is inserted in the torque command between the position-speed control system 100 and the servo driver 2. The notch filter 50 is configured by connecting two independent notch filters 51 and 52 in series.
In this embodiment, the notch filter 51 is connected to the frequency ωx and the amplitude Vx, which are vibration parameters of the X-axis sensor, and the notch filter 52 is connected to the vibration parameter ωy and the amplitude Vy of the Y-axis sensor.
Here, the orders of the two filters are considered to be second order, and the transfer functions of the two filters are

H (s) = (s ² + ω0 ² ) / (s ² + (ω0 / Q) * s + ω0 ² ) (Expression 4)
Suppose that
The notch filters 51 and 52 are configured such that the center frequency ω 0 and the Q value that determines the width of the notch filter can be changed using an external signal.
The wireless sensor module 15 calculates the vibration parameters (ωx, Vx, ωy, Vy) of the main components of the two-axis sensor and transmits them to the robot controller 20.
The center frequency ω 0 of the notch filter can be changed to match the frequency ωx (or ωy) of the input vibration parameter. The Q value is configured such that the Q value is high when the amplitude Vx (or Vy) of the vibration parameter is large, and the Q value is low when the amplitude Vx is small.

  ωx and ωy are considered to be the resonance frequencies of the robot in the X and Y directions, and the excitation of vibrations that are likely to occur in the robot arm 2 is prevented by removing signals having these frequencies from the torque command of each axis motor. Can do. Since the center frequency ω0 and the Q value of the notch filter can be changed with the change of the vibration parameter, the effect of preventing vibration excitation can be improved.

  As described above, the vibration parameters (frequency and amplitude) frequency-analyzed by the signal analysis device 152 of the wireless sensor module 15 are transmitted to the robot controller 20 and the vibration component is removed by the notch filter mounted on the robot controller 20. Therefore, the effect of vibration suppression control can be improved, and the power consumption of the battery 154 mounted on the wireless sensor module 15 can be reduced by reducing the number of times the sensor signal is wirelessly transmitted.

In this type of robot, the lengths of the first arm 4 and the second arm 5 are often the same. In the fifth embodiment, the case where the length is d is described in FIG.
FIG. 10 is a diagram showing the configuration of the fifth embodiment. The controller has a built-in vibration frequency calculator that calculates the vibration frequency of each motor from the S-axis position command θS and the L-axis position command θL given by the command generator. The vibration frequency calculator calculates the posture of the robot from θS and θL, and calculates the inertia of each axis motor. The vibration frequency calculator can calculate the parameters of the motor and the speed reducer and the parameters that change with the posture of the robot. Using the inertia, spring constant, and damping constant obtained from this, vibration frequencies ωS and ωL generated in the reduction gears of each motor shaft are calculated and input to the radio circuit 70. The wireless circuit 70 transmits the vibration frequencies ωS and ωL to the wireless circuit 71 of the wireless sensor module.
The wireless sensor module includes a biaxial acceleration sensor, a bandpass filter, and a wireless circuit 71. Vibration frequencies ωS and ωL obtained from the radio circuit 71 are input to the bandpass filter. The band-pass filter is configured in parallel so as to obtain the ωS frequency component and the ωL frequency component of the Y-axis sensor signal, and the center frequency is changed to ωS and ωL. The acceleration sensor has a function of calculating the ωS frequency component Ay (ωS), the phase θy (ωS), the ωL frequency component Ay (ωL), and the phase θy (ωL) of the Y-axis component Ay.
The four signals obtained by the band pass filter are transmitted to the controller via the radio circuit.
In the S-axis vibration amplitude calculator, Ay (ωS), θL is input and X calculated from θL is used (see FIG. 11), and the vibration acceleration As (ωS) of the S-axis motor is As (ωS) = − Ay. (ΩS) / X (Expression 5)
Is calculated. Here, X = √ (2 · d ² + 2 · d ² * Cos (θL)).
In the L-axis vibration amplitude calculator, Ay (ωL) and θL are input and Al (ωL) = 2 · Ay (ωL) / √ (2 · d ² + 2 · d ² · Cos (θL)
... (6 formulas)
Is calculated.
The two phase compensators have a built-in means for obtaining a delay time from the two-axis acceleration sensor to the radio circuit 70 for each vibration frequency obtained in an off-line state, and calculate a delay time at the calculated ωS and ωL frequencies. There is a function to do.
As (ωS), θs (ωS), ωs, and the output of the phase compensator are input to the S-axis signal generator, and an approximate waveform of the S-axis vibration waveform is calculated.
An approximate waveform is similarly calculated for the L axis.
An approximate waveform of the S-axis and L-axis vibration waveforms is input to the motor speed command to compensate for the vibration component of each axis motor and suppress vibrations at the tip of the robot arm.
In this embodiment, the phases of the two vibration frequency components are obtained and transmitted to the controller, but the amplitude transmission timing is performed at a time determined by the phase information, and the phase information is obtained by measuring this time at the time of reception. It can also be configured. As a result, the amount of transmission information can be reduced, and the power consumption of the wireless sensor module can be reduced.

DESCRIPTION OF SYMBOLS 1 Robot system 2 Robot 3 Base 4 S-axis arm 5 L-axis arm 6 Fork 7 S-axis motor 17 S-axis encoder 8 L-axis motor 18 L-axis encoder 15 Wireless sensor module 151 2-axis acceleration sensor 152 Signal analysis apparatus 20 Robot control Device 70, 71 Radio circuit 81, 82 Signal generator 90 Jacobian inverse transform 50 Notch filter 51, 52 Secondary notch filter
W Workpiece 100 Position-speed control system 201 Multi-axis control arithmetic device 202 for robot Multi-axis amplifier 300, 301 for robot Band pass filter

Claims (7)

In a wireless transmission device that transmits a sensor signal to a controller using a pair of wireless circuits,
The sensor signal is frequency-analyzed to obtain a parameter composed of vibration frequency, amplitude, and phase as main components, and when the parameter changes, a signal analysis device that sends the parameter from the radio circuit, and a received vibration A radio transmission apparatus comprising a signal generator that generates an approximate waveform of the sensor signal using the parameter.   2. The wireless transmission device according to claim 1, a Jacobian reverse conversion means for obtaining a motor speed of each axis of a robot from two-axis sensor signals to be transmitted, and a speed generated when each axis motor is operated from a position command of the robot. The motor speed vibration component is subtracted from the motor speed obtained from the sensor to obtain the motor speed vibration component, and the motor position-speed control loop speed command compensates for the motor speed vibration component. A vibration suppression control device for a robot characterized by suppressing vibrations generated in the arm. The wireless transmission device according to claim 1, and means for obtaining a frequency component of a motor speed generated when the robot operates from a result of frequency analysis of a position command generated by a motion unit,
Subtract the frequency component of the speed generated when operating from the frequency component of the sensor speed signal to obtain the frequency component of the vibration component of the motor speed, and approximate the vibration of the motor speed from the obtained vibration frequency, amplitude, and phase parameters A vibration suppression control device for a robot, which generates a waveform and compensates for a vibration component in a motor position-speed control loop to suppress vibration generated in an arm. 2. The vibration frequency of the main component of the sensor signal from the torque command of the motor using a notch filter capable of changing the center frequency and the Q value according to the frequency and amplitude of the vibration of the main component of the sensor signal transmitted with the signal analysis device according to claim 1. A vibration suppression control device for a robot characterized by removing a component.   In the robot controller, the vibration frequency of each axis motor constituting the arm is calculated and transmitted to the wireless sensor module, and the wireless sensor module calculates the amplitude and phase of the two vibration frequencies of the Y-axis sensor signal. The robot controller transmits to the robot controller, and the robot controller uses the transmitted amplitude, phase, vibration frequency obtained by calculation, and posture information of the robot to calculate the S-axis vibration amplitude calculator and the L-axis vibration amplitude calculator. The vibration amplitude and the vibration amplitude of the L-axis motor are calculated, the S-axis vibration waveform is synthesized by the signal generator from the vibration amplitude of the S-axis motor, the phase of the Y-axis signal at the S-axis vibration frequency, and the S-axis vibration frequency, and the L-axis The signal generator generates the L-axis vibration from the vibration amplitude of the motor, the phase of the Y-axis signal at the L-axis vibration frequency, and the L-axis vibration frequency. It was synthesized waveform, robot controller and performs vibration suppression control using the two synthetic waveform.   The amplitude information is transmitted based on the phase information based on the amplitude and phase information obtained by the frequency analysis of the sensor signal, and the robot controller performs the frequency analysis based on the arrival time of the transmitted amplitude information. 6. The robot control apparatus according to claim 5, wherein a signal generator is configured to calculate a phase obtained and synthesize a vibration waveform from the amplitude, the calculated phase, and a vibration frequency inside the controller.   It has a phase compensator with a built-in offline measurement result of transmission delay time corresponding to each frequency, and generates signal for transmission delay time at S-axis vibration frequency ωS and L-axis vibration frequency ωL obtained by vibration frequency calculator 7. The robot control apparatus according to claim 5, wherein a signal generator configured to compensate for transmission delay time generated by the wireless sensor module and the two wireless circuits is configured.

Images