JP4287242B2 - Robot hand control device - Google Patents

Description

  The present invention relates to a control device for a robot hand.

In a conventional robot hand control device, generally, a contact sensor or a force sensor is arranged on the palm side or fingertip of the finger provided on the hand, and whether or not the object is gripped based on the detected value. Determination is made to control the rotation angle (joint angle) of the finger joint (see, for example, Patent Documents 1 to 3).
Japanese Patent Laid-Open No. 9-300258 (FIG. 5 etc.) Japanese Patent Laid-Open No. 10-100089 (FIG. 3 etc.) Japanese Patent No. 2665894 (FIG. 2 etc.)

  However, when a contact sensor or a force sensor is arranged on the palm side or fingertip of the finger as in the prior art, there is a problem that the configuration becomes complicated and the hand is increased in size and weight.

  In addition, in robot hands mounted on installation type industrial robots, etc., where the usage environment is limited, the position of the object in the work area usually hardly changes. There is a low possibility that the finger part will come into contact with the object when performing the operation. On the other hand, a robot hand mounted on a humanoid robot capable of autonomous walking has a wide range of usage environments. For example, if the robot hand coexists in a human living environment, the position of the object in the work area is It may change irregularly. For this reason, in a robot hand mounted on a humanoid robot or the like, not only the presence of contact between a finger and a target when performing a gripping operation, but also a finger when performing an opening operation It is desirable that the presence or absence of contact with the object can also be determined.

  However, in the above-described prior art, since the contact sensor and the force sensor are arranged on the palm side of the finger part or the fingertip, it is determined whether or not the finger part is in contact with the object during the opening operation. I couldn't. In addition, if a contact sensor or the like is arranged on the back side of the hand of the finger part in order to determine whether or not the finger part is in contact with the object when the opening operation is performed, the configuration becomes more complicated and the above-described problems are more likely to occur. Needless to say, it becomes prominent.

  Accordingly, an object of the present invention is to solve the above-described problem, and whether or not the finger part and the object are in contact with each other when the gripping operation is performed, and the contact between the finger part and the object when the opening operation is performed. An object of the present invention is to provide a control device for a robot hand that determines presence / absence with a simple configuration and thus prevents an increase in the size and weight of the hand.

In order to achieve the first object described above, according to claim 1, a finger portion comprising a plurality of finger links and a finger joint connecting them is provided, and the finger joint is rotated around its joint axis. And a rotation angle detecting means for detecting a rotation angle (joint angle) of the finger joint, and a final target rotation of the finger joint according to an external command. A final target rotation angle determining means for determining an angle, and based on the final target rotation angle and the detected rotation angle, the finger joints are guided toward the final target rotation angle. An intermediate target rotation angle calculating means for calculating the intermediate target rotation angle, and a control means for controlling the drive of the actuator based on the intermediate target rotation angle. A first difference calculating means for calculating a first difference which is a difference between the intermediate target rotation angle and the detected rotation angle; and a first difference calculating means which is a difference between the final target rotation angle and the detected rotation angle. A second difference calculating unit that calculates a difference between the first difference, a determination unit that determines whether each of the first difference and the second difference is within a predetermined range, and the first difference and the Determination means for determining that the finger has contacted the object before the rotation angle reaches the final target rotation angle when it is determined that both of the second differences are not within the predetermined range. The final target rotation angle determining means sets the final target rotation angle to the rotation angle when it is determined that the finger portion has contacted the object before the rotation angle reaches the final target rotation angle. Before correcting When the final target rotation angle predetermined time has elapsed since been fixed to the rotation angle, was constructed the corrected final target rotation angle so as to return to the final target rotation angle is set according to the external command.

  According to a second aspect of the present invention, when the determination unit determines that both the first difference and the second difference are within the predetermined range, the rotation angle of the finger joint is set to the final target. The final target rotation angle determination means is configured to hold the final target rotation angle when it is determined that the finger joint has reached the final target rotation angle.

  According to a third aspect of the present invention, when the determination unit determines that the first difference is within the predetermined range and the second difference is outside the predetermined range, the finger joints Is determined to be rotated toward the final target rotation angle, and the final target rotation angle determination means determines that the finger joint is rotated toward the final target rotation angle. The final target rotation angle is maintained.

According to a fourth aspect of the present invention, the actuator is an electric motor, and the control means causes the determination means to return the final target rotation angle to a final target rotation angle set according to the external command. when I et been, and configured to supply current to the electric motor in the low power consumption is a predetermined duty ratio possible.

