JP5353581B2 - robot - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a robot which can be controlled with high accuracy while eliminating the effect of bias drift of an angular velocity sensor and the effect of noise resulting from disturbance or the like. <P>SOLUTION: The robot includes: a link rotatable with respect to a base; an actuator for driving the link; a torque transmission mechanism for transmitting the torque of the actuator to the link at a prescribed reduction ratio; an angle sensor for detecting a rotation angle of the actuator; the angular velocity sensor for detecting an angular velocity of the link with respect to the base; and an arithmetic unit for calculating the angle of the link by using low-frequency components equal to or smaller than a cut-off frequency among the products of the prescribed reduction ratio and rotation angles, high-frequency components equal to or greater than the cut-off frequency among the values of integrals of angular velocities, and a filter having a specific frequency characteristic in the high-frequency components. <P>COPYRIGHT: (C)2011,JPO&amp;INPIT

Description

  The present invention relates to a robot.

  A robot having a multi-link structure includes an actuator (for example, an electric motor) that drives each link and an angle sensor (for example, an encoder) that detects a rotation angle of each actuator. Conventionally, a technique has been used in which the rotation angle of each link is indirectly controlled by controlling the rotation angle of an actuator. However, the torque transmission mechanism that transmits the torque of the actuator to the link generally has a spring characteristic rather than a rigid body. Therefore, in a robot having a multi-link mechanism, there is a problem that actuators and links have mechanical resonance characteristics and vibrate with each other.

Various techniques for solving such problems have been developed. For example, in Patent Document 1, a low-pass filter that removes high-frequency components higher than the second-order mode and passes the first-mode frequency components and DC components is used. Used. That is, the acceleration signal in the secondary mode or higher is cut by the low-pass filter, and the arm is vibrated by suppressing the vibration in the secondary mode or higher and exciting the arm.
Further, in Patent Document 2, an inertia sensor is provided in the vicinity of a workpiece gripping portion (chuck portion) to detect the operation (swing state) of the chuck portion. A command signal is input to an inertial sensor and a control circuit connected to a driving body serving as a joint of each arm, thereby controlling the driving of each arm and controlling vibration of the chuck portion.

  In these Patent Documents 1 and 2, it is possible to prevent the actuator and the link from vibrating with each other, but there is a problem that a steady deviation (offset) such as a speed deviation and a position deviation occurs due to the influence of the bias drift of the inertial sensor. On the other hand, in Patent Document 3, a low frequency equal to or lower than the cutoff frequency among products of a high frequency component equal to or higher than the cutoff frequency in the integral value of the angular velocity and a rotation angle of the actuator and a predetermined reduction ratio of the torque transmission mechanism. By using the frequency component, the influence of the bias drift of the angular velocity sensor is eliminated.

