JP2013061834A - Robot device and control method of the same - Google Patents

Abstract

[PROBLEMS] A conventional robot apparatus control method has a problem in that the amount of calculation in the control apparatus increases and processing takes time, and the cost of the control apparatus increases in order to increase the processing speed.
A first calculation unit for calculating an angular velocity of an arm operated by an actuator including the angle sensor from rotation angle detection data of the angle sensor, and the base body connection device and the arm connection from angular velocity detection data of an inertial sensor. A second calculator that calculates an angular velocity of the arm that is operated by the actuator with the device as an axis; and the angular velocity of the arm that is operated by the actuator and the angular velocity detection data of the inertia sensor that is calculated by the angular velocity detection data of the arm. A robot apparatus comprising: a third calculation unit that extracts a frequency component of vibration for each arm from a difference in angular velocity and calculates a torsional angular velocity between the actuator and the arm.
[Selection] Figure 1

Description

  The present invention relates to a robot apparatus and a method for controlling the robot apparatus.

  A robot apparatus having a multi-joint structure, which is often used as a part of an IC handler or an assembly apparatus, has been widely used in various industrial sites. Therefore, it has become an important performance specification and quality for a robot apparatus to be able to move an arm quickly and accurately to a required position more than ever.

  In general, in order to move the arm at high speed and accurately, it is also necessary to reduce the inertial force applied to the arm and not to increase the load of the driving actuator. In order to reduce the inertial force applied to the arm, weight reduction of the arm is used as the most effective method. However, reducing the weight of the arm causes a decrease in arm rigidity, making it difficult to suppress arm vibration that occurs when the arm is stopped, and it is determined that the arm tip has been stopped at the target position based on the control signal. However, in practice, there is a positional shift corresponding to the amplitude of the vibration of the arm itself, and there is a problem that the next operation cannot be started until the vibration is attenuated.

  For this problem, an acceleration sensor is installed at the tip of the arm and the arm is operated based on the acceleration signal to suppress vibration (for example, Patent Document 1), an angular velocity sensor is installed at the tip of the arm and the arm, and the angular velocity signal is output. A method (for example, Patent Document 2) for controlling the arm operation based on the above is proposed.

