JP2012051042A - Robot system and robot control device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a robot system and a robot control device capable of reducing external force generated by contact between a robot arm and a peripheral object.SOLUTION: This robot system includes the robot arm 2, one or more of actuators 41A-47A arranged in the robot arm 2 and driving the robot arm 2, a sensor part 4 for detecting the external force applied to at least any of the robot arm 2 and the actuators 41A-47A, and a controller 3 for controlling operation of the actuators 41A-47A and limiting a torque command value to the actuators 41A-47A based on a detection result of the sensor part 4.

Description

  The present invention relates to a robot system having a robot arm and a robot control apparatus.

In the field of robots, there is a demand for avoiding an excessive load (external force) on the robot itself or objects existing around the robot.
For example, in Patent Document 1, a force detector that detects contact force (external force) is attached to the base end of the robot arm, and the operation of the robot arm is stopped based on the detection result of the force detector, or excessive external force is applied. In such a case, a technique such as operating a robot arm in a direction in which the external force is reduced is disclosed.

JP 2006-21287 A

However, as in Cited Document 1, when the operation of the robot arm is stopped after the external force is detected, a large impact force is generated even if the robot arm is stopped when the contacted object is moving. Even when the robot is operated in a direction to reduce the external force, the robot arm generates a new operation command after the external force is detected and operates. There is a problem that the response time until operation becomes long and it is difficult to mitigate the impact at the time of collision.
The present invention has been made in view of such problems, and an object of the present invention is to provide a robot system and a robot control apparatus capable of reducing external force generated by contact between a robot arm and a surrounding object. And

  In order to solve the above problems, a robot system according to the present invention includes a robot arm, one or more actuators provided on the robot arm for driving the robot arm, and a sensor for detecting an external force applied to at least one of the robot arm and the actuator. And a controller that controls the operation of the actuator and restricts the torque command value to the actuator based on the detection result of the sensor unit.

  Moreover, it has a position sensor which detects the position and attitude | position of a control point which is a point where a position and attitude | position are controlled by a controller among robot arms or actuators, and a controller respond | corresponds to the position and attitude | position which a control point should take Based on the operation command setting unit that sets the position of each actuator as a target position and sets the operation command of each actuator, and at least the operation command and the detection result of the position sensor, On the other hand, based on the detection result of the servo unit and the sensor unit that generates a torque command, an external force detection unit that detects the direction of the external force when an external force is generated, and 1 based on the direction of the external force detected by the external force detection unit Among the above actuators, the avoidance axis selection unit that selects the actuator to perform the avoidance operation and the servo unit The torque command generated by the servo unit for each of the gravity compensation torque adding unit for adding the gravity compensation torque corresponding to its own weight to the generated torque command and the actuator selected by the avoidance axis selection unit, the friction of the actuator itself It is preferable to have a friction compensation torque adding section that adds a friction compensation torque for operating in the direction of the external force against the force.

Moreover, it is preferable that the controller has an operation command blocking unit that blocks an operation command given to the servo unit when an external force is detected by the external force detection unit.
In addition, when the external force detection unit does not detect an external force for a preset period after the operation command is cut off by the operation command cut-off unit, the controller cancels the cut-off of the operation command by the operation command cut-off unit. It is preferable to have.
Further, the controller has a torque command limiting unit that limits a torque command value to the actuator, and the torque command limiting unit is based on a sum of the gravity compensation torque and the friction compensation torque. Is preferably set.

Moreover, it is preferable that the sensor part is formed including a plurality of crystal resonators using quartz as a piezoelectric body.
The sensor unit includes a disk-shaped sensor fixing jig provided at the base of the actuator on the most proximal side of the robot arm, and a plurality of sensors embedded in the sensor fixing jig along the same arc. It is preferable.

The external force detection unit preferably extracts high-frequency vibration components of signals from a plurality of sensors, and detects the presence or absence of an external force based on the extracted high-frequency vibration components.
It is preferable that the external force detector obtains the peak value of the extracted high-frequency vibration component for each sensor and detects the direction of the external force based on a positive / negative combination of the peak values for each sensor.
It is preferable that the avoidance axis selection unit calculates the avoidance target joint axis and the direction by performing threshold processing on each component of the vector obtained by multiplying the direction vector of the external force obtained by the external force detection unit by the transposed matrix of the Jacobian matrix.