JP-A-1-173116 Japanese Patent Laid-Open No. 7-9374 JP 2005-242794 A

  However, Patent Documents 1 to 3 do not consider the influence of noise due to other disturbances other than the influence of the bias drift of the inertial sensor. For this reason, a steady-state deviation (offset) occurs due to the influence of noise due to disturbance or the like, and high-precision control cannot be performed.

  The present invention has been made in view of such circumstances, and a robot capable of performing high-precision control by removing the influence of bias drift of the angular velocity sensor and removing the influence of noise due to disturbance or the like. The purpose is to provide.

  In order to solve the above-described problems, a robot according to the present invention includes a link that can rotate with respect to a base, an actuator that drives the link, and torque transmission that transmits torque of the actuator to the link at a predetermined reduction ratio. A low frequency component equal to or lower than a cut-off frequency among products of a mechanism, an angle sensor that detects a rotation angle of the actuator, an angular velocity sensor that detects an angular velocity of the link with respect to the base, and a predetermined reduction ratio and the rotation angle And an arithmetic unit that uses a high frequency component equal to or higher than the cut-off frequency in the integral value of the angular velocity and calculates a link angle using a filter having a specific frequency characteristic in the high frequency component. Features.

  According to the robot of the present invention, a low frequency component equal to or lower than the cutoff frequency among products of a high frequency component equal to or higher than the cutoff frequency in the integral value of the angular velocity and a rotation angle of the actuator and a predetermined reduction ratio of the torque transmission mechanism. Therefore, the influence of the bias drift of the angular velocity sensor can be eliminated. In addition, since the filter having a specific frequency characteristic is used in the high frequency component equal to or higher than the cutoff frequency in the integrated value of the angular velocity, the influence of noise due to disturbance or the like can be removed. Therefore, it is possible to provide a robot capable of removing the influence of the bias drift of the angular velocity sensor and removing the influence of noise due to disturbance and the like and performing highly accurate control.

  Further, in this robot, the filter has a characteristic of attenuating a frequency higher than at least the lowest natural frequency among the natural frequencies of the system including the link, the actuator, and the torque transmission mechanism in the high frequency component. You may have.

  According to this configuration, a frequency higher than at least the lowest natural frequency among the natural frequencies of the system including the link, the actuator, and the torque transmission mechanism is attenuated. In addition, it is possible to remove frequency components having a large influence of noise due to disturbances and the like that are not necessary for control. Therefore, highly accurate control with little influence of noise can be performed.

  In this robot, the filter may have a characteristic that a gain of a natural frequency of at least a frequency to be measured among the natural frequencies of the system in the high frequency component is relatively large.

  According to this configuration, since the gain of the natural frequency to be measured is relatively large among the natural frequencies of the system composed of the link, the actuator, and the torque transmission mechanism, the effect of increasing the sensitivity of the sensor in the portion to be measured of the system is effective. Yes, it is possible to perform measurement while emphasizing the vibration of the link and actuator. Therefore, by performing control based on the vibration data measured with emphasis, the vibration of the link and the actuator can be reliably suppressed.

  Further, in this robot, the filter may have a characteristic in which a gain of at least a disturbance noise having a frequency with the highest energy intensity among electric disturbance noises in the high frequency component is relatively small.

  According to this configuration, the gain of disturbance noise having a frequency with the highest energy intensity among electrical disturbance noises is relatively small. For this reason, since the sensitivity of the sensor is lowered in a portion that is not desired to be detected by the sensor, it is possible to reliably remove signals that are not necessary for control such as disturbance noise. Therefore, it is possible to perform control with extremely high accuracy.

  Further, in this robot, even if the filter has a relatively small gain of a physical vibration at a frequency having the highest energy intensity among the physical vibrations from the outside in the high frequency component. Good.

  According to this configuration, the gain of the physical vibration of the frequency having the largest energy intensity among the physical vibrations from the outside becomes relatively small. For this reason, there is an effect that the sensitivity of the sensor is lowered in a portion that is not desired to be detected by the sensor, so that it is possible to reliably remove signals that are not necessary for control such as physical vibration. Therefore, it is possible to perform control with extremely high accuracy.

  In this robot, the filter has a characteristic in which the gain of the natural frequency of the frequency having the highest energy intensity among the natural frequencies of the robot casing or the installation base in the high frequency component is relatively small. May be.

  According to this configuration, the gain of the natural frequency of the frequency having the highest energy intensity among the natural frequencies of the robot casing or the installation base becomes relatively small. For this reason, there is an effect of reducing the sensitivity of the sensor in a portion that is not desired to be detected by the sensor, so that it is possible to surely remove a signal that is not necessary for controlling the natural frequency of the housing or the installation table. Therefore, it is possible to perform control with extremely high accuracy.

  Further, in this robot, a vibration gyro is used for the angular velocity sensor, and the filter has a driving frequency, a detection frequency, a detuning frequency, or an element natural frequency of the vibration gyro in the high frequency component. Of these, at least the drive frequency, the detection frequency, the detuning frequency, or the natural frequency gain of the element having the highest energy intensity may have a relatively small characteristic.

  According to this configuration, when the vibration type gyro is used for the angular velocity sensor, at least the drive frequency and detection frequency of the vibration type gyro having the highest energy intensity among the drive frequency, detection frequency, detuning frequency, and element natural frequency. The gain of the detuning frequency or the natural frequency of the element becomes relatively small. For this reason, there is an effect that the sensitivity of the sensor is lowered in the part that the sensor does not want to detect. It can be reliably removed. Therefore, it is possible to perform control with extremely high accuracy.

