JP6254911B2 - Hand control device - Google Patents

Description

  The present invention relates to a hand control device, and more specifically to a hand control device capable of determining an abnormality of a sensor that detects a physical quantity acting on a robot hand.

  Conventionally, for example, a technique described in Patent Document 1 is known as a robot capable of determining an abnormality such as a force sensor provided in a hand. The technique described in Patent Document 1 monitors the output of a force sensor or the like, and determines that the sensor is abnormal when these outputs exceed a specified value.

JP 2001-150374 A

  However, when determining sensor abnormality based on whether or not the sensor output exceeds a specified value, if the specified value is too small, it is not inherently caused by the sensor abnormality, for example, due to the influence of harness swinging, etc. There is a possibility that the output of the sensor may be determined to be abnormal. On the other hand, if the specified value is too large, the sensor may output an abnormal value, but it may not be detected and may be determined to be normal. As described above, there is a disadvantage that the sensor abnormality cannot be appropriately determined by the method of determining the sensor abnormality based on whether or not the output of the sensor simply exceeds the specified value.

  Accordingly, an object of the present invention is to solve the above-described problems and provide a hand control device that can accurately determine abnormality of a sensor that detects a physical quantity acting on the hand.

In order to solve the above-described problem, in claim 1, a hand operable by the power of the drive source, a sensor that generates an output indicating a physical quantity acting on the hand, and an operation of the drive source are controlled. In the robot hand control device including the drive source control unit, the drive source control unit executes an initial operation of moving the hand from the initial position to a position in contact with the target object along a predetermined trajectory. Initial operation execution means for controlling the operation of the drive source; and detecting the maximum value of the output of the sensor when the hand is caused to execute the initial operation; and detecting the maximum value of the hand when the maximum value is detected Position calculating means for calculating a position in the predetermined trajectory; distance calculating means for calculating a distance from the calculated position to the target; and Said sensor is configured as having a determining whether the sensor abnormality judgment means is abnormal based on the detected maximum value.

  In the hand control device according to claim 2, the sensor abnormality determination unit is configured such that when the calculated distance is less than a specified value and the detected maximum value is outside a predetermined range, the sensor It was configured to determine that it was abnormal.

  The hand control device according to claim 3 is configured such that the sensor abnormality determination unit determines that the sensor is abnormal when the calculated distance is equal to or greater than the specified value.

  In the hand control device according to claim 4, the hand has a plurality of finger mechanisms, and the sensor is provided in at least one of the plurality of finger mechanisms. Configured.

  In the hand control device according to claim 5, the target is configured to be any one of the plurality of finger mechanisms.

  In the hand control device according to claim 6, the drive source control means determines that the sensor is abnormal by the sensor abnormality determination means, and causes the hand to perform a gripping operation. When controlling the movement of the second finger mechanism other than the finger mechanism provided with the sensor determined to be abnormal, it is determined whether the gripping operation is possible. The operation of the drive source is controlled so that the gripping operation is performed by a second finger mechanism.

  8. The hand control device according to claim 7, wherein the drive source control means controls the operation of the drive source so as not to perform the gripping operation when it is determined that the gripping operation is not possible. did.