The friction compensation torque adding unit adds the maximum static friction torque of the actuator as the friction compensation torque when the actuator selected by the avoidance axis selection unit is stationary, and when the actuator is rotating, It is preferable to add the dynamic friction torque as the friction compensation torque.
The external force detector preferably detects the presence or absence of an external force based on a moving average value obtained by moving and averaging the extracted high-frequency vibration components for each sensor.
It is preferable that the external force detection unit obtains an integrated value of the high-frequency vibration component for each sensor after the point in time when it is detected that there is an external force, and detects the direction of the external force based on a positive / negative combination of the integrated values.

  The controller has a data table in which the positive / negative combination of the integrated value for each sensor or the positive / negative combination of the moving average value for each sensor is associated with the direction of the external force. It is possible to detect the direction of external force based on a positive / negative combination of integrated values for each sensor or a positive / negative combination of moving average values for each sensor at a plurality of time points after the detection of the presence. preferable.

  The robot control apparatus of the present invention is a robot control apparatus having a robot arm having one or more actuators, and a sensor unit for detecting an external force applied to at least one of the robot arm and the actuator. On the other hand, an actuator control unit that transmits a torque command and a torque command limiting unit that limits a torque command value transmitted to the actuator based on an input result from the sensor unit are characterized.

According to the present invention, even if a disturbance caused by, for example, a ripple of a reduction gear included in an actuator acts on a sensor built in a robot arm, the external force can be more accurately detected without erroneously detecting the presence or absence of the external force. Presence / absence and direction can be detected. Since the torque command value for the actuator is limited when an external force is detected, the actuator is displaced following the external force when the external force exceeds the torque command value for each actuator, so that the impact due to a sudden external force load can be reduced. Can do.
Also, even when an external force is continuously applied to the robot arm, such as when an object around the robot arm continues to move toward the robot arm, the posture of the robot arm is determined by the actuator being displaced following the external force. By changing, it is possible to avoid objects around the robot arm.

Schematic configuration diagram for explaining the overall configuration of a robot system according to an embodiment of the present invention It is a side view showing the composition of the robot concerning the embodiment of the present invention. Schematic structure diagram showing the configuration of the sensor unit Block diagram for explaining the functional configuration of the contact detection unit Block diagram for explaining the functional configuration of the external force direction detector Schematic diagram showing table data Diagram showing the relationship between sensor placement and external force direction vector Block diagram for explaining the functional configuration of the avoidance axis selection unit Block diagram for explaining the functional configuration of the avoidance compensation unit Chart for explaining functions of torque limiter The block diagram of the contact detection part concerning 2nd Embodiment of this invention. The block diagram of the external force direction detection part concerning 2nd Embodiment of this invention. The figure which shows an example of the signal integration process result concerning 2nd Embodiment of this invention. The figure which shows an example of the external force direction narrowing-down condition table concerning 2nd Embodiment of this invention. The figure which shows an example of the external force direction narrowing-down process concerning 2nd Embodiment of this invention. Typical block diagram for demonstrating the whole structure of the robot system concerning 3rd Embodiment of this invention.

<First Embodiment>
Hereinafter, embodiments of the present invention will be described with reference to the drawings.
In the present embodiment, an embodiment of the present invention will be described using a robot system that performs operations such as inspection, disinfection, and milking of livestock using the robot arm 2 as an example.

As shown in FIG. 1, the robot system 100 according to the present embodiment includes a floor F in which livestock 1 is stored, a robot arm 2, a controller 3, and a sensor unit 4.
As shown in FIG. 2, the robot arm 2 includes a base 40 fixed to an installation surface (wall surface, floor, etc.) 101, a first structural member 41, and a first structural member 41 in order from the base 40 to the tip of the robot arm 2. The two structural members 42, the third structural member 43, the fourth structural member 44, the fifth structural member 45, the sixth structural member 46, and the flange 47 are connected via an actuator (rotary joint) that is driven to rotate.

  The base 40 and the first structural member 41 are connected via a first actuator (first joint) 41A, and the first structural member 41 is rotated by driving the first actuator 41A. . The first structural member 41 and the fourth structural member 42 are connected via a second actuator (second joint) 42A, and the second structural member 42 turns by driving the second actuator 42A. ing.

  The second structural member 42 and the third structural member 43 are connected via a third actuator (third joint) 43A, and the third structural member 43 is rotated by driving the third actuator 43A. ing. The third structural member 43 and the fourth structural member 44 are connected via a fourth actuator (fourth joint) 44A, and the fourth structural member 44 turns by driving the fourth actuator 44A. ing.

The fourth structural member 44 and the fifth structural member 45 are connected via a fifth actuator (fifth joint) 45A, and the fifth structural member 45 is rotated by driving the fifth actuator 45A. ing. The fifth structural member 45 and the sixth structural member 46 are connected via a sixth actuator (sixth joint) 46A, and the sixth structural member 46 turns by driving the sixth actuator 46A. ing.
The sixth structural member 46 and the flange 47 are connected via a seventh actuator (seventh joint) 47A, and the end effector 2A such as a hand attached to the flange 47 and the flange 47 by driving the seventh actuator 47A. Is designed to rotate.

