JP5807604B2 - Control device, control method, and robot - Google Patents

Description

  The present invention relates to a control device and a control method, and more particularly to a control method at the time of a robot collision and a control device using the control method. The present invention also relates to a robot that uses the control method.

  Usually, in the field of robot control and mechatronics control, a torque command is given to a motor amplifier through a low-pass filter (LPF) to prevent excitation of mechanical vibration. An LPF may also be used for a noise or noise detection signal for force or torque.

  Patent Document 1 discloses a technique for performing operation compensation by performing a low-pass filter operation on a disturbance load torque signal. The control device of Patent Document 1 detects the current value of the servo motor that drives the robot manipulator and the rotation speed of the servo motor, and detects the disturbance load torque related to the robot manipulator from the motor current value and the motor rotation speed. Then, the control device performs a motion compensation by performing a low-pass filter calculation that transmits only a low frequency signal among the detected disturbance load torque signals, calculates a position command value of the robot manipulator from the motion compensation calculation result, and performs compliance. Perform the action.

  With this configuration, even when a noise signal having a high frequency is included in the disturbance load torque signal, it is possible to obtain an effect that the compliance operation is not performed due to a malfunction.

JP 11-042576 A

  As shown in FIG. 7, the characteristics of the LPF related to the background art do not change with respect to the direction of the torque command. Therefore, the response is the same in both directions of increasing and decreasing the pressing force or pressing torque generated by the collision.

  Thus, since the response to the torque command does not depend on the torque command direction, the pressing force or the pressing torque generated by the collision cannot be reduced quickly.

  In view of the above problems, the present invention provides a control device, a control method, and a robot that can quickly reduce the pressing force or the pressing torque generated by the collision in a case where a collision with an unknown environment occurs. With the goal.

  The control device according to an aspect of the present invention includes a generation unit that generates a drive command signal that commands the drive to a drive unit that drives a manipulator, and a first cutoff of the drive command signal generated by the generation unit A first filter that attenuates or blocks a frequency band higher than the frequency, a collision determination unit that determines whether a collision has occurred in the manipulator based on a detection result in a detection unit that detects a predetermined physical quantity generated in the manipulator, and A collision direction determining means for determining the direction of the collision; and a setting means for setting the first cutoff frequency of the first filter based on the direction of the collision.

  The control method according to one aspect of the present invention includes a collision determination step for determining whether a collision has occurred in the manipulator based on a detection result in a detection unit that detects a predetermined physical quantity generated in the manipulator, and the collision has occurred. A collision direction determining step for determining the direction of the collision when it is determined, a setting step for setting a first cutoff frequency based on the direction of the collision, and driving the driving means for driving the manipulator. A generation step for generating a drive command signal to be commanded, and a first filter step for attenuating or blocking a frequency band higher than the first cutoff frequency set in the setting step of the drive command signal generated in the generation step; Have.

  The robot according to one aspect of the present invention includes a manipulator, a driving unit that drives the manipulator, a generation unit that generates a drive command signal that commands the driving unit to perform the driving, and a generation unit that generates the robot. A first filter for attenuating or blocking a frequency band higher than the first cut-off frequency of the driven command signal, a detection means for detecting a predetermined physical quantity generated in the manipulator, and the manipulator based on a detection result in the detection means Collision determination means for determining whether a collision has occurred, collision direction determination means for determining the direction of the collision, setting means for setting the first cutoff frequency of the first filter based on the direction of the collision, Are provided.

  According to the present invention, in the case where a collision with an unknown environment occurs, it is possible to quickly reduce the pressing force or the pressing torque generated by the collision.

It is a figure explaining the case of (a) collision detection by the force sensor of the robot which concerns on Embodiment 1, and (b) collision detection by a torque sensor. 2 is a block diagram illustrating a configuration of a control device according to Embodiment 1. FIG. FIG. 3 is a flowchart showing an operation flow of the control device according to the first embodiment. FIG. 3 is a block diagram illustrating an overall configuration of a robot according to a second embodiment. FIG. 6 is a flowchart showing an operation flow of the control device according to the second embodiment. 6 is a diagram illustrating an example of filter characteristics of a low-pass filter according to Embodiment 2. FIG. It is a figure which shows an example of the filter characteristic of the conventional low-pass filter.

  Embodiments of the present invention will be described below with reference to the drawings. The following description shows preferred embodiments of the present invention, and the scope of the present invention is not limited to the following embodiments. In the following description, the same reference numerals indicate substantially the same contents.

