JP5906825B2 - robot - Google Patents

Description

  The present invention relates to a robot.

  2. Description of the Related Art Conventionally, a device such as a robot that moves a terminal device attached to the tip of an arm to a desired position by rotating the arm or the like and operates the terminal device at the position is known. For example, a feeding / dispensing material device that includes a gripping terminal and feeds / unloads a workpiece to a processing device, a painting robot that includes a painting terminal, a welding robot that includes a welding terminal, and the like are known.

  When driving a robot, a control method is used in which the rotation angle of a drive source such as a motor that drives the robot arm is measured, and the tip position of the arm is controlled based on the measured angle information. However, the transmission mechanism and the arm that transmit the driving force from the driving source to the arm are not rigid bodies. Due to this, the transmission mechanism and the arm are deformed, and the position on the tip side of the arm whose position is controlled based on the angle information may not always coincide with the target position. In addition, there is a problem that vibration is generated by the deformation of the transmission mechanism and the arm during operation.

  In order to solve these problems, a method has been proposed in which an inertial sensor is attached to the tip of an arm to measure the movement of the tip, and angular velocity information obtained by the obtained inertial sensor is used for control. Patent Document 1 discloses a multi-joint robot control method and an articulated robot that controls the operation of an arm by an output signal of an inertial sensor. By using the inertial sensor, it is possible to perform positioning with high accuracy even when the rigidity is low, and to prevent a decrease in accuracy due to shake.

  However, in order to obtain the arm rotation angle from the output of the inertial sensor, it is necessary to integrate the output of the inertial sensor. If the integration is repeated, there is a problem that the control device is likely to be influenced by the drift of the reference potential of the inertial sensor, and the possibility that the control device erroneously recognizes information increases.

  Patent Document 2 discloses a robot control device and a robot control method in which a control switching determination unit determines a weighted multiplier of inertial force information according to an operation state during control. By determining the weighting multiplier of the inertial force information, error factors such as noise influence and reference potential drift are eliminated. However, when the weighting multiplier is switched, an abrupt change in the input to the drive source may occur and vibration may occur, or a mechanical load may be applied to the drive source or the speed reducer. As a result, there is a problem that the possibility of affecting the machine life is increased.

  As a method of not changing the weighting multiplier, a method of performing a filter process using a digital filter on the inertial sensor can be considered. Patent Document 3 discloses a gyro apparatus that removes noise components of an inertial sensor by performing filter processing using a digital filter. According to this, the digital filter has characteristics as a low-pass filter. When the vehicle is stopped, the cutoff frequency of the digital filter is set low, and when the vehicle is running, the cutoff frequency is set high. As a result, noise was sufficiently reduced during the stop period, and high-speed response was ensured during the travel period.

Japanese Patent Laid-Open No. 7-9374 JP 2010-284770 A JP-A-6-194170

  The operation of the arm includes steps of movement (acceleration, constant speed movement, deceleration, stop), settling, and standby. In Patent Document 3, it is effective for movement and standby, but it is difficult to be an effective method for settling. Therefore, a robot that dampens the arm with high quality when the arm is set is required.

  SUMMARY An advantage of some aspects of the invention is to solve at least a part of the problems described above, and the invention can be implemented as the following forms or application examples.

[Application Example 1]
The robot according to this application example detects an angle supported at one end, a drive source for rotating the arm, a rotation angle of the drive source, and outputs rotation angle information of the drive source. An angular sensor that is attached to the arm, detects an angular velocity at which the arm rotates and outputs angular velocity information of the arm, and a control unit that controls the drive source, and the control unit includes: A control command generation unit that outputs a control command for instructing the pivoting operation of the arm, a filter unit that removes a high-frequency component of the angular velocity information, and a filter control unit that controls a cutoff frequency of the high-frequency component. It is characterized by that.

  According to the robot according to this application example, the control command generation unit instructs the rotation operation of the arm, and the drive source rotates the arm. Since the angle sensor detects the rotation angle of the drive source, the rotation angle of the arm can be detected. The inertial sensor detects the angular velocity of the arm. Since the arm vibrates as it moves, the inertial sensor detects the angular velocity including the vibration of the arm.

  The control unit controls the operation of the arm using the control command, the rotation angle information, and the angular velocity information subjected to the filter process. At this time, the control unit determines whether the arm is moving, settling, or waiting using the rotation angle information and the angular velocity information. The settling indicates a state where the vibration is attenuated and stopped. And control according to each situation is performed.

  The filter control unit controls the filter characteristics of the filter unit according to the operating state of the arm. A filter part performs the filter process which removes a high frequency component with respect to angular velocity information. Thereby, the frequency range of the high frequency component to be removed is controlled in the angular velocity information. The filter control unit determines the filter characteristics by comprehensively considering the effect of using the angular velocity information and the influence of the error of the angular velocity information. Thereby, the influence which the error of angular velocity information exerts becomes small. Therefore, the control unit can control the robot using the angular velocity information including an appropriate frequency component. As a result, the robot can control the arm when the arm is set.