Various tools (not shown) such as an inspection device, a milking device, and a disinfection device are attached to the end effector, and operations such as inspection, disinfection, and milking are performed on the target portion 1A of the livestock 1. ing.
Further, an object detection sensor (in the present embodiment, a camera but other various sensors can be applied) 2B is attached to the end effector (in other words, the tip portion of the robot arm 2) 2A.

The object detection sensor 2B is oriented with respect to a sufficiently wide detection region including the target portion 1A of the livestock 1 that moves irregularly within the floor F partitioned on all sides.
The image acquired by the object detection sensor 2B is input to the target recognition unit 2C of the controller 3, and the target recognition unit 2C detects the position of the target unit 1A of the livestock 1 and uses the detection result as a target position in the operation command unit 5 described later. It is designed to input in real time.
In the present embodiment, for ease of explanation, the controller 3 is shown as an integrated control device. However, the controller 3 is actually composed of a plurality of computers and the like, and a target recognition unit 2C and an external force detection unit (described later). ) May be configured by a separate control device from the robot controller that controls the operation of the robot arm 2.

  Each of the first to seventh actuators 41A to 47A is constituted by a speed reducer-integrated servo motor having a hollow portion through which a cable (not shown) can be inserted, and the rotational position of each actuator depends on the actuator. A signal from a built-in encoder is input to the controller 3 via a cable.

  As shown in FIG. 3, the sensor unit 4 includes a sensor fixing jig 15 and four sensors S <b> 1 to S <b> 4. The disk-shaped sensor fixing jig 15 is a stator of the first actuator 41 </ b> A of the robot arm 2. At the base of the.

The sensors S1 to S4 (16a to 16d) are embedded in a disk-shaped sensor fixing jig 15, and the sensors S1 to S4 are arranged at equal intervals along the same arc (virtual circle). Each of the sensors S1 to S4 uses a crystal as a piezoelectric body, and each of the sensors S1 to S4 detects the amount of strain in the radial direction of the sensor fixing jig 15 as a voltage, and the obtained voltage is an amplifier unit. The signal is amplified via 17 and input to the controller 3.
By using a crystal resonator (quartz piezoelectric element) for each of the sensors S1 to S4, the response time is smaller than when a strain gauge or other piezoelectric element is used, and a satisfactory response compared to the calculation cycle of the servo unit 7 described later. Thus, it is possible to sufficiently mitigate an impact caused by contact with an object around the robot arm.

  In the present embodiment, as described above, a crystal resonator is used for reasons such as fast response time. However, any type of sensor S1 to S4 can be used as long as good response can be obtained. Can be applied. In addition, the quartz piezoelectric sensor is superior in durability compared to a strain gauge and a normal collision sensor, and sufficiently accurate detection is possible even when the sensor unit 4 is provided at the base end where the load due to the weight of the robot arm 2 is the largest. There is also an advantage of being able to.

The operation of each drive part including each actuator 41 </ b> A to 47 </ b> A of the robot arm 2 is controlled by the controller 3.
The controller 3 is composed of a computer having a storage device, an electronic calculator, and an input device (all not shown in detail), and is connected to each drive part of the robot arm 2 so as to be able to communicate with each other.
As shown in FIG. 1, the controller 3 includes a target recognition unit 2C, an operation command unit 5, a smoothing processing unit 6, a servo unit 7, a contact detection unit (external force detection unit) 8, a position command cutoff unit (operation A command cutoff unit) 9, an external force direction detection unit 10, an avoidance shaft selection unit 11, an avoidance compensation unit (friction compensation torque addition unit) 12, a gravity torque compensation unit (gravity compensation torque addition unit) 13, and a torque limitation unit 14. ing.

As described above, the target recognition unit 2C obtains the position of the target unit 1A of the livestock 1 based on the detection result of the object detection sensor 2B, and the position of the target unit 1A is delivered to the operation command unit 5 as the target position.
Various methods can be applied to the method for recognizing the position of the target unit 1A by the target recognition unit 2C.

The operation command unit 5 calculates position commands (operation commands) for the actuators 41 </ b> A to 47 </ b> A based on the input of target positions from the target recognition unit 2 </ b> C and pools them in the smoothing processing unit 6.
In the smoothing processing unit 6, the pooled position commands are sequentially delivered to the servo unit 7 side every calculation cycle T (mm / sec).
For each actuator 41A to 47A, the servo unit 7 uses the joint angle feedback circuit Fp based on the detection value of the encoder of each actuator 41A to 47A and the joint angle based on the angular velocity detection value obtained from the detection value of the encoder of each actuator 41A to 47A. A feedback circuit Fv is provided, and a torque command Tref is output every calculation cycle T.