It is a figure which shows typically the robot which concerns on this invention. It is a flowchart figure which shows the process which calculates the angle of a link. It is a graph which shows the frequency characteristic of the filter which concerns on 1st Embodiment of this invention. It is a graph which shows the frequency characteristic of the filter which concerns on 2nd Embodiment of this invention. It is a graph which shows the frequency characteristic of the filter which concerns on 3rd Embodiment of this invention. It is a graph which shows the frequency characteristic of the filter following FIG. It is a flowchart figure which shows the modification of the process which calculates the angle of a link.

  Embodiments of the present invention will be described below with reference to the drawings. This embodiment shows one aspect of the present invention, and does not limit the present invention, and can be arbitrarily changed within the scope of the technical idea of the present invention. Moreover, in the following drawings, in order to make each structure easy to understand, an actual structure and a scale, a number, and the like in each structure are different.

(First embodiment)
FIG. 1 is a schematic cross-sectional view of a robot 100 provided with angular velocity sensors 12 and 13 according to the first embodiment of the present invention. As shown in FIG. 1, the robot 100 includes a gantry (base) 1, links 3 and 8, motors (actuators) 4 and 9, speed reducers (torque transmission mechanisms) 5 and 11, and angle sensors 6 and 10. And angular velocity sensors 12 and 13 and a calculation unit 15.

  One end of the link 3 is attached to the upper part of the gantry 1, and one end of the link 8 is attached to the other end of the link 3. The link 3 is rotatable relative to the gantry 1 about the shaft 2. The link 8 is rotatable relative to the link 3 about the shaft 7.

  The motor 4 drives the link 3 via the speed reducer 5. The motor 9 drives the link 8 via the speed reducer 11. Here, a predetermined reduction ratio in the reduction gear 5 is N1, and a predetermined reduction ratio in the reduction gear 11 is N2.

  The angle sensor 6 is provided on the top of the motor 4. The angle sensor 6 has a function of detecting the rotation angle Θm1 of the motor 4 around the axis 2. The angle sensor 10 is provided on the top of the motor 9. The angle sensor 10 has a function of detecting the rotation angle Θm2 of the motor 9 around the axis 7.

  The angular velocity sensor 12 is provided at the other end of the link 3. The angular velocity sensor 12 has a function of detecting the angular velocity w1 of the link 3 with respect to the gantry 1. The angular velocity sensor 13 is provided at the other end of the link 8. The angular velocity sensor 13 has a function of detecting the angular velocity w2 of the link 8 with respect to the gantry 1. The angular velocity w1 of the link 3 with respect to the gantry 1 is an angular velocity component in the rotation plane of the link 3. The angular velocity w2 of the link 8 with respect to the gantry 1 is an angular velocity component in the rotation plane of the link 8.

  The calculation unit 15 is provided in the vicinity of the gantry 1. The arithmetic unit 15 calculates the low frequency component below the cut-off frequency and the angular velocity w1, (w2−) of the products Θm1, N1, Θm2, and N2 of the rotation angles Θm1 and Θm2 of the motors 4 and 9 and the predetermined reduction ratios N1 and N2. It has a function of calculating the link angles ΘA1 and ΘA2 using a high frequency component equal to or higher than the cut-off frequency in the integral value of w1) and using a filter having a specific frequency characteristic in the high frequency component.

  In this embodiment, two links, a motor, a speed reducer, an angle sensor, and an angular velocity sensor are provided, but the present invention is not limited to this. For example, one link, one motor, one speed reducer, one angle sensor, and one angular velocity sensor may be provided, or any number such as three or more may be provided.

  Moreover, although the angular velocity sensors 12 and 13 are arrange | positioned at the front-end | tip of the links 3 and 8, respectively, it is not restricted to this. For example, the angular velocity sensors 12 and 13 may be arranged at arbitrary positions on the links 3 and 8, respectively. Moreover, although the calculating part 15 is arrange | positioned in the vicinity of the mount frame 1, it is not restricted to this. For example, the calculation unit 15 may be arranged at an arbitrary position such as the outside of the robot 100.

  FIG. 2 is a flowchart showing a process of calculating the angle ΘA1 of the link 3 and the angle ΘA2 of the link 8. Arithmetic processes other than steps S1 and S6 may be executed by the arithmetic unit 15 arranged in the robot 100, or may be executed by the arithmetic unit 15 provided outside the robot 100.

  First, the angular velocity w1 of the link 3 with respect to the gantry 1 is detected by the angular velocity sensor 12. Further, the angular velocity w2 of the link 8 with respect to the gantry 1 is detected by the angular velocity sensor 13 (S1).

  Next, the calculation unit 15 calculates the single angular velocity dΘA1 / dt of the link 3. Further, the calculation unit 15 calculates the single angular velocity dΘA2 / dt of the link 8. Since the angular velocities w1 and w2 are angular velocities of the links 3 and 8 with respect to the gantry 1, respectively, the angular velocities dΘA1 / dt and dΘA2 / dt can be expressed as the following formula (1).

  dΘA1 / dt = w1, dΘA2 / dt = w2-w1 (1)

  Next, the angular velocities dΘA1 / dt and dΘA2 / dt of the link are integrated by the calculation unit 15 (S3).

  Next, these integration results are passed through the high-pass filter FH and through a filter having a specific frequency component (S4). As a result, a high frequency component of the cut-off frequency 1 / Tf or higher is obtained from the link angles ΘA1 and ΘA2.

  Here, the reason for removing the low frequency component of the link angles ΘA1 and ΘA2 is that the link angles ΘA1 and ΘA2 are accurately obtained by the influence of bias drift in the integration of the link angular velocities dΘA1 / dt and dΘA2 / dt. This is because the influence of this bias drift is eliminated. The reason why the link angles ΘA1 and ΘA2 are passed through a filter having a specific frequency component is that the link angles ΘA1 and ΘA2 are affected by noise caused by disturbances or the like in the integration of the link angular velocities dΘA1 / dt and dΘA2 / dt. This is because the influence of noise due to this disturbance or the like is removed because it cannot be obtained accurately.

  On the other hand, when the robot 100 is stopped or operating in the extremely low frequency band, the speed reducers 5 and 11 are hardly twisted, and the following expression (2) is established.

  ΘA1 = Θm1 · N1, ΘA2 = Θm2 · N2 (2)

  Therefore, the motor angle Θm1 is detected by the angle sensor 6. Further, the motor angle Θm2 is detected by the angle sensor 10 (S6).

  Next, the calculation unit 15 multiplies the motor angle Θm1 by a predetermined reduction ratio N1 (N1 <1) to calculate Θm1 · N1. Further, the calculation unit 15 multiplies the motor angle Θm2 by a predetermined reduction ratio N2 (N2 <1) to calculate Θm2 · N2 (S7).

  Next, these calculation results are passed through the low-pass filter FL (S8). As a result, a low frequency component having a cut-off frequency of 1 / Tf or less is obtained from the link angles ΘA1 and ΘA2. Here, the reason why the high-frequency components of the link angles ΘA1 and ΘA2 are removed is because the reduction gears 5 and 11 are twisted at the high-frequency components, so that equation (2) is not established.

  When the filter coefficient of the high-pass filter FH is HF and the filter coefficient of the low-pass filter FL is LF, the following equation (3) is established.

  HF = 1-LF (3)

  Thus, by using the high-pass filter FH and the low-pass filter FL having the relationship of Expression (3), the high-frequency components of the link angles ΘA1 and ΘA2 obtained in steps S1 to S4 and the steps S6 to S8 are obtained. The low-frequency components of the link angles ΘA1 and ΘA2 thus added can be added (S10). By this calculation, the angle ΘA1 of the link 3 with respect to the gantry 1 and the angle ΘA2 of the link 8 with respect to the link 3 can be calculated.