[Application Example 2]
In the robot according to the application example, it is preferable that the filter unit is a low-pass filter, and the cutoff frequency is variably controlled.

  According to this application example, the filter unit has a filter characteristic as a low-pass filter. The cutoff frequency is variably controlled. When high-speed responsiveness of angular velocity information is required, the cutoff frequency is set higher than the arm operating frequency. Thereby, it is possible to easily detect the vibration of the arm.

  In order to reduce the error of the angular velocity information and the influence of noise, the cutoff frequency is set higher than the arm operating frequency. Thereby, it is possible to perform the optimum filter processing on the angular velocity information. Therefore, the error of the angular velocity information and the influence of the high frequency component can be adjusted according to the situation.

[Application Example 3]
In the robot according to the application example, the filter control unit determines a state of movement, settling, and standby of the arm according to the control command, and sets the cutoff frequency of the filter unit when the operation of the arm is moving. Set higher than the natural frequency of the arm, set the cutoff frequency of the filter unit lower than the natural frequency of the arm when the operation of the arm is in standby, and when the operation of the arm is settling, It is preferable to control the cutoff frequency.

  According to this application example, the filter control unit determines the state of movement, settling, and standby according to the control command. The filter control unit controls the cutoff frequency according to each state. The filter unit performs filtering on the angular velocity information according to the determination. When the filter control unit determines that it is settling, the arm settling state is detected from the angular velocity information, and the cutoff frequency is controlled according to the arm settling state. When the vibration of the arm is large, the cut-off frequency is set high to increase the high-speed response, and when the vibration converges, the cut-off frequency is set low to reduce the error of angular velocity information and the influence of noise. Thereby, the filter part can output useful angular velocity information according to the settling state of the arm.

  When the operation of the arm is moving, it is not necessary to set the cut-off frequency high and to control the cut-off frequency in detail in order to increase the responsiveness to the behavior of the arm during movement. Thereby, compared with the case where a filter control part judges that it is settling, the load of control can be lightened. When the filter control unit determines that it is standby, it is not necessary to set the cut-off frequency low and reduce the cut-off frequency finely in order to reduce the influence of error and noise in the angular velocity information. Thereby, compared with the case where a filter control part judges that it is settling, the load of control can be lightened. As a result, it is possible to perform optimum filter processing on the angular velocity information according to the operating state of the arm.

[Application Example 4]
In the robot according to the application example, a determination value for determining the angular velocity information is set in advance, and the filter control unit controls the cutoff frequency by comparing the angular velocity information with the determination value when the operation of the arm is settling. It is preferable to do.

  According to this application example, the cut-off frequency for the angular velocity information is controlled by comparing the angular velocity information with the determination value. For this reason, when the angular velocity information exceeds the determination value, the cutoff frequency is set high. Thereby, angular velocity information with high responsiveness can be obtained. When the angular velocity information falls below the determination value, the cutoff frequency is set low. In the control using angular velocity information, when the angular velocity information becomes small, the effect of using the angular velocity information is reduced, and accuracy is lowered due to the influence of errors in the angular velocity information due to noise or the like. By comparing the angular velocity information with the determination value and performing a filtering process on the angular velocity information, it is possible to reduce the error of the angle information and the influence of noise. Therefore, the influence of the error of the angular velocity information is reduced, and the robot can be controlled so that the vibration damping effect can be maximized.

[Application Example 5]
In the robot according to the application example, a determination value for determining the rotation angle information is set in advance, and when the operation of the arm is settling, the filter control unit compares the rotation angle information with the determination value, It is preferable to control the cutoff frequency.

  According to this application example, the rotation angle information is compared with the determination value, and the cutoff frequency for the angular velocity information is controlled. For this reason, when the rotation angle exceeds the determination value, the cutoff frequency is set high, and when the rotation angle falls below, the cutoff frequency is set low. In the control using angular velocity information, when the angular velocity information becomes small, the effect of using the angular velocity information is reduced, and accuracy is lowered due to the influence of errors in the angular velocity information due to noise or the like. The rotation angle information is compared with the determination value, and the angular velocity information is filtered. When the rotation angle information exceeds the determination value, angular velocity information with high responsiveness is obtained. If the rotation angle information falls below the determination value, the angle information error and the influence of noise can be reduced. Therefore, it is possible to reduce the influence of the error of the angular velocity information.

[Application Example 6]
In the robot according to the application example described above, a determination value for determining an integral value of the angular velocity information is set in advance, and when the operation of the arm is settling, the filter control unit calculates one or more integral values of the angular velocity information. It is preferable to control the cut-off frequency in comparison with the determination value.

  According to this application example, the cutoff frequency for the angular velocity information is controlled by comparing one or more integral values of the angular velocity information with the determination value. The frequency is set low. In the control using angular velocity information, when the angular velocity information becomes small, the effect of using the angular velocity information is reduced, and accuracy is lowered due to the influence of errors in the angular velocity information due to noise or the like.

  One or more integral values of the angular velocity information are compared with a determination value, and the angular velocity information is filtered. Angular velocity information with high responsiveness is obtained when the judgment value is exceeded. When the value is lower than the judgment value, the error of angle information and the influence of noise can be reduced. Therefore, it is possible to reduce the influence of the error of the angular velocity information. Comparing one or more integral values of angular velocity information with a determination value can be the same criterion as when comparing angular velocity information with a determination value. By handling one or more integral values of angular velocity information, the unit can be handled in common with the rotation angle information and the like.