In the robot hand control apparatus according to claim 1, the robot hand includes a finger portion including a plurality of finger links and a finger joint connecting the finger links, and the finger joint is rotated around its joint axis to An actuator for changing the relative angle of the linked finger links, a rotation angle detecting means for detecting a rotation angle of the finger joint, and a final target rotation angle determination for determining a final target rotation angle of the finger joint according to an external command Means for calculating an intermediate target rotation angle of the finger joint so that the rotation angle is guided toward the final target rotation angle based on the means, the final target rotation angle and the detected rotation angle A control device for a robot hand, comprising: a target rotation angle calculation unit; and a control unit that controls driving of the actuator based on the intermediate target rotation angle, A first difference calculating means for calculating a first difference that is a difference between the target rotation angle and the detected rotation angle, and a second difference that is a difference between the final target rotation angle and the detected rotation angle. Second difference calculating means for determining, determining means for determining whether each of the first difference and the second difference is within a predetermined range, and the first difference and the second difference are And determining means for determining that the finger portion has contacted the object before the rotation angle reaches the final target rotation angle when it is determined that both are not within the predetermined range, and the final target The rotation angle determining means corrects the final target rotation angle to the rotation angle when it is determined that the finger portion has contacted the object before the rotation angle reaches the final target rotation angle, and Final target rotation angle When There a predetermined time has elapsed since the corrected on the rotation angle, since it is configured the corrected final target rotation angle so as to return to the final target rotation angle which is set in response to the external command, performing a gripping operation With a simple configuration (without providing a dedicated sensor), it is possible to determine the presence or absence of contact between the finger part and the target object when touching and the presence or absence of contact between the finger part and the target object when performing an opening operation, Therefore, an increase in the size of the hand and an increase in weight can be prevented. Further, by correcting the final target rotation angle based on the determination result, it is possible to prevent damage to the object and damage to the hand drive system.

  Further, in the robot hand control device according to claim 2, when the determination unit determines that the first difference and the second difference are both within the predetermined range, the finger joint And the final target rotation angle determination means determines the final target rotation angle when it is determined that the finger joint has reached the final target rotation angle. In addition to the above-described effects, the operator does not need to change the operation instruction of the hand according to the presence or absence of contact between the finger part and the object, thus reducing the burden on the operator. it can.

  In the robot hand control device according to claim 3, the determination unit determines that the first difference is within the predetermined range and the second difference is outside the predetermined range. When determined, the finger joint is determined to be rotated toward the final target rotation angle, and the final target rotation angle determination means is configured to rotate the finger joint toward the final target rotation angle. Since it is configured to hold the final target rotation angle when it is determined that the user is moving, in addition to the above-described effects, the operator changes the operation instruction of the hand according to the presence or absence of contact between the finger part and the object. This is unnecessary, and thus the burden on the operator can be reduced.

Also, when an additional note the effect of claim 1, when the front Symbol first difference and the second difference are both determined not within the predetermined range, before the rotation angle reaches the final target rotation angle The final target rotation angle determination means determines that the finger has contacted the object before the detected rotation angle reaches the final target rotation angle. The final target rotation angle is corrected to the detected rotation angle, and when the predetermined target time has elapsed after the final target rotation angle is corrected to the rotation angle, the corrected final target rotation angle is determined. since the angle was configured to return to the final target rotation angle which is set in response to the external command, Misao authors do not have to change the operation instruction of the hand in accordance with the presence or absence of contact of the fingers and the object, The burden on the operator can be reduced me.

In the robot hand control device according to claim 4 , the actuator is an electric motor, and the control means sets the final target rotation angle according to the external command by the determination means. when were found to return to the final target rotation angle, the so constructed as to supply current to the electric motor at a predetermined duty ratio that can reduce power consumption, in addition to the effects mentioned above, continue to grip the object It is possible to reduce the power consumption when doing and to reduce the burden on the electric motor.

  The best mode for carrying out the robot hand control apparatus according to the present invention will be described below with reference to the accompanying drawings.

  FIG. 1 is a front view of a robot on which a control device for a robot hand according to the present invention is mounted, and FIG. 2 is a right side view thereof.

  As shown in FIG. 1, the robot 10 includes two legs 12, and an upper body (base body) 14 is provided thereabove. A head 16 is formed further above the upper body 14, and two arms 18 are connected to both sides of the upper body 14. A hand (robot hand) 20 is connected to the tips of the left and right arms 18. Further, as shown in FIG. 2, a storage unit 22 is provided on the back of the upper body 14, and an ECU (electronic control unit) 24, a battery 26, and the like are accommodated therein.

  The robot 10 is given six degrees of freedom for each of the left and right legs 12, and operates the electric motor that drives these 6 × 2 = 12 joints based on the control value calculated by the ECU 24. Thus, a desired movement can be given to the entire foot, and the robot 10 can be arbitrarily moved in the three-dimensional space. Also, each of the left and right arm portions 18 is given five degrees of freedom, and by operating an electric motor that drives these 5 × 2 = 10 joints based on a control value calculated by the ECU 24, A desired movement can be given to the whole part. Further, each of the left and right hands 20 is given 14 degrees of freedom as will be described later, and the electric motors that drive these 14 × 2 = 28 joints are operated based on the control values calculated by the ECU 24. Thus, a desired movement such as gripping can be given to the hand 20. Thus, the robot 10 is a humanoid robot capable of autonomous walking imitating the shape of a person.

  FIG. 3 is a plan view of the hand 20 viewed from the palm side.

  As shown in FIG. 3, the hand 20 includes a palm part 30 and first to fifth finger parts 32 to 40 connected to the palm part 30. The first to fifth finger portions 32 to 40 correspond to a thumb, an index finger, a middle finger, a ring finger, and a little finger of a human hand, respectively.

  The palm portion 30 includes a palm forming member 42 that forms a surface on the palm side, and a back forming portion 44 that forms a surface on the back side of the hand, and a base end portion thereof is connected to the arm portion 18 described above.