Further, the angular accelerations d ² ΘA1 / dt ² and d ² ΘA2 / dt ² of the links 3 and 8 can be calculated by approximating the angular velocities dΘA1 / dt and dΘA2 / dt obtained in step S2 once. Yes (S5). Further, the motor angular velocities dΘm1 / dt and dΘm2 / dt of the motors 4 and 10 can be calculated by approximating the motor angles Θm1 and Θm2 obtained in step S6 once (S9).

Thus, the motor angles Θm1 and Θm2, the motor angular velocities dΘm1 / dt and dΘm2 / dt, the link angles ΘA1 and ΘA2, the link angular velocities dΘA1 / dt and dΘA2 / dt, and the link angle The accelerations d ² ΘA1 / dt ² and d ² ΘA2 / dt ² can be determined. That is, since it is not necessary to perform two-time integration or two-time differentiation that is easily affected by bias drift, accurate control can be performed.

  FIG. 3 is a graph showing the frequency characteristics of the filter according to the present embodiment. FIG. 3A shows a response characteristic of a system including a link, a motor, and a speed reducer, and FIG. 3B shows a filter characteristic according to the present embodiment. FIG. 3 is a graph with frequency on the horizontal axis and gain on the vertical axis. In FIG. 3, reference numeral f0 is a measurement target portion in a natural frequency band of a system including a link, a motor, and a speed reducer, reference numeral f1 is a first cutoff frequency, and reference numeral f2 is a second cutoff frequency. Note that the first cutoff frequency f1 and the second cutoff frequency f2 have a relationship of f1 <f2.

  As shown in FIG. 3A, the natural frequency of the system including the link, the motor, and the speed reducer is generated at three locations. As shown in FIG. 3B, the filter according to this embodiment has a characteristic of attenuating a frequency higher than at least the lowest natural frequency among the natural frequencies of the system generated in three places. As a result, it is possible to remove frequency components having a large influence of noise due to disturbances and the like that are not necessary for control while measuring the lowest natural frequency that contributes greatly to vibration.

  In addition, the band between the first cutoff frequency f1 and the second cutoff frequency f2 is a natural frequency band of the system from which the influence of noise due to disturbances and the like that are not necessary for control is removed. The measurement target part f0 is included in a band between the first cutoff frequency f1 and the second cutoff frequency f2. For this reason, a signal in the natural frequency band of the system necessary for control can be taken between the first cutoff frequency f1 and the second cutoff frequency f2.

  According to the robot 100 of the present invention, a high frequency component having a cut-off frequency of 1 / Tf or higher among integral values of the angular velocities dΘA1 / dt and dΘA2 / dt of the link, a rotation angle of the motor, and a predetermined reduction gear ratio of the reduction gear. Of the products Θm1 · N1 and Θm2 · N2 and a low frequency component having a cut-off frequency of 1 / Tf or less, the influence of the bias drift of the angular velocity sensors 12 and 13 can be eliminated. In addition, since a filter having a specific frequency characteristic is used in a high frequency component having a cut-off frequency of 1 / Tf or higher among the integral values of the angular velocities dΘA1 / dt and dΘA2 / dt of the link, the influence of noise due to disturbance or the like is eliminated. can do. Therefore, it is possible to provide the robot 100 that can remove the influence of the bias drift of the angular velocity sensors 12 and 13 and remove the influence of noise due to disturbance or the like, and perform highly accurate control.

  In addition, according to this configuration, a frequency higher than at least the lowest natural frequency among the natural frequencies of the system including the link, the motor, and the speed reducer is attenuated. While taking a signal, it is possible to remove a frequency component having a large influence of noise due to a disturbance or the like which is not necessary for the control. Therefore, highly accurate control with little influence of noise can be performed.