JP-A-1-173116 JP 2005-242794 A

  However, in the conventional control method of the robot apparatus, the control signal for suppressing the vibration is generated by using either the angular velocity sensor or the acceleration sensor, so that an error such as bias drift is included in the sensor signal. In some cases, an error occurs in the control signal, and accurate control cannot be performed.

  For example, in Patent Document 2, two types of filters, a low-pass filter that removes high-frequency components of an angle sensor and a high-pass filter that removes low-frequency components of an angular velocity sensor, are used to reduce the influence of sensor errors. For this reason, there is a problem in that the amount of calculation in the control device increases, processing takes time, and the cost of the calculator increases to increase the processing speed.

  Therefore, an inertial sensor called a gyro sensor is placed on the part that you want to stop accurately, for example, the part equipped with a robot hand that holds the workpiece and performs the specified work, and occurs when the arm stops. A robot apparatus and a control method for controlling the driving means for driving the arm so as to suppress the arm vibration and stopping the arm at an accurate position in a short time are realized.

  The present invention can be realized as the following forms or application examples so as to solve at least one of the above-described problems.

  [Application Example 1] In the robot apparatus of this application example, an actuator, an arm connecting device including an angle sensor for detecting a rotation angle of the actuator, and a plurality of arms are connected in series and rotatably by the arm connecting device. A base body in which the arm body is rotatably connected by a base body connecting device including an arm body, an actuator provided at one end of the arm body, and an angle sensor for detecting a rotation angle of the actuator; Among the arms, an inertial sensor including at least an angular velocity sensor provided in a work arm to which a work holding means is attached is operated by the actuator including the angle sensor based on rotation angle detection data of the angle sensor. A first calculation unit for calculating an angular velocity of the arm; A second calculation unit for calculating an angular velocity of the arm operated by the actuator having the base connecting device and the arm connecting device as axes based on the angular velocity detection data of the sir; and the arm operated by the actuator in the arm A frequency component of vibration is extracted for each arm based on the difference between the angular velocity of the arm calculated from the angular velocity and the angular velocity detection data of the inertial sensor, and the actuator and the arm connected to the actuator are extracted. And a third calculation unit for calculating a torsional angular velocity therebetween.

  According to the robot apparatus of this application example, accurate control is possible by using the torsional angular velocity as data that is the basis of vibration suppression control of the robot apparatus.

  Application Example 2 In the application example described above, the frequency component is extracted by a band-pass filter including an anti-resonance frequency and a resonance frequency among the mechanical system natural frequencies of the robot apparatus.

  According to the above application example, according to the above application example, by cutting the low frequency component without cutting the anti-resonance frequency and the resonance frequency, even when using an inertial sensor including an error, A torsional angular velocity close to a true value obtained by an inertial sensor that does not include an error in a region with a large amplitude can be obtained, and a robot apparatus that accurately controls vibration by the torsional angular velocity can be obtained.

  Application Example 3 In the application example described above, the fourth calculation unit further adds the angular velocity of the arm calculated by the first calculation unit and the torsional angular velocity calculated by the third calculation unit. It is characterized by providing.

  According to the application example described above, the actual operation is not included in the low frequency component of the torsional angular velocity. Accordingly, the removal of the low frequency component of the torsional angular velocity is the removal of the error (noise) of the inertial sensor, and the torsional angular velocity from which the low frequency component has been removed or the angular velocity of the actuator to the torsional angular velocity from which the low frequency component has been removed. By using the arm angular velocity obtained by adding as control data, it is possible to perform accurate vibration suppression control of the robot apparatus.

  [Application Example 4] A control method of a robot apparatus according to this application example includes an actuator, an arm coupling device including an angle sensor for detecting a rotation angle of the actuator, and a plurality of arms that can be rotated in series by the arm coupling device. A base body in which the arm body is rotatably connected by a base body connecting device including an arm body connected to the arm body, an actuator provided at one end of the arm body, and an angle sensor for detecting a rotation angle of the actuator. And an inertial sensor including at least an angular velocity sensor provided in a work arm to which a work holding means is attached among the plurality of arms, and a rotation angle of the angle sensor. From the detected data, before operating by the actuator with the angle sensor A first calculation step of calculating an angular velocity of the arm, and a second calculation step of calculating an angular velocity of the arm operated by the actuator having the base connecting device and the arm connecting device as an axis, from angular velocity detection data of the inertial sensor. And a frequency component of vibration is extracted for each arm from a difference between the angular velocity of the arm operated by the actuator in the arm and the angular velocity detection data of the arm calculated from the angular velocity detection data of the inertial sensor. And a third calculation step of calculating a torsional angular velocity between the actuator and the arm connected to the actuator.

  According to the application example described above, it is possible to accurately control the torsional angular velocity by using the data as the basis of vibration suppression control of the robot apparatus.

  Application Example 5 In the application example described above, the frequency component is extracted by a band-pass filter including an anti-resonance frequency and a resonance frequency among mechanical natural frequencies of the robot apparatus.

  According to the application example described above, the low-frequency component is cut without cutting the anti-resonance frequency and the resonance frequency, so that even if an inertial sensor including an error is used, an error is included in a region with a large amplitude. It is possible to obtain a torsional angular velocity close to a true value obtained by an inertial sensor, and to accurately control the vibration of the robot apparatus due to the torsional angular velocity.

  Application Example 6 In the above application example, the angular velocity of the arm is calculated by adding the angular velocity of the arm calculated in the first calculation step and the torsional angular velocity calculated in the third calculation step. The fourth operation step is further provided.

  According to the application example described above, the actual operation is not included in the low frequency component of the torsional angular velocity. Accordingly, the removal of the low frequency component of the torsional angular velocity is the removal of the error (noise) of the inertial sensor, and the torsional angular velocity from which the low frequency component has been removed or the angular velocity of the actuator to the torsional angular velocity from which the low frequency component has been removed. By using the arm angular velocity obtained by adding as the control data, vibration of the robot apparatus can be accurately controlled and suppressed.

