JP5614232B2 - Robot hand control device, control method, and control program - Google Patents

Description

  The present invention relates to control of a robot hand, and more particularly to safety control when a fingertip contacts a surrounding object.

  A robot hand that performs various operations by autonomously controlling the link mechanism requires safety control so as not to harm the object when it comes into contact with a surrounding object.

  FIG. 5 shows a functional configuration of a general danger avoidance control apparatus 101. The danger avoidance control device 101 includes a robot operation planning unit 102, an operation procedure / operation command generation unit 103, an operation mode information unit 104, a signal processing circuit unit 105, each axis torque change amount detection unit 106, and a contact / collision determination handling command generation. Unit 107, each axis target value generation unit 108, and each axis motor drive servo control unit 109.

  The robot operation planning unit 102 decomposes a robot operation command given from a person or as a program into operation procedures necessary for each operation process. The motion procedure / motion command generation unit 103 generates a motion command according to the motion procedure generated by the robot motion planning unit 102.

  The operation mode information unit 104 corresponds to the operation mode at that time among the determination reference information (threshold value of change amount of each shaft torque) set for each operation mode based on the command from the operation procedure / operation command generation unit 103. Output things.

  The signal processing circuit unit 105 inputs each axis torque from an axis torque detector installed on each axis of the arm-mounted mobile robot 110. Each shaft torque change amount detection unit 106 detects a change amount of each shaft torque from the signal processing circuit unit 105 (hereinafter referred to as “each shaft torque change amount”).

  The contact / collision determination handling instruction generation unit 107 compares each axis torque change amount from each axis torque change amount detection unit 106 with the determination reference information from the operation mode information unit 104, and changes in each axis torque of a certain joint portion. When the amount becomes larger than the determination reference information, it is determined that contact or collision has occurred in the arm portion located on the distal end side with respect to the joint axis, and each axis target value generation unit 108 is determined according to the operation mode at that time. The trajectory target value generated according to the command from the operation procedure / operation command generation unit 103 is adjusted, or each axis motor drive servo control unit 109 stops the motor.

  The arm-mounted mobile robot 110 detects each axis torque by an axis torque detector controlled by each axis motor drive servo control unit 109 and installed on the joint axis.

  Further, the arm-mounted mobile robot according to Patent Document 1 has a mechanism as described above, and when the amount of change in the torque of each axis of the robot arm is larger than a preset threshold, the robot arm comes into contact with surrounding people or objects. Judgment is made to correct the motion trajectory or to stop the motor drive.

JP 2008-229800 A

  However, when the risk avoidance control as described above is used, the contact of an object at the fingertip of the robot hand may not be detected. For example, there is a high possibility that contact cannot be detected when an object is poke with the fingertip of a robot hand. Further, each object has physical properties, and ideally, risk avoidance control adapted to the physical properties of each contact object should be performed.

  Therefore, an object of the present invention is to reliably detect contact of an object at the fingertip of a robot hand, estimate physical properties of the object, and realize danger avoidance control suitable for the physical property.

  A first aspect of the present invention is a control device for a robot hand comprising a plurality of links and a plurality of joints connecting the links, and an operation command generating means for outputting an operation command in response to the received operation request; A torque command generating means for outputting a torque command for driving each joint based on a target angle of each joint determined according to the operation command and a gain set for each joint; A first finger obtained by subtracting a torque value for resisting the weight and inertial force of the first finger link from the torque value according to the torque command output to the first finger joint provided at the base portion of the first finger link. From the joint contact torque and the torque value by the torque command output to the second finger joint provided at the base portion of the second finger link connected to the first finger joint, the second finger joint is ahead. Detecting the contact between the first finger link or the second finger link and an object based on the weight of the side portion and the second finger joint contact torque obtained by subtracting the torque value for resisting the inertial force, The first finger joint contact torque and the second finger joint contact torque are both 0 when performing a contact determination operation for displacing the first finger link so that the position of the fingertip that is the tip of the first finger link does not change. If it is determined that the fingertip and the object are in contact with each other, contact determination means for determining that the fingertip is in contact with the object, and determining that the fingertip is in contact with the object, the first finger joint contact torque, Contact force estimation means for estimating a contact force, which is a reaction force from the object to the fingertip, based on a detected value of a two-finger joint contact torque and an angle of the first finger joint; and the fingertip contacts the object If it is determined that Contact angle estimation means for estimating a contact angle formed by the direction of the contact force and the length direction of the first finger link based on the first finger joint contact torque and the contact force; and When it is determined that the object is in contact with the object, the Young of the object is determined based on the amount of minute deformation of the object and the contact force when performing a Young's modulus estimation operation for moving the fingertip toward the object by a small amount. Based on the Young's modulus estimating means for estimating the rate, and the Young's modulus and the contact angle, the control stiffness when the fingertip comes into contact with the object does not exceed the stiffness of the object. A control device for a robot hand comprising: gain adjustment means for adjusting the gain and the gain for the second finger joint.

  According to the apparatus according to the above aspect, it is determined whether or not the fingertip of the robot hand has touched the object, and the Young's modulus of the object is estimated. Then, based on the estimated Young's modulus, the control gains of the first finger joint and the second finger joint are adjusted so as not to harm the object.

  In addition, the contact determination means determines the contact determination to the operation command generation means when it is detected that the first finger joint contact torque and the second finger joint contact torque are not 0 during execution of the normal operation. It is preferable to output a contact determination operation request for executing the operation.

  In addition, when the first finger joint contact torque and the second finger joint contact torque are both 0 at the time of executing the contact determination operation, the contact determination unit is configured such that the other than the fingertip of the first finger link. It is preferable to determine that the part is in contact with the object.

  Further, the contact determination means may be configured to detect the second finger link and the object when the first finger joint contact torque is 0 and the second finger joint contact torque is not 0 when the contact determination operation is performed. Is preferably determined to be in contact.

  Furthermore, the contact determination means is configured such that when the normal operation is performed, the N-1th finger joint contact torque (N is an integer equal to or greater than 2) is 0 from the first finger joint contact torque, and the Nth finger joint contact torque. When is not 0, it is preferable to determine that the N-th finger link and the object are in contact with each other.

  The contact determination means preferably determines that all the links are not in contact with the object when the joint contact torque for all the joints is 0 when the normal operation is performed.

  In the embodiment of the following invention, the contact force estimation by the contact force estimation means, the contact angle estimation by the contact angle estimation means, the Young's modulus estimation by the Young's modulus estimation means, and the gain adjustment The method of adjusting the gain by the means is specifically shown by using mathematical expressions.

  A second aspect of the present invention is a control method based on the same technical idea as that of the control device.

  Furthermore, a third aspect of the present invention is a control program based on the same technical idea as the control device and the control method.

  ADVANTAGE OF THE INVENTION According to this invention, the contact of the object in the fingertip of a robot hand is detected, and danger avoidance control suitable for the physical property of the said object is realizable. As a result, for example, for a soft object, the output torque for driving the robot hand is sufficiently suppressed even at the sacrifice of the original operation of the robot hand, thereby reliably preventing harm to the object. Can do. For a hard object, it is possible to continue the original operation of the robot hand while preventing harm to the object by making the suppression of the driving torque of the robot hand relatively small.