[Application Example 7]
In the robot according to the application example described above, a determination value for determining a differential value of the rotation angle information is set in advance, and when the operation of the arm is settling, the filter control unit performs one or more differentiations of the rotation angle information. It is preferable to control the cutoff frequency by comparing a value with the determination value.

  According to this application example, the cutoff frequency for the angular velocity information is controlled by comparing one or more differential values of the rotation angle information with the determination value. Therefore, when the determination value is exceeded, the cutoff frequency is set higher and lower. And the cutoff frequency is set low. In the control using angular velocity information, when the angular velocity information becomes small, the effect of using the angular velocity information is reduced, and accuracy is lowered due to the influence of errors in the angular velocity information due to noise or the like.

  By comparing one or more differential values of the rotation angle information with the judgment value and applying a filter process to the angular velocity information, if the judgment value is exceeded, angular velocity information with high responsiveness is obtained, and if the judgment value is below Angle information errors and noise effects can be reduced. Therefore, it is possible to reduce the influence of the error of the angular velocity information. Comparing one or more differential values of the rotation angle information with the determination value can be the same criterion as when comparing the rotation angle information with the determination value. By handling one or more differential values of the rotation angle information, the unit can be handled in common with the angular velocity information and the like.

[Application Example 8]
In the robot according to the application example described above, a determination value for determining a difference between the angular velocity indicated by the control command and the angular velocity information is set in advance, and when the arm operation is on standby, the filter control unit displays the angular velocity indicated by the control command. It is preferable that the filter control unit determines whether to use the value of the angular velocity information by comparing the difference between the angular velocity information and the angular velocity information.

  According to this application example, the difference between the angular velocity indicated by the control command and the angular velocity information is compared with the determination value to determine whether to use the angular velocity information. When the difference between the angular velocity indicated by the control command and the angular velocity information exceeds the determination value, the angular velocity information is not used. In the control using angular velocity information, if the difference between the angular velocity and the angular velocity information indicated by the control command is reduced, the effect of using the angular velocity information is reduced, and the operation is unstable due to the influence of errors in the angular velocity information due to noise or the like. Become. Therefore, it is possible to reduce the influence of the error of the angular velocity information.

The schematic perspective view which shows the structure of the feeding / dispensing material apparatus concerning 1st Embodiment. The block diagram which shows the structure of the function which drives a robot mechanism. The flowchart which shows the process of rotating a feeding / dispensing material arm. The figure for demonstrating the judgment value of the angular velocity used as the reference | standard which switches control. The schematic front view of the robot system in connection with 2nd Embodiment.

  Hereinafter, an embodiment of a robot control method using a robot as a transfer device and an inertial sensor will be described with reference to the drawings. In the drawings referred to in the following description, the vertical and horizontal scales of the members or parts and the scales of the parts may be shown differently from actual ones in order to display the constituent members in an easy-to-understand manner.

(First embodiment)
An embodiment of a control method using a robot as a transfer device and an inertial sensor will be described. The present embodiment will be described with reference to an example of a material supply / discharge material device that is an example of a transport device. The supply / discharge material apparatus according to the present embodiment is, for example, a supply / discharge material apparatus that handles a wafer in which a plurality of semiconductor chips constituting the semiconductor device are partitioned in a semiconductor device manufacturing process.