The outline | summary of the robot apparatus which concerns on 1st Embodiment is shown, (a) is a typical top view, (b) is typical sectional drawing. FIG. 3 is a control block diagram of the robot apparatus according to the first embodiment. The block diagram which showed the robot apparatus shown in FIG. 1 with the connection apparatus represented with the characteristic model. FIG. 3A is a Bode diagram showing a frequency response characteristic of a first arm and (b) a frequency response characteristic of a second arm of the robot apparatus according to the first embodiment. FIG. 6A is a characteristic diagram of a bandpass filter in which (a) extracts the angular velocity component of the first arm and (b) extracts the angular velocity component of the second arm of the robot apparatus according to the first embodiment. The Bode diagram of the angular velocity response of an actuator, an arm, and a twist to the torque of the actuator of the robot apparatus concerning a 1st embodiment. The graph in the case of the inertial sensor which does not have an error, (b) shows the inertial sensor which has a low frequency error, before and after the removal of the low frequency component of the twist angular velocity frequency component at the time of operation | movement which concerns on 1st Embodiment. The flowchart which shows the control method of the robot apparatus which concerns on 2nd Embodiment. FIG. 6 is a control block diagram illustrating a second embodiment.

  Embodiments according to the present invention will be described below with reference to the drawings.

(First embodiment)
1A and 1B show a robot apparatus according to a first embodiment, wherein FIG. 1A is a schematic plan view, and FIG. 1B is a schematic cross-sectional view. The robot apparatus according to this embodiment is a so-called two-axis horizontal articulated robot 100 (hereinafter referred to as a robot apparatus 100) in which two arms are connected so as to be rotatable in the horizontal direction.

  The robot apparatus 100 includes an arm body 10 configured by a first arm 11 and a second arm 12 being rotatably connected by an arm connecting device 20. The arm body 10 is rotatably connected to the base body 40 fixed to the base by the base body connecting device 30 to constitute the robot apparatus 100.

  The arm coupling device 20 includes an actuator 51 and a torque transmission device 61 that transmits the torque of the actuator 51 at a predetermined reduction ratio. Moreover, the base | substrate connection apparatus 30 contains the torque transmission apparatus 62 which transmits the actuator 52 and the torque of the actuator 52 with a predetermined reduction ratio. At the end of the second arm 12 which is the end opposite to the base body 40 of the arm body 10, a work holding device 70 is provided as a work holding means for holding a processing tool or a work.

  The actuator 51 included in the arm coupling device 20 is provided with an angle sensor 81 that detects a rotation angle. Further, the actuator 52 is also provided with an angle sensor 82 in the base body connection device 30. Further, an inertia sensor 90 is provided at a position corresponding to a position where the work holding device 70 of the second arm 12 is provided. The inertial sensor 90 includes at least an angular velocity sensor, and can detect the angular velocity at the mounting position of the inertial sensor 90, that is, the position where the work holding device 70 is disposed.

  FIG. 2 is a control block diagram illustrating a configuration of the robot apparatus 100 according to the present embodiment. The CPU 200 includes a first calculation unit 510, a second calculation unit 520, a third calculation unit 530, a fourth calculation unit 540, and a control unit 600, which will be described later, and reads and executes a program stored in the ROM 300. The RAM 400 stores data obtained by executing the program in the CPU 200, and sends necessary data from the stored data to the CPU 200.

  The rotation angle data of the actuators 51 and 52 detected by the angle sensors 81 and 82 provided in the robot apparatus 100 are converted into the rotation angle θ1 of the actuator 51 and the rotation angle θ2 of the actuator 52 by the first calculation unit 510 and converted. Each of the rotation angles θ1 and θ2 is differentiated once with respect to time, and the rotation angular velocity of the actuator is calculated.