Each of the first to fifth finger parts includes a plurality of finger links and a finger joint connecting them. Specifically, the first finger part 32 (thumb) is fixed to the palm part 30 with the terminal joint link 32a, the base joint link 32b, the first joint 32A connecting them, and the base joint link 32b. The second joint 32B is connected to the middle hand link 32c. The second finger 34 (index finger) includes a terminal link 34a, a middle link 34b, a base link 34c, a first joint 34A connecting the terminal link 34a and the middle link 34b, and a middle link. The second joint 34 </ b> B connecting the proximal link 34 c and the proximal link 34 c, and the third joint 34 </ b> C connecting the proximal link 34 c to the middle hand link 34 d fixed to the palm 30. The third finger part 36, the fourth finger part 38, and the fifth finger part 40 are configured in the same manner as the second finger part 34.

  FIG. 4 is an exploded perspective view of the second finger portion 34.

  As shown in FIG. 4, the middle link 34d includes a first electric motor (actuator, specifically, a stepping motor) 34d1 and a first reduction mechanism 34d2 including a planetary gear reducer that reduces the output thereof. And a first encoder 34d3 for detecting the rotation angle (joint angle) of the third joint 34C. The output shaft of the first electric motor 34d1, the input shaft of the first speed reduction mechanism 34d2, and the rotation shaft of the first encoder 34d3 are connected via a first belt 34d4.

  The rotation output portion 34d5 of the first reduction mechanism 34d2 is provided with a coupling portion 34d6. The coupling portion 34d6 is fixed to the proximal link 34c by a bolt (not shown). In this way, the middle link 34d and the proximal link 34c are coupled via the joint shaft 34CS of the third joint 34C (the output shaft of the first reduction mechanism 34d2) so that the relative angle can be changed.

  Further, the base link 34c includes a second electric motor (actuator, specifically, a stepping motor) 34c1, a second reduction mechanism 34c2 including a planetary gear reducer that reduces the output thereof, and a second A second encoder 34c3 that detects the rotation angle (joint angle) of the joint 34B is disposed. The output shaft of the second electric motor 34c1, the input shaft of the second reduction mechanism 34c2, and the rotation shaft of the second encoder 34c3 are connected via a second belt 34c4.

  The rotation output portion 34c5 of the second reduction mechanism 34c2 is provided with a coupling portion 34c6. The coupling portion 34c6 is fixed to the middle link 34b by a bolt (not shown). In this way, the base joint link 34c and the middle joint link 34b are coupled via the joint shaft 34BS of the second joint 34B (the output shaft of the second reduction mechanism 34c2) so that the relative angle can be changed.

  Further, the middle link 34b includes a third electric motor (actuator, specifically, a stepping motor) 34b1, a third reduction mechanism 34b2 including a planetary gear reducer that reduces the output, and the first A third encoder 34b3 that detects the rotation angle (joint angle) of the joint 34A is arranged. The output shaft of the third electric motor 34b1, the input shaft of the third reduction mechanism 34b2, and the rotation shaft of the third encoder 34b3 are connected via a third belt 34b4.

  A pin hole 34b6 is formed in the rotation output portion 34b5 of the third reduction mechanism 34b2. A pin hole 34a1 corresponding to the pin hole 34b6 is formed in the end node link 34a, and a pin (not shown) is inserted into the end node link 34a to connect the middle node link 34b and the end node link 34a.

  The middle link 34b and the end link 34a are further connected via an arm 34a2. One end of the arm 34a2 is rotatably attached to an appropriate position of the end node link 34a, and the other end is rotatably attached to an appropriate position of the middle node link 34b. Arranged in an inclined posture.

  As described above, the middle joint link 34b and the end joint link 34a have two joint shafts 34AS1 (the output shaft of the third reduction mechanism 34b2) and 34AS2 (the central shafts of the pin holes 34b6 and 34a1) constituting the first joint 34A. Through which the relative angle can be changed. A fingertip portion 34a3 having high flexibility and a friction coefficient is attached to the tip of the end link 34a.

  The rotation output of the first electric motor 34d1 is transmitted to the first speed reduction device 34d2 via the first belt 34d4, so that the third joint 34C is rotated around its joint axis 34CS, and the intermediate link 34c and The relative angle of the hand link 34d is changed. Further, the rotation output of the second electric motor 34c1 is transmitted to the second reduction gear 34c2 via the second belt 34c4, so that the second joint 34B is rotated around its joint axis 34BS, and the middle link 34b. And the relative angle of the proximal link 34c is changed.

  Further, the rotational output of the third electric motor 34b1 is transmitted to the third reduction gear 34b2 via the third belt 34b4, so that the first joint 34A is rotated around the one joint shaft 34AS1 and the distal link 34a. And the relative angle of the middle link 34b is changed. At this time, the relative angle of the end joint link 34a and the middle joint link 34b is further changed around the other joint axis 34AS2 by the action of the arm 34a2. That is, the rotation angle of the first joint 34A is the sum of the rotation angles of the two joint axes 34AS1 and 34AS2.

  The third finger part 36, the fourth finger part 38, and the fifth finger part 40 are also configured in the same manner as the second finger part 34, and three electric motors (not shown) are arranged on each finger part. To drive the first to third joints around their joint axes, thereby changing the relative angles of the links. The first finger portion 32 is configured in the same manner as the second finger portion 34 except that the first finger portion 32 does not include a third joint and a middle joint link (including an electric motor, a speed reducer, and an encoder arranged there). Then, by driving two electric motors (not shown) disposed on the middle link 32a and the proximal link 32b, the first and second joints are rotated to change the relative angle of each link.

  As described above, in the hand 20 according to this embodiment, by driving three or two electric motors arranged on each finger, each finger is bent and extended, A gripping operation or the like can be performed.

  Next, the operation of the robot hand control apparatus according to the present invention will be described with reference to FIG.

  FIG. 5 is a block diagram functionally showing the operation. As shown in FIG. 5, the finger joint instruction rotation angle θouter_input [rad] corresponding to the operation instruction (external input) of the hand 20 input from the operator via the controller 50 is input to the final target rotation angle determination unit 52. Is done. The controller 50 is arranged at a position separated from the robot 10 and is configured to be communicable with the ECU 24 via a wire or wirelessly.

  The final target rotation angle determination unit 52 determines the final target rotation angle θaim [rad] (the rotation angle (joint angle) of the finger part to be finally targeted) based on the input instruction rotation angle θouter_input and each value described later. Is output to the ramp wave trajectory generator 54. The ramp wave trajectory generator 54 generates a target rotation angle θcmd_ramp [rad] for each minute interval based on the input final target rotation angle θaim and the like. Here, “minute interval” means a control period, and in this embodiment, it is 5 msec. The processes of the final target rotation angle determination unit 52 and the ramp wave trajectory generation unit 54 will be described in detail later.

  The target rotation angle θcmd_ramp output from the ramp wave trajectory generation unit 54 passes through the first filter 56 and the second filter 58 (first-order lag element), and then passes the target rotation angle θcmd [rad] (intermediate target described above). A rotation angle, specifically, an intermediate target rotation angle (joint angle) that guides the rotation angle of the finger toward the final target rotation angle θaim. The subtractor 60 subtracts the detected rotation angle θact [rad] of the finger joint detected by the encoder from the post-filter target rotation angle θcmd, and then the multiplier 62 sets the position gain Kp [1 / sec]. The target rotational angular velocity ωcmd [rad / sec] is calculated by multiplying and adding the speed FF (feed forward) rotational angular velocity ωff [rad / sec] (described later) by the adder 64. The target rotational angular velocity ωcmd is input to a motor driver (not shown), and the motor driver supplies a current corresponding to the input target rotational angular velocity ωcmd to the electric motor described above.

  Further, the post-filter target rotation angle θcmd is input to the previous value storage unit 66 and stored as the previous target rotation angle θcmd_p [rad]. The stored previous target rotational angle θcmd_p is input to the ramp wave trajectory generating unit 54 together with the rotational angular velocity limit ωlim [rad / sec] corresponding to the external input and the final target rotational angle θaim.