<Feeding material equipment>
First, the mechanical configuration of the supply / discharge material apparatus 10 will be described with reference to FIG. FIG. 1 is a schematic perspective view showing the configuration of the supply / discharge material apparatus. As shown in FIG. 1, the supply / discharge material apparatus 10 includes a holding hand 12, a robot mechanism 20, a supply / discharge material apparatus control unit 30, an angular velocity sensor 32, and an angle sensor 34 (see FIG. 2). ing.

  The robot mechanism 20 includes a hand holding mechanism 24, a supply / discharge material arm 21, an arm shaft portion 26, and a machine base 28. The machine base 28 supports the arm shaft portion 26 so as to be rotatable around the rotation axis of the arm shaft portion 26 and precisely positioned and fixed via a built-in bearing mechanism (not shown). The arm shaft portion 26 is connected to an arm drive motor 22 (see FIG. 2) built in the machine base 28 via an arm drive mechanism 23 and is rotated by the arm drive motor 22. An angle sensor 34 is connected to the arm drive motor 22, and the rotation angle of the arm drive motor 22 is measured by the angle sensor 34.

  One end of the supply / discharge material arm 21 is fixed to the end of the arm shaft portion 26 opposite to the side supported by the machine base 28. The feed / release material arm 21 is rotated about the rotation axis of the arm shaft portion 26 by the arm drive motor 22. The rotation angle of the supply / discharge material arm 21 is approximately measured by measuring the rotation angle of the arm drive motor 22 by the angle sensor 34.

  A hand holding mechanism 24 is fixed to the opposite end of the arm shaft portion 26 in the supply / discharge material arm 21. The hand holding mechanism 24 includes a holding bearing 24a fixed to the supply / discharge material arm 21 and a holding mechanism shaft 24b supported by the holding bearing 24a. The holding mechanism shaft 24b is slidable in the axial direction of the holding mechanism shaft 24b with respect to the holding bearing 24a by a vertical drive source (not shown). The axial direction of the holding mechanism shaft 24 b is substantially parallel to the axial direction of the arm shaft portion 26. The holding mechanism shaft 24b can be accurately positioned and fixed in the axial direction.

  A holding hand 12 is attached to the lower end of the holding mechanism shaft 24b in the drawing. The holding hand 12 can be positioned at a position facing the object to be conveyed by rotating the supply / discharge material arm 21. By sliding the holding mechanism shaft 24b with respect to the holding bearing 24a, the holding hand 12 is separated from and brought into contact with the object to be conveyed, and the object to be conveyed held by the holding hand 12 is lifted from the placing place, or the placing place It is possible to make it approach.

  An angular velocity sensor 32 is fixed to the hand holding mechanism 24 to which the holding hand 12 is attached on the side opposite to the holding hand 12. That is, the angular velocity sensor 32 is fixed to the distal end of the supply / discharge material arm 21, and can measure the angular velocity at which the supply / discharge material arm 21 is rotated.

  The material supply / control device controller 30 performs overall control of the operation of each part of the material supply / discharge material device 10 based on a control program input in advance via an information input / output device (not shown). For example, signals output from the angle sensor 34 and the angular velocity sensor 32 are input to drive the arm drive motor 22.

<Functional configuration of robot mechanism drive>
Next, the configuration of the function for driving the robot mechanism 20 will be described with reference to FIG. FIG. 2 is a block diagram showing a configuration of functions for driving the robot mechanism. As described above, in order to rotate the feed / release material arm 21, the feed / release material device 10 includes an arm drive motor 22, an arm drive mechanism 23, an angular velocity sensor 32, an angle sensor 34, and a feed / release material device. And a control unit 30.

  As the angular velocity sensor 32, for example, a gyro sensor can be used. For the angle sensor 34, for example, an encoder can be used. The feed / release material arm 21 corresponds to an arm, the robot mechanism 20 corresponds to a robot, the arm drive motor 22 corresponds to a drive source, and the angular velocity sensor 32 corresponds to an inertial sensor.

  The material supply / control device control unit 30 includes a robot control unit 31 as a control unit for controlling the arm drive motor 22. The robot control unit 31 includes a control command generation unit 36, a filter control unit 37, a filter unit 35, an arm operation control unit 38, and a motor driver 39.

  The control command generator 36 calculates an operation command for the robot mechanism 20 based on the operation command for material supply or material removal. Then, an operation command for the supply / discharge material arm 21 is output to the filter control unit 37 and the arm operation control unit 38. The operation command is input to the material supply / dispensing device 10 from an input device (not shown) by the operator. An operation command of the robot mechanism 20 based on the operation command is output from the overall control unit (not shown) included in the supply / discharge material apparatus control unit 30 to the control command generation unit 36. The operation command of the supply / discharge material arm 21 output by the control command generation unit 36 is, for example, a command as a change in angle at which the supply / discharge material arm 21 moves so that the trajectory at the tip of the supply / discharge material arm 21 becomes an operation command. Is done.

  The arm operation control unit 38 outputs a signal for controlling the arm drive motor 22 to the motor driver 39. As a result, the operation command for the supply / discharge material arm 21 output by the control command generator 36 is executed. The arm operation control unit 38 includes angle information and angular velocity information use control unit 38a. The angle information and angular velocity information use control unit 38 a inputs angle information from the angle sensor 34 and angular velocity information from the angular velocity sensor 32. Then, the angle information and angular velocity information use control unit 38a generates and outputs a control signal for the arm drive motor 22 that is optimal for executing an operation command for the material supply / discharge material arm 21.

  The filter unit 35 inputs angular velocity information from the angular velocity sensor 32. The filter unit 35 performs a filtering process that applies a low-pass filter that removes high-frequency components on the angular velocity information, and outputs the filtered information to the angular information and angular velocity information use control unit 38a. The angle information and angular velocity information use control unit 38a uses the angle information and the filtered angular velocity information when generating a control signal for the arm drive motor 22.

  Control of filter characteristics used by the filter unit 35 is performed by a filter control unit 37. The filter characteristic corresponds to the cutoff frequency. The angle information and angular velocity information use control unit 38a included in the arm operation control unit 38 or the arm operation control unit 38 corresponds to the arm operation control unit.

  The filter control unit 37 inputs angle information from the angle sensor 34, angular velocity information from the angular velocity sensor 32, an operation command for the supply / discharge material arm 21 from the control command generation unit 36, and the like. Then, the filter characteristic is determined and output to the filter unit 35. Specifically, the filter control unit 37 controls the cutoff frequency of the filter unit 35.