From the obtained rotational angular velocity of the actuator, the rotational angular velocity of the arm driven by the actuator is obtained. In the case of the first arm 11, since it is driven by the torque transmission device 62 having a reduction ratio 1 / N2 from the actuator 52, the rotational angular velocity ω2 of the output portion of the base body coupling device 30 is
ω2 = (dθ2 / dt) × (1 / N2)
It becomes.

Similarly, the rotational angular velocity ω1 of the output unit of the arm coupling device 20 including the actuator 51 that drives the second arm 12 is:
ω1 = (dθ1 / dt) × (1 / N1)
1 / N1: A reduction ratio of the torque transmission device 61.

In the second calculation unit 520, the position where the inertial sensor 90 having the rotation axis of the base body coupling device 30 as the angular velocity of the arm body 10, that is, the workpiece is detected from the detection value detected by the inertial sensor 90 provided on the second arm 12. The angular velocity ω _a of the holding device 70 is calculated.

In the third calculation unit 530, the angular velocity ω1 of the second arm 12, the angular velocity ω2 of the first arm 11, and the second arm 12 having the connecting device including the actuator by the rotation of the actuator calculated as described above as the rotation axis. The torsional angular velocity ω _b, which is the difference from the angular velocity ω _a of the second arm with the base body coupling device 30 as the rotation axis obtained from the attached inertial sensor 90,
ω _b = ω _a −ω2−ω1
Obtained by. The obtained twist angular velocity ω _b is considered to be a combination of the twist angular velocity ω _b1 caused by the first arm 11 and the twist angular velocity ω _b2 caused by the second arm 12,
ω _b = ω _b1 + ω _b2
It is expressed. In other words, by decomposing the torsional angular velocity ω _b into the torsional angular velocities ω _b1 and ω _b2 caused by the arms 11 and 12, an actuator control signal for suppressing the vibration of the arm body 10, that is, a feedback signal is generated. can do.

The torsional angular velocity omega _b, torsional angular velocity omega _b1 due to the arms 11 and 12, illustrating a method for decomposing the omega _b2. FIG. 3 is a diagram illustrating the arm coupling device 20 and the base body coupling device 30 of the robot apparatus 100 illustrated in FIG. 1 by a characteristic model of spring characteristics and damping characteristics (damper characteristics). As shown in FIG. 3, the arm connecting device 20 of the robot apparatus 100 includes a virtual spring 20a and a virtual damping device 20b, and the first arm 11 and the second arm 12 are connected. The robot apparatus 100 can be schematically shown as having the first arm 11 connected to the base body 40 with the 30a and the virtual damping device 30b. In the modeled robot apparatus 100, the first arm 11 and the second arm 12 are generally different in weight, length, rigidity, and the like, so that the first arm 11 is provided in the base connecting device 30. Calculation is made using the frequency response when driven by the actuator 52 and the configurations of the virtual springs 20a and 30a and the virtual damping devices 20b and 30b when the second arm 12 is driven by the actuator 51 provided in the arm coupling device 20. It shows different characteristics with the frequency response.

FIG. 4 is a Bode diagram showing the frequency response of the first arm 11 and the second arm 12. 4A shows the frequency response of the first arm 11, and FIG. 4B shows the frequency response of the second arm 12. Since the actuator 52 provided in the base body coupling device 30 can be provided with a high output, the first arm 11 can be provided with high rigidity and weight. On the other hand, since the second arm 12 includes the arm coupling device 20 in the first arm 11, the second arm 12 needs to be a small and low-power actuator 51, so that the weight is reduced. With such a configuration, the frequency characteristics of the first arm 11 shown in FIG. 4A and the second arm 12 shown in FIG. 4B are different. It is noted that the frequency characteristics are different, and the torsion angular velocity ω _b can be decomposed into torsion angular velocities ω _b1 and ω _b2 caused by the arms 11 and 12.