The contact detection unit 8 detects the presence or absence of contact between the livestock 1 and the robot arm 2 based on the output of the sensor unit 4.
The configuration of the contact detection unit 8 will be described in more detail. As shown in FIG. 4, the contact detection unit 8 includes a high-pass filter unit 18 and a threshold value determination unit 19.
The high-pass filter unit 18 includes high-pass filters (or band-pass filters) F1 to F4 that extract only high-frequency components for each sensor signal from the amplifier unit 17 of each sensor unit 4, and are caused by an external force. The signal component is separated and delivered to the external force direction detection unit 10.

For the adjustment of the high-pass filters F1 to F4, a contact experiment (for example, hitting a robot arm with a hammer or the like) is performed in advance, and the frequency of the detection signal of each sensor S1 to S4 when an external force is generated by the contact is measured. The cutoff frequency of each filter is determined so that frequency components other than the frequency caused by the external force can be removed.
By doing so, signal components included in the detection signals of the sensors S1 to S4 and peripheral objects or the like due to disturbance factors generated inside the robot arm 2, such as ripples of the reducers built in the actuators 41A to 47A It is possible to separate signal components caused by external force due to contact with the robot arm itself.

The threshold judgment unit 19 judges whether or not the absolute value of the detection signal (after passing through the high-pass filters F1 to F4) of each sensor S1 to S4 exceeds a preset threshold (adjusted by a collision experiment), and each sensor S1 to S4. If any one of the detection signals exceeds the threshold, it is determined that contact has occurred (external force is present).
When the threshold judgment unit 19 detects that the contact has occurred, the position command blocking unit 9 (see FIG. 1) is activated to block the delivery of the position command from the operation command unit 5 to the smoothing processing unit 6, The position command transmitted to the servo unit 7 is gradually reduced by the smoothing processing unit 6, and the position command to the servo unit 7 is blocked.
When the position command to the servo unit 7 is interrupted, the value of the torque command Tref output by feedback is naturally reduced, and the robot arm 2 immediately enters a stop operation.

As shown in FIG. 5, the external force direction detection unit 10 includes a peak calculation unit 20, a positive / negative determination unit 21, and a table reference unit 22 as functional configurations.
The peak calculation unit 20 calculates the peak value of the shock wave for each detection signal for each of the sensors S1 to S4 output from the high-pass filter unit 18. The peak value is the maximum of the absolute value of the detection signal (that is, the maximum value within a preset time).
The positive / negative determining unit 21 outputs the positive / negative (1 or −1) or zero of each calculated peak value.
That is, when the absolute value of the peak value is below a preset threshold value, zero (determination impossible) is output.

The table reference unit 22 refers to a data table set in the storage device in advance for the positive and negative combinations of the shock wave peaks of each sensor signal, so that the direction vector (external force acting upon contact with the livestock 1 or the like) ( External force direction vector) is calculated.
For example, as shown in FIGS. 6 and 7, the data table may be configured as a matrix in which positive and negative combinations of detection signals for the sensors S1 to S4 are associated with the direction of external force.
In this way, by referring to the data table and the positive / negative combination of the detection signals for each of the sensors S1 to S4, the direction of the collision can be roughly determined from the positive / negative of the shock wave peak at the time of collision using a plurality of sensor signals (this embodiment). In the example, it can be calculated in 8 directions). Since the sensor unit 4 is attached to the base 40 side of the robot arm 2, even if the contact object such as the livestock 1 contacts not only the vicinity of the distal end of the robot arm 2 but also a position on the more proximal side, the collision occurs. Can be calculated.

As illustrated in FIG. 8, the rotation axis selection unit 11 includes a vector conversion unit 23 and a joint vector positive / negative determination unit 24 as functional configurations.
The vector conversion unit 23 multiplies the external force direction vector described by the work coordinates of the robot arm 2 by the transposed matrix of the Jacobian matrix of the robot arm 2 and converts the vector into the vectors for the respective actuators 41A to 47A.
Here, since the robot arm 2 is a redundant manipulator, the vector of the actuator 43A (redundant axis) can be set to zero.
Then, the actuators 41A and 42A are converted into the respective axis vectors as the first axis and the second axis, respectively, and the actuators 44A to 47A are respectively converted as the third axis to the sixth axis. If the first to sixth axes are not specified, they are also simply referred to as axes.

As the Jacobian matrix, since the livestock 1 and the robot arm 2 are likely to contact each other in the vicinity of the wrist of the robot arm 2, the Jacobian matrix for the wrist point of the robot arm 2 may be calculated.
Moreover, although an inverse matrix can be used instead of the transposed matrix, the transposed matrix requires less calculation load than the inverse matrix.
The joint vector positive / negative determining unit 24 outputs positive / negative (1 or −1) for each component (component of each joint axis) of the external force direction vector converted into the joint space. If the absolute value is below a preset threshold value, zero is output.