<Rotation control procedure of feed / supply material arm>
Next, the step of rotating the supply / discharge material arm 21 by controlling the drive of the arm drive motor 22 to position the holding hand 12 disposed at the tip of the supply / discharge material arm 21 at a predetermined position. This will be described with reference to FIGS. FIG. 3 is a flowchart showing a process of rotating the supply / discharge material arm. FIG. 4 is a diagram for explaining an angular velocity determination value as a reference for switching control. The upper part of FIG. 4 is a diagram illustrating an example of the relationship between the elapsed time and the angular velocity and the angular velocity determination value during the rotation of the feeding / dispensing material arm. The lower part of FIG. 4 is a diagram showing an example of the relationship between the elapsed time and the angular velocity immediately before the supply / discharge material arm stops and the determination value of the angular velocity.

  In FIG. 3, the first step S21 is a step of determining whether or not “motor stop command is present”. The motor stop command is a command for stopping the arm drive motor 22 and terminating the control. When there is a motor stop command (YES in step S21), the supply / discharge material arm 21 is rotated by controlling the drive of the arm drive motor 22 to be held at the tip of the supply / discharge material arm 21. The step of positioning the hand 12 at a predetermined position is completed. If there is no motor stop command (NO in step S21), the process proceeds to step S22.

  Next, step S22 is a step of “obtaining control command value, angle information, and angular velocity information”. Specifically, the control command value output from the control command generator 36 is input to the arm operation controller 38 and the filter controller 37. Then, the angle sensor 34 detects the rotation angle of the arm drive motor 22 and calculates angle information of the rotation angle of the supply / discharge material arm 21. Next, the angle sensor 34 outputs angle information of the rotation angle of the supply / discharge material arm 21 to the filter control unit 37 and the arm operation control unit 38.

  The angular velocity sensor 32 detects the angular velocity of the feeding / discharging material arm 21 near the tip of the feeding / discharging material arm 21. Then, the angular velocity sensor 32 outputs angular velocity information of the angular velocity of the material supply / discharge material arm 21 to the filter control unit 37 and the angle information and angular velocity information use control unit 38a.

  Next, step S23 is a step of “arm operation parameters, movement, settling, standby”. In this step, the operating state of the arm is determined from the acquired control command value. Specifically, the control command generator 36 outputs an operation parameter after putting an operation parameter in the control command value. The filter control unit 37 determines from the operation parameters whether the state of the supply / discharge material arm 21 is moving, settling, or waiting. The operation parameter is an operation parameter P.

  If the operation parameter P is moving, the process proceeds to step S24. Step S24 is a step of “setting a high cut-off frequency”. In this step, the filter control unit 37 sets the cutoff frequency of the filter unit 35 high. As a result, the filtered angular velocity information becomes information including the angular velocity of high-frequency vibration. Next, the process proceeds to step S29.

  If the operation parameter P is standby, the process proceeds to step S25. Step S25 is a step of “setting a low cutoff frequency”. In this step, the filter control unit 37 sets the cutoff frequency of the filter unit 35 low. As a result, the filtered angular velocity information is information that does not include the angular velocity of high-frequency vibration. Next, the process proceeds to step S29.

  If the operation parameter P is settling, the process proceeds to step S26. Step S26 is a step of determining “exceeding angular velocity information determination value”. In this step, the filter control unit 37 determines from the angular velocity information output from the angular velocity sensor 32 whether or not the angular velocity of the supply / discharge material arm 21 has exceeded a predetermined determination value. The predetermined determination value is denoted as determination value S. A determination value having the same value in the direction opposite to the angular velocity of the determination value S is expressed as determination value-S.

  The determination value S is set in advance before performing step S26. An appropriate value is set as the determination value S by conducting an experiment in advance. If the absolute value of the angular velocity of the material supply / discharge material arm 21 exceeds the predetermined determination value S (YES in step S26), the process proceeds to step S27. Step S27 is a step of “setting a high cut-off frequency”. In this step, the filter control unit 37 sets the cutoff frequency of the filter unit 35 high. As a result, the filtered angular velocity information becomes information including the angular velocity of high-frequency vibration. Next, the process proceeds to step S29.

  If the absolute value of the angular velocity of the supply / discharge material arm 21 is smaller than the predetermined determination value S in step S26 (NO in step S26), the process proceeds to step S28. Step S28 is a step of “setting a low cut-off frequency”. In this step, the filter control unit 37 sets the cutoff frequency of the filter unit 35 low. As a result, the filtered angular velocity information is information that does not include the angular velocity of high-frequency vibration. Next, the process proceeds to step S29.

  In FIG. 4, the horizontal axis indicates the passage of time. The vertical axis in the upper stage shows the angular velocity, and the angular velocity transition line 42 shows the transition of the angular velocity detected by the angular velocity sensor 32. The lower vertical axis indicates the cutoff frequency, and the cutoff frequency transition line 43 indicates the transition of the cutoff frequency output from the filter control unit 37 to the filter unit 35. The arm operating frequency indicating line 44 is a line indicating the arm operating frequency. The arm operating frequency is a natural frequency of the supply / discharge material arm 21 and indicates a frequency at which the supply / discharge material arm 21 easily vibrates.

  The operation of the feeding / discharging material arm 21 is a repetition of three states of movement, settling, and standby. In the standby section indicated by P3, the cutoff frequency transition line 43 is set to the first frequency 43a lower than the arm operating frequency indicating line 44. Then, when the supply / discharge material arm 21 starts to rotate, the arm operation parameter is changed to move in the section indicated by P1. And the angular velocity transition line 42 rises. As indicated by the cut-off frequency transition line 43, the cut-off frequency is set to the second frequency 43 b higher than the arm operating frequency indicating line 44. As a result, the filtered angular velocity information becomes information including the angular velocity of high-frequency vibration.

  When the command value reaches the set target position, the process proceeds to the section indicated by P2. Then, the arm operation parameter changes to settling, and the angular velocity is attenuated. When the angular velocity falls within the determination value S to the determination value −S, the cutoff frequency is set to the first frequency 43 a lower than the arm operating frequency indicating line 44 as indicated by the cutoff frequency transition line 43. As a result, the filtered angular velocity information is information that does not include the angular velocity of high-frequency vibration. When a preset settling time is passed, the process moves to a section indicated by P3. Then, the arm operation parameter is changed to standby, and the cutoff frequency is set to the first frequency 43 a lower than the arm operation frequency indicating line 44.

  Returning to FIG. 3, after step S24, step S25, step S27, or step S28, the process proceeds to step S29. Step S29 is a step of "calculating a torque command from the control command value, angle information, and filtered angular velocity information". In this step, the angle information and angular velocity information use control unit 38a filters the angular velocity information using the control command value and the angle information. Then, the arm operation control unit 38 calculates a torque command using the filtered angular velocity information and angle information. Next, the process proceeds to step S30.

  Next, step S30 is a step of “inputting a torque command value to the motor driver”. In this step, the torque command value calculated and obtained by the arm operation control unit 38 is input to the motor driver 39. Next, the process proceeds to step S31.

  Step S31 is a step of “generating the torque of the arm drive motor”. In this step, electric power corresponding to the torque command value is supplied to the arm drive motor 22 by the motor driver 39. The arm drive motor 22 generates torque corresponding to the supplied power. Next, the process proceeds to step S32.

  Step S32 is a step of “arm mechanism operation”. In this step, the arm drive mechanism 23 connected to the arm drive motor 22 is operated by the torque generated by the arm drive motor 22. Then, the angular velocity of the supply / discharge material arm 21 is accelerated or decelerated by the torque generated by the arm drive motor 22.

  After step S32, the process returns to step S21, and if there is a motor stop command in step S21 (YES in step S21), the feed / release material arm 21 is rotated by controlling the drive of the arm drive motor 22. The process of positioning the holding hand 12 disposed at the tip of the supply / discharge material arm 21 at a predetermined position is completed.