Specifically, a bandpass filter that extracts the twist angular velocity ω _b1 component of the first arm 11 from the twist angular velocity ω _b by providing the third arithmetic unit 530 with a band pass filter having the characteristics shown in FIG. The torsional angular velocity ω _b1 of the first arm 11 and the torsional angular velocity ω of the second arm 12 are filtered by a bandpass filter that extracts the torsional angular velocity ω _b2 component of the second arm 12 having the characteristics shown in FIG. _b2 can be extracted and obtained.

FIG. 6 illustrates the response of the angular velocity Ω _act for driving the arm from the actuator via the torque transmission device, the angular velocity Ω _{arm of the} arm obtained by the inertial sensor, and the torsion angular velocity Ω _tor to the torque of the actuator in the robot apparatus 100. It is a Bode diagram. As shown in FIG. 6, in Ω _act and Ω _arm , the low-frequency component includes the original operation component, but Ω _tor includes almost no actual operation component in the low-frequency component. Focusing on this point, the present invention regards the low-frequency component of the torsional angular velocity Ω _tor as an effect due to an error due to the bias drift of the sensor and removes it, thereby removing the low-frequency noise of the robot device 100, and The vibration can be accurately controlled.

FIG. 7 is a diagram conceptually showing the frequency component of the torsional angular velocity during operation, and shows how the low frequency component is removed by means such as a high-pass filter. 7A shows the case of an inertial sensor having no error, and FIG. 7B shows the case of an inertial sensor having a low-frequency error such as bias drift. FIGS. 7A and 7B. The difference between the two indicates a component due to an inertial sensor error. The torsional angular velocity Ω _tor before removal of the low frequency component shown in FIG. _7A becomes Ω _tor ′ by removing the low frequency component with a high-pass filter. In FIG. 4B, the torsional angular velocity Ω ′ _tor before the removal of the low frequency component becomes Ω ′ _tor by removing the low frequency component in the same _manner . As shown in FIG. _7A , the torsional angular velocity Ω _tor ′ from which the low-frequency component has been removed becomes smaller in amplitude than the torsional angular velocity Ω _tor before the removal, below the removed frequency. However, the region S is a region having a minute value, for example, 10 ⁻⁵ or less as shown in the figure, and the removal amount is negligible.

  On the other hand, in the case of the inertial sensor having the error shown in FIG. 7B, the difference region T generated before and after the removal of the low frequency component by the high pass filter is in a region having a large amplitude. Therefore, by removing the low frequency component, it is possible to approach the value U that does not include an error before removing the low frequency component in FIG. Therefore, by removing the low-frequency component of the torsional angular velocity, it is possible to obtain a torsional angular velocity that is error-free, in other words, close to the true value, even when using an inertial sensor that has errors in low-frequency components such as bias drift. Thus, it is possible to accurately control the vibration of the robot apparatus 100 due to the torsional angular velocity.

That is, for the torsional angular velocities ω _b1 and ω _b2 obtained as described above, the torsional angular velocities Ω _b1 and Ω _{b2 are obtained} by removing low frequency components of the frequency components by a high-pass filter (hereinafter referred to as HPF) (not shown). Is calculated. Here, the HPF filtering frequency is preferably a lower frequency HPF with respect to the lowest frequency among the anti-resonance frequency and the resonance frequency among the mechanical natural frequencies of the robot apparatus 100. Further, by using a band-pass filter instead of a high-pass filter, it is possible to remove unnecessary frequency components that also exist in high-frequency components while removing unnecessary low-frequency components.