It is a block diagram which shows the functional structure of the robot hand which concerns on Embodiment 1 of this invention. FIG. 3 is a free body diagram of the hand unit according to the first embodiment. 3 is a flowchart showing a flow of control by a driving force control unit according to the first embodiment. 6 is a graph showing temporal changes in the first finger joint angle and the contact angle in the first embodiment. 3 is a graph showing the amount of deformation of an object in the first embodiment. 3 is a graph showing a first finger joint contact torque and a second finger joint contact torque in the first embodiment. 3 is a graph showing a contact force and a contact force estimated value in the first embodiment. 3 is a graph showing a contact angle and a contact angle estimated value in the first embodiment. 4 is a graph showing contact object Young's modulus and contact object Young's modulus estimated value in the first embodiment. It is a block diagram which shows the functional structure of a general danger avoidance control apparatus.

Embodiment 1
Embodiments of the present invention will be described below with reference to the drawings. FIG. 1 shows a functional configuration of the robot hand 1 according to the first embodiment. The robot hand 1 includes a hand unit 11 and a driving force control unit 12.

  The hand unit 11 includes a link mechanism 21, a joint motor 22, and a joint angle detection unit 23. The link mechanism 21 includes a plurality of links, a joint mechanism that connects the links, and the like. The link mechanism 21 is designed according to the purpose of use of the robot hand 1 and is configured so as to be able to grip and transport an object, for example. The drive shaft of the joint motor 22 is controlled in accordance with the electric signal, and changes the relative positional relationship between adjacent links. The joint angle detector 23 detects the amount and direction of rotation of the drive shaft of each joint motor 23, converts the detection result into an electrical signal, and outputs the electrical signal.

  The driving force control unit 12 includes an operation command generation unit 31, a torque generation command unit 32, a contact determination unit 33, a contact force estimation unit 34, a Young's modulus estimation unit 35, a contact angle estimation unit 36, and a gain adjustment unit 37. The driving force control unit 12 determines whether or not the fingertip of the hand unit 11 has contacted the object, and when the contact is confirmed, estimates the Young's modulus (contact object Young's modulus) of the object, and the contact object In accordance with the Young's modulus, the drive torques of the first and second finger joints of the link mechanism 21 that are directly involved in the movement of the fingertips are controlled within a range in which harm to the object can be reliably prevented.

  The operation command generation unit 31 outputs a contact determination operation command Oc according to the contact determination operation request Rc from the contact determination unit 33 at the time of fingertip contact determination. In addition, when the contact object Young's modulus is estimated, the operation command generation unit 31 outputs a Young's modulus estimation operation command Oy according to the Young's modulus estimation operation request Ry from the Young's modulus estimation unit 35. Further, the operation command generator 31 outputs a normal operation command Ow in cases other than the above, that is, when the original operation of the hand unit 11 is executed.

  At the time of fingertip contact determination, the torque generation command unit 32 outputs a torque command Tc such that the joint angle detection value θ follows the contact determination operation command Oc. Further, the torque generation command unit 32 outputs a torque command Ty such that the joint angle detection value θ follows the Young's modulus estimation operation command Oy when the contact object Young's modulus is estimated. Then, when the fingertip of the hand unit 11 is in contact with an object, the torque generation command unit 32 is set by the gain adjustment unit 37 according to the contact object Young's modulus estimated value E2 calculated by the Young's modulus estimation unit 35. Using the first finger joint control gain G1 and the second finger joint control gain G2, a torque command Tw is output so that the detected joint angle value θ follows the normal operation command Ow. On the other hand, when the fingertip of the hand unit 11 is not in contact with the contact object, the joint angle detection value θ is normally operated using the first finger joint control gain G1 and the second finger joint control gain G2 set in advance. A torque command Tw that follows the target angle included in the command Ow is output.

  The contact determination unit 33 determines whether or not an object has contacted the fingertip or other part of the hand unit 11 and outputs the determination result as a fingertip contact determination result S.

  Here, first, a configuration example of the hand unit 11 will be described with reference to FIG. FIG. 2 is a free body diagram of a finger of the hand unit 11 according to the present embodiment. In FIG. 2, a first finger link 41, a first finger joint 51, a second finger link 42, and a second finger joint 52 are shown.

  One end of the first finger link 41 is a fingertip 50. When the fingertip 50 contacts the object, a contact force F, which is a reaction force from the object, is applied to the fingertip 50. Although not shown, normally, the second finger joint 52 is followed by the third finger link, the third finger joint, the back of the robot hand, the wrist joint of the robot hand, the forearm of the robot arm, and the like.

  The contact determination unit 33 detects contact between the fingertip 50 and the object or contact between a part other than the fingertip 50 and the object by the following methods (1) to (5).

  (1) The component of the torque command Tw obtained by subtracting the component due to gravity and inertia from the first finger joint torque command T1r for driving the first finger joint 51 (hereinafter referred to as “first finger joint contact torque T1”). When both of the second finger joint torque command T2r for driving the second finger joint 52 subtracting components due to gravity and inertia (hereinafter referred to as “second finger joint contact torque T2”) are non-zero, Since it can be determined that there is a possibility that somewhere on the first finger link 41 is in contact with the object, the contact determination operation request Rc is output. The operation command generation unit 31 that has received the contact determination operation request Rc outputs a contact determination operation command Oc that satisfies sgn (Δθ1) = − sgn (ΔT1) without changing the position of the fingertip 50 of the first finger link 41. To do. Thereafter, when both the first finger joint contact torque T1 and the second finger joint contact torque T2 remain non-zero (when the first finger link 41 is still torqued even when the first finger joint 51 is moved). Is determined as “the fingertip 50 has touched the object”.

  (2) On the other hand, after outputting the contact determination operation command Oc that satisfies the above sgn (Δθ1) = − sgn (ΔT1), both the first finger joint contact torque T1 and the second finger joint contact torque T2 are 0. If it becomes (when the first finger link 41 is moved away from the object by moving the first finger joint 51), it is determined that “a part other than the fingertip 50 of the first finger link 41 is in contact with the object”. .

  (3) Also, after outputting a contact determination operation command Oc that satisfies the above sgn (Δθ1) = − sgn (ΔT1), the first finger joint contact torque T1 is 0 and the second finger joint contact torque T2 is When it becomes non-zero, it is determined that “the second finger link 42 has contacted the object”.

  (4) Further, after outputting a contact determination operation command Oc that satisfies the above sgn (Δθ1) = − sgn (ΔT1), the first finger joint contact torque T1 to the N-1th finger joint contact torque TN-1 When (N is an integer of 2 or more) is all 0 and only the N-th finger joint contact torque TN is non-zero, it is determined that “the N-th finger link is in contact with the object”.

  (5) Furthermore, when all the finger joint contact torques are 0, it is determined that “the hand unit 11 is not in contact with an object”.

  The contact force estimation unit 34 estimates the absolute value of the contact force based on the first finger joint contact torque T1, the second finger joint contact torque T2, and the joint angle detection value θ, and outputs the contact force estimated value F. .

  When the Young's modulus estimator 35 receives the contact force estimated value F from the contact force estimator 34 and the contact angle estimated value θf from the contact angle estimator 36, the Young's modulus estimator 35 sends a Young's modulus estimated operation request Ry to the operation command generator 31. Output. Upon receiving the Young's modulus estimation operation request Ry, the operation command generation unit 31 operates a minute amount (hereinafter referred to as “a small deformation amount d”) in a direction of pressing the position of the fingertip 50 against the object based on the estimated contact angle value θf. A Young's modulus estimation operation command Oy is output. The Young's modulus estimator 35 then calculates a contact object Young's modulus estimated value E 2 that is an estimated value of the Young's modulus of the object, based on the contact force estimated value F during operation based on the small deformation amount d and the Young's modulus estimation operation command Oy. Is output.

  The contact angle estimator 36 outputs a contact angle estimated value θf that is an estimated value of an angle at which the contact force F is applied to the fingertip 50 based on the first finger joint contact torque T1 and the contact force estimated value F.

  Based on the contact object Young's modulus estimated value E2 and the contact angle estimated value θf, the gain adjusting unit 37 controls the first finger joint control gain of the torque generation command unit 32 so as not to damage the object in contact with the finger of the hand unit 11. G1 and the second finger joint control gain G2 are adjusted. At this time, other control gains, that is, control gains for controlling other joints of the robot hand 1 are not adjusted.

  The driving force control unit 12 includes a programmable electronic control system using a microprocessor or the like, a combination of various electric circuits (logic circuits), and the like.

  Hereinafter, a specific operation principle of each block shown in FIG. 1 will be described.

The first finger joint contact torque T1 and the second finger joint contact torque T2 are expressed by the following equations (1) and (2) from FIG.

Here, l 1 is the length of the first finger link 41, and l 2 is the length of the second finger link 42. When Expressions (1) and (2) are solved for the contact force F and the contact angle θf, the following Expressions (3) and (4) are obtained, respectively.

  The contact force estimation unit 34 estimates the contact force estimated value F using equation (3), and the contact angle estimation unit 36 estimates the contact angle estimated value θf using equation (4). Here, for simplicity, both the contact force and the estimated contact force value are represented by the same symbol F, and both the contact angle and the estimated contact angle value are represented by the same symbol θf.

On the other hand, the contact force F is expressed by the following formula (5) by KL Johnson, “Contact Mechanics”, Cambridge University Press, 1985.

Where E * is the effective Young's modulus, R is the fingertip radius of curvature, d is the amount of minute deformation, E1 is the Young's modulus of the fingertip 50, ν1 is the Poisson's ratio of the fingertip 50, E2 is the contact Young's modulus, and ν2 is the Poisson of the contact. Is the ratio. Here, the fingertip Young's modulus E1 and the fingertip Poisson's ratio ν1 are known from the design values of the robot hand 1. When Formula (5) is solved for contact material Young's modulus E2, the following Formula (Formula 6) is obtained.

  The Poisson's ratio takes a value between 0 and 0.5, but the contact Poisson's ratio ν2 in the equation (6) is expressed in a square form, so if it is fixed at ν2 = 0.25, the error of the contact Young's modulus E2 Is within (0.5 ^ 2-0.25 ^ 2) / (1-0.5 ^ 2) · 100 = 25 [%]. In the present embodiment, the contact object Young's modulus E2 is a physical property value that varies greatly depending on the object to be contacted, and it is considered that an error of 25 [%] can be ignored even in the control gain setting described later (contact object Young's modulus). The 25 [%] error of the estimated value E2 corresponds to 15 [%] of the control gain described later). Accordingly, the contact object Poisson's ratio ν2 is fixed at 0.25. The Young's modulus estimator 35 calculates the contact object Young's modulus estimated value E2 by the equation (6). Here, for simplicity, both the contact object Young's modulus and the contact object Young's modulus estimated value are represented by the same symbol E2.

In order not to damage the object with which the finger of the hand unit 11 comes into contact, the rigidity (hereinafter referred to as “control rigidity”) when the finger is feedback-controlled may be equal to or less than the rigidity at the contact portion of the object. For example, when position proportional velocity proportional control is performed on the first finger joint 51, the closed-loop motion equation of the finger of the robot hand in FIG. 2 is expressed by the following equations (7) and (8).

  However, J1 is the moment of inertia of the first finger link 41 viewed from the first finger joint 51 (hereinafter referred to as “first finger link inertia moment”), and Kp1 is the position proportional control gain (hereinafter referred to as “first finger link inertia moment”). Kvj1 is a speed proportional control gain of the first finger joint 51 (hereinafter referred to as “first finger joint speed proportional control gain”), and J2 is a second finger joint 52. Moment of inertia (hereinafter referred to as “first second finger link inertia moment”) of the first finger link 41 and the second finger link 42 as viewed from above, and Kp2 is a position proportional control gain (hereinafter referred to as “first finger link inertia moment”). Kvj2 is a speed proportional control gain of the second finger joint 52 (hereinafter referred to as “second finger joint speed proportional control gain”). Further, in the expressions (7) and (8), the term of gravity applied to the first finger link 41 and the second finger link 42 and the components of the first finger joint torque command Tr1 and the second finger joint torque command Tr2 that support the term. Has been offset.

The above-described minute deformation amount d resulting from only the first finger joint angle θ1 is expressed by the following equation (9) from FIG.

The following formula (10) is obtained from the formulas (1), (7), and (9).

  Where M1 is the equivalent mass of the first finger link 41, D1 is the equivalent viscosity coefficient of the first finger link 41, and K1 is the equivalent stiffness of the first finger link 41.

Similarly, the above-described minute deformation amount d caused only by the second finger joint angle θ2 is expressed by the following equation (11) from FIG.

Here, θ2 represents a second finger joint angle that is an angle of the second finger joint 52. From the formulas (2), (8), and (11), the following formula (12) is obtained.

Where M2 is the equivalent mass of the second finger link 42, D2 is the equivalent viscosity coefficient of the second finger link 42, K2 is the equivalent stiffness of the second finger link 42, δ is the first finger joint angle θ1, the contact angle θf, It is a constant determined by the one finger link length l1 and the second finger link length l2. The condition that the control rigidity is the same as the rigidity at the contact portion of the contact object is expressed by the following expression (13) from the expression (10).

However, l is the surface layer thickness of the contact object, and in this embodiment, it is the same as the fingertip radius of curvature R, which is a thickness that can be affected by contact with the fingertip 50 (l = R). Further, the desired attenuation coefficient of the first finger joint 51 (hereinafter referred to as “first finger joint attenuation coefficient ζ1”) is expressed by the following expression (14) from Expression (10).

When equations (13) and (14) are solved for the first finger joint position proportional control gain Kp1 and the first finger joint velocity proportional control gain Kvj1, the following equations (15) and (16) are obtained.

Similarly, the second finger joint position proportional control gain Kp2 and the second finger joint velocity proportional control gain Kvj2 are obtained from the following equations (17) and (18) from the equation (12).

  Here, ζ 2 is a desired attenuation coefficient of the second finger joint 52 (hereinafter referred to as “second finger joint attenuation coefficient”).

  The joint gain adjustment unit 37 calculates only the control gains Kp1, Kvj1, Kp2, and Kvj2 for controlling the first finger joint 51 and the second finger joint 52 among the control gains of the torque generation command unit 32 from the equations (15) to (18). ), The hand unit 11 is controlled with an appropriate control rigidity (for example, a control rigidity in Expression (7)) that does not damage the contact object according to the contact object Young's modulus E2.

  Here, the maximum error of 25 [%] generated in the contact object Young's modulus estimated value E2 is 1−√1.25 = − for each control gain as shown in the equations (15) to (18). A ratio of 0.15 or 1−√0.75 = 0.15, that is, a maximum error of 15% appears. However, 15% is a negligible error with respect to the operation accuracy required for the service robot.

  FIG. 3 shows a flow of control by the driving force control unit 12 according to the present embodiment. The contact determination unit 33 (see FIG. 1) determines whether or not both the first finger joint contact torque T1 and the second finger joint contact torque T2 are non-zero (S1). The contact determination operation request Rc is output (S2). In response to the output of Rc, the operation command generation unit 31 outputs a contact determination operation command Oc, and the torque generation command unit 32 outputs a torque command Tc according to Oc (S3).

  Next, the contact determination unit 33 determines whether T1 and T2 included in Tc are non-zero (S4). If non-zero (Y), the fingertip 50 is in contact with the object. (S5) and the fingertip contact determination result S is output (S6). Upon receiving the output of S, the contact force estimating unit 34 estimates and outputs the contact force estimated value F, and the contact angle estimating unit 36 estimates and outputs the contact angle estimated value θf (S7). In response to the outputs of F and θf, the Young's modulus estimator 35 outputs a Young's modulus estimation operation request Ry (S8). In response to the output of Ry, the operation command generation unit 31 outputs a Young's modulus estimation operation command Oy, and the torque generation command unit 32 outputs a torque command Ty according to Oy (S9). Thereafter, the Young's modulus estimator 35 estimates and outputs the contact object Young's modulus estimated value E2 based on F and the detected joint angle value θ (S10). The gain adjusting unit 37 adjusts the first finger joint control gain G1 and the second finger joint control gain G2 based on E2 and θf (S11). Then, the operation command generation unit 31 outputs a normal operation command Ow using the adjusted G1 and G2, and the torque generation command unit 32 outputs a torque command Tw according to Ow (S12).

  If both T1 and T2 are not non-zero in step S4 (N), the contact determination unit 33 determines whether both T1 and T2 are 0 (S15). In step S15, when both T1 and T2 are 0 (Y), it is determined that a portion other than the fingertip 50 of the first finger link 41 is in contact with the object (S16), and other portions are responded accordingly. The appropriate control is executed (S17). On the other hand, if both T1 and T2 are not 0 in step S15 (N), it is determined whether T1 is 0 and T2 is non-zero (S20). In step S20, if T1 is 0 and T2 is not non-zero (N), the process returns to step S1, and if T1 is zero and T2 is non-zero (Y), the second finger link It is determined that 42 is in contact with the object (S21), and other appropriate control is executed accordingly (S22).

  In step S1, when both T1 and T2 are not non-zero (N), the contact determination unit 33 determines not only T1 and T2 but also contact torques T3 to TN of the third to Nth joints (not shown). If T1 to TN-1 is 0 and TN is non-zero (Y), it is determined that the Nth finger link is in contact with the object (S31). In response, other appropriate control is executed (S32). On the other hand, if T1 to TN-1 is 0 and TN is not non-zero (N) in step S30, it is determined whether or not T1 to TN are all 0 (S35). If T1 to TN are all 0 in step S35 (Y), it is determined that none of the links constituting the hand unit 11 is in contact with the object (S36), and Ow and Tw for normal operation are output. If it is determined that T1 to TN are not all zero (N), the process returns to step S1.

  As described above, according to the robot hand 1 according to the present embodiment, it is determined whether or not the fingertip 50 of the hand unit 11 has touched the object, the Young's modulus of the object is estimated, and the Young's modulus of the contact object Based on the control gains G1 and G2 of the first finger joint 51 and the second finger joint 52 adjusted according to the above, the robot hand 1 can be controlled so as not to damage the object.

The simulation result of the robot hand 1 according to Embodiment 1 is shown below. The numerical values used for the simulation are as follows.
l1 = 20 × 10 ^ −3 [m],
l2 = 20 × 10 ^ −3 [m],
E1 = 106 × 10 ^ 9 [Pa],
ν1 = 0.39,
E2 = 2 × 10 ^ 3 [Pa],
ν2 = 0.45,
R = 5 × 10 ^ −3 [m],
ζ1 = 1,
ζ2 = 1,
m1 = 30 × 10 ^ −3 [kg],
m2 = 30 × 10 ^ −3 [kg],
r1 = 5 × 10 ^ −3 [m],
r2 = 5 × 10 ^ −3 [m]

  Here, m1 is the mass of the first finger link 41, m2 is the mass of the second finger link 42, r1 is the radius of the first finger link 41, and r2 is the radius of the second finger link 42.

  This simulation shows an example of numerical calculation when an object is touched during the operation of bending the finger of the robot hand 1 shown in FIG.

  FIG. 4A shows temporal changes in the first finger joint angle θ1 and the contact angle θf. The solid line represents the first finger joint angle θ1, and the broken line represents the contact angle θf. As shown in FIG. 4A, the first finger joint 51 was bent in accordance with the normal operation command Ow described above, and the object and the finger of the hand unit 11 contacted each other at 0.5 [s]. After confirming that the fingertip 50 is in contact with the above-described fingertip contact determination command Oc, the operation is performed according to the above-described contact object Young's modulus estimation command Oy up to 0.55 [s]. The control gains Kp1, Kvj1, Kp2, and Kvj2 of the first finger joint 51 and the second finger joint 52 were adjusted. Thereafter, the fingertip 50 is kept in the same position.

  FIG. 4B shows the deformation amount of the object. As shown in FIG. 4B, since no force is applied to the object from the hand unit 11 until the finger of the hand unit 11 contacts the object at 0.5 [s], the deformation amount of the object is zero. It is determined that the finger touches the object and the fingertip 50 touches at 0.5 [s]. Thereafter, the deformation amount of the object starts to increase and increases until the deformation amount of the object becomes 1 × 10 ^ −3 [m] by the contact object Young's modulus estimation command Oy. The estimation of the contact object Young's modulus E2 is completed at 0.55 [s], and the control gains Kp1, Kvj1, Kp2, Kvj2 of the first finger joint 51 and the second finger joint 52 are adjusted. Thereafter, the fingertip 50 is kept at the same position, and the deformation amount of the object is kept constant.

  FIG. 4C shows the first finger joint contact torque T1 and the second finger joint contact torque T2. The solid line is the first finger joint contact torque T1, and the broken line is the second finger joint contact torque T2. Even when the first finger joint angle θ1 is increased by the fingertip contact determination command Oc at 0.5 [s], both the first finger joint contact torque T1 and the second finger joint contact torque T2 do not become zero. Therefore, the contact determination unit 33 determines that the fingertip 50 has contacted the object, and continues to estimate the contact force F, the contact angle θf, and the contact object Young's modulus E2. Both the first finger joint contact torque T1 and the second finger joint contact torque T2 are between 0.5 [s] and 0.55 [s], and the fingertip 50 is pushed into the object by the contact object Young's modulus estimation command Oy. Since the position of the fingertip 50 is maintained thereafter, a constant value is taken. After the fingertip 50 contacts the object at 0.5 [s], the second finger joint contact torque T2 is larger than the first finger joint contact torque T1 in the second finger joint 52. This is because the first finger joint 51 is farther from the object.

  FIG. 4D shows the contact force and the estimated contact force value. The solid line indicates the estimated contact force, and the broken line indicates the contact force. As shown in the figure, the estimated contact force value calculated by the contact force estimation unit 34 matches the contact force.

  FIG. 4E shows the contact angle and the estimated contact angle value. A solid line indicates a contact angle estimated value, and a broken line indicates a contact angle. As shown in the figure, the contact angle estimated value calculated by the contact angle estimating unit 36 matches the contact angle.

  FIG. 4F shows the contact object Young's modulus and the contact object Young's modulus estimated value. The solid line indicates the contact object Young's modulus estimated value, and the broken line indicates the contact object Young's modulus. In the figure, it is shown that the contact object Young's modulus estimated value is estimated immediately after confirming the contact of the fingertip 50 at 0.5 [s]. While the contact object Poisson's ratio ν2 is 0.45, the Young's modulus estimator 35 fixes ν2 in equation (6) at 0.25 as described above. (0.45 ^ 2-0.25 ^ 2) / (1-0.45 ^ 2) · 100 = 17 [%] larger than the contact object Young's modulus. An error of 17 [%] of the contact object Young's modulus estimated value has a ratio of 1-√1.17 = −0.08 to the control gain, that is, an error of 8 [%]. This error is negligible for the accuracy of operation.

  Based on the results of FIGS. 4A to 4F, the control gains calculated by the joint gain adjustment unit 37 using the equations (15) to (18) are Kp1 = 8.49 [s ^ −1] and Kvj1 = 1, respectively. .42 × 10 ^ −4 [N · m · s / rad], Kp2 = 9.39 [s ^ −1], Kvj2 = 1.13 × 10 ^ −3 [N · m · s / rad] It was. Therefore, the first finger joint 51 has a velocity loop of 5.41 [Hz], a position loop of 8.49 [Hz], the second finger joint 52 has a velocity loop of 5.98 [Hz], and a position loop of 9 It can be seen that the control of the flexible hand portion 11 is performed with a control rigidity of the same degree as the rigidity according to the contact object Young's modulus with a bandwidth of .39 [Hz].

However, the first finger link inertia moment J1 and the first second finger link inertia moment J2 used in the calculation of the control gain were calculated by the following equations (19) and (20).

  Here, J10 is the moment of inertia around the center of gravity of the first finger link 41, J20 is the moment of inertia around the center of gravity of the second finger link 42, and the positions of the center of gravity are 2 for the first finger link 41 and the second finger link 42, respectively. Suppose that it is located at an equally divided point.

  As described above, according to the robot hand 1 according to the present embodiment, it is determined whether or not the fingertip 50 of the hand unit 11 has touched the object, the Young's modulus of the object is estimated, and the Young's modulus of the contact object Based on the control gains Kp1, Kvj1, Kp2, and Kvj2 of the first finger joint 51 and the second finger joint 52 adjusted according to the above, the robot hand 1 can be controlled so as not to damage the object.

  Note that the present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention.

  The present invention can be applied to all service robots including robot hands such as a robot for helping with housework, a customer service robot, a care robot, and a rehabilitation robot.

DESCRIPTION OF SYMBOLS 1 Robot hand 11 Hand part 12 Driving force control part 21 Link mechanism 22 Joint motor 23 Joint angle detection part 31 Operation command generation part 32 Torque command generation part 33 Contact determination part 34 Contact force estimation part 35 Young's modulus estimation part 36 Contact angle estimation 36 Unit 37 gain adjusting unit 41 first finger link 42 second finger link 50 fingertip 51 first finger joint 52 second finger joint Oc contact determination operation command Oy Young's modulus estimation operation command Ow normal operation command Tc, Ty, Tw torque command T1 First finger joint contact torque T2 Second finger joint contact torque Rc Contact determination operation request Ry Young's modulus estimation operation request S Fingertip contact determination result F Contact force estimation value θ Joint angle detection value θf Contact angle estimation value E2 Contact object Young's modulus estimation Value G1 First finger joint control gain G2 Second finger joint control gain

Claims (23)

A control device for a robot hand comprising a plurality of links and a plurality of joints connecting the links,
An operation command generating means for outputting an operation command in response to the received operation request;
Torque command generating means for outputting a torque command for driving each joint based on a target angle of each joint determined according to the operation command and a gain set for each joint;
A torque value for resisting the weight and inertial force of the first finger link is subtracted from the torque value according to the torque command output to the first finger joint provided at the base portion of the first finger link located at the tip. From the first finger joint contact torque and the torque value by the torque command output to the second finger joint provided at the base portion of the second finger link connected to the first finger joint, the tip from the second finger joint Detecting the contact between the first finger link or the second finger link and an object based on the weight of the side portion and the second finger joint contact torque obtained by subtracting the torque value for resisting the inertial force, The first finger joint contact torque and the second finger joint contact torque are both 0 when performing a contact determination operation for displacing the first finger link so that the position of the fingertip that is the tip of the first finger link does not change. Not The case, the contact determination unit and said and said fingertip object is in contact,
When it is determined that the fingertip is in contact with the object, the object is based on detected values of the first finger joint contact torque, the second finger joint contact torque, and the angle of the first finger joint. Contact force estimation means for estimating a contact force that is a reaction force from the fingertip to the fingertip;
When it is determined that the fingertip is in contact with the object, a direction of the contact force and a length direction of the first finger link are formed based on the first finger joint contact torque and the contact force. Contact angle estimating means for estimating a contact angle to be performed;
When it is determined that the fingertip is in contact with the object, based on the amount of minute deformation of the object and the contact force when performing a Young's modulus estimation operation for moving the fingertip to the object side by a minute amount, A Young's modulus estimating means for estimating the Young's modulus of the object;
Based on the Young's modulus and the contact angle, the gain for the first finger joint and the second finger joint are controlled so that the control rigidity when the fingertip contacts the object does not exceed the rigidity of the object. Gain adjusting means for adjusting the gain;
A robot hand control device comprising: The contact determination unit performs the contact determination operation on the operation command generation unit when it is detected that the first finger joint contact torque and the second finger joint contact torque are not 0 during execution of the normal operation. Output a contact judgment operation request to be executed,
The robot hand control device according to claim 1. When the first finger joint contact torque and the second finger joint contact torque are both 0 at the time of executing the contact determination operation, the contact determination unit is configured to detect a portion of the first finger link other than the fingertip. Determining that the object is in contact;
The robot hand control device according to claim 1 or 2. When the first finger joint contact torque is 0 and the second finger joint contact torque is not 0 when the contact determination operation is performed, the contact determination unit determines that the second finger link and the object are Determine that you are in contact,
The control apparatus of the robot hand as described in any one of Claims 1-3. When the normal operation is performed, the contact determination unit is configured such that all the N-1th finger joint contact torques (N is an integer equal to or greater than 2) are 0 and the Nth finger joint contact torque is not 0. In the case, it is determined that the N-th finger link and the object are in contact with each other.
The control apparatus of the robot hand as described in any one of Claims 1-4. The contact determination means determines that all the links are not in contact with the object when the joint contact torque for all the joints is 0 when the normal operation is performed.
The control apparatus of the robot hand as described in any one of Claims 1-5. The contact force estimating means estimates the contact force by the following equation (1).
The robot hand control device according to any one of claims 1 to 6.

However, the meaning of the symbols is as follows.
F: the contact force l 1: the length between the fingertip and the first finger joint l 2: the length between the first finger joint and the second finger joint θ 1: the angle of the first finger joint The contact angle estimation means estimates the contact angle by the following equation (2):
The robot hand control device according to any one of claims 1 to 7.

However, the meaning of the symbols is as follows.
θf: contact angle T1: the first finger joint contact torque F: the contact force l1: the length between the fingertip and the first finger joint The Young's modulus estimating means estimates the Young's modulus by the following equation (3).
The control apparatus of the robot hand as described in any one of Claims 1-8.

However, the meaning of the symbols is as follows.
R: radius of curvature of the fingertip E1: Young's modulus of the fingertip ν1: Poisson's ratio ν2 of the fingertip: fixed parameter d: the amount of minute deformation The gain adjusting means only determines the gains of the first finger joint and the second finger joint so that the rigidity of the closed loop system of the robot hand matches the rigidity of the object based on the estimated Young's modulus. Adjust the
The control apparatus of the robot hand as described in any one of Claims 1-9. The torque command generation means controls the first finger joint and the second finger joint by position proportional speed proportional control,
The gain adjusting means adjusts the gains of the first finger joint and the second finger joint according to the following formulas (4) to (7).
The control apparatus of the robot hand as described in any one of Claims 1-10.

However, the meaning of the symbols is as follows.
Kp1: Position proportional control gain of the first finger joint Kvj1: Speed proportional control gain of the first finger joint Kp2: Position proportional control gain of the second finger joint Kvj2: Speed proportional control gain of the second finger joint J1: Moment of inertia J2 between the fingertip and the first finger joint viewed from the first finger joint: Moment of inertia ζ1 between the first finger joint and the second finger joint viewed from the second finger joint: Desired damping Coefficient ζ2: Desired damping coefficient A method for controlling a robot hand comprising a plurality of links and a plurality of joints connecting the links,
An operation command generation step for outputting an operation command in response to the received operation request;
A torque command generation step for outputting a torque command for driving each joint based on a target angle of each joint determined according to the operation command and a gain set for each joint;
A torque value for resisting the weight and inertial force of the first finger link is subtracted from the torque value according to the torque command output to the first finger joint provided at the base portion of the first finger link located at the tip. From the first finger joint contact torque and the torque value by the torque command output to the second finger joint provided at the base portion of the second finger link connected to the first finger joint, the tip from the second finger joint Detecting the contact between the first finger link or the second finger link and an object based on the weight of the side portion and the second finger joint contact torque obtained by subtracting the torque value for resisting the inertial force, The first finger joint contact torque and the second finger joint contact torque are both 0 when performing a contact determination operation for displacing the first finger link so that the position of the fingertip that is the tip of the first finger link does not change. Not The case, the contact determination step and said and said fingertip object is in contact,
When it is determined that the fingertip is in contact with the object, the object is based on detected values of the first finger joint contact torque, the second finger joint contact torque, and the angle of the first finger joint. A contact force estimation step for estimating a contact force that is a reaction force from the fingertip to the fingertip;
When it is determined that the fingertip is in contact with the object, a direction of the contact force and a length direction of the first finger link are formed based on the first finger joint contact torque and the contact force. A contact angle estimation step for estimating a contact angle to be performed;
When it is determined that the fingertip is in contact with the object, based on the amount of minute deformation of the object and the contact force when performing a Young's modulus estimation operation for moving the fingertip to the object side by a minute amount, A Young's modulus estimation step for estimating the Young's modulus of the object;
Based on the Young's modulus and the contact angle, the gain for the first finger joint and the second finger joint are controlled so that the control rigidity when the fingertip contacts the object does not exceed the rigidity of the object. A gain adjustment step for adjusting the gain;
A method for controlling a robot hand comprising: In the contact determination step, when it is detected that the first finger joint contact torque and the second finger joint contact torque are not 0 at the time of executing the normal operation, the contact determination operation is performed on the operation command generation unit. Outputting a contact determination operation request to be executed;
The method for controlling a robot hand according to claim 12. In the contact determination step, when the first finger joint contact torque and the second finger joint contact torque are both 0 at the time of executing the contact determination operation, a portion other than the fingertip of the first finger link Determining that the object is in contact;
The method of controlling a robot hand according to claim 12 or 13. In the contact determination step, when the first finger joint contact torque is 0 and the second finger joint contact torque is not 0 when the contact determination operation is performed, the second finger link and the object are Determining that they are in contact with each other;
The method for controlling a robot hand according to any one of claims 12 to 14. In the contact determination step, the N-1th finger joint contact torque (N is an integer equal to or greater than 2) from the first finger joint contact torque is all 0 and the Nth finger joint contact torque is not 0 when the normal operation is performed. A step of determining that the Nth finger link and the object are in contact with each other;
The method of controlling a robot hand according to any one of claims 12 to 15. The contact determination step includes a step of determining that all the links are not in contact with the object when the joint contact torque for all the joints is 0 when the normal operation is performed.
The method of controlling a robot hand according to any one of claims 12 to 16. In the contact force estimation step, the contact force is estimated by the following equation (8).
The method for controlling a robot hand according to any one of claims 12 to 17.

However, the meaning of the symbols is as follows.
F: the contact force l 1: the length between the fingertip and the first finger joint l 2: the length between the first finger joint and the second finger joint θ 1: the angle of the first finger joint In the contact angle estimation step, the contact angle is estimated by the following equation (9).
The method for controlling a robot hand according to any one of claims 12 to 18.

However, the meaning of the symbols is as follows.
θf: contact angle T1: the first finger joint contact torque F: the contact force l1: the length between the fingertip and the first finger joint In the Young's modulus estimation step, the Young's modulus is estimated by the following equation (10).
The method for controlling a robot hand according to any one of claims 12 to 19.

However, the meaning of the symbols is as follows.
R: radius of curvature of the fingertip E1: Young's modulus of the fingertip ν1: Poisson's ratio ν2 of the fingertip: fixed parameter d: the amount of minute deformation In the gain adjustment step, based on the estimated Young's modulus, only the gains of the first finger joint and the second finger joint are set so that the rigidity of the closed loop system of the robot hand matches the rigidity of the object. Adjust the
The method for controlling a robot hand according to any one of claims 12 to 20. The torque command generation step includes a step of controlling the first finger joint and the second finger joint by position proportional speed proportional control,
The gain adjusting step includes a step of adjusting the gains of the first finger joint and the second finger joint according to the following formulas (11) to (14):
The control method of the robot hand according to any one of claims 12 to 21.

However, the meaning of the symbols is as follows.
Kp1: Position proportional control gain of the first finger joint Kvj1: Speed proportional control gain of the first finger joint Kp2: Position proportional control gain of the second finger joint Kvj2: Speed proportional control gain of the second finger joint J1: Moment of inertia J2 between the fingertip and the first finger joint viewed from the first finger joint: Moment of inertia ζ1 between the first finger joint and the second finger joint viewed from the second finger joint: Desired damping Coefficient ζ2: Desired damping coefficient A program for controlling a robot hand that operates on a computer that controls a robot hand including a plurality of links and a plurality of joints connecting the links ,
In the computer,
A method according to any one of claims 12-22 is performed.
Robot hand control program.

Images