As shown in FIG. 9, the avoidance compensation unit 12 includes a friction compensation vector calculation unit 25 and a friction compensation value storage unit 26 as functional configurations.
The friction compensation vector calculation unit 25 multiplies each component (± 1 or zero) of the external force direction vector in each joint space output from the avoidance axis selection unit 11 and outputs the result by multiplying the friction compensation value of each axis.
The friction compensation value for each axis is stored in the friction compensation value storage unit after being determined in advance.
The friction compensation value is based on the maximum static friction torque of the actuator corresponding to the shaft when the corresponding shaft is completely stationary, and the dynamic friction torque of the actuator corresponding to the shaft when the corresponding shaft is rotating. Thus, the friction compensation value is calculated.

The friction compensation vector (torque) output from the avoidance compensation unit 12 is added to the torque command Tref in the servo unit 7 together with the gravity compensation torque as shown in FIG.
The gravity compensation torque is calculated by the gravity torque compensation unit 13. Hereinafter, the sum of the friction compensation torque and the gravity compensation torque is referred to as an avoidance compensation torque Tavo.

Then, the total of the original torque command Tref and the avoidance compensation torque Tavo output from the feedback circuit of the servo unit 7 is input to the torque limiting unit 14.
Further, the output of the rotation axis selection unit 11 is input not only to the avoidance compensation unit 12 but also to the torque limiting unit 14. The torque limiter 14 limits the torque command value to the actuator corresponding to the axis whose output from the rotation axis selector 11 is +1 or −1.

  As shown in FIG. 10, the torque limiter 14 has a torque command value for the corresponding actuator within a torque limit width 27 (Wtrq) determined in advance through experiments or the like with the avoidance compensation torque 26 (Tavo) as the center value. It is set to limit to. Therefore, the motor torque is limited (limited) between the torque upper limit 28 (TU) and the torque lower limit 29 (TL), and the limited torque is sent to the actuators 41A to 47A as final torque command values. .

As described above, the avoidance compensation torque Tavo supports each actuator with respect to the moment load caused by its own weight (gravity compensation torque), and the robot arm 2 moves in the direction in which an external force acts on the static friction force or dynamic friction force of the actuator. 2 is a torque for operating 2 (friction compensation torque), and if the value of the torque upper limit 28 (TU) is too large, excessive torque is allowed to be generated, and on the contrary, it becomes a load on the robot arm 2. .
On the other hand, if the value of the torque lower limit 29 (TL) is too small, the joint is unintentionally rotated by gravity without supporting its own weight.

Therefore, by making the torque limit width Wtrq sufficiently small, it is possible to cause each joint of the robot arm 2 to follow the impact and external force caused by the collision without causing unnecessary force to act on the livestock 1. The torque limit width Wtrq is preferably as small as possible (the difference between the torque upper limit TU and the torque lower limit TL is small). However, since the preset gravity compensation torque and friction compensation torque are not always optimal and include errors, in order to allow an error in the preset gravity compensation torque and friction compensation torque, it is necessary to conduct experiments beforehand. The robot arm 2 can be controlled more stably by providing the obtained torque limit width Wtrq.
The torque limitation may be performed immediately after the avoidance axis is selected and the avoidance compensation torque is determined.

  Since the robot system according to the first embodiment of the present invention is configured as described above, at the start of work, the position of the target unit 1A of the livestock 1 is detected at any time based on the detection image from the object detection sensor 2B. By giving feedback to the servo unit 7 as a target position for each calculation cycle, even when the position of the target unit 1A changes irregularly, the end effector tool is moved toward the target unit 1A following the target unit 1A. In addition, operations such as inspection, disinfection, and milking can be performed on the target portion 1A of the livestock 1.

When contact (external force) is detected by the contact detection unit 8 due to, for example, the livestock 1 moving greatly during such work and contacting the robot arm 2, the position command blocking unit 9 is activated and the operation command Since the position command from the unit 5 is cut off and the position command is not transmitted to the servo unit 7, the robot arm 2 enters a stop operation.
As described above, at this time, the command data held in the smoothing processing unit 8 is sequentially delivered to the servo unit 7, so that an impact caused by the sudden stop of the robot arm 2 is reduced.

  On the other hand, the sensor output is passed from the contact detection unit 8 to the external force direction detection unit 10 where the direction of collision between the livestock 1 and the robot arm 2 is detected. The avoidance axis selection unit 11 selects which joint axis of the robot arm 2 is to be avoided based on the detected external force direction vector. The avoidance compensation unit 12 reduces the collision by performing motion compensation on the selected joint axis. The avoidance shaft selection unit 11 also acts on the torque limiting unit 14 to limit the torque of the avoidance shaft.