(Embodiment 1)
Embodiments of the present invention will be described below with reference to the drawings. FIG. 1 is a diagram illustrating a state in which the arm 12 collides with an obstacle in the robot 10 according to the first embodiment.

  The motor 11 drives the arm 12 based on a drive command signal from the control device 20 described later.

  The arm 12 is connected to the motor 11 via a joint shaft 13. 1A shows a case where the force sensor 14 is installed on the arm 12, and FIG. 1B shows a case where the torque sensor 15 is installed on the joint shaft 13. .

  The force sensor 14 detects the collision with the obstacle by detecting an external force applied to the arm 12. The torque sensor 15 detects a collision with an obstacle by detecting a torque generated in the joint shaft 13.

  In the following description, the entire part including the joint shaft 13 in the arm 12 will be referred to as a manipulator. Further, the manipulator used in this specification includes various types of movable parts connected to a driving device such as a motor such as a leg part corresponding to a leg and a head part corresponding to a neck in addition to an arm part of a robot. Shall be included.

  Further, in the following description, the force sensor 14 or the torque sensor 15 is collectively described as a sensor 16 that is a detection unit that detects a predetermined physical quantity applied to the manipulator.

  Further, in the following description, the counterclockwise rotation shown in FIG. 1 is defined as the positive direction, and the direction of the force / torque generated by colliding with the obstacle when rotating in this direction is defined as the positive direction. explain.

  In addition, the robot used in this specification has a machine that performs automatic work that continues autonomously to some extent, such as an industrial robot, and a shape and function similar to humans and animals, such as a humanoid robot and an animal robot. Including machines that have it.

  Next, the control system of the robot 10 will be described. FIG. 2 is a block diagram illustrating a configuration of the control device 20 that controls the robot 10. The control device 20 outputs a drive command signal for issuing a drive command to the motor 11 which is a driving means for driving the arm 12 which is a manipulator, and inputs a detection signal which is a measured value of a physical quantity measured by the sensor 16. To do. Further, an operation command signal is input from the outside, and a corresponding drive command signal is generated based on the operation command signal and output. Here, the control device 20 specifically inputs a detection signal that is a detection result of force or torque from the sensor 16.

  The control device 20 includes a drive command signal generation unit 21, a first filter unit 22, a collision determination unit 23, a collision direction determination unit 24, and a filter setting unit 25.

  The drive command signal generation unit 21 generates a drive command signal that commands the motor 11 that drives the manipulator. Specifically, the drive command signal generation unit 21 outputs a torque command signal for generating torque. The motor 11 generates torque based on the torque command signal.

  Here, the drive command signal generation unit 21 generates a drive command signal according to the control mode. The control mode includes a collision handling mode when a collision occurs, in addition to a normal mode during normal operation. The control mode is switched from the normal mode to the collision handling mode based on the control mode switching instruction from the collision determination unit 114.

  The first filter unit 22 attenuates or blocks a frequency band higher than the first cutoff frequency (first cutoff frequency) of the drive command signal generated by the drive command signal generation unit 21, and is a frequency band lower than the cutoff frequency. Transparent. The 1st filter part 22 outputs the drive command signal after the filter from which the high frequency component was removed.

  The collision determination unit 23 determines whether a collision has occurred in the arm 12 based on the detection signal input from the sensor 16. For example, the sensor 16 detects the force or torque generated when the arm 12 collides with an obstacle, and the collision determination unit 23 compares the detection signal from the sensor 16 with the value of the force or torque during normal operation. To determine if there is a collision.

  When it is determined that a collision has occurred, the collision determination unit 23 switches the control mode in the drive command signal generation unit 21 from the normal mode to the collision handling mode. The drive command signal generation unit 21 that has received the control mode switching instruction signal from the collision determination unit 23 switches the control mode from the normal mode to the collision handling mode.

  The collision direction determination unit 24 determines the direction of the collision based on the detection signal from the sensor 16. Here, the collision direction determination unit 24 determines whether the collision direction is positive or negative according to the positive or negative value of the detection signal from the sensor 16.

  The filter setting unit 25 sets the first cutoff frequency of the first filter unit 22 based on the direction of collision. When the collision direction determination unit 24 determines that the collision direction is the positive direction, the filter setting unit 25 sets the positive cutoff frequency of the first filter unit 22 low and sets the negative cutoff frequency high. On the other hand, when the collision direction determination unit 24 determines that the collision direction is negative, the filter setting unit 25 sets the positive cutoff frequency of the first filter unit 22 high and sets the negative cutoff frequency low. To do. By setting the filter in this way, it is possible to quickly leave the collision state.

  Next, the operation of the control device 20 will be described. FIG. 3 is a flowchart showing an operation flow of the control device 20.

  When the control cycle is started, the collision determination unit 23 determines whether a collision has occurred in the manipulator based on the detection signal input from the sensor 16 (step S101).

  When it is determined that no collision has occurred (No in step S101), the collision determination unit 23 maintains the control mode of the drive command signal generation unit 21 in the normal mode, and the drive command signal generation unit 21 operates in the normal mode. A drive command signal for performing various controls such as torque control is generated (step S102). The drive command signal generated in step S102 is output after filter processing for attenuating or reducing a frequency band higher than the first cutoff frequency is performed by the first filter 22 (step S103).

  On the other hand, when it is determined that a collision has occurred (Yes in step S101), the collision determination unit 23 switches the control mode of the drive command signal generation unit 21 to the collision handling mode (step S104).

  Subsequently, the collision direction determination unit 24 determines the collision direction based on the sign of the detected value of the force or torque indicated by the detection signal (step S105).

  Next, the filter setting unit 25 sets the first cutoff frequency of the first filter 22 based on the collision direction determined by the collision direction determination unit 24 (step S106).

  Next, the drive command signal generation unit 21 that has been switched to the collision handling mode in step S104 generates a drive command signal for performing control for leaving the collision state (step S107). In the form of FIG. 1, a collision occurs in the arm 12 due to the rotation operation around the joint shaft 13 driven by the motor 11. Therefore, the drive command signal generation unit 21 generates a torque command signal in order to perform torque control for specifically leaving the collision state.

  The first filter 22 performs a process of attenuating or blocking a frequency band greater than the first cutoff frequency set in step S106 with respect to the torque command signal generated in step S107 (step S108).

  The motor 11 drives the arm 12 based on the filtered drive command signal processed in step S103 or step S108.

  As described above, according to the control device of the first embodiment, when a collision occurs, the high-frequency component of the command signal that drives the manipulator away from the collision target is the high-frequency component of the command signal in the normal mode. Means is provided for setting the cutoff frequency of the first filter high so that it remains. With this configuration, when a collision is detected, a drive command in a direction away from the object of failure is quickly transmitted to the drive means, so that it is possible to quickly leave the collision state.

  Further, the collision determination in the collision determination unit is performed using the detection signal before the filter, and the generation of the drive command signal in the drive command signal generation unit is performed based on the detection signal after the filter. Detachment control can be realized.