(Second Embodiment)
Next, an embodiment of the robot will be described with reference to the schematic front view of the robot system in FIG. This embodiment is different from the first embodiment in that the robot is a double-arm robot. Note that description of the same points as in the first embodiment is omitted.

  That is, in this embodiment, as shown in FIG. 5, the robot system 50 includes a robot main body 51, a photographing device 52, a gripping unit 53, and a robot control device 54. Specifically, the robot main body 51 includes a main body 51a installed movably with respect to the ground, a neck portion 51b connected to the main body 51a so as to be capable of turning, and a head portion 51c fixed to the neck portion 51b. I have. Further, the robot body 51 includes a first arm part 51d connected to the head part 51c so as to be capable of turning and bending, and a second arm part 51e connected to the head part 51c so as to be capable of turning and bending. . Further, the robot main body 51 includes a transfer unit 51f attached to the main body 51a so that the robot main body 51 can move with respect to the installation surface of the robot main body 51.

  A grip 53 is attached to the hand that is the open end of the first arm 51d. A photographing device 52 is attached to the hand that is the open end of the second arm portion 51e. The first arm part 51d and the second arm part 51e include a shoulder joint part 55 connected to the head part 51c. An upper arm 56 is installed in connection with the shoulder joint 55, and an elbow joint 57 is installed in connection with the upper arm 56. Further, a lower arm arm 58 is installed in connection with the elbow joint 57.

  And in the 1st arm part 51d, it connects with the lower arm arm 58 and the holding part 53 is installed. In the second arm portion 51e, an imaging device 52 is installed in connection with the lower arm 58. An arm drive motor 22, an arm drive mechanism 23, and an angle sensor 34 are installed on the shoulder joint portion 55 and the elbow joint 57. An angular velocity sensor 32 is installed on the elbow joint 57 side of the upper arm 56. Further, in the first arm portion 51d, the angular velocity sensor 32 is installed on the grip portion 53 side of the lower arm arm 58. In the second arm portion 51e, the angular velocity sensor 32 is installed on the imaging device 52 side of the lower arm 58.

  The conveyance unit 51f supports the robot main body 51 so that the robot main body 51 can move in a certain direction or freely in a direction with respect to the installation surface of the robot main body 51. The conveyance unit 51f is realized by four sets of wheels, four sets of casters, a pair of endless tracks, and the like.

  The robot body 51 is, for example, a vertical articulated robot (double-arm robot) having two arms. The robot body 51 takes in the robot control command supplied from the robot control device 54, and changes the position and orientation of the imaging device 52 and the gripper 53 in the three-dimensional space as desired by drive control based on the robot control command. Further, the claw portion of the grip portion 53 is opened and closed.

  The imaging device 52 captures a subject, acquires a captured image that is a still image or a moving image, and supplies the captured image to the robot control device 54. The photographing device 52 is realized by, for example, a digital camera device or a digital video camera device. The grip portion 53 includes a claw portion that can grip or clamp an object. In the figure, the gripping portion 53 is schematically shown in order to show the function.

  The robot control device 54 controls the operations of the neck portion 51b, the head portion 51c, and the second arm portion 51e of the robot main body 51, and changes the position and posture of the photographing device 52. A robot control unit 31 is installed inside the robot control device 54.

  As in the first embodiment, the robot control unit 31 inputs angle information output from the angle sensor 34 and angular velocity information output from the angular velocity sensor 32. And the filter control part 37 controls the cutoff frequency of the filter part 35, and the filter part 35 is performing the filter process of angular velocity information. Therefore, the robot system 50 can control the first arm part 51d or the second arm part 51e in a short time when the first arm part 51d or the second arm part 51e is set.