As a result, when the livestock 1 and the robot arm 2 come into contact with each other, the responsiveness is good and the robot arm 2 actively avoids the control in the series of servo units in the direction of the external force, so that the shock at the time of contact is reduced. Can do. Moreover, since the robot arm 2 stops after avoiding in the direction of external force from the position where the contact occurs, the possibility that the livestock 1 and the robot arm 2 continuously collide can be reduced.
Further, the torque limiting unit 14 of the livestock robot control apparatus according to the present embodiment is limited within the torque limit width Wtrq centered on the avoidance compensation torque with respect to the avoidance target axis. Even when moving to the side 2, the robot arm 2 can follow the direction of receiving impact force without applying unnecessary force to the livestock 1, and safety can be achieved without causing excessive external force to the livestock 1 and robot arm 2. There is an effect of improving.

In addition, since the collision detection unit 8 according to the present embodiment extracts only high-frequency vibration at the time of collision from the output signal of the sensor unit 4 by the high-pass filter unit 18, even when a disturbance such as a reducer ripple acts on the sensor, the collision detection unit 8 can operate at a high speed Sensitivity has an effect that the contact between the livestock 1 and the robot arm 2 can be detected.
In addition, the avoidance compensation unit 12 of the livestock robot control apparatus according to the present embodiment adds an avoidance compensation torque to the torque command, so that there is an effect that a response can be made faster than avoidance compensation by the position command.

[Second Embodiment]
Next, a second embodiment will be described. The robot system according to the present embodiment is different from that of the first embodiment only in the functional configuration of the controller 3, and the same components as those in the first embodiment are appropriately omitted from description by using the same reference numerals. explain.

FIG. 11 shows a detailed block diagram of the contact detection unit 8 in the present embodiment.
In the present embodiment, the contact detection unit 8 includes a moving average filter unit 30 between the high-pass filter unit 18 and the threshold determination unit 19.
The moving average filter unit 30 includes moving average filters 1 to 4 that output an average value of sensor signals at a predetermined time retroactively from each sensor signal output from the high-pass filter unit 18. Yes.
As a result, when a disturbance signal such as servo noise having a frequency component equal to or higher than the signal component of the shock wave passes through the high-pass filter, only the disturbance signal is removed by the moving average filter. As a result, it is possible to avoid the threshold judgment unit 19 from erroneously detecting contact other than during a collision. As described above, when the threshold judgment unit 19 detects contact, the position command blocking unit 9 is activated, and at the same time, the sensor signal passing unit 31 is activated, and the sensor signal output from the high-pass filter unit 18 is detected as the external force direction detection unit 10. Output to.

FIG. 12 shows a detailed block diagram of the external force direction detector 10 in the present embodiment.
The signal integrating unit 32 calculates the integrated value of the sensor signals input up to now for each sensor signal output from the sensor signal passing unit 31 of FIG. By using the integrated value of the signal, the positive / negative change of the signal becomes smooth, and the certainty of positive / negative discrimination at each time is improved.
As an example of the signal integration processing result, as shown in FIG. 13, the signal 34 before integration is input to the signal integration unit 32 to obtain a signal 35 after integration. The signal 35 after integration has a lower frequency of positive and negative changes than the signal 34 before integration.

The narrowing-down condition reference unit 33 refers to a predetermined external force direction narrowing-down condition table (data table) with respect to the positive / negative of each signal integrated value output by the positive / negative determining unit 21, and the direction of collision (external force direction vector). ) Is calculated.
As shown in FIG. 14, the narrowing-down condition table is data in which candidates for external force direction vectors corresponding to positive and negative of each sensor signal integrated value at a plurality of predetermined times are associated with each other and stored in the storage device of the controller 3. .

In the example of FIG. 14, the table includes four times of X1 to X4 (provided that X1 <X2 <X3 <X4).
The arrangement relationship between the sensors S1 to S4 and the external force direction vectors V1 to V8 is as shown in FIG. The narrow-down condition reference unit 33 extracts external force direction vector candidates that satisfy all the conditions for the positive and negative values of each sensor signal integrated value at each time. For example, as shown in FIG. 15, the case where the positive / negative combination of the integrated values of S1 to S4 at time X1 is (negative, negative, positive, positive) will be described.