The hand control device according to claim 1 detects the maximum value of the output of the sensor when the hand is caused to execute an initial operation of moving from the initial position to a position in contact with the target object along a predetermined trajectory. Calculating the position of the hand in the predetermined trajectory when the maximum value is detected and the distance from the position to the target, and whether the sensor is abnormal based on the calculated distance and the detected maximum value For example, when the calculated distance becomes a value near zero, and the sensor does not output the original normal value corresponding to the hand even though the hand is in contact with the target, Alternatively, it is determined that the sensor is abnormal, for example, when the calculated distance is equal to or greater than a predetermined value and the maximum value of the sensor output is detected even though the hand is not in contact with the target. Bets since it is possible, it is possible to determine an abnormality of the sensor accurately.

  The hand control device according to claim 2 is configured to determine that the sensor is abnormal when the calculated distance is less than a specified value and the detected maximum value is outside a predetermined range, In addition to the above effects, the sensor outputs the original normal value when the hand touches the target because the hand is intentionally brought into contact with the target and the output of the sensor at that time is judged. Otherwise, it is possible to determine that the sensor is abnormal, and thus it is possible to determine the sensor abnormality more accurately.

  In the hand control device according to claim 3, since the sensor is determined to be abnormal when the calculated distance is equal to or greater than the specified value, the calculated distance is specified in addition to the effect described above. It is possible to determine that the sensor is abnormal even when the maximum value of the sensor output is detected even though the hand is not in contact with the target, so that the sensor abnormality can be determined with higher accuracy. Can be determined.

  In the hand control device according to claim 4, the hand has a plurality of finger mechanisms, and the sensor is configured to be provided in at least one of the plurality of finger mechanisms. In addition to the effect described above, an abnormality of the sensor provided in the finger mechanism can be determined. Therefore, for example, whether or not the gripping operation can be appropriately performed can be determined based on the determination result.

  In the hand control device according to claim 5, since the target is configured to be any one of a plurality of finger mechanisms, in addition to the above-described effects, for example, specially prepared for determination Even without using a member (a jig) having a contact surface (fixed surface) that has been made, it is possible to determine abnormality of the sensor simply by moving the finger mechanism.

  In the hand control device according to claim 6, the sensor is determined to be abnormal, and a sensor determined to be abnormal is provided when controlling the operation of the drive source so that the hand performs a gripping operation. It is determined whether or not a gripping operation can be performed by a second finger mechanism other than the finger mechanism, and when the gripping operation is determined to be possible, the operation of the drive source is controlled so that the gripping operation is performed by the second finger mechanism. Thus, in addition to the above-described effects, even when the sensor is determined to be abnormal when performing the gripping operation, the gripping operation can be performed by the remaining finger mechanisms that are not determined to be abnormal.

  The hand control device according to claim 7 is configured to control the operation of the drive source so that the gripping operation is not performed when it is determined that the gripping operation is not possible. It is possible to avoid inconveniences such as an unexpected movement of the hand or damage to the object to be grasped when trying to perform the grasping operation even though the operation is impossible.