(Second Embodiment)
FIG. 4 is a graph showing frequency characteristics of the filter according to the second embodiment of the present invention. FIG. 4 is a graph showing filter characteristics in the second embodiment corresponding to FIG. As shown in FIG. 4, the filter of this embodiment is different from the filter described in the first embodiment in that it has a characteristic of increasing the gain in a specific band. Since the other points are the same as in the first embodiment, the same elements as those in FIG. 3B are denoted by the same reference numerals, and detailed description thereof is omitted.

  As shown in FIG. 4, the filter according to the present embodiment has a characteristic that the gain of the natural frequency of at least the frequency to be measured among the natural frequencies of the system is relatively large in the high frequency component equal to or higher than the first cutoff frequency f1. Have In other words, the filter has a relatively large gain in a specific band in a high frequency component equal to or higher than the first cut-off frequency f1, and the measurement target part f0 is included in the band in which the gain is relatively large. Yes. Note that increasing the gain in a specific band is equivalent to increasing the sensitivity of the sensor in the specific band.

  According to the configuration of the present embodiment, since the gain of at least the natural frequency to be measured among the natural frequencies of the system is relatively large, there is an effect of increasing the sensitivity of the sensor in the portion to be measured in the system. 8 and motors 4 and 10 can be emphasized to perform measurement. Therefore, the vibrations of the links 3 and 8 and the motors 4 and 10 can be reliably suppressed by performing the control based on the vibration data measured with emphasis.