Using the torsional angular velocities Ω _b1 and Ω _{b2 from} which the low-frequency components have been removed, the control unit 600 can generate a control signal and control the actuators 51 and 52. In this case, the second arithmetic unit 540 is provided, and the second arm 12 and the first arm 12 are connected to the torsional angular velocities Ω _b1 and Ω _b2 from which the low frequency component is removed via the torque transmission devices 61 and 62 of the coupling devices 20 and 30. It is also possible to add the angular velocities ω1 and ω2 transmitted to the arm 11 and use the result to generate a control signal in the control unit 600.

(Second Embodiment)
A control method of the robot apparatus 100 according to the first embodiment will be described as a second embodiment. FIG. 8 is a flowchart showing a control method of the robot apparatus 100 according to the present embodiment.

[First calculation step]
In the operating state of the robot apparatus 100, the first calculation step (S111) is first executed. In the first calculation step (S111), rotation angle data is obtained from the angle sensors 81 and 82 provided in the actuators 51 and 52 in the first calculation unit 510. The obtained rotation angle data is converted into a rotation angle, the rotation angle is differentiated once with respect to time, the rotation angular velocity ω1 of the output portion of the arm coupling device 20 including the actuator 51 that drives the second arm 12, the base body coupling The rotational angular velocity ω2 of the output unit of the device 30 is calculated.

[Second calculation step]
At the same time, in the second calculation step (S112), the angular velocity data is obtained from the inertial sensor 90 provided in the second arm 12 in the second calculation unit 520, and the connecting devices 20 and 30 for connecting and driving the arm body 10 are used as the rotation axes. The angular velocities of the arms 11 and 12 are calculated. That is, the angular velocity ω _a at the position where the inertial sensor 90 of the arm body 10 having the base body coupling device 30 as the rotation axis is calculated from the data obtained from the inertial sensor 90 as described above.

[Third calculation step]
Next, the process proceeds to the third calculation step (S120). In the third calculation step (S120), the angular velocity ω1, ω2 calculated in the first calculation step (S111) and the angular velocity ω of the arm body 10 calculated from the detection data of the inertial sensor 90 in the second calculation step (S112). _{From a}
ω _b = ω _a −ω2−ω1
To calculate the torsional angular velocity ω _b . The calculated torsional angular velocity ω _b is filtered by a bandpass filter, and the torsional angular velocity ω _b1 of the first arm 11 and the torsional angular velocity ω _b2 of the second arm 12 are extracted and decomposed. From the calculated and extracted torsional angular velocities ω _b1 and ω _b2 , the torsional angular velocities Ω _b1 and Ω _b2 from which low-frequency components are removed from the mechanical system natural frequency of the robot apparatus 100 are calculated.

[Fourth calculation step]
Next, the process proceeds to the fourth calculation step (S130). In the fourth calculation step (S130), the torque transmission devices of the coupling devices 20 and 30 are converted to the torsional angular velocities Ω _b1 and Ω _b2 calculated in the third calculation step (S120) by removing the low-frequency components. The angular speeds ω1 and ω2 transmitted to the second arm 12 and the first arm 11 through 61 and 62 are added, and the process proceeds to a control signal generation step (S140) for generating a control signal based on the result.

[Control signal generation process]
In the control signal generation step (S140), the control unit 600 generates control signals for the actuators 51 and 52 based on the obtained calculation result in the fourth calculation step (S130). That is, the angular velocities transmitted to the second arm 12 and the first arm 11 via the torque transmission devices 61 and 62 of the coupling devices 20 and 30 to the torsional angular velocities Ω _b1 and Ω _{b2 from} which low-frequency components have been removed by the band pass filter Based on the angular velocity obtained by adding ω1 and ω2, a signal for controlling the actuators 51 and 52 is generated, and the process proceeds to the next robot apparatus operation stop confirmation step (S150).