It is a side view of the robot provided with the hand of the control apparatus of the hand which concerns on the Example of this invention. It is a top view of the hand of the robot shown in FIG. It is side surface sectional drawing of the finger mechanism of the finger mechanisms of the hand shown in FIG. 2 is a flowchart showing the operation of the ECU shown in FIG. It is explanatory drawing explaining the initial operation | movement of the finger mechanism shown in FIG. It is explanatory drawing explaining the case where it determines with the sensor provided in the finger mechanism shown in FIG. 2 being abnormal. It is explanatory drawing explaining the case where holding | grip operation | movement is performed by finger mechanisms other than the finger mechanism provided with the sensor determined to be abnormal. It is explanatory drawing similar to FIG. 5 explaining the modification of initial operation | movement. It is explanatory drawing explaining the state of the finger mechanism in the modification shown in FIG.

  DESCRIPTION OF EXEMPLARY EMBODIMENTS Hereinafter, embodiments for implementing a hand control device according to the invention will be described with reference to the accompanying drawings.

  FIG. 1 is a side view of a robot including a hand of a hand control device according to an embodiment of the present invention.

  As shown in the figure, the robot (denoted by reference numeral 10) is configured as a legged mobile robot having two legs 12 on the left and right.

  The leg 12 is connected to the lower part of the base body (upper body) 14 and includes a thigh link 16, a crus link 18, and a foot 20, respectively. The thigh link 16 is connected to the base body 14 via a hip joint. Three electric motors each having a rotation axis about the Z axis (yaw axis), the Y axis (pitch axis) (the front-rear direction of the robot 10), and the X axis (roll axis) (the left-right direction of the robot 10). A motor is arranged.

  The lower leg link 18 is connected to the thigh link 16 via a knee joint, and the foot 20 is connected to the lower leg link 18 via an ankle joint. An electric motor having a rotation axis around the Y axis is arranged at the knee joint, and an electric motor having rotation axes around the Y axis and the X axis is arranged at the ankle joint.

  A head portion 22 is connected to the upper portion of the base body 14, and left and right arm portions (links) 24 are connected to the side of the base body 14. Hands (end effectors) 26 are connected to the tips of the left and right arm portions 24, respectively.

  The left and right arm portions 24 include an upper arm link 28 and a lower arm link 30, respectively. The upper arm link 28 is connected to the base body 14 via a shoulder joint 32, and the lower arm link 30 is connected to the upper arm link 28 via an elbow joint 34. The hand 26 is connected to the lower arm link 30 via a wrist joint 36.

  Three electric motors having rotation axes around the Z axis, Y axis, and X axis are arranged at the shoulder joint 32, and an electric motor having a rotation axis around the Y axis is arranged at the elbow joint 34. The wrist joint 36 is provided with three electric motors having rotation axes around the Z axis, the Y axis, and the X axis.

  A storage unit 38 is provided on the back of the base 14, and an ECU (Electronic Control Unit) 40, a battery (not shown), and the like are accommodated therein. The ECU 40 includes a microcomputer having a CPU, an input / output circuit, a ROM, a RAM, and the like (not shown). The ECU 40 moves the robot 10 by controlling the operation of the electric motor of the leg 12 and the like. Control the behavior.

  2 is a plan view of the hand 26 of the robot 10, and FIG. 3 is a side sectional view of the finger mechanism 44B of the finger mechanism 44 of the hand 26. As shown in FIG.

  As shown in FIG. 2, the hand 26 is manufactured by imitating a human hand, and a palm 42 corresponding to the palm or back of the hand, and five hydraulic finger mechanisms 44 extending from the palm 42, specifically, Comprises a thumb finger mechanism 44A, an index finger mechanism 44B, a middle finger mechanism 44C, a ring finger mechanism 44D, and a little finger mechanism 44E.

  Hereinafter, the finger mechanism 44 will be described. Since each finger mechanism 44 has the same structure, only the index finger mechanism 44B will be described below.

  As shown in FIG. 3, the index finger mechanism 44B includes a DIP joint (distal interphalangeal joint) 44B1 and a PIP joint (a joint between distal phalanges) that rotate the finger mechanism 44B in the direction toward the palm side in order from the fingertip side. A proximal interphalangeal joint) 44B2 and an MPI joint (metacarpophalangeal joint) 44B3.

  Slave side fluid pressure cylinders 46a1 and 46a2 are arranged on the fingertip side and the palm side of the MPI joint 44B3, respectively. The slave side fluid pressure cylinders 46a1 and 46a2 are connected to the master side fluid pressure cylinders 50a1 and 50a2 (the master side fluid pressure cylinder 50a1 is not shown) via the fluid pressure transmission tubes 48a1 and 48a2 (the fluid pressure transmission tube 48a1 is not shown). Connected to. The master side fluid pressure cylinders 50 a 1 and 50 a 2 are installed in the vicinity of the shoulder joint 32 of the arm portion 24 of the robot 10.

  The cylinder body 46a21 of the slave fluid pressure cylinder 46a2 installed on the palm side of the MPI joint 44B3 is fixed to the palm portion 42, and the rod 46a23 attached to the piston 46a22 is connected to the metacarpal segment.

  The cylinder body 46a11 of the slave-side fluid pressure cylinder 46a1 installed on the fingertip side with respect to the MPI joint 44B3 is fixed to the middle finger phalanx, and the rod 46a13 attached to the piston 46a12 is distant from the proximal proximal phalanx. It is connected to the distal phalanx via a connecting member 46a14 and a link member 46a15.

  In the above configuration, when the working fluid pressure is supplied to the slave side fluid pressure cylinder 46a2 via the fluid pressure transmission pipe 48a2 as the master side fluid pressure cylinder 50a2 moves in the forward direction, the piston 46a22 also moves in the forward direction. Then, the distal end side portion from the middle finger segment is bent around the MPI joint 44B3.

  When the master fluid pressure cylinder 50a1 (not shown) moves in the forward direction, the working fluid pressure is supplied to the slave fluid pressure cylinder 46a1 via the fluid pressure transmission pipe 48a1 arranged around the MPI joint 44B3. The piston 46a12 also moves in the forward direction, and the proximal phalanx is bent around the PIP joint 44B2, and the distal phalanx is bent through the connecting member 46a14 and the link member 46a15.

  On the other hand, as the master side fluid pressure cylinders 50a1 and 50a2 move in the backward direction, the working fluid pressure is transferred from the slave side fluid pressure cylinders 46a1 and 46a2 to the master side fluid pressure cylinders 50a1 and 50a2 via the fluid pressure transmission pipes 48a1 and 48a2. When it is discharged, the index finger mechanism 44B operates to extend in the reverse direction.

  Returning to the description of FIG. 2, a six-axis force sensor (sensor; hereinafter referred to as “force sensor”) 54 is attached to the fingertip of each finger mechanism 44, more specifically, the so-called finger belly inside the fingertip or the vicinity thereof. It is done. The force sensor 54 can detect when an object is gripped with a fingertip or when an object comes into contact with the fingertip, and generates an output indicating components in three directions of force and moment acting on the fingertip. The output of the force sensor 54 is input to the ECU 40.

  As described above, in the finger mechanism 44 of the hand 26, the joint bends and extends (bends / extends) in response to supply / discharge of hydraulic oil (working fluid pressure) from the master side fluid pressure cylinder 50a to the slave side fluid pressure cylinder 46a. For example, an operation such as gripping an object or pointing in an appropriate direction can be performed.

  Meanwhile, in the robot 10, the electric motor of the arm portion 24, the fluid pressure cylinder 50a, and the like are controlled based on the output values of the force sensor 54 and the like, and operations such as gripping an object are performed. Therefore, it is necessary to always output a normal value from the force sensor 54. If the force sensor 54 is abnormal, an operation such as gripping an object cannot be performed accurately. Accordingly, an object of the present invention is to accurately determine (detect) an abnormality of the force sensor 54. This will be described below with reference to FIG.

  FIG. 4 is a flowchart illustrating the operation of the ECU 40, FIG. 5 is an explanatory diagram for explaining an initial operation of the finger mechanism 44, and FIG. 6 is an explanatory diagram for explaining a case where the force sensor 54 is determined to be abnormal. The program shown in FIG. 4 is executed when abnormality determination of the force sensor 54 is performed after the robot 10 is activated. In FIG. 4, S indicates a processing step of the program.

  In the following, the finger mechanism 44 is caused to execute an initial operation in S10. Specifically, as shown in FIG. 5, the finger mechanism 44 (one of the finger mechanisms 44A to 44E) that is the target of abnormality determination is moved along a predetermined trajectory from a predetermined initial position. Move to the position where it touches (assumed contact position).

  The target 60 is a part or member prepared separately from the robot 10 and is fixed at a predetermined position, and is made of a part such as a wall or floor having a contact surface 60a, or a member such as a plate or a rectangular parallelepiped. . Since the initial position and the position of the target 60 are known by placing the finger mechanism 44 and the target 60 in a predetermined position in advance, when the initial operation is executed, a predetermined predetermined for the initial operation is performed. The finger mechanism 44 may be operated according to the target joint angle at each point on the trajectory. During execution of the initial operation, the joint angle of the finger mechanism 44 and the output value (force component and moment component) of the force sensor 54 corresponding to the joint angle are sequentially stored every predetermined time.

  Next, in S12, the magnitude F of the force component is calculated based on the output value of the force sensor 54 stored during the initial operation, and the maximum value F (sensor The maximum value (Fmax) of the output (Fmax) is searched (detected).

The magnitude F of the force component is calculated by the following equation (1) using the force component values Fx, Fy, and Fz in the x, y, and z directions output from the force sensor 54.
^{F = (Fx 2 + Fy 2} + Fz 2) 1/2 ··· formula (1)

  Next, in S14, the position of the finger mechanism 44 in the predetermined trajectory when the maximum value Fmax is detected and the distance Df from the position to the target 60 are calculated.

  The position of the finger mechanism 44 in the predetermined trajectory when the maximum value Fmax is detected is specifically the fingertip position of the finger mechanism 44 when the maximum value Fmax is detected (more specifically, the finger mechanism 44 is the target). The fingertip position is obtained by converting the fingertip position in the sensor coordinate observation into the fingertip position in the origin coordinate observation. Here, the fingertip position in the sensor coordinate observation is a fingertip position calculated (estimated) based on the force component and the moment component output from the force sensor 54, and the fingertip position in the origin coordinate observation is the MPI joint 44B3 or the like. This is the fingertip position when the rotation center position is the origin coordinate. The reason for converting the fingertip position in the sensor coordinate observation into the fingertip position in the origin coordinate observation is that the assumed contact position of the contact surface 60a of the target 60 is described with reference to the origin coordinates.

  Since the coordinate conversion from the fingertip position in the sensor coordinate observation to the fingertip position in the origin coordinate observation can be obtained by a known coordinate conversion using a predetermined determinant, detailed description is omitted, but in the sensor coordinate observation described above It can be performed based on the fingertip position and the sensor coordinate position in the origin coordinate observation calculated by forward kinematics calculation based on the joint angle and the link length between the joints.

For the distance Df, the coordinates (Pfcx, Pfcy, Pfcz) of the position (fingertip position) of the finger mechanism 44 in the predetermined trajectory when the maximum value Fmax is detected are (Pwx, (Pwy, Pwz), it is calculated by the following equation (2).
Df = {(Pfcx−Pwx) ² + (Pfcy−Pwy) ² + (Pfcz−Pwz) ² } ^1/2 Equation (2)

  Next, in S16, it is determined whether or not the distance Df is less than the specified value Dft and the maximum value Fmax is within the range from the minimum threshold value Ftmin to the maximum threshold value Ftmax (Ftmin <Fmax <Ftmax).

  The case where the distance Df is less than the specified value Dft means that the finger mechanism 44 is substantially in contact with the target 60. Therefore, the specified value Dft is set to a value near zero. The specified value Dft is set to a value close to zero instead of zero because the calculated fingertip position of the finger mechanism 44 when the finger mechanism 44 contacts the target 60 and the estimated contact position of the actual target 60 This is because, for example, the influence of the case where the fingertip is elastic and the fingertip is slightly crushed when the fingertip presses the contact surface 60a of the target 60 is considered.

  The minimum threshold value Ftmin and the maximum threshold value Ftmax are lower limits and upper limits of the normal output value of the force sensor 54 when the finger mechanism 44 comes into contact with the target 60, which are determined in advance through experiments or the like. means. From the above, S16 is processing for determining whether or not the force sensor 54 outputs a normal value corresponding to the finger mechanism 44 when it contacts the target 60.

  When the result in S16 is negative, the program proceeds to S18, in which it is determined that the force sensor 54 is abnormal, and the process is terminated. In the case of negative in S16, the distance Df is less than the prescribed value Dft, and the maximum value Fmax is outside the range from the minimum threshold value Ftmin to the maximum threshold value Ftmax (Fmax ≦ Ftmin or Fmax ≧ Ftmax (outside the predetermined range). )), That is, when the finger mechanism 44 is in contact with the target 60 but the force sensor 54 outputs an abnormal value (for example, the output value of the force sensor 54 becomes zero or some value due to disconnection or the like). If the distance Df is greater than or equal to the specified value Dft, that is, as shown in FIG. 6, the finger mechanism 44 is not in contact with the target 60 but the output value of the force sensor 54 is used. This means that the maximum value Fmax is searched. Therefore, in such a case, it is determined that the force sensor 54 is abnormal.

  On the other hand, when the result in S16 is affirmative, the process proceeds to S20 and subsequent steps, and the same processing (S12 to S16) as the force component described above is performed for the moment component in the output value of the force sensor 54. That is, it is determined whether the force sensor 54 is abnormal based on the moment component. By determining not only the force component but also the moment component, the abnormality of the force sensor 54 can be determined with higher accuracy.

  In the following description, the magnitude M of the moment component is calculated based on the output value of the force sensor 54 stored during the initial operation in S20, and the maximum value M of the magnitude M of the calculated moment components ( The maximum value (Mmax) of the sensor output is searched.

The magnitude M of the moment component is calculated by the following expression (3) using the moment component values Mx, My, Mz in the x direction, the y direction, and the z direction.
M = (Mx ² + My ² + Mz ² ) ^1/2 ... Formula (3)

  Next, in S22, the position of the finger mechanism 44 in a predetermined trajectory when the maximum value Mmax is detected and the distance Dm from the position to the target 60 are calculated.

The distance Dm is calculated by the following equation (4), where (Pmcx, Pmcy, Pmcz) is the coordinate of the position (fingertip position) in the predetermined trajectory of the finger mechanism 44 when the maximum value Mmax is detected. .
Dm = {(Pmcx−Pwx) ² + (Pmcy−Pwy) ² + (Pmcz−Pwz) ² } ^1/2 Equation (4)

  Next, in S24, it is determined whether or not the distance Dm is less than the specified value Dmt and the maximum value Mmax is within the range from the minimum threshold value Mtmin to the maximum threshold value Mtmax (Mtmin <Mmax <Mtmax). When the distance Dm is less than the specified value Dmt, it means that the finger mechanism 44 is substantially in contact with the target 60 as in S16, and the minimum threshold value Mtmin and the maximum threshold value Mtmax are also Similar to the minimum threshold value Ftmin and the maximum threshold value Ftmax, it means the lower limit value and upper limit value of the normal output value of the force sensor 54 when the finger mechanism 44 contacts the target 60, which is determined in advance through experiments or the like. To do. From the above, S24 is a process of determining whether or not the force sensor 54 outputs a normal value corresponding to the finger mechanism 44 when it contacts the target 60.

  When the result in S24 is negative, the process proceeds to S18, where it is determined that the force sensor 54 is abnormal and the process is terminated. When the result is affirmative, the process proceeds to S26, where the force sensor 54 is determined to be normal and the process is performed. finish.

  Next, a gripping operation when the force sensor 54 is determined to be abnormal by any of the five finger mechanisms 44 (44A to 44E) will be described.

  FIG. 7 is an explanatory diagram for explaining a case where a gripping operation is performed by a finger mechanism 44 other than the finger mechanism 44 provided with the force sensor 54 determined to be abnormal.

  The initial operation for determining the abnormality of the force sensor 54 is performed mainly at the time of system check immediately after the power is turned on, between specific operations of the robot 10, when an instruction is given from the operator, or immediately before gripping an object.

  In this embodiment, the ECU 40 determines a finger mechanism (second finger mechanism) other than the finger mechanism 44 in which the force sensor 54 is determined to be abnormal, regardless of which case the force sensor 54 is determined to be abnormal. It is determined at 44 whether or not gripping is possible.

  Specifically, for example, when it is determined that the force sensor 54 of the ring finger mechanism 44D or the little finger mechanism 44E is abnormal, it is determined whether or not the remaining finger mechanism 44A, the index finger mechanism 44B, and the middle finger mechanism 44C can grip the object to be gripped. To do. When it is determined that gripping is possible, the gripping operation is continued with the three finger mechanisms 44 of the thumb finger mechanism 44A, the index finger mechanism 44B, and the middle finger mechanism 44C. Whether or not the object can be grasped is determined based on the size and shape of the object to be grasped, the arrangement and the movable range of the remaining finger mechanisms 44, and the like.

  In addition, when there is a finger mechanism 44 for which the force sensor 54 is determined to be abnormal and it is difficult to grip only with the remaining finger mechanism 44 for which the force sensor 54 is determined to be normal, at least one of the remaining finger mechanisms 44 It is determined whether or not gripping is possible by changing the fingertip position when gripping the finger mechanism 44 to a position different from the original position. As a result, when it is determined that gripping is possible, the gripping operation is continued after changing the fingertip position of the finger mechanism 44 for which the force sensor 54 is determined to be normal. The original position means, for example, a basic gripping position in which the interval between the finger mechanisms 44 determined in the gripping operation program is considered in advance, or a final target position on the gripping trajectory generated at the start of the gripping operation.

  Specifically, as shown in FIG. 7 (in FIG. 7, for convenience of explanation, the three finger mechanisms 44 are designated as the first finger, the second finger, and the third finger from the drawing), the first finger force sensor When 54 is determined to be abnormal and the force sensor 54 of the second finger and the third finger is determined to be normal, the gripping operation is performed by moving the fingertip position of the second finger to the original fingertip position of the first finger. . Thereby, in the example shown in the figure, the object to be grasped can be grasped with the second finger and the third finger without using the first finger.

  On the other hand, if there is only one finger mechanism 44 for which the force sensor 54 is determined to be normal, or there are two or more finger mechanisms 44, but it is still difficult to grip even if the fingertip positions of these finger mechanisms 44 are changed Judge that gripping is impossible and do not perform gripping operation.

  In this way, when trying to grip an object to be gripped, it is determined whether the force sensor 54 can be gripped by a finger mechanism 44 other than the finger mechanism 44 determined to be abnormal, and only when gripping is possible. While the gripping operation is performed, the gripping operation is not performed when the gripping operation is impossible, so that the gripping operation can be performed as much as possible. By trying to do so, the inconveniences such as unexpected movement of the hand and damage to the grasped object were prevented.

  It should be noted that when the system is checked immediately after the power is turned on, during a specific operation of the robot 10 or when an instruction is given from the operator, etc., when the force sensor 54 is determined to be abnormal, The operation of the robot 10 may be temporarily stopped, and an operator may be notified that the force sensor 54 is abnormal by an alarm or the like. That is, as described above, only the notification may be performed without necessarily continuing the gripping operation. In this case, the operator who has received the alarm can restart the robot 10 after repairing or replacing the abnormal force sensor 54 by temporarily shutting off the power supply of the robot 10 or the like.

  Next, a modified example of the initial operation will be described. FIG. 8 is an explanatory diagram for explaining a modified example of the initial operation, and FIG. 9 is an explanatory diagram for explaining a state of the finger mechanism 44 in the modified example.

  In the above-described embodiment, as the target 60 when performing the initial operation, a member such as a predetermined wall or floor prepared separately from the robot 10 or a member such as a plate shape or a rectangular parallelepiped shape is shown. In the example, as the target 60, a finger mechanism 44 other than the finger mechanism 44 that is the target of abnormality determination, specifically, a thumb mechanism 44A is used.

  As shown in FIGS. 8 and 9, in the modified example, the finger mechanism 44 (finger mechanism 44 </ b> B in FIG. 9) that is the target of abnormality determination and the thumb mechanism 44 </ b> A that is the target 60 are respectively moved from the initial position to a predetermined path. It is moved until the fingertips of the finger mechanism 44 and the thumb mechanism 44A contact each other at the assumed contact position (from the state of FIG. 9A to the state of FIGS. 9B and 9C). If the force sensor 54 of the finger mechanism 44 is normal, the output of the force sensor 54 indicates a normal value when the finger mechanism 44 contacts the thumb mechanism 44A. Accordingly, in this modified example as well, the abnormality of the force sensor 54 is determined by monitoring the output of the force sensor 54 when the finger mechanism 44 contacts the thumb mechanism 44A, which is the target 60, as in the above-described embodiment. can do.

  In the modification, both the finger mechanism 44 and the thumb mechanism 44A are moved toward the assumed contact position. However, the thumb mechanism 44A is fixed without being moved, and only the finger mechanism 44 is moved. Good. Further, in the modified example, the index finger mechanism 44B, the middle finger mechanism 44C, the ring finger mechanism 44D, and the little finger mechanism 44E are sequentially brought into contact with the thumb mechanism 44A, and the abnormality of the force sensor 54 of each finger mechanism 44 is determined.

As described above, in the embodiment of the present invention, the hand 26 that can be operated by the power of the drive source (electric motor, fluid pressure cylinder 50a, etc.) and the sensor (force) that generates an output indicating the physical quantity acting on the hand. Sensor) and a drive source control means (EUC) 40 for controlling the operation of the drive source. In the control device for the hand 26 of the robot 10, the drive source control means moves the hand from an initial position to a predetermined trajectory. And an initial operation executing means (ECU 40. S10) for controlling the operation of the drive source so as to execute an initial operation for moving the object 60 to a position in contact with the target 60 , and causing the hand to execute the initial operation. Detecting (retrieving) the maximum values Fmax and Mmax of the sensor output of the hand and in the predetermined trajectory of the hand when the maximum value is detected. Position calculating means (ECU 40. S 12, S 14, S 20, S 22) for calculating the position, and distance calculating means (ECU 40. S 14, S 22) for calculating distances Df, Dm from the calculated position to the target. Since it is configured to have sensor abnormality determination means (ECU 40. S16, S18, S24, S26) for determining whether or not the sensor is abnormal based on the calculated distance and the detected maximum value. For example, when the calculated distances Df and Dm are values close to zero and the hand 26 is in contact with the target 60 but the force sensor 54 does not output the corresponding normal value, or The calculated distances Df and Dm are equal to or greater than the prescribed values Dft and Dmt, and the output of the force sensor 54 is detected even though the hand 26 is not in contact with the target 60. The maximum value Fmax, such as when Mmax is detected, a force sensor 54 it is possible to determine that there is an abnormality, it is possible to determine an abnormality of the force sensor 54 accurately.

  Further, the sensor abnormality determining means is configured such that the calculated distance is less than a predetermined value Dft, Dmt, and the detected maximum value is out of a predetermined range (Fmax (Mmax) ≦ Ftmin (Mtmin) or Fmax (Mmax) ≧ Ftmax (Mtmax)) is configured to determine that the sensor is abnormal, that is, the hand 26 is intentionally brought into contact with the target 60, and the output of the force sensor 54 at that time is determined. Therefore (ECU 40. S16, S18, S24), it is possible to determine that the force sensor 54 is abnormal if the force sensor 54 does not output the original normal value when the hand 26 contacts the target 60. Therefore, the abnormality of the force sensor 54 can be determined with higher accuracy.

  Further, since the sensor abnormality determination means is configured to determine that the sensor is abnormal when the calculated distance is equal to or greater than the specified value (ECU 40. S16, S18, S24), the calculated distance Df , Dm is equal to or greater than the prescribed values Dft, Dmt, that is, the force sensor 54 is abnormal even when the maximum values Fmax, Mmax of the output of the force sensor 54 are detected even though the hand 26 is not in contact with the target 60. Therefore, the abnormality of the force sensor 54 can be determined with higher accuracy.

  Since the hand has a plurality of finger mechanisms 44 (44A to 44E), and the sensor is configured to be provided in at least one of the plurality of finger mechanisms, the finger mechanism Since it is possible to determine an abnormality of the force sensor 54 provided at 44, it is possible to determine, for example, whether or not the gripping operation can be appropriately performed based on the determination result.

  In addition, since the target is configured to be any one of the plurality of finger mechanisms, for example, a member (a jig) having a contact surface (fixed surface) 60a specially prepared for determination. Even if it is not used, the abnormality of the force sensor 54 is determined only by moving the finger mechanism 44, specifically, the thumb mechanism 44A and any of the other finger mechanisms 44B, 44C, 44D, 44E. Can do.

  The drive source control means is determined to be abnormal when the sensor abnormality determination means determines that the sensor is abnormal and controls the operation of the drive source to cause the hand to perform a gripping operation. It is determined whether the gripping operation is possible by a second finger mechanism other than a finger mechanism provided with a sensor, and when the gripping operation is determined to be possible, the gripping operation is performed by the second finger mechanism. Thus, when the gripping operation is performed, even if the force sensor 54 is determined to be abnormal, the gripping is performed by the remaining finger mechanism 44 that is not determined to be abnormal. The action can be performed.

  In addition, the drive source control means is configured to control the operation of the drive source so that the gripping operation is not performed when it is not determined that the gripping operation is possible. It is possible to avoid inconveniences such as an unexpected operation of the hand 26 or damage to the object to be grasped when trying to perform the grasping operation.

  In the above description, the six-axis force sensor is shown as the sensor 54 provided in the finger mechanism 44, but a three-axis force sensor and a pressure distribution sensor may be used instead of the six-axis force sensor.

  Further, although the legged mobile robot is shown as the robot 10, other than such a so-called humanoid robot, for example, an industrial fixed robot (arm) installed in a production line of a factory or the like may be used.

  10 robot, 26 hands, 40 ECU (electronic control unit), 42 palm part, 44 (44A, 44B, 44C, 44D, 44E) finger mechanism, 46a (46a1, 46a2) slave side hydraulic cylinder, 50a (50a1, 50a2) ) Master side fluid pressure cylinder, 54 6-axis force sensor (sensor), 60 Target