The ramp wave trajectory generator 54 calculates the target rotation angle θcmd_ramp described above based on each input value described above. Specifically, first, the final target rotation angle θaim is substituted into the target rotation angle θcmd_ramp. Then, the rotational angular velocity ω is calculated according to the following formula 1. In Equation 1, DELTA_T means a control period (a minute period), and in this embodiment, it is set to 5 [msec].
ω = (θcmd_ramp−θcmd_p) / DELTA_T Equation 1

Next, a limit check of the rotational angular velocity ω is performed. Specifically, it is determined whether or not the rotational angular velocity ω is within the range of the rotational angular velocity limit ωlim. When the rotational angular velocity ω is within the range, the calculated value of the rotational angular velocity ω is retained, and when it is out of the range, Substitute the rotation angular velocity limit ωlim. Then, based on the rotation angular velocity ω and the previous target rotation angle θcmd_p, the target rotation angle θcmd_ramp is calculated according to the following equation 2.
θcmd_ramp = θcmd_p + ω × DELTA_T Equation 2

The previous target rotation angle θcmd_p is also input to the speed FF (feed forward) rotation angular velocity calculator 68 together with the post-filter target rotation angle θcmd. The speed FF rotational angular speed calculation unit 68 calculates the above-described speed FF rotational angular speed ωff according to the following formula 3.
ωff = (θcmd−θcmd_p) / DELTA_T Equation 3

  In FIG. 5, the internal configuration of the ECU 24 is shown except for the controller 50, the electric motor, and the encoder.

  The previous target rotation angle θcmd_p is also input to the final target rotation angle determination unit 52. The final target rotation angle determination unit 52 further receives the detected rotation angle θact, and determines the final target rotation angle θaim described above based on each input value.

  FIGS. 6 and 7 are flowcharts showing the final target rotation angle θaim correction process in the final target rotation angle determination unit 52. The illustrated program is executed in the ECU 24 every control cycle (5 msec) described above.

  Hereinafter, the processing will be described with reference to FIGS. 6 and 7. First, in S10, it is determined whether or not the value of the first timer servo_time exceeds zero. Note that the first timer servo_time is counted up at the last step of this flowchart. That is, the determination in S10 corresponds to determining whether or not the illustrated program (the process of correcting the final target rotation angle θaim) is not the first time.

  When the result in S10 is negative, the process proceeds to S12, where the status status indicating the operating state of the hand 20 is set to REACH (described later), and the process proceeds to S14 where the status status is set to the previous value (hereinafter referred to as “previous status status_p”) save. If the result in S10 is affirmative, the status status set in the later-described step during the previous program execution is stored as the previous status status_p.

  Next, in S16, the detected rotation angle θact is subtracted from the previous target rotation angle θcmd_p to calculate the first difference Δθ1 [rad]. Then, the process further proceeds to S18, where the detected rotation angle θact is subtracted from the final target rotation angle θaim to calculate the second difference Δθ2 [rad]. Note that the initial value of the final target rotation angle θaim is the instruction rotation angle θouter_input.

  Next, in S20, whether or not the absolute value of the first difference Δθ1 is equal to or smaller than the lock detection angle detect_th [rad] (for example, 0.01 [rad] (about 0.6 [deg])), in other words, the first difference. It is determined whether or not the difference Δθ1 is within a predetermined range (within plus / minus detect_th). Here, as shown in FIG. 8, the lock detection angle detect_th is set so as to increase as the rotational speed limit ωlim increases.

  Affirmative in S20, the first difference Δθ1 that is the difference between the previous target rotation angle θcmd_p and the detected rotation angle θact is within the predetermined range, in other words, the detected rotation angle θact follows the previous target rotation angle θcmd_p. When the determination is made, the process proceeds to S22, and whether or not the absolute value of the second difference Δθ2 is equal to or smaller than the lock detection angle detect_th, in other words, the second difference Δθ2 is within a predetermined range (plus / minus detect_th). In).

  When the result in S22 is affirmative and it is determined that the second difference Δθ2 that is the difference between the final target rotation angle θaim and the detected rotation angle θact is within the predetermined range, the process proceeds to S24, and the value of the first counter reach_cnt is set to 1. In step S26, it is determined whether or not the value of the first counter reach_cnt exceeds a predetermined value detect_cnt (for example, 3).

  When the result in S26 is negative, the process proceeds to S28, where the previous status status_p is set to the current value (the previous status status_p is held). When the result is S26, the process proceeds to S30, where the detected rotation angle θact is the final target rotation angle. It is determined that θaim has been reached and the status status is set to REACH. That is, the status “REACH” means that the detected rotation angle θact has reached the final target rotation angle θaim. Then, the process proceeds to S32, and the values of the first counter reach_cnt, the second counter move_cnt (described later), and the third counter lock_cnt (described later) are all reset to zero.

  Thus, in this embodiment, the detected rotation angle θact approximates the previous target rotation angle θcmd_p and the final target rotation angle θaim (that is, the first difference Δθ1 and the second difference Δθ2 are both within a predetermined range. When the state (within plus / minus detect_th) continues for a predetermined time (when the value of the first counter reach_cnt reaches the predetermined value detect_cnt), the detected rotation angle θact reaches the final target rotation angle θaim. Judgment was made.

  On the other hand, when it is determined in S22 that the second difference Δθ2 that is the difference between the final target rotation angle θaim and the detected rotation angle θact is outside the predetermined range, the process proceeds to S34 and the value of the second counter move_cnt is determined. Is incremented by 1, and the process further proceeds to S36 to determine whether or not the value of the second counter move_cnt has exceeded the predetermined value detect_cnt.

  When the result in S36 is negative, the process proceeds to S38 and the previous status status_p is held. When the result in S36 is positive, the process proceeds to S40, where it is determined that the finger joint is rotating toward the final target rotation angle θaim. Set status to MOVE. That is, the status “MOVE” means that the finger joint is rotating toward the final target rotation angle θaim. In S42, the values of the first counter reach_cnt, the second counter move_cnt, and the third counter lock_cnt are all reset to zero.

  Thus, the detected rotation angle θact approximates to the previous target rotation angle θcmd_p (the first difference Δθ1 is within a predetermined range (within plus / minus detect_th)) and has not reached the final target rotation angle θaim (first When the difference Δθ2 of 2 is outside the predetermined range (outside of plus / minus detect_th) and continues for a predetermined time (when the value of the second counter move_cnt reaches the predetermined value detect_cnt), the finger joint is rotated at the final target rotation. It was determined that the motor was rotating toward the angle θaim.

  Further, the first difference Δθ1 which is negative in S20 and is the difference between the previous target rotation angle θcmd_p and the detected rotation angle θact is outside the predetermined range, in other words, the detected rotation angle θact follows the previous target rotation angle θcmd_p. If it is determined that there is not, then the process proceeds to S44, where it is determined whether or not the absolute value of the second difference Δθ2 is equal to or smaller than the lock detection angle detect_th.

  When the result in S44 is affirmative (usually impossible), the process proceeds to S38 to hold the previous status status_p, while the result is negative in S44 and the second difference Δθ2 that is the difference between the final target rotation angle θaim and the detected rotation angle θact is obtained. If it is determined that the value is outside the predetermined range, the process proceeds to S46, the value of the third counter lock_cnt is incremented by one, and the process proceeds to S48, where the value of the third counter lock_cnt exceeds the predetermined value detect_cnt described above. Judge whether or not.

  When the result is negative in S48, the previous status status_p is held in S38, while when the result is positive in S48, the process proceeds to S50, where the finger touches the object and stops before reaching the final target rotation angle θaim. It is determined that (locked) and the status status is set to LOCK. That is, the status status “LOCK” means that the finger has stopped in contact with the object before reaching the final target rotation angle θaim. In S52, the values of the first counter reach_cnt, the second counter move_cnt, and the third counter lock_cnt are all reset to zero.

  Thus, the detected rotation angle θact does not approximate the previous target rotation angle θcmd_p and the final target rotation angle θaim (the first difference Δθ1 and the second difference Δθ2 are both outside the predetermined range (outside plus / minus detect_th)). When the state continues for a predetermined time (when the value of the third counter lock_cnt reaches the predetermined value detect_cnt), it is determined that the finger portion has come into contact with the object and stopped.

  As described above, since the lock detection angle detect_th is set to a larger value as the rotation speed limit ωlim increases, even when the driving speed of the hand 20 is high, the operation state is the above three types. ("REACH", "MOVE", "LOCK") can be accurately determined.

  The program then proceeds to S54 shown in FIG. 7 and determines whether the status status is set to LOCK. When the result in S54 is negative, the program proceeds to S56, in which a bit of a rewrite flag rewrite_flg (described later) is reset to 0.

  On the other hand, when the result in S54 is affirmative, the process proceeds to S58, and it is determined whether or not the value of the second timer rewrite_time has reached a predetermined time detect_time, specifically 500 [msec]. When the result in S58 is affirmative, the process proceeds to S56, and the bit of the rewrite flag rewrite_flg is reset to 0. On the other hand, when the result in S58 is negative, the process proceeds to S60, and the bit of the rewrite flag rewrite_flg is set to 1.

  Next, in S62, it is determined whether or not the bit of the rewrite flag rewrite_flg is set to 1. When the result in S62 is negative, the program proceeds to S64, where the command rotation angle θouter_input is set as the final target rotation angle θaim. That is, the final target rotation angle θaim is determined to be the designated rotation angle θouter_input. Since the initial value of the final target rotation angle θaim is the designated rotation angle θouter_input as described above, the processing here corresponds to holding the value of the final target rotation angle θaim. Then, the process proceeds to S66, the value of the second timer rewrite_time is reset to 0, and then the process proceeds to S68, where the value of the first timer servo_time is counted up by 5 [msec] and the process ends.

  On the other hand, when the result in S62 is affirmative, the program proceeds to S70 where the detected rotation angle θact is set to the final target rotation angle θaim. That is, the final target rotation angle θaim is corrected (determined) from the instruction rotation angle θouter_input to the detected rotation angle θact. In step S72, the value of the second timer rewrite_time is incremented by 5 [msec], and in step S68, the first timer servo_time is incremented by 5 [msec] and the process ends.

  As described above, in the first embodiment of the present invention, the first difference Δθ1 that is the difference between the previous target rotation angle θcmd_p (the previous value of the post-filter target rotation angle θcmd) and the detected rotation angle θact, and the final A second difference Δθ2 that is a difference between the target rotation angle θaim and the detected rotation angle θact is calculated, and it is determined whether or not they are within a predetermined range. Based on the determination result, the contact between the finger and the object is determined. More specifically, when it is determined that the first difference Δθ1 and the second difference Δθ2 are both outside the predetermined range, it is determined that the finger has contacted the object (S50), and Since it is determined that the finger part and the object are not in contact with each other (S30, S40), the presence or absence of contact between the finger part and the object when the gripping operation is performed, and the opening operation are performed. With a simple configuration, the presence or absence of contact between the finger and the object It is possible to make a determination (without providing a dedicated sensor), and thus it is possible to prevent the hand 20 from being enlarged and increased in weight.

  Further, the final target rotation angle θaim is corrected based on the determination result. Specifically, it is determined that the finger has touched the object before reaching the final target rotation angle θaim, and the status status is set to LOCK. When the bit of the rewrite flag rewrite_flg is set to 1 (S60), the final target rotation angle θaim determined according to the instruction rotation angle θouter_input is corrected to the detected rotation angle θact (that is, the finger portion Since the operation is stopped (S70)), it is possible to prevent damage to the object and damage to the drive system of the hand 20 (such as a planetary gear reducer or an electric motor).

  Further, when the elapsed time (the value of the second timer rewrite_time) after the final target rotation angle θaim is corrected to the detected rotation angle θact has reached a predetermined time detect_time (for example, 500 [msec]) (Yes in S58) ), The bit of the rewrite flag rewrite_flg is reset to 0 (S56), and the final target rotation angle θaim is returned to the designated rotation angle θouter_input, thereby releasing the contact between the finger and the object as shown in FIG. The detected rotation angle θact is quickly made to reach the instruction rotation angle θouter_input (final target rotation angle θaim) input by the operator. When the contact between the finger and the object is continued, the final target rotation angle θaim is corrected again to the detected rotation angle θact according to the processing of the flowcharts shown in FIGS.

  Thus, when it is determined that the finger has contacted the object before the detected rotation angle θact reaches the final target rotation angle θaim, the final target rotation angle θaim is corrected to the detected rotation angle θact to The operation is stopped, and when the contact between the finger and the object is released, the final target rotation angle θaim is returned to the designated rotation angle θouter_input. Thus, there is no need to change the operation instruction (instructed rotation angle θouter_input) of the hand 20, and the burden on the operator can be reduced.

  FIG. 10 is a time chart showing a current waveform supplied to the electric motor when the finger part and the object are in contact with each other.

  In FIG. 10, the period in which the current waveform (pulse waveform) rises is a period from when the final target rotation angle θaim is restored to the command rotation angle θouter_input until it is corrected again to the detected rotation angle θact.

  Hereinafter, the period during which the current waveform rises will be described in detail with reference to FIG. When the final target rotation angle θaim is returned to the command rotation angle θouter_input, energization of each electric motor is resumed. At this time, if the finger part and the object are in contact, the detected rotation angle θact does not follow the previous target rotation angle θcmd_p, so that the first difference Δθ1 gradually increases as shown in the figure. The degree of increase in the first difference Δθ1 is determined according to the rotational speed of the electric motor, the reduction ratio of the reduction gear, and the time constants of the first filter 56 and the second filter 58.

  When the first difference Δθ1 reaches the lock detection angle detect_th, the counter lock_cnt starts counting up (note that, under the situation where the first difference Δθ1 reaches the lock detection angle detect_th, the second difference Δθ2 is already The lock detection angle detect_th has been reached). Then, when the value of the counter lock_cnt reaches the predetermined value detect_cnt, it is determined that the finger has contacted the object, and the final target rotation angle θaim is set to the detected rotation angle θact.

  When the final target rotation angle θaim is set to the detection rotation angle θact, the first difference Δθ1 and the second difference Δθ2 are both within the lock detection angle detect_th (the detection rotation angle θact becomes the final target rotation angle θaim). Therefore, energization to each electric motor is stopped (deenergized). Stopping energization of the electric motor is continued until the value of the timer rewrite_time reaches the predetermined time detect_time.

  Here, as can be understood from the above configuration, the lock detection angle detect_th, the predetermined value detect_cnt, the predetermined time detect_time, the rotation speed of the electric motor, the reduction ratio of the reduction gear, the first filter 56 and the second filter 58. By setting the time constant to an appropriate value, the period during which the current waveform rises, that is, the duty ratio of the supply current can be adjusted to an appropriate value. In this embodiment, the above values were set so that the duty ratio of the supply current was about 20%. Thereby, it is possible to reduce power consumption when the finger unit and the object continue to contact (more specifically, when holding the object), and to reduce the burden on the electric motor. . In particular, in an autonomous biped robot such as the robot 10 according to this embodiment, the battery capacity to be mounted is limited, and a large amount of power is required to move in the environment. Therefore, it is very effective to reduce the power consumption of an operation that may be performed continuously, such as gripping an object.

  In addition, when electric power is not supplied to the electric motor, the self-locking phenomenon (self locking of the reduction gears including the planetary gear reducer described above can be driven from the input shaft side to the output shaft side. By intentionally using the characteristic that the input shaft side cannot be driven from the side, the posture (rotation angle) of the finger portion is maintained.

As described above, in the first embodiment of the present invention, a plurality of finger links (terminal link 32a-40a, middle link 34b-40b, base link 32b, 34c-40c, middle link 32c, 34d). To 40d) and finger parts (first to fifth finger parts 32 to 40) composed of finger joints (first joints 32A to 40A, second joints 32B to 40B, and third joints 34C to 40C) connecting them. ) And an actuator (electric motors 34b1, 34c1, 34d1, etc.) that rotate the finger joints around their joint axes (34AS1, 34AS2, 34BS, 34CS, etc.) to change the relative angle of the connected finger links. Rotation angle detection means (encoders 34b3, 34c3, 34d3, etc.) for detecting the rotation angle (θact) of the finger joint, and an external command (instructed rotation angle θ final target rotation angle determination means (ECU 24, final target rotation angle determination unit 52) for determining a final target rotation angle (θaim) of the finger joint according to outer_input), the final target rotation angle and the detected rotation angle Based on the above, intermediate target rotation angle calculation means for calculating an intermediate target rotation angle (filtered target rotation angle θcmd) of the finger joint so that the rotation angle is guided toward the final target rotation angle ( ECU 24, ramp wave activation generation unit 54, first filter 56, second filter 58), and control means (ECU 24) for controlling the driving of the actuator based on the intermediate target rotation angle The control device of the hand 20, wherein the intermediate target rotation angle (specifically, the previous target rotation angle θcmd which is the previous value of the target rotation angle θcmd after passing through the filter) _p) and a first difference calculation means (ECU 24, final target rotation angle determination unit 52, S16) for calculating a first difference (Δθ1) as a difference between the detected rotation angle, the final target rotation angle, and the Second difference calculation means (ECU 24, final target rotation angle determination unit 52, S18) that calculates a second difference (Δθ2) that is a difference between the detected rotation angles, the first difference, and the second difference Determining means (ECU 24, final target rotation angle determination unit 52, S20, S22, S44) for determining whether or not each is within a predetermined range, and both the first difference and the second difference are the predetermined when it is determined that it does not fall within the range, the rotation angle is the final target rotation determination means and the finger is in contact with the object before reaching the angle (ECU 24, the final target rotation angle determining section 52, 50) provided with a, the final target rotational angle determining means, when the fingers before the rotation angle reaches the final target rotation angle is determined to be in contact with the object, the final target rotation The angle is corrected to the rotation angle (S70), and when the predetermined target time has elapsed since the final target rotation angle is corrected to the rotation angle, the corrected final target rotation angle is set according to the external command. The final target rotation angle is restored (S58, S56, S62, S64) .

  The determination means determines that the rotation angle of the finger joint has reached the final target rotation angle when it is determined that the first difference and the second difference are both within the predetermined range. Along with (S30), the final target rotation angle determination means is configured to hold the final target rotation angle when it is determined that the finger joint has reached the final target rotation angle (S64).

  In addition, when the determination unit determines that the first difference is within the predetermined range and the second difference is outside the predetermined range, the finger joint is directed toward the final target rotation angle. The final target rotation angle determining means determines that the finger joint is rotated toward the final target rotation angle when the final target rotation angle is determined to be rotated (S40). The angle was held (S64).