[Robot device stop confirmation process]
In the robot apparatus operation stop confirmation step (S150), it is confirmed whether the robot apparatus 100 is in an operation state. If the robot apparatus 100 is in an operation state (No), the first operation step (S111) and the second operation step (S112). Return to) and repeat the control. If the operation is stopped (Yes), the control ends.

  As described above, the torsional angular velocity of each arm is calculated by one inertial sensor 90 provided at the position where the work holding device 70 of the robot apparatus 100 is arranged, and the obtained torsional angular velocity is used as control data. By using the torsional angular velocity from which the component has been removed as control data, it is possible to perform accurate vibration suppression control of the robot apparatus from which the inertia sensor error has been removed.

Example 1
An example of control based on the torsional angular velocity of the robot apparatus 100 according to the first embodiment will be described. The equation for calculating the torque command value τ for the actuators 51 and 52 uses four state quantities of an actuator angle, an arm angle (link angle), an actuator angular velocity, and an arm angular velocity. Of these, the arm angular velocity is described in the above embodiment. The “torsional angular velocities (Ω _b1 , Ω _b2 ) from which low-frequency components have been removed + the angular velocities (ω2, ω1) transmitted to the arms via the torque transmission devices 61 and 62 of the coupling device” are used.

  Includes the frequency components of the actuator state, arm angle (link angle), actuator angular velocity, and mechanical natural frequency (anti-resonance frequency and resonance frequency) when viewed from the actuator of each arm, which are the above-mentioned four state quantities. The arm angular velocity obtained by adding the angular velocity transmitted to the arm via the torque transmission devices 61 and 62 of the coupling device to the torsional angular velocity from which the low-frequency component has been removed is measured, and the following ( A state feedback control system is constructed by Equation 1).

K _{1 to} k ₅ in (Expression 1) are control gains inside the control device, and can be determined by a pole placement method, an optimal regulator, or the like. By controlling using the torque command value τ obtained in this way, vibration of the robot apparatus 100 is suppressed and an accurate operation is enabled.

In the above-described example, the angular velocity transmitted to the arm via the “torsional angular velocity (Ω _b1 , Ω _b2 ) from which low-frequency components have been removed” + the torque transmission devices 61 and 62 of the coupling device described in the above-described embodiment. Although an example using (ω2, ω1) ”has been described, a control system with a small amount of calculation can be constructed by using“ twist angular velocity ”instead of arm angular velocity as a state quantity.

That is, using the arm angle θ _a in (Equation 1) and the arm angular velocity dθ _a / dt of one-time differentiation as the torsional angular velocities (Ω _b1 and Ω _b2 in the embodiment), the control system is expressed by the following (Equation 2). To construct.

K _{t1 to} k _t5 in (Expression 2) are control gains inside the control device, and can be determined by a pole placement method, an optimal regulator, or the like. By controlling using the torque command value τ obtained in this way, vibration of the robot apparatus 100 is suppressed and an accurate operation is enabled.

(Example 2)
Another embodiment will be described with reference to a control block diagram shown in FIG. 9 differs from the control block diagram shown in FIG. 2 in the first embodiment described above in that the position control system 610a and the speed control system 610b in the control unit 610 acquire and control the calculation results. Accordingly, the same components as those in the embodiment are denoted by the same reference numerals, and description thereof is omitted.

  Also in the present embodiment illustrated in FIG. 9, the CPU 210 includes a first calculation unit 510, a second calculation unit 520, a third calculation unit 530, and a control unit 610, which will be described later, and reads and executes a program stored in the ROM 310. . The RAM 410 stores data obtained by executing the program in the CPU 210, and sends necessary data from the stored data to the CPU 210.

  As shown in the control block diagram of FIG. 9, in the control of the present embodiment, the position control system 610a acquires the rotation angle data of the actuators 51 and 52 converted by the first calculation unit 510. Similarly, the velocity control system 610b acquires the angular velocity data of the actuators 51 and 52 calculated by the first calculator. The torsional angular velocity data calculated by the third calculation unit 530 is acquired by the velocity control system 610b, and the velocity control system 610b outputs a torque command to the robot apparatus 100 together with the angular velocity command input from the position control system 610a. . By controlling in this way, vibration of the robot apparatus 100 is suppressed and an accurate operation is enabled.

  In the above-described embodiment and examples, the horizontal articulated arm body 10 having two arms has been described as shown in FIG. 1, but the present invention is not limited to this, and a robot apparatus having three or more arms is used. Also applies.

  DESCRIPTION OF SYMBOLS 10 ... Arm body, 11, 12 ... Arm, 20 ... Arm coupling device, 30 ... Base body coupling device, 40 ... Base body, 51, 52 ... Actuator, 61, 62 ... Torque transmission device, 70 ... Work holding device, 81, 82 ... An angle sensor, 90 ... Inertia sensor, 100 ... Robot device.