When the integrated value of S1 is negative, the external force direction vector candidate is one of V1, V3, and V7. When the integrated value of S2 is negative, the external force direction vector candidate is one of V1, V5, and V7. At this time, either of the external force direction vector candidates corresponds to either V1 or V7. Similarly, one of V1 and V7 corresponds to an external force direction vector candidate for positive and negative of the integrated values of S3 and S4.
If the number of extracted external force direction vectors is 1, the extraction result (external force direction vector) is input to the avoidance axis selection unit 11. If the number of candidates is 2 or more, an external force direction vector candidate is calculated using both the extraction result of the external force direction vector candidate at the next time and the extraction result up to the current time.

  In the case of this example, the extraction of the external force direction vector is continued with reference to the combination of positive and negative integrated values at the next time X2 and the narrowing-down condition table. In this way, the narrowing is repeated until the number of candidates becomes 1 or less. By using sensor signals at a plurality of times, it is possible to improve the detection rate in the external force direction. Also, if the number of candidates becomes 0 by narrowing down, or if the number of candidates is 2 or more when all tables up to a predetermined narrowing end time are referenced, it is determined that the external force direction is unknown, and the external force All elements of the direction vector are set to zero and input to the avoidance axis selection unit 11.

Since the robot system according to the present embodiment is configured as described above, the collision detection unit 8 suppresses the disturbance that occurs in a very short time after passing through the high-pass filter unit 18 by the moving average filter unit 30. Thus, there is an effect that the erroneous detection that the contact has occurred when the object such as the robot arm 2 is not in contact with the robot arm 2 is further suppressed.
In addition, the external force direction detection unit 10 has an effect of improving the certainty of positive / negative identification at each time by reducing the frequency of signal positive / negative changes by the signal integrating unit 32. In addition, since the narrow-down condition reference unit 33 refers to sensor signals at a plurality of times, there is an effect of improving the detection rate in the external force direction.

[Third Embodiment]
Next, a third embodiment will be described. The robot system according to the present embodiment is different from that of the first embodiment only in the functional configuration of the controller 3, and the same components as those in the first embodiment are appropriately omitted from description by using the same reference numerals. explain.
As shown in FIG. 16, the present embodiment is different from the first embodiment in that an operation command return unit 38 is provided as a functional configuration of the controller 3.

The operation command return unit 38 contacts within a predetermined period (for example, a predetermined time after the collision avoiding operation) in a state where the contact detection unit 8 detects contact and blocks the position command from the operation command unit 5. When the detection unit 8 does not newly detect external force, the position command blocking unit 9 cancels the blocking of the position command,
The position command from the operation command unit 5 is set to be sent to the servo unit 7 side through the smoothing processing unit 6.

  Since the robot system according to the third embodiment of the present invention is configured as described above, by setting a predetermined period to an optimum value in advance by experiment or the like, for example, during the operation of the robot arm 2, for example, livestock 1 When the robot arm 2 comes into contact with the robot arm 2, the robot arm can avoid the excessive contact between the livestock 1 and the robot arm 2 by performing an avoiding operation in the direction of the external force for a predetermined period, and the predetermined period has elapsed. Thereafter, the approaching operation to the target unit 1A is resumed based on the detection result of the object detection sensor 2B. Therefore, the robot arm 2 does not stop even if contact occurs, or the livestock 1 can be stopped for a short time. Thus, inspection, disinfection and milking can be continued.

That is, depending on the application application of the robot system 100, while allowing slight contact between the robot arm 2 and surrounding objects, the robot system 100 is stable with respect to the work target (or the robot position target) without stopping the work. Work may be required.
In the case of this embodiment, for example, when the livestock 1 moves frequently and contacts the robot arm 2 frequently, the work efficiency may be significantly reduced if the robot arm 2 stops each time. By setting a predetermined period, when the livestock 1 and the robot arm 2 come into contact with each other, the robot arm 2 is operated to avoid the excessive external force (or impact) by avoiding the robot arm 2 in the direction of the external force. By continuing the work, there is an advantage that a reduction in work efficiency due to contact can be reduced.

  The embodiments of the present invention have been described above. However, the present invention is not limited to the above-described embodiments, and can be appropriately changed from the above embodiments without departing from the spirit of the present invention. Moreover, it is also possible to use combining the method of said each embodiment suitably. That is, it goes without saying that even a technique with such changes is included in the technical scope of the present invention.

For example, in the above-described embodiment, an example in which four crystal piezoelectric sensors are embedded in the fixing jig of the robot base unit as the sensor unit 4 has been described. However, the number of sensors may be three or less or five or more. Is also possible. Moreover, although the example which embedded the sensor in the jig | tool was demonstrated, a sensor can also be affixed on the robot surface. Furthermore, it is possible to use a sensor element other than the quartz piezoelectric sensor.
In addition, although the example in which the number of times of external force direction narrowing is set to a maximum of 4 times, the embodiment can be implemented even if it is 3 times or less or 5 times or more.
In addition, this invention is not limited to the use which performs work with respect to livestock, but can be applied to various uses. For example, the present invention can be applied to a control device when a human and a robot collaborate or collaborate.