Also, together with the actuator is an electric motor, said control means, when the final target rotation angle was found to return to the final target rotation angle which is set in response to the external command by the determining means, the power consumption A current is supplied to the electric motor at a predetermined duty ratio that can be reduced .

  In the above description, a force sensor or a contact sensor may be disposed on the finger portion, and the presence / absence of contact between the finger portion and the object may be determined in consideration of the detected values.

  In addition, the stepping motor is used as an actuator that changes the relative angle of the finger links that are connected by rotating the finger joints. However, if the actuator can rotate the finger joints, other motors such as DC motors and rotary solenoids may be used. An actuator may be used.

1 is a front view of a robot on which a control device for a robot hand according to a first embodiment of the present invention is mounted. It is a right view of the robot shown in FIG. It is the top view which looked at the hand shown in FIG. 1 from the palm side. It is a disassembled perspective view of the 2nd finger | toe part shown in FIG. It is a block diagram which shows the operation | movement of the apparatus shown in FIG. 1 functionally. It is a flowchart which shows the first half part of operation | movement of the apparatus shown in FIG. It is a flowchart which shows the latter half part of operation | movement of the apparatus shown in FIG. 6 is a characteristic diagram showing characteristics of a rotational angular velocity limit with respect to a lock detection angle used in the flowchart of FIG. FIG. 8 is a time chart showing a tracking relationship of a detected rotation angle with respect to a final target rotation angle determined in the flowchart of FIG. 7. It is a time chart which shows the electric current waveform supplied to an electric motor when the finger | toe part shown in FIG. 3 is contacting the target object. It is a time chart explaining the duty ratio of the electric current supplied to an electric motor when the finger | toe part shown in FIG. 3 is contacting the target object.