(Third embodiment)
5 and 6 are graphs showing the frequency characteristics of the filter according to the third embodiment of the present invention. FIG. 5A shows the energy intensity of noise, and FIG. 5B shows the filter characteristics according to the present embodiment. FIGS. 5B and 6 are graphs showing filter characteristics in the third embodiment corresponding to FIG. As shown in FIGS. 5 and 6, the filter of this embodiment is different from the filter described in the first embodiment in that it has a characteristic of reducing the gain in a specific band. Since the other points are the same as in the first embodiment, the same elements as those in FIG. 3B are denoted by the same reference numerals, and detailed description thereof is omitted.

  As shown in FIGS. 5B and 6, the filter according to the present embodiment is a disturbance noise having a frequency with at least the highest energy intensity among the electrical disturbance noises in the high frequency component of the first cutoff frequency f1 or higher. Have a relatively small gain. In the present embodiment, the filter has a relatively small gain in the band of the second cut-off frequency f2, and disturbance noise is included in the band in which the gain is relatively small. Note that reducing the gain in a specific band is equivalent to reducing the sensitivity of the sensor in the specific band.

  That is, as shown in FIG. 5, the electrical disturbance noise is included in the vicinity of the band of the second cutoff frequency f2 in which the gain is relatively reduced by the filter of the present embodiment. As a result, electrical disturbance noise can be brought to a band where the gain is relatively small.

  According to the configuration of the present embodiment, the gain of disturbance noise having a frequency having the highest energy intensity among electrical disturbance noises is relatively small. For this reason, since the sensitivity of the sensor is lowered in a portion that is not desired to be detected by the sensor, it is possible to reliably remove signals that are not necessary for control such as disturbance noise. Therefore, it is possible to perform control with extremely high accuracy.

  In the present embodiment, an example in which the gain of the electrical disturbance noise of the filter is relatively small has been described. However, the present invention is not limited to this. For example, in place of electrical disturbance noise, the present invention can also be applied to a signal that is not desired to be detected by a sensor, such as physical vibration from the outside, the natural frequency of a robot casing or installation base, and the like. In addition, when using a vibrating gyroscope for the angular velocity sensor, it can also be applied to signals that you do not want to detect with the sensor, such as the driving frequency, detection frequency, detuning frequency, or element natural frequency of the vibrating gyroscope. is there.