DESCRIPTION OF SYMBOLS 1 Livestock 2 Robot 3 Controller 4 Sensor part 5 Operation command part 6 Smoothing process part 7 Servo part 8 Contact detection part 9 Position command interruption | blocking part 10 External force direction detection part 11 Avoidance axis selection part 12 Avoidance compensation part 13 Gravity torque compensation part 14 Torque limiter 40 Base 41-46 Arm structure material 41A-47A Actuator 47 Flange

Claims (15)

A robot arm,
One or more actuators provided on the robot arm for driving the robot arm;
A sensor unit for detecting an external force applied to at least one of the robot arm and the actuator;
And a controller for controlling an operation of the actuator and limiting a torque command value to the actuator based on a detection result of the sensor unit. A position sensor for detecting the position and posture of a control point, which is a point whose position and posture are controlled by the controller, of the robot arm or actuator;
The controller is
An operation command setting unit for setting the position of each actuator corresponding to the position and posture to be taken by the control point as a target position, and setting an operation command for each of the actuators;
Based on at least the operation command and the detection result of the position sensor, the external force is generated based on the detection result of the servo unit that generates a torque command for each of the actuators for each predetermined calculation cycle and the detection result of the sensor unit. An external force detector that detects the direction of the external force when it occurs,
An avoidance axis selection unit that selects an actuator to perform an avoidance operation among the one or more actuators based on the direction of the external force detected by the external force detection unit;
A gravity compensation torque adding unit for adding a gravity compensation torque corresponding to its own weight to the torque command generated by the servo unit;
Friction compensation torque for operating in the direction of the external force against the frictional force of the actuator itself against the torque command generated by the servo unit for each of the actuators selected by the avoidance axis selection unit The robot system according to claim 1, further comprising: a friction compensation torque adding unit that adds The controller is
3. The robot system according to claim 2, further comprising an operation command blocking unit that blocks the operation command given to the servo unit when an external force is detected by the external force detection unit. The controller is
An operation command return unit for canceling the block of the operation command by the operation command blocking unit when an external force is not detected by the external force detection unit for a preset period after the operation command is blocked by the operation command blocking unit. The robot system according to claim 3, further comprising: The controller has a torque command limiting unit that limits a torque command value to the actuator,
The torque command limiting unit sets an upper limit value and a lower limit value of the torque command value based on a sum of the gravity compensation torque and the friction compensation torque. The robot system according to item. The sensor unit is
The robot system according to claim 2, wherein the robot system includes a plurality of sensors using quartz as a piezoelectric body. The sensor unit is
A disc-shaped sensor fixing jig provided at the base of the actuator on the most proximal side of the robot arm;
The robot system according to claim 6, comprising a plurality of the sensors embedded in the sensor fixing jig along the same arc. The external force detector is
The robot system according to claim 7, wherein high-frequency vibration components of signals from the plurality of sensors are extracted, and the presence or absence of the external force is detected based on the extracted high-frequency vibration components. The external force detector is
9. The robot system according to claim 8, wherein a peak value of the extracted high-frequency vibration component is obtained for each sensor, and the direction of the external force is detected based on a positive / negative combination of the peak values for each sensor. . The avoidance axis selection unit includes:
The avoidance target joint axis and direction are calculated by performing threshold processing on each component of a vector obtained by multiplying the direction vector of the external force obtained by the external force detection unit by a transposed matrix of a Jacobian matrix. Robot system. The friction compensation torque adding unit is
When the actuator selected by the avoidance axis selection unit is stationary, the maximum static friction torque of the actuator is added as the friction compensation torque,
11. The robot system according to claim 10, wherein when the actuator is rotating, the dynamic friction torque of the actuator is added as the friction compensation torque. The external force detector is
9. The robot system according to claim 8, wherein the presence or absence of the external force is detected based on a moving average value obtained by moving averaging the extracted high-frequency vibration components for each sensor. The external force detector is
9. The integrated value of the high-frequency vibration component for each sensor after the time point when the presence of the external force is detected is obtained, and the direction of the external force is detected based on a positive / negative combination of the integrated values. The robot system described. The controller is
A data table in which the positive / negative combination of the integrated values for each sensor, or the positive / negative combination of the moving average value for each sensor, and the direction of the external force are associated with each other;
The external force detector is
At a plurality of time points after the time point when the presence of the external force is detected, the external force based on the positive / negative combination of the integrated values for each sensor or the positive / negative combination of the moving average value for each sensor, respectively. The robot system according to claim 12 or 13, wherein a direction is detected. A robot control device comprising: a robot arm having one or more actuators; and a sensor unit for detecting an external force applied to at least one of the robot arm and the actuator,
An actuator controller for sending a torque command to the actuator;
A robot control device, comprising: a torque command limiting unit that limits the torque command value sent to the actuator based on an input result from the sensor unit.

Images