Explanation of symbols

20 robot hand 24 ECU (control means, final target rotation angle determination means, intermediate target rotation angle calculation means, first difference calculation means, second difference calculation means, determination means, determination means)
32 to 40 First to fifth fingers 32a to 40a End link (finger link)
34b-40b Middle link (finger link)
32b, 34c-40c Prosthetic link (finger link)
32c, 34d to 40d Middle hand link (finger link)
34b1, 34c1, 34d1 Electric motor (actuator)
34b3, 34c3, 34d3 encoder (rotation angle detection means)
32A-40A 1st joint (finger joint)
32B-40B second joint (finger joint)
34C-40C 3rd joint (finger joint)
34AS1, 34AS2, 34BS, 34CS Joint shaft 52 Final target rotation angle determination unit (final target rotation angle determination means, first difference calculation means, second difference calculation means, determination means, determination means)
54 Ramp wave activation generator (intermediate target rotation angle calculation means)
56 1st filter (intermediate target rotation angle calculation means)
58 second filter (intermediate target rotation angle calculation means)

Claims (4)

While having a finger part composed of a plurality of finger links and a finger joint connecting them,
a. An actuator for rotating the finger joint around its joint axis to change the relative angle of the connected finger links;
b. Rotation angle detecting means for detecting the rotation angle of the finger joint;
c. Final target rotation angle determination means for determining a final target rotation angle of the finger joint in response to an external command;
d. An intermediate target rotation angle for calculating an intermediate target rotation angle of the finger joint so that the rotation angle is guided toward the final target rotation angle based on the final target rotation angle and the detected rotation angle. A calculation means;
And e. Control means for controlling the drive of the actuator based on the intermediate target rotation angle;
A control device for a robot hand comprising:
f. First difference calculation means for calculating a first difference which is a difference between the intermediate target rotation angle and the detected rotation angle;
g. Second difference calculating means for calculating a second difference which is a difference between the final target rotation angle and the detected rotation angle;
h. A judgment means each of said first difference and said second difference you determined whether within a predetermined range,
And i. When it is determined that both the first difference and the second difference are not within the predetermined range, it is determined that the finger has contacted the object before the rotation angle reaches the final target rotation angle. Determination means to perform,
And the final target rotation angle determining means determines that the final target rotation angle is rotated when it is determined that the finger has contacted an object before the rotation angle reaches the final target rotation angle. In addition to correcting the final target rotation angle to a final target rotation angle set according to the external command when a predetermined time has elapsed since the final target rotation angle was corrected to the rotation angle. A control device for a robot hand, characterized by being configured to allow   The determination means determines that the rotation angle of the finger joint has reached the final target rotation angle when it is determined that the first difference and the second difference are both within the predetermined range; The robot according to claim 1, wherein the final target rotation angle determination unit is configured to hold the final target rotation angle when it is determined that the finger joint has reached the final target rotation angle. Hand control device.   The determination means rotates the finger joint toward the final target rotation angle when it is determined that the first difference is within the predetermined range and the second difference is outside the predetermined range. The final target rotation angle determination means holds the final target rotation angle when it is determined that the finger joint is rotated toward the final target rotation angle. 3. The robot hand control device according to claim 1, wherein the control device is a robot hand. Together with the actuator is an electric motor, said control means, when the final target rotation angle is the et to return to the final target rotation angle is set in accordance with the external command by the determination unit, a reduction in power consumption possible predetermined control apparatus for a robot hand according to any one of claims 1 to 3, characterized by being configured to supply current to the electric motor at duty ratio.

Images