(Modification)
FIG. 7 is a flowchart showing a modification of the process of calculating the link angle. FIG. 7 is a flowchart showing a modification of the link angle calculation process corresponding to FIG. As shown in FIG. 7, the calculation process of the link angle of the present modification includes the high frequency component of the link angular velocity dΘA1 / dt and dΘA2 / dt integrated value of the cutoff frequency 1 / Tf or more, and the rotation of the motor. A filter having a specific frequency characteristic is used after using a low-frequency component having a cutoff frequency of 1 / Tf or less of the products Θm1 · N1 and Θm2 · N2 of the angle and a predetermined reduction gear ratio of the reducer. This is different from the link angle calculation process described in the first embodiment. Since the other points are the same as in the first embodiment, the same elements as those in FIG. 2 are denoted by the same reference numerals, and detailed description thereof is omitted.

  In step S4 of this modification, in order to eliminate bias drift, the calculation unit 15 passes the integration results of the link angular velocities dΘA1 / dt and dΘA2 / dt to the high-pass filter FH. As a result, a high frequency component of the cut-off frequency 1 / Tf or higher is obtained from the link angles ΘA1 and ΘA2. The high frequency components of the link angles ΘA1 and ΘA2 obtained in steps S1 to S4 and the low frequency components of the link angles ΘA1 and ΘA2 obtained in steps S6 to S8 are added, and the influence of noise due to disturbance or the like is added. In order to eliminate it, it is passed through a filter having a specific frequency characteristic (S10). By this calculation, the angle ΘA1 of the link 3 with respect to the gantry 1 and the angle ΘA2 of the link 8 with respect to the link 3 are calculated (S11).

  Thus, by using the high-pass filter FH and the low-pass filter FL, the high-frequency components of the link angles ΘA1 and ΘA2 obtained in steps S1 to S4 and the link angles ΘA1 and ΘA2 obtained in steps S6 to S8 are used. Even when a filter having a specific frequency characteristic is used after adding the low frequency components, the influence of bias drift of the angular velocity sensors 12 and 13 is removed and the influence of noise due to disturbance or the like is removed. In addition, highly accurate control can be performed.

DESCRIPTION OF SYMBOLS 1 ... Base (base | substrate) 3,8 ... Link, 4,9 ... Motor (actuator) 5,11 ... Reduction gear (torque transmission mechanism), 6,10 ... Angle sensor, 12, 13 ... Angular velocity sensor, 100 ... Robot, w1, w2 ... Link angular velocity, ΘA1, ΘA2 ... Link angle, Θm1, Θm2 ... Motor rotation angle, 1 / Tf ... Cutoff frequency, N1, N2 ... Reduction ratio

Claims (7)

A link capable of rotating relative to the substrate;
An actuator for driving the link;
A torque transmission mechanism for transmitting the torque of the actuator to the link at a predetermined reduction ratio;
An angle sensor for detecting a rotation angle of the actuator;
An angular velocity sensor for detecting an angular velocity of the link with respect to the substrate;
Of the product of the predetermined reduction ratio and the rotation angle, a low frequency component equal to or lower than a cut-off frequency and a high frequency component equal to or higher than the cut-off frequency among integrated values of the angular velocities and a specific frequency in the high frequency component are used. And a calculation unit that calculates an angle of the link using a filter having characteristics.   The filter has a characteristic of attenuating a frequency higher than at least the lowest natural frequency among the natural frequencies of the system including the link, the actuator, and the torque transmission mechanism in the high frequency component. Item 2. The robot according to Item 1.   The robot according to claim 2, wherein the filter has a characteristic that a gain of a natural frequency of at least a frequency to be measured among the natural frequencies of the system in the high frequency component is relatively large.   4. The filter according to any one of claims 1 to 3, wherein the filter has a characteristic in which a gain of a disturbance noise having a frequency having the highest energy intensity among electric disturbance noises in the high frequency component is relatively small. The robot described in 1.   5. The filter according to claim 1, wherein the filter has a characteristic in which a gain of a physical vibration having a frequency having the largest energy intensity among physical vibrations from outside in the high frequency component is relatively small. The robot according to any one of claims.   The filter according to any one of claims 1 to 5, wherein the high frequency component has a characteristic in which a gain of a natural frequency having the highest energy intensity is relatively small among the natural frequencies of the robot casing or installation base. The robot according to any one of claims.   The vibration gyro is used for the angular velocity sensor, and the filter has at least the highest energy intensity among the driving frequency, the detection frequency, the detuning frequency, or the element natural frequency of the vibration gyro in the high frequency component. The robot according to any one of claims 1 to 6, wherein the drive frequency, the detection frequency, the detuning frequency, or the natural frequency gain of the element has a relatively small characteristic.

Images