(Embodiment 2)
The control device according to the second embodiment is characterized in that, in addition to the cutoff frequency for the drive command signal, the cutoff frequency for the detection signal input from the sensor is controlled based on the direction of the collision.

  In the second embodiment, specifically, a description will be given on the assumption that a torque command is issued as a drive command, and a force or torque is detected by a detection means. The driving means is specifically a motor, and the manipulator is an arm. However, the second embodiment is not limited to this configuration. Hereinafter, description will be given with reference to the drawings. However, a part of the description already given in Embodiment 1 is omitted for the sake of clarifying the invention.

  FIG. 4 is a block diagram showing a configuration of the robot 100 according to the second embodiment. The robot 100 includes a control device (controller) 110, a motor amplifier 120, a motor 130, an encoder 140, an arm 150, and a sensor 160.

  The control device 110 performs various controls such as position control, speed control, torque control, and force control according to the control mode. The control device 110 has at least two control modes of a normal mode and a collision handling mode, and performs control according to the control mode.

  The motor amplifier 120 generates torque according to the command by controlling the current of the motor 130 according to the filtered torque command signal output from the control device 110.

  The motor 130 is driven by the motor amplifier 120 to generate torque.

  The encoder 140 obtains the current position of the arm 150 based on the rotation of the motor 130 and outputs it to the control device 110 as a position signal.

  The arm 150 is coupled to the motor 130 via a joint shaft, and exchanges force with the external environment.

  The sensor 160 detects a force or torque generated between the arm 150 and the external environment. The sensor 160 may be composed of two sensors, a force sensor that detects the force and a torque sensor that detects torque. A signal relating to the force or torque detected by the sensor 160 is output to the control device 110 as a detection signal.

  Next, the control device 110 will be described in detail. The control device 110 includes a drive command signal generation unit 111, a first filter 112, a second filter 113, a collision determination unit 114, a collision direction determination unit 115, and a filter setting unit 116. Except for the second filter 113, each has substantially the same functions as the drive command signal generation unit 21, the first filter 22, the collision determination unit 23, the collision direction determination unit 24, and the filter setting unit 25 in the first embodiment. Therefore, some explanation is omitted.

  The drive command signal generation unit 111 performs various controls performed in the next control cycle based on an operation command signal input from the outside, a position signal input from the encoder 140, a post-filter detection signal input from the second filter unit 113, and the like. A torque command signal for performing is generated. Here, when a control mode switching instruction signal is input from the collision determination unit 114, the drive command signal generation unit 111 changes the control mode from the normal mode in which various normal controls are performed to the collision handling mode.

  In the collision handling mode, the drive command signal generation unit 111 generates a torque command signal according to a predetermined algorithm for leaving the collision state when a collision occurs.

  The first filter unit 112 and the second filter unit 113 perform filter processing at the cutoff frequency set individually by the filter setting unit 116. The first filter unit 112 outputs the filtered torque command signal to the motor amplifier 120. The second filter unit 113 outputs the detection signal after filtering to the drive command signal generation unit 111.

  Since the first filter unit 112 is a filter that transmits a frequency band equal to or lower than the first cutoff frequency with respect to the torque command signal, the first filter unit 112 may be referred to as a torque command LPF (torque command low-pass filter) in the following description. In addition, the second filter unit 113 is a filter that transmits a frequency band equal to or lower than the second cutoff frequency with respect to the detection signal, and may be referred to as a detection signal LPF (detection signal low-pass filter) in the following description.

  The collision determination unit 114 determines the presence or absence of a collision between the arm 150 and the external environment based on the detection signal from the sensor 160, and switches the control mode to the normal mode or the collision handling mode. Here, the collision determination unit 114 performs the collision determination based on the detection signal before being filtered by the second filter unit 113. Accordingly, the drive command signal generation unit 111 switches the control mode based on the pre-filter detection signal, and generates a torque command signal corresponding to the control mode based on the post-filter detection signal.

  When the collision determination unit 114 detects a collision, the collision direction determination unit 115 determines the collision direction from the value of the pre-filter detection signal, and outputs the determination result to the filter setting unit 116.

  The filter setting unit 116 sets the first cutoff frequency of the first filter unit 112 and the second cutoff frequency of the second filter unit 113 based on the determination result in the collision direction determination unit 115, respectively. Here, the filter setting unit 116 can set the first cutoff frequency and the second cutoff frequency to the same frequency, but it is preferable to select and set the optimum frequencies individually.

  Subsequently, a control operation in the robot 100 will be described. FIG. 5 is a flowchart showing a flow of control operation of the robot 100.

  When the control cycle is started, the collision determination unit 114 determines whether a collision has occurred in the manipulator based on the detection signal input from the sensor 160 (step S201).

  Here, as an example, the collision determination unit 114 directly detects a detection signal (force / torque detection value) from the sensor 160 without passing through the second filter unit 113, and a force / torque value to be output in the normal mode ( It is determined whether or not a collision has occurred by comparing with an assumed detection reference value.

  If the detection value is large as a result of comparison between the value indicated by the detection signal and the reference value, the collision determination unit 114 determines that a collision has occurred (Yes in step S201), and the processing flow in the collision countermeasure mode is as follows. The process proceeds to a certain step S204 to step S210. On the other hand, as a result of the comparison between the value indicated by the detection signal and the reference value, if the detected value is substantially the same value as the reference value or less than the reference value, the collision determination unit 114 It is determined that it has not occurred (No in step S201), and the process proceeds to steps S202 to S203, which is a process flow in the normal mode.

  When it is determined in step S201 that no collision has occurred, the drive command signal generation unit 111 generates a drive command signal in the normal mode (step S202). Here, the drive command signal generation unit 111 receives the torque value that should be generated at present to perform the operation content instructed by the operation command signal input from the outside, the position signal input from the encoder 140, and the second filter unit 113. A torque command signal is generated by calculation based on a signal (information) such as a post-filter detection signal to be input. Further, the drive command signal generation unit 111 can generate various drive command signals for performing speed control, position control, force control, and the like in addition to the torque command signal generated for performing torque control.

  The first filter unit 112 performs a filter process that transmits the low-frequency region on the drive command signal (torque command signal) generated in step S202, and outputs the filtered torque command signal to the motor amplifier 120 ( Step S203). Here, the first filter unit 112 performs a filter process according to the cutoff frequency set by the filter setting unit 116.

  On the other hand, when it is determined in step S201 that a collision has occurred, the drive command signal generation unit 111 switches the control mode from the normal mode to the collision handling mode (step S204).

  Specifically, when the collision determination unit 114 determines that a collision has occurred in step S201, the collision determination unit 114 outputs a control mode switching instruction signal to the drive command signal generation unit 111, and the drive command signal generation unit 111 performs the control. Based on the mode switching instruction signal, the control mode is switched from the normal mode to the collision handling mode.

  Next, the collision direction determination unit 115 determines the collision direction (step S205). Specifically, the collision direction is determined based on the positive / negative of the input detection signal (force / torque detection value). After determining the collision direction after the collision occurrence determination in the collision determination unit 114, the collision direction determination unit 115 determines that the direction determination remains unchanged until the collision handling mode is exited, and the process can be omitted. That is, until the control mode is switched from the collision handling mode to the normal mode, the determination result of the collision direction determination performed by the collision direction determination unit 115 after the collision determination unit 114 determines that a collision has occurred is retained. A configuration in which the determination is not performed again can be adopted.

  Next, the filter setting unit 116 sets the cutoff frequencies of the first filter unit 112 and the second filter unit 113 based on the determination result of the collision direction determination in step S205 (steps S206 to S209).

  If the result of the collision direction determination in step S205 is a positive collision (Yes in step S205), the filter setting unit 116 sets the cutoff frequency for the drive command signal in the positive direction of the first filter unit 112 low, A cut-off frequency for the drive command signal in the negative direction is set high (step S206).

  Subsequently, the filter setting unit 116 sets the cutoff frequency for the detection signal in the positive direction of the second filter unit 113 high, and sets the cutoff frequency for the detection signal in the negative direction low (step S207).

  On the other hand, if the result of the collision direction determination in step S205 is a negative collision (No in step S205), the filter setting unit 116 sets a high cutoff frequency for the positive direction drive command signal of the first filter unit 112. Then, the cut-off frequency for the negative direction drive command signal is set low (step S208).

  Subsequently, the filter setting unit 116 sets the cutoff frequency for the detection signal in the positive direction of the second filter unit 113 low, and sets the cutoff frequency for the detection signal in the negative direction high (step S209).

  Note that the order of step S206, step S207, step S208, and step S209 can be changed.

  Next, the drive command signal generation unit 111 generates a drive command signal in the collision handling mode (step S210). Specifically, the drive command signal generation unit 111 is a signal such as a position signal input from the encoder 140, a post-filter detection signal input from the second filter unit 113, and the torque value necessary for leaving the collision state. Based on (information), a torque command signal is generated.

  At this time, the post-filter detection signal input from the second filter unit 113 is filtered at the cutoff frequency set by the filter setting unit 116 in step S207 or step S208. In step S207 and step S209, the setting is such that detection in the direction in which the pressing force / pressing torque decreases, that is, in the direction away from the collision target is delayed. A desired operation can be obtained when the drive command signal generation unit 111 generates a torque command signal after filtering the detection signal based on the setting.

  This point will be described in detail. When a collision occurs, if the pressing force / torque becomes excessive, it will lead to destruction of the object and itself, so avoiding such a situation is the top priority. Here, if a signal in a direction in which the pressing force / torque decreases is detected early, a torque command in a direction in which the pressing force / torque decreases, that is, in a direction away from the collision target, in order to prevent excessive reduction of the pressing force / torque. On the other hand, it becomes smaller and does not work as desired. Therefore, by setting so that the detection of the direction in which the pressing force / torque decreases is delayed, in order to reduce the pressing force / torque more quickly, the torque command in the direction in which the pressing force / torque decreases increases, resulting in a collision. This makes it easier to move away from the target and obtains a desired action.

  The torque command signal generated in step S210 is output to the motor amplifier 120 after being filtered in the first filter unit 112 (step S211).

  Here, the cut-off frequency of the first filter unit 112 is set so that the torque command in the direction in which the pressing force / torque is reduced by the filter setting unit 116, that is, the direction away from the collision target is quickly executed in step S206 and step S208. Is set to The first filter unit 112 performs a filtering process on the torque command signal based on the setting and then outputs the filtered signal to the motor amplifier 120, so that the first filter unit 112 can easily move away from the collision target and obtain a desirable operation. become.

  The motor amplifier 120 performs current control on the motor 130 based on the torque command signal after the filter processing to drive the motor 130, and the motor 130 drives the arm 150 according to current control from the motor 120.

  FIG. 6 shows filter characteristics of the first filter unit 112 with respect to the torque command signal. Compared with the filter characteristics of the conventional torque command low-pass filter shown in FIG. 7, a quick response is made to the torque command for the operation in the direction away from the colliding object, and the operation in the direction of further pressing the colliding object is performed. Response to the torque command is slow.

  As described above in each embodiment, the collision control device of the present invention includes a collision direction determination unit that determines the direction of the collision, and a cutoff frequency of the low-pass filter (LPF) for the drive command signal based on the direction of the collision. And a filter setting unit for setting. According to this configuration, in addition to the fact that a collision has occurred, the collision direction is judged, and the filter process that reflects the collision direction is performed and a drive command is issued to appropriately speed up the departure from the collision state. Can do.

  Here, the filter setting unit sets the cutoff frequency of the first LPF for the drive command signal for driving the manipulator in the direction opposite to the direction of the collision, and the cutoff of the first LPF for the drive command signal for driving the manipulator in the direction of the collision. It is still better to set higher than the frequency. With this setting, it is possible to realize a filter characteristic in which priority is given to leaving from a collision state.

  It further includes a second LPF that attenuates or cuts off a frequency band higher than the second cutoff frequency of the detection signal output from the detection unit such as a torque sensor, and commands the drive to drive the manipulator. The generation unit that generates the drive command signal is favorable because the mechanical vibration can be further suppressed when the drive command signal is generated based on the detection signal after the filter processing in the second LPF. Here, as with the first LPF, the filter setting unit may set the second cutoff frequency of the second LPF based on the direction of collision.

  At this time, the filter setting unit increases the second cutoff frequency of the second LPF with respect to the detection signal in the collision direction higher than the second cutoff frequency of the second LPF with respect to the detection signal in the direction opposite to the collision direction. Even better when set. By adopting such a configuration, it is possible to realize a filter characteristic in which priority is given to leaving from a collision state.

  Further, as described above, the collision determination in the collision determination unit is performed using the detection signal before the filter, and the generation of the drive command signal in the drive command signal generation unit is performed based on the detection signal after the filter, Detachment control from an appropriate collision state can be realized.

  In the above description, a case has been described in which torque is detected by a detection unit such as a sensor and a torque command is issued as a drive command to be output to a drive unit such as a motor amplifier. However, the present invention is not limited to this. For example, the detection unit can detect vibration generated in a manipulator such as an arm, and the collision determination unit can determine whether a collision has occurred based on the detected vibration waveform, amplitude, or the like. .

  That is, in the above description, the collision determination unit determines whether a collision has occurred in the manipulator based on the detection result in the detection unit that detects the force or torque applied to the manipulator, and the collision direction determination unit determines whether the force applied to the manipulator The case where the direction of the collision is determined based on the detection result in the detecting means for detecting the torque has been described. On the other hand, the present invention is not limited to this, and it is possible to determine a collision and a collision direction based on information from sensors that detect various physical quantities.

  In the above description, the case where the control mode is switched from the normal mode to the collision handling mode has been mainly described. However, when the control mode is released from the collision state, the control device appropriately switches from the collision handling mode to the normal mode. Whether or not the vehicle has left the collision state can be determined by providing a predetermined reference value for the time or force / torque detection value and comparing it with the reference value in each control cycle in the collision handling mode. Is possible. As a result of the comparison, when the collision state has been resolved, the collision determination unit switches the control mode from the collision handling mode to the normal mode.

  Note that the present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention. For example, in the above description, each filter unit and the filter setting unit have been described as different functional configurations, but each filter unit and the filter setting unit may be integrally formed.

  That is, the control device of the present invention includes a collision determination unit that determines whether or not a collision has occurred in the manipulator based on a detection result of a sensor that detects a predetermined physical quantity generated in the manipulator, and a collision that determines the direction of the collision. A direction determination unit, a generation unit that generates a drive command signal that commands the drive to a drive unit that drives the manipulator, and the drive command signal that drives the manipulator in a direction opposite to the direction of the collision. On the other hand, the structure which comprises the filter part which performs a filter process with the cutoff frequency higher than the drive command signal which drives the said manipulator in the direction of the said collision is contained. More specifically, the filter unit is a drive command signal for driving the manipulator in a direction opposite to the collision direction determined by the collision direction determination unit. In some cases, the filter processing is performed at a higher cutoff frequency than the drive command signal that drives the manipulator in the direction of the collision, thereby allowing higher frequency band components to pass. By setting it as the said structure, the detachment | leave from a collision state can be accelerated.

  The same applies to the filter unit related to the detection signal. That is, the control device of the present invention includes a detection signal low-pass filter unit that performs a filtering process on a detection signal in a direction opposite to the collision direction at a cutoff frequency lower than that of the detection signal in the collision direction. The generation unit that generates the drive command signal that commands driving generates the drive command signal for the drive unit that drives the manipulator based on the detection signal after the filter processing in the detection signal low-pass filter unit. Of course it is possible.

  Each function in the control device described above may be realized by hardware such as an individual device or an electronic circuit, or may be realized by causing a CPU (Central Processing Unit) to execute a computer program. Further, the above-described program can be stored using various types of non-transitory computer readable media and supplied to a computer. Non-transitory computer readable media include various types of tangible storage media. Examples of non-transitory computer-readable media include magnetic recording media (for example, flexible disks, magnetic tapes, hard disk drives), magneto-optical recording media (for example, magneto-optical disks), CD-ROM (Read Only Memory) CD-R, CD -R / W, semiconductor memory (for example, mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM (Random Access Memory)). The program may also be supplied to the computer by various types of transitory computer readable media. Examples of transitory computer readable media include electrical signals, optical signals, and electromagnetic waves. The temporary computer-readable medium can supply the program to the computer via a wired communication path such as an electric wire and an optical fiber, or a wireless communication path. Moreover, it is also possible to combine each embodiment mentioned above.

DESCRIPTION OF SYMBOLS 10 Robot 11 Motor 12 Arm 13 Joint axis 14 Force sensor 15 Torque sensor 16 Sensor 20 Control device 21 Drive command signal generation part 22 Filter part 23 Collision judgment part 24 Collision direction judgment part 25 Filter setting part 25 Setting part 100 Robot 110 Control apparatus DESCRIPTION OF SYMBOLS 111 Drive command signal generation part 112 Filter part 113 Filter part 114 Collision judgment part 115 Collision direction judgment part 116 Filter setting part 120 Motor amplifier 130 Motor 140 Encoder 150 Arm 160 Sensor

Claims (10)

Generating means for generating a drive command signal for instructing the drive to drive means for driving the manipulator;
A first filter that attenuates or cuts off a frequency band higher than the first cut-off frequency of the drive command signal generated by the generating means;
A collision determination means for determining whether a collision has occurred in the manipulator based on a detection result in a detection means for detecting a predetermined physical quantity generated in the manipulator;
A collision direction judging means for judging the direction of the collision;
Setting means for determining a value of the first cutoff frequency of the first filter according to a direction of the collision;
A control device comprising: The setting means sets the first cutoff frequency of the first filter for a drive command signal that drives the manipulator in a direction opposite to the direction of the collision, and a drive command signal that drives the manipulator in the direction of the collision. It is set higher than the first cutoff frequency of the first filter,
The control device according to claim 1. A second filter for attenuating or blocking a frequency band higher than the second cutoff frequency of the detection signal output from the detection means;
The setting means determines a value of the second cutoff frequency of the second filter according to a direction of the collision;
The control device according to claim 1 or 2. The setting means sets the second cutoff frequency of the second filter for the detection signal in the direction of the collision, and the second cutoff frequency of the second filter for the detection signal in the direction opposite to the direction of the collision. It is characterized by being set higher than
The control device according to claim 3. When the collision determination unit determines that the collision has occurred, the generation unit drives the driving based on the detection signal in which a frequency band higher than the second cutoff frequency is attenuated or blocked by the second filter. Generating a command signal,
The control device according to claim 3 or 4. The generation means generates a torque command signal that is a torque command for the drive means as the drive command signal,
The first filter attenuates or blocks a frequency band higher than the first cutoff frequency set by the setting unit among the torque command signals generated by the generation unit.
The control device according to any one of claims 1 to 5. The collision determination means determines whether a collision has occurred in the manipulator based on a detection result in a detection means for detecting a force or torque applied to the manipulator;
The collision direction determination means determines the direction of the collision based on a detection result in a detection means for detecting a force or torque applied to the manipulator;
The control device according to any one of claims 1 to 6. A collision determination step of determining whether or not a collision has occurred in the manipulator based on a detection result in a detection means for detecting a predetermined physical quantity generated in the manipulator;
A collision direction determination step of determining a direction of the collision when it is determined that the collision has occurred;
A setting step of determining a value of the first cutoff frequency according to the direction of the collision;
A generation step of generating a drive command signal for instructing the drive to drive means for driving the manipulator;
A first filter step for attenuating or blocking a frequency band higher than the first cutoff frequency set in the setting step of the drive command signal generated in the generation step;
A control method. A manipulator,
Driving means for driving the manipulator;
Generating means for generating a drive command signal for instructing the drive to the drive;
A first filter that attenuates or cuts off a frequency band higher than the first cut-off frequency of the drive command signal generated by the generating means;
Detecting means for detecting a predetermined physical quantity generated in the manipulator;
A collision determination means for determining whether a collision has occurred in the manipulator based on a detection result in the detection means;
A collision direction judging means for judging the direction of the collision;
Setting means for determining a value of the first cutoff frequency of the first filter according to a direction of the collision;
A robot comprising: The driving means is a motor for driving the manipulator;
The manipulator
An arm,
A joint shaft connecting the arm part and the motor;
Including
The detecting means detects either a force applied to the arm portion or a torque applied to the joint shaft;
The robot according to claim 9.

Images