Claims (7)

In the robot hand control device, comprising: a hand operable by the power of the driving source; a sensor that generates an output indicating a physical quantity acting on the hand; and a driving source control unit that controls the operation of the driving source. A source control unit configured to control the operation of the driving source so as to execute an initial operation of moving the hand from an initial position to a position in contact with the target object along a predetermined path; Position calculating means for detecting a maximum value of the output of the sensor when the initial operation is executed, and calculating a position of the hand in the predetermined trajectory when the maximum value is detected; A distance calculating means for calculating a distance from the determined position to the target, and the sensor is abnormal based on the calculated distance and the detected maximum value. Controller of the hand, characterized in that it comprises a whether determining sensor abnormality judgment means.   The sensor abnormality determining means determines that the sensor is abnormal when the calculated distance is less than a prescribed value and the detected maximum value is outside a predetermined range. The hand control device described.   The hand control device according to claim 2, wherein the sensor abnormality determination unit determines that the sensor is abnormal when the calculated distance is equal to or greater than the specified value.   4. The hand according to claim 1, wherein the hand has a plurality of finger mechanisms, and the sensor is provided in at least one of the plurality of finger mechanisms. Hand control device.   The hand control device according to claim 4, wherein the target is any one of the plurality of finger mechanisms.   The drive source control unit determines that the sensor is abnormal when the sensor abnormality determination unit determines that the sensor is abnormal, and controls the operation of the drive source so that the hand performs a gripping operation. It is determined whether or not the gripping operation can be performed by a second finger mechanism other than the finger mechanism provided with the first finger mechanism, and when it is determined that the gripping operation is possible, the second finger mechanism causes the gripping operation to be performed. 6. The hand control device according to claim 4, wherein the operation of the drive source is controlled.   7. The hand control device according to claim 6, wherein the drive source control unit controls the operation of the drive source so as not to perform the gripping operation when it is not determined that the gripping operation is possible.

Images