In addition, this embodiment is not limited to embodiment mentioned above, A various change and improvement can also be added. A modification will be described below.
(Modification 1)
In the first embodiment, the filter control unit 37 controls the cutoff frequency by comparing the angular velocity output from the angular velocity sensor 32 with the determination value S and the determination value −S. The filter control unit 37 may control the cutoff frequency using the rotation angle information output from the angle sensor 34. A determination value for determining the rotation angle information is set in advance. When the rotation angle exceeds the determination value, the cutoff frequency is set high, and when the rotation angle falls below, the cutoff frequency is set low. In the control using angular velocity information, when the angular velocity information becomes small, the effect of using the angular velocity information is reduced, and accuracy is lowered due to the influence of errors in the angular velocity information due to noise or the like. The rotation angle information is compared with the determination value, and the angular velocity information is filtered. When the rotation angle information exceeds the determination value, angular velocity information with high responsiveness is obtained. If the rotation angle information falls below the determination value, the angle information error and the influence of noise can be reduced. Therefore, it is possible to reduce the influence of the error of the angular velocity information.

(Modification 2)
In the first embodiment, the filter control unit 37 controls the cutoff frequency by comparing the angular velocity output from the angular velocity sensor 32 with the determination value S and the determination value −S. The filter control unit 37 may control the cutoff frequency using an integrated value of angular velocity information output from the angular velocity sensor 32. A determination value for determining the integral value of the angular velocity information is set in advance. When the operation of the feeding / discharging material arm 21 is settling, the filter control unit 37 controls the cutoff frequency of the filter unit 35 by comparing one or more integral values of the angular velocity information with the determination value.

  When one or more integral values of angular velocity information exceed the determination value, the cutoff frequency is set high, and when it is lower, the cutoff frequency is set low. In the control using angular velocity information, when the angular velocity information becomes small, the effect of using the angular velocity information is reduced, and accuracy is lowered due to the influence of errors in the angular velocity information due to noise or the like.

  One or more integral values of the angular velocity information are compared with a determination value, and the angular velocity information is filtered. Angular velocity information with high responsiveness is obtained when the judgment value is exceeded. When the value is lower than the judgment value, the error of angle information and the influence of noise can be reduced. Therefore, it is possible to reduce the influence of the error of the angular velocity information. Comparing one or more integral values of angular velocity information with a determination value can be the same criterion as when comparing angular velocity information with a determination value. By handling one or more integral values of angular velocity information, the unit can be handled in common with the rotation angle information and the like.