Claims (6)

An arm coupling device including an actuator and an angle sensor for detecting a rotation angle of the actuator;
A plurality of arms connected in series and rotatably by the arm connecting device;
A base body in which the arm body is rotatably connected by a base body connecting device including the actuator provided at one end of the arm body and an angle sensor for detecting a rotation angle of the actuator;
An inertial sensor including at least an angular velocity sensor provided in a work arm to which a work holding means is attached among the plurality of arms;
A first calculation unit for calculating an angular velocity of the arm operated by the actuator including the angle sensor from rotation angle detection data of the angle sensor;
A second calculator that calculates an angular velocity of the arm that is operated by the actuator with the base connecting device and the arm connecting device as axes, from the angular velocity detection data of the inertial sensor;
From the difference between the angular velocity of the arm operated by the actuator in the arm and the angular velocity of the arm calculated by the angular velocity detection data of the inertial sensor, vibration frequency components are extracted for each arm, A third calculator that calculates a torsional angular velocity between an actuator and the arm connected to the actuator;
A robot apparatus characterized by that. The frequency component is extracted by a bandpass filter including an anti-resonance frequency and a resonance frequency among the mechanical natural frequencies of the robot apparatus.
The robot apparatus according to claim 1. The angular velocity of the arm calculated by the first calculating unit;
A fourth calculation unit that adds the torsional angular velocity calculated by the third calculation unit;
The robot apparatus according to claim 1 or 2, wherein An arm coupling device including an actuator and an angle sensor for detecting a rotation angle of the actuator;
A plurality of arms connected in series and rotatably by the arm connecting device;
A base body in which the arm body is rotatably connected by a base body connecting device including the actuator provided at one end of the arm body and an angle sensor for detecting a rotation angle of the actuator;
A control method for a robot apparatus, comprising: an inertial sensor including at least an angular velocity sensor provided in a work arm to which a work holding means is attached among the plurality of arms,
A first calculation step of calculating an angular velocity of the arm operated by the actuator including the angle sensor from rotation angle detection data of the angle sensor;
A second calculation step of calculating an angular velocity of the arm operated by the actuator having the base connecting device and the arm connecting device as axes, from the angular velocity detection data of the inertial sensor;
From the difference between the angular velocity of the arm operated by the actuator in the arm and the angular velocity of the arm calculated by the angular velocity detection data of the inertial sensor, vibration frequency components are extracted for each arm, A third calculation step of calculating a torsional angular velocity between an actuator and the arm connected to the actuator,
A method for controlling a robot apparatus, comprising: The frequency component is extracted by a bandpass filter including an anti-resonance frequency and a resonance frequency among the mechanical natural frequencies of the robot apparatus.
The robot apparatus control method according to claim 4, wherein: The angular velocity of the arm calculated in the first calculation step;
A fourth calculation step of calculating the angular velocity of the arm by adding the torsional angular velocity calculated in the third calculation step;
The method for controlling a robot apparatus according to claim 4 or 5, wherein

Images