(Modification 3)
In the first embodiment, the filter control unit 37 controls the cutoff frequency by comparing the angular velocity output from the angular velocity sensor 32 with the determination value S and the determination value −S. The filter control unit 37 may control the cutoff frequency using a differential value of the rotation angle information output from the angle sensor 34. A determination value for determining the differential value of the rotation angle information is set in advance. When the operation of the supply / discharge material arm 21 is settling, the filter control unit 37 controls the cutoff frequency of the filter unit 35 by comparing one or more differential values of the rotation angle information with the determination value.

  The cut-off frequency for the angular velocity information is determined by comparing one or more differential values of the rotation angle information with the determination value. For this reason, if it exceeds the determination value, the cutoff frequency is set high, and if it is lower, the cutoff frequency is set low. In the control using angular velocity information, when the angular velocity information becomes small, the effect of using the angular velocity information is reduced, and accuracy is lowered due to the influence of errors in the angular velocity information due to noise or the like.

  One or more differential values of the rotation angle information are compared with the determination value, and the angular velocity information is filtered. When one or more differential values of the rotation angle information exceed the determination value, angular velocity information with high responsiveness is obtained, and when the rotation angle information falls below the determination value, the error of angle information and the influence of noise can be reduced. Therefore, it is possible to reduce the influence of the error of the angular velocity information. Comparing one or more differential values of the rotation angle information with the determination value can be the same criterion as when comparing the rotation angle information with the determination value. By handling one or more differential values of the rotation angle information, the unit can be handled in common with the angular velocity information and the like.

(Modification 4)
In the first embodiment, the filter control unit 37 controls the cutoff frequency by comparing the angular velocity output from the angular velocity sensor 32 with the determination value S and the determination value −S. The filter control unit 37 may control the cutoff frequency using a difference between the angular velocity indicated by the control command and the angular velocity information. A determination value for determining the difference between the angular velocity and the angular velocity information indicated by the control command generator 36 is set in advance. When the operation of the supply / discharge material arm 21 is on standby, the filter control unit 37 compares the difference between the angular velocity and the angular velocity information indicated by the control command generation unit 36 with a determination value. Next, the filter control unit 37 determines whether or not to use the value of the angular velocity information.

  The difference between the angular velocity and the angular velocity information indicated by the control command generator 36 is compared with the determination value to determine whether to use the angular velocity information. When the difference between the angular velocity and the angular velocity information exceeds the determination value, the angular velocity information is not used. In the control using angular velocity information, if the difference between the angular velocity and the angular velocity information indicated by the control command is reduced, the effect of using the angular velocity information is reduced, and the operation is unstable due to the influence of errors in the angular velocity information due to noise or the like. Become. Therefore, it is possible to reduce the influence of the error of the angular velocity information.

  DESCRIPTION OF SYMBOLS 20 ... Robot mechanism as a robot, 22 ... Arm drive motor as a drive source, 31 ... Robot control part as a control part, 32 ... Angular velocity sensor as an inertial sensor, 34 ... Angle sensor, 35 ... Filter part, 36 ... Control Command generation unit, 37... Filter control unit.

Claims (7)

An arm that is rotatably supported at one end;
A drive source for rotating the arm;
An angle sensor that detects a rotation angle of the drive source and outputs rotation angle information of the drive source;
An inertial sensor that is attached to the arm and detects angular velocity at which the arm rotates and outputs angular velocity information of the arm;
A control unit for controlling the drive source,
The control unit is a control command generation unit that outputs a control command that instructs a rotation operation of the arm;
A filter unit for removing high frequency components of the angular velocity information;
E Bei and a filter control unit for controlling the cutoff frequency of the high frequency components,
The filter control unit determines whether the arm operation is moving, settling, or waiting according to the control command. When the arm operation is moving, the cutoff frequency of the filter unit is higher than the natural frequency of the arm. The cut-off frequency of the filter unit is set lower than the natural frequency of the arm when the operation of the arm is on standby, and the cut-off frequency is controlled by the angular velocity information when the operation of the arm is settling. Robot.   The robot according to claim 1, wherein the filter unit is a low-pass filter, and the cutoff frequency is variably controlled. The determination value for determining the angular velocity information is set in advance, and the filter control unit controls the cut-off frequency by comparing the angular velocity information with the determination value when the operation of the arm is settling. The robot according to 1 or 2 . A determination value for determining the rotation angle information is set in advance, and when the operation of the arm is settling, the filter control unit compares the rotation angle information with the determination value to control the cutoff frequency. The robot according to claim 1 or 2 , characterized in that: A determination value for determining an integral value of the angular velocity information is set in advance, and when the operation of the arm is settling, the filter control unit compares the angular velocity information one or more integral values with the determination value, and The robot according to claim 1 or 2 , wherein a cutoff frequency is controlled. A determination value for determining a differential value of the rotation angle information is set in advance, and when the operation of the arm is settled, the filter control unit compares the differential value of the rotation angle information once or more with the determination value. Te, robot according to claim 1 or 2, wherein the controller controls the cut-off frequency. A determination value for determining a difference between the angular velocity indicated by the control command and the angular velocity information is set in advance, and when the operation of the arm is on standby, the filter control unit calculates a difference between the angular velocity indicated by the control command and the angular velocity information. 3. The robot according to claim 1, wherein the filter control unit determines whether or not to use a value of angular velocity information as compared with the determination value.

Images