JP2012179679A - System for controlling robot hand - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a system for controlling a robot hand that is compatible with contact objects different in hardness by a simple configuration.SOLUTION: The system for controlling a robot hand includes: an in-contact joint state estimator 131 for estimating a joint state when a finger comes into contact with the contact object; a not in-contact joint state estimator 132 for estimating a joint state when the finger does not come into contact with the contact object; a contact determiner 133 for determining whether the finger comes into contact with the contact object; a torque disturbance estimator 134 for estimating a torque disturbance applied to a joint; a rigidity estimator 135 for estimating the rigidity of the contact object; a control rigidity computing unit 137 for computing a minimum value of a control rigidity of the joint so that the finger can maintain a pressed state of the contact object thereby, when the result of the determination of contact is a contact state; and a control unit 138 for controlling a joint motor 121.

Description

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

  A general robot hand includes a plurality of fingers and a base to which the fingers are movable (rotatable) at the root portion. The finger includes a plurality of links, a joint that operably connects two adjacent links, and a joint motor that drives the joint. With such a configuration, the robot hand can be configured to imitate a part of the function of a human hand.

FIG. 7 shows a schematic diagram of the robot hand. In the robot hand shown in FIG. 7, the first to fourth fingers are operably connected to the base 723.
Specifically, the first finger includes a first finger first link 701, a first finger MP (metacarpophalangeal) joint 702, a first finger second link 703, and a first finger IP (interphalangeal) joint 704. Prepare.

  The second finger includes a second finger first link 705, a second finger MP joint 706, a second finger second link 707, a second finger PIP (proximal interphalangeal) joint 708, and a second finger third link 709. And a second finger DIP (distal interphalangeal) joint 710.

  The third finger includes a third finger first link 711, a third finger MP joint 712, a third finger second link 713, a third finger PIP joint 714, a third finger third link 715, and a third finger. A finger DIP joint 716.

  The fourth finger includes a fourth finger first link 717, a fourth finger MP joint 718, a fourth finger second link 719, a fourth finger PIP joint 720, a fourth finger third link 721, and a fourth finger. A finger DIP joint 722.

  These finger joints are provided with torsion springs. Two adjacent joints operate in conjunction with an interlocking link member (not shown). Each finger is bent and stretched by rotating the interlocking link member with a joint motor (not shown). At this time, if the finger contacts the contacted body and receives a reaction force during the bending and stretching operation of the finger, the above-described torsion spring absorbs the reaction force.

  Utilizing such a configuration of the robot arm, the robot hand of Patent Document 1 uses a finger with low joint rigidity when handling a soft contacted object by changing the joint rigidity of each of a plurality of fingers. When a hard contacted object is handled, a finger with high joint rigidity is used.

  Also, Patent Document 2 discloses a robot hand configured to be able to satisfactorily grip a contacted body having different hardness. The robot hand of Patent Document 2 performs contact detection and slip detection on the contacted object based on detection information of the probe provided at the tip of the finger, and the contacted object moves relative to the gravitational direction. In order not to occur, the contacted object is gripped with a minimum gripping force.

  Incidentally, Patent Documents 3 and 4 disclose a robot hand configured to be able to grip a contacted object satisfactorily. The robot hand of Patent Document 3 is configured such that a finger link is covered with a covering member having low rigidity and further covered with a covering member having a high friction coefficient. The robot hand of Patent Document 4 is configured to increase the gripping force when the external force applied to the robot hand detected by the force detection means increases.

JP 2009-125888 A JP 2010-149262 A JP 2005-88096 A Japanese Patent Laid-Open No. 10-100089

Since the robot hand of Patent Document 1 has predetermined joint rigidity, the robot hand cannot flexibly cope with contacted bodies having different hardnesses.
The robot hand of Patent Document 2 requires a probe at the tip of the robot hand, and the configuration becomes complicated.

  An object of the present invention is to solve such a problem, and is to provide a control device for a robot hand that can deal with contacted bodies having different hardnesses with a simple configuration.

  A robot hand control device according to an aspect of the present invention is a robot hand control device including a finger having a link and a joint of the link, and is based on a joint angle detection value that is an angle detection value of the joint. The joint state estimator for estimating the joint state, which is the joint angle, joint speed, and joint acceleration when the finger is in contact with the contacted object, and the finger is not in contact with the contacted object. Non-contact joint state estimator for estimating the joint state at the time, the joint angle detection value, the contact joint state estimation value output by the contact joint state estimator, and the non-contact joint state estimation A contact determination unit that determines whether or not the finger has contacted the contacted body based on a non-contact joint state estimation value output by the device, and a determination result output by the contact determination device is a contact state In the case of A torque disturbance estimator that estimates a torque disturbance applied to the joint based on a joint state estimated value at the time of touch, and when the contact determination result is a contact state, the joint state estimated value at the time of contact and the torque disturbance estimation A stiffness estimator that estimates the stiffness of the contacted object based on a torque disturbance estimated value output by the device, and when the contact determination result is a contact state, the joint angle detection value and the stiffness estimated value, The joint control stiffness is calculated to a minimum value that allows the finger to maintain the pressed state of the contacted body, and when the contact determination result is a non-contact state, the joint control stiffness is calculated in advance. Based on a control stiffness calculator that calculates a value larger than the control stiffness of the joint when the estimated finger is in contact with the contacted object, and a control stiffness calculation value output by the control stiffness calculator ,in front And a control unit for controlling a joint motor for driving the joint, the.

  The finger of the robot hand includes a first joint, a second joint, and a third joint, the contacted body includes a home appliance key or a musical instrument keyboard or button, and an elastic body, and the control device includes the contact When the determination result is a contact state, the determination unit further includes an arrival determination unit that determines whether or not the contacted body has reached the pushing bottom based on the estimated joint state value at the time of contact. When the contact determination result is a non-contact state, the stiffness estimation value is larger than a value obtained by equivalently converting the rotation stiffness around the first joint and the second joint, and the control stiffness of the first joint and the second joint of the finger When the contact determination result is in the contact state, the first joint and the second joint are set to a value substantially equal to a value obtained by equivalently converting the rigidity estimated value to the rotational stiffness around the first joint and the second joint. Calculate the joint control stiffness and When the arrival determination result output from the determiner is in a state where the push-in bottom is not reached, the control stiffness of the third joint is set to a value larger than the value obtained by equivalently converting the estimated stiffness value to the rotational stiffness around the third joint. When the arrival determination result reaches the indentation bottom, the control stiffness of the third joint is calculated to be equal to the value obtained by equivalently converting the estimated stiffness value to the rotational stiffness around the third joint. It is preferable to do.

  The contact determination unit is configured such that a time average value of an absolute value of a contact joint angle estimation error obtained by subtracting a contact joint angle estimation value which is a component of the contact joint state estimated value from the joint angle is calculated from the joint angle. If the absolute value of the non-contact joint angle estimation error, which is the component of the non-contact joint state estimated value subtracted from the non-contact joint angle estimated value, is smaller than the time average value, it is determined that the contact state is present. In the case, it is preferable to determine that it is in a non-contact state.

ただし、記号の意味は以下の通りであり、ｄ＾、ｑ＾、ｆ_ｃ＾、Ｔはベクトル、Ｊは行列である。
ｄ＾：トルク外乱推定値
Ｊ：慣性行列
ｆ_ｃ＾：接触時の非線形力の推定値（接触時非線形力推定値）
ｑ＾：接触時の関節の状態の推定値（一般化座標推定値）
Ｔ：関節モータのトルク The torque disturbance estimator preferably calculates the torque disturbance estimated value by [Equation 1].

However, the meaning of the symbols are as _{follows, d ^, q ^, f} c ^, T is a vector, J is a matrix.
d ^: Estimated value of torque disturbance J: Inertia matrix f _c ^: Estimated value of non-linear force during contact (estimated value of non-linear force during contact)
q ^: Estimated value of joint state at the time of contact (generalized coordinate estimated value)
T: Torque of the joint motor

ただし、記号の意味は以下の通りである。
ｋ＾：被接触体の剛性推定値[Ｎ／ｍ]
ｍ_１：第１関節の質量[ｋｇ]
Ｊ_１０：第１関節の慣性モーメント[ｋｇ・ｍ^２]
θ_１：第１関節の角度[ｒａｄ]
θ_２：第２関節の角度[ｒａｄ]
θ_３：第３関節の角度[ｒａｄ]
ｍ：キー等の質量[ｋｇ]
ｇ：重力加速度[ｍ／ｓ^２]、
ｌ_１：第１リンクの長さ[ｍ]
ｌ_２：第２リンクの長さ[ｍ]
ｌ_３：第３リンクの長さ[ｍ]
ｌ_１０：第１リンクの重心と第１関節との距離[ｍ]
ｌ_２０：第２リンクの重心と第２関節との距離[ｍ]
ｌ_３０：第３リンクの重心と第３関節との距離[ｍ]
Ｔ_１：第１関節の関節モータが発生する反時計回りを正としたトルク（第１関節モータトルク）[Ｎ・ｍ]
Ｔ_ｄ１＾：第１関節に加わる反時計回りを正としたトルク外乱の推定値（第１関節トルク外乱推定値）[Ｎ・ｍ]
ｙ_０：被接触体を支持する弾性体の自然長 The rigidity estimator preferably calculates the rigidity estimated value by [Equation 2].

However, the meaning of the symbols is as follows.
k ^: Estimated stiffness of contacted object [N / m]
m ₁ : mass of the first joint [kg]
J ₁₀ : Moment of inertia of the first joint [kg · m ² ]
θ ₁ : first joint angle [rad]
θ ₂ : second joint angle [rad]
θ ₃ : third joint angle [rad]
m: Mass of keys, etc. [kg]
g: Gravitational acceleration [m / s ² ],
l ₁ : length of the first link [m]
l ₂ : length of the second link [m]
l ₃ : length of the third link [m]
l ₁₀ : distance [m] between the center of gravity of the first link and the first joint
l ₂₀ : distance [m] between the center of gravity of the second link and the second joint
l ₃₀ : distance [m] between the center of gravity of the third link and the third joint
T ₁ : Torque with positive counterclockwise rotation generated by the joint motor of the first joint (first joint motor torque) [N · m]
T _d1 ^: Estimated value of torque disturbance with positive counterclockwise rotation applied to the first joint (first joint torque disturbance estimated value) [N · m]
y ₀ : natural length of the elastic body that supports the contacted body

  The arrival determination unit includes a time average value of a contact joint angle estimation error obtained by subtracting a contact joint angle estimation value that is a component of the contact joint state estimated value from the joint angle, and the non-contact time from the joint angle. When both the time average value of the non-contact joint angle estimation error obtained by subtracting the non-contact joint angle estimation value, which is a component of the joint state estimation value, diverges, the finger has reached the bottom of the contacted object. It is preferable to determine that it is not reached at other times.

ただし、記号の意味は以下の通りである。
ｋ_１：第１関節の制御剛性（第１関節制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ_２：第２関節の制御剛性（第２関節制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ_３：第３関節の制御剛性（第３関節制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ_１ｎ：非接触時の第１関節制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ_２ｎ：非接触時の第２関節制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ_３ｎ：押し込み底未到達時の第３関節制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ_１０：第１関節回りにおいて被接触体の剛性を等価変換した値（第１関節等価制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ_２０：第２関節回りにおいて被接触体の剛性を等価変換した値（第２関節等価制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ_３０：第３関節回りにおいて被接触体の剛性を等価変換した値（第３関節等価制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ＾：キー剛性推定値[Ｎ／ｍ]
ｋ：キー剛性
ｃ_１：第１制御剛性のパラメータ
ｃ_２：第２制御剛性のパラメータ
ｃ_３：第３制御剛性のパラメータ
θ_１：第１関節の角度検出値［ｒａｄ］
θ_２：第２関節の角度検出値［ｒａｄ］
θ_３：第３関節の角度検出値［ｒａｄ］ It is preferable that the control stiffness calculator calculates a control stiffness calculation value of the first joint, the second joint, and the third joint by [Equation 3].

However, the meaning of the symbols is as follows.
k ₁ : Control stiffness of the first joint (first joint control stiffness) [N · m / rad]
k ₂ : Control stiffness of the second joint (second joint control stiffness) [N · m / rad]
k ₃ : control stiffness of the third joint (third joint control stiffness) [N · m / rad]
k _1n : First joint control rigidity at the time of non-contact [N · m / rad]
k _2n : second joint control stiffness at non-contact [N · m / rad]
k _3n : Third joint control rigidity when the indentation bottom is not reached [N · m / rad]
k ₁₀ : Value obtained by equivalently converting the rigidity of the contacted object around the first joint (first joint equivalent control rigidity) [N · m / rad]
k ₂₀ : a value obtained by equivalently converting the rigidity of the contacted object around the second joint (second joint equivalent control rigidity) [N · m / rad]
k ₃₀ : Value obtained by equivalently converting the rigidity of the contacted body around the third joint (third joint equivalent control rigidity) [N · m / rad]
k ^: Key stiffness estimated value [N / m]
k: Key stiffness c ₁ : Parameter of first control stiffness c ₂ : Parameter of second control stiffness c ₃ : Parameter of third control stiffness θ ₁ : Angle detection value of first joint [rad]
θ ₂ : second joint angle detection value [rad]
θ ₃ : third joint angle detection value [rad]

ただし、記号の意味は以下の通りである。
ｋ_１：第１関節の制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ_２：第２関節の制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ_３：第３関節の制御剛性[Ｎ・ｍ／ｒａｄ]
ｄ_１：第１関節における制御減衰[Ｎ・ｍ・ｓ／ｒａｄ]
ｄ_２：第２関節における制御減衰[Ｎ・ｍ・ｓ／ｒａｄ]
ｄ_３：第３関節における制御減衰[Ｎ・ｍ・ｓ／ｒａｄ]
ｌ_１０：第１リンクの重心と第１関節の距離[ｍ]
ｌ_２０：第２リンクの重心と第２関節の距離[ｍ]
ｌ_３０：第３リンクの重心と第３関節の距離[ｍ]
ｌ_１：第１リンクの長さ[ｍ]
ｌ_２：第２リンクの長さ[ｍ]
ｌ_３：第３リンクの長さ[ｍ]
ｍ_１：第１関節の質量[ｋｇ]
θ_１：第１関節の角度[ｒａｄ]
θ_１＾：第１関節の角度推定値[ｒａｄ]
ｍ_２：第２関節の質量[ｋｇ]
θ_２：第２関節の角度[ｒａｄ]
θ_２＾：第２関節の角度推定値[ｒａｄ]
ｍ_３：第３関節の質量[ｋｇ]
θ_３：第３関節の角度[ｒａｄ]
θ_３＾：第３関節の角度推定値[ｒａｄ]
ｍ：キー等の質量[ｋｇ]
ｇ：重力加速度[ｍ／ｓ^２]
Ｔ_ｄ１＾：第１関節に加わる反時計回りを正としたトルク外乱の推定値[Ｎ・ｍ]
Ｔ_ｄ２＾：第２関節に加わる反時計回りを正としたトルク外乱の推定値[Ｎ・ｍ]
Ｔ_ｄ３＾：第３関節に加わる反時計回りを正としたトルク外乱の推定値[Ｎ・ｍ]
θ_１ｒ：第１関節の角度指令[ｒａｄ]
θ_２ｒ：第２関節の角度指令[ｒａｄ]
θ_３ｒ：第３関節の角度指令[ｒａｄ] The joint motor that drives the first joint generates a first joint motor torque T ₁ expressed by [Equation 4], and the joint motor that drives the second joint is expressed by [Equation 5]. the second joint motor torque T ₂ occurs, joint motor for driving the third joint, generating a third joint motor torque T ₃ expressed by Equation 6], is preferred.

However, the meaning of the symbols is as follows.
k ₁ : Control stiffness of the first joint [N · m / rad]
k ₂ : Control stiffness of the second joint [N · m / rad]
k ₃ : Control stiffness of the third joint [N · m / rad]
d ₁ : Control attenuation in the first joint [N · m · s / rad]
d ₂ : Control attenuation in the second joint [N · m · s / rad]
d ₃ : Control attenuation in the third joint [N · m · s / rad]
l ₁₀ : Distance between the center of gravity of the first link and the first joint [m]
l ₂₀ : distance between the center of gravity of the second link and the second joint [m]
l ₃₀ : Distance between the center of gravity of the third link and the third joint [m]
l ₁ : length of the first link [m]
l ₂ : length of the second link [m]
l ₃ : length of the third link [m]
m ₁ : mass of the first joint [kg]
θ ₁ : first joint angle [rad]
θ ₁ ^: Estimated angle of the first joint [rad]
m ₂ : mass of the second joint [kg]
θ ₂ : second joint angle [rad]
θ ₂ ^: second joint angle estimate [rad]
m ₃ : third joint mass [kg]
θ ₃ : third joint angle [rad]
θ ₃ ^: Estimated angle of the third joint [rad]
m: Mass of keys, etc. [kg]
g: Gravity acceleration [m / s ² ]
T _d1 ^: Estimated value of torque disturbance with positive counterclockwise rotation applied to the first joint [N · m]
T _d2 ^: Estimated value of torque disturbance with the counterclockwise rotation applied to the second joint being positive [N · m]
T _d3 ^: Estimated value of torque disturbance with positive counterclockwise rotation applied to the third joint [N · m]
θ _1r : First joint angle command [rad]
θ _2r : Second joint angle command [rad]
θ _3r : Angle command for third joint [rad]

  As described above, according to the present invention, it is possible to provide a control device for a robot hand capable of handling a contacted body having different hardness with a simple configuration.

It is a functional block diagram showing a control device of a robot hand and its peripheral devices according to an embodiment of the present invention. It is a free body figure which shows the finger mechanism in the robot hand which concerns on embodiment of this invention. It is a figure which shows the time change of the angle of the 1st joint in the simulation of this invention. It is a figure which shows the time change of the angle of the 2nd joint in the simulation of this invention. It is a figure which shows the time change of the angle of the 3rd joint in the simulation of this invention. It is a figure which shows the time change of the control rigidity in the simulation of this invention. It is the schematic which shows a robot hand.

  The best mode for carrying out the present invention will be described below with reference to the accompanying drawings. However, the present invention is not limited to the following embodiments. In addition, for clarity of explanation, the following description and drawings are simplified as appropriate.

  A robot hand control device according to an embodiment of the present invention (hereinafter sometimes simply referred to as a control device) is a robot hand that operates, for example, a key of a household appliance or a button of a piano keyboard or a trumpet. Apply well to control. However, the control device is not limited to touching the contacted body with a pressing operation, such as key operation of home appliances, piano keyboard operation, trumpet button operation, etc. Applicable well. Incidentally, the contacted body of the present embodiment includes a key of a household appliance or a keyboard or button of a musical instrument, and an elastic body that returns these to the position before the contact.

First, the configuration of the robot hand controlled by the control device will be described. FIG. 2 is a free body diagram showing the finger mechanism of the robot hand.
The finger mechanism includes a first link 201, a second link 203, and a third link 205. The first link 201 and the second link 203 are operably connected via the first joint 202. The second link 203 and the third link 205 are operably connected via the second joint 204. The third link 205 is operatively connected to the base, for example, via the third joint 206.

Each of the first joint 202, the second joint 204, and the third joint 206 includes a joint motor (not shown), and the first joint angle θ ₁ , the second joint angle θ ₂ , and the third joint angle are driven by rotation of the joint motor. changing the θ ₃ by operating the key 207.

  Here, as shown in FIG. 2, the finger mechanism of this embodiment operates keys 207 which are keys on a keyboard of a personal computer, buttons installed on an operation unit of an electrical appliance, a keyboard of a keyboard instrument, and the like. The key 207 is supported by a spring 208.

  Next, the configuration of the control device that controls the finger mechanism and its peripheral devices will be described. FIG. 1 is a functional block diagram showing a control device and its peripheral devices. As shown in FIG. 1, a joint angle command generator 110 and a control target 120 are provided as peripheral devices of the control device 130.

The joint angle command generator 110 generates the first joint angle θ ₁ and the second joint when the finger mechanism 122 presses the key 207 based on the key contact determination value and the key bottom arrival determination value input from the control device 130. Before the first link 201 touches the key 207, the joint angle command that is the target value of the angle θ ₂ and the third joint angle θ ₃ is pressed while being in contact with the key 207, and the pressed key 207 has reached the key bottom. Generate and output for later. Here, the key contact determination value is a result of determining whether or not the key 207 is in contact with the first link 201. The key bottom arrival determination value is a result of determining whether or not the pressed key 207 has reached the key bottom.

The control target 120 is a general robot hand. The control target 120 includes a finger mechanism 122 that is a finger mechanical mechanism and a joint motor 121 that drives the first joint 202, the second joint 204, and the third joint 206 based on the motor current input from the control device 130. And a joint angle detector 123 that detects the first joint angle θ ₁ , the second joint angle θ ₂ , and the third joint angle θ ₃ and outputs them as joint angle detection values.

  The control device 130 includes a key contact joint state estimator 131, a key non-contact joint state estimator 132, a key contact determiner 133, a torque disturbance estimator 134, a stiffness estimator 135, and a key bottom arrival determination. A calculator 136, a control stiffness calculator 137, and a controller 138.

The key contact joint state estimator 131 detects joint angle detection values (first joint angle θ ₁ , second joint angle θ ₂ , third joint angle θ ₃ ) input from the joint angle detector 123 of the control target 120. Is used to estimate the joint angle, joint speed, and joint acceleration (ie, joint state) based on the equation of motion while the first link 201 is in contact with the key 207 to estimate the joint state at the time of key contact. Output as a value.

  The key non-contact joint state estimator 132 is derived using the joint angle detection value or the like input from the joint angle detector 123 of the control target 120, and exercises while the first link 201 is not in contact with the key 207. Based on the equations, the joint angle, joint speed, and joint acceleration are estimated and output as a joint state estimated value when the key is not in contact.

The key contact determination unit 133 includes a joint angle detection value input from the joint angle detector 123 of the control target 120, a key contact joint state estimation value input from the key contact joint state estimator 131, and a key non-contact. a key non-contact during articulation state estimation value input from the time the joint state estimator 132, based on the generalized coordinates estimated in the first joint angle estimation error theta _{1c ~} ^(at key contact at key contact error q ₁ ^~ = _q 1 and _-q ^{1 ^} first component), the first joint angle estimation error theta _1nc ^~ of the key non-contact time (generalized coordinates estimated at key contactless error ^{_{q 1 ~ = q 1 -q 1}} ^ of First component) is calculated. When the absolute value of the first joint angle estimation error θ _1c ^˜ when the key is touched is smaller than the absolute value of the first joint angle estimation error θ _1nc ^˜ when the key is not touched (abs (θ _1c ^˜ ) <Abs (θ _1nc ^˜ )), it is determined that the first link 201 is in contact with the key 207, otherwise it is determined that it is not in contact, and the determination result is output as a key contact determination result. .

When the key contact determination result input from the key contact determiner 133 is a determination result that the first link 201 is in contact with the key 207, the torque disturbance estimator 134 receives the key contact joint state estimator 131. Based on the input key contact joint state estimated value, a torque disturbance applied to each of the first joint 202, the second joint 204, and the third joint 206 is estimated, and a first joint torque disturbance estimated value T _d1 ^{^} , second The joint torque disturbance estimated value T _d2 ^{^} and the third joint torque disturbance estimated value T _d3 ^{^} are output. On the other hand, the torque disturbance estimator 134 outputs nothing when the key contact determination result input from the key contact determiner 133 is a determination result that the first link 201 is not in contact with the key 207.

The stiffness estimator 135 receives the torque disturbance estimated value (first joint torque disturbance estimated value T _d1 ^{^)} , the second joint input from the torque disturbance estimator 134 only when the first link 201 is in contact with the key 207. Torque disturbance estimated value T _d2 ^{^} , third joint torque disturbance estimated value T _d3 ^{^} ) and the key contact joint state estimated value input from the key contact joint state estimator 131, The rigidity (in this embodiment, the spring constant of the spring 208) is estimated and output as the estimated rigidity value k ^.

  When the key contact determination result input from the contact determination device 133 is a determination result that the first link 201 is in contact with the key 207, the key bottom arrival determination device 136 determines that the key contact arrival state determination device 136 When the absolute value of the joint state estimated value when the key is input diverges, it is determined that the key 207 has reached the key bottom, otherwise it is determined that the key bottom has not been reached, and the determination result is used as the key. Output as bottom arrival determination result.

  The control stiffness calculator 137 includes a joint angle detection value input from the joint angle detector 123, a key contact determination result input from the key contact determination device 133, and a stiffness estimation value k ^ input from the stiffness estimator 135. And whether or not the first link 201 is in contact with the key 207 based on the key bottom arrival determination result input from the key bottom arrival determiner 136, and if it is in contact with the key 207, The control stiffness of each of the first joint 202, the second joint 204, and the third joint 206 according to the judgment result is determined based on the stiffness estimation value k ^ and the joint angle detection value. And output as a control stiffness calculation value. Here, the control rigidity is a rigidity obtained by adding the rigidity of a joint mechanical mechanism and the rigidity of a joint motor, a speed reducer, and the like included in the joint.

The controller 138 includes target values for the first joint angle θ ₁ , the second joint angle θ ₂ , and the third joint angle θ ₃ , which are joint angle commands input from the joint angle / posture generator 110, and a control stiffness calculator. In order to realize the control stiffness calculation value input from 137, control gains for controlling the first joint angle θ ₁ , the second joint angle θ ₂ , and the third joint angle θ ₃ are set, and the control gain is set as the control gain. The motor current calculated based on this is output.

  The control device 130 is realized as an electric / electronic / programmable electronic control system. For example, the control device 130 includes a CPU (Central Processing Unit) that performs arithmetic processing, a ROM (Read Only Memory) that stores arithmetic programs executed by the CPU, and a RAM (Random) that temporarily stores processing data and the like. The hardware configuration is centered on a microcomputer composed of (Access Memory). Further, each unit of the control device 130 may be realized by hardware such as an ASIC (Application Specific Integrated Circuit).

Next, a detailed operation principle of each functional block constituting the control device 130 will be derived and described.
First, the equation of motion of the contacted body and the finger mechanism 122 is derived using FIG. From FIG. 2, the equation of motion of the contacted body can be derived as [Equation 7].

However, the meaning of the symbols is as follows.
y ₀ : Natural length [m] of the spring 208 with the upper direction in FIG. 2 being positive
y ₁ : Vertical position (fingertip vertical position) [m] of the tip (fingertip) of the first link 201 with the third joint 206 as a reference and the upward direction in FIG.
f: Keystroke force [N] with the upward direction in FIG. 2 being positive
m: Key mass [kg]
g: Gravity acceleration [m / s ² ]
k: rigidity of the spring 208 [N / m]

  Next, the equation of motion of the finger mechanism 122 is derived based on a method of analytical mechanics. The kinetic energy K and potential energy U of the finger mechanism 122 can be derived as [Equation 8] and [Equation 9].

However, the meaning of the symbols is as follows.
m ₁ : mass of the first joint 202 (first joint mass) [kg]
m ₂ : mass of the second joint 204 (second joint mass) [kg]
m ₃ : mass of the third joint 206 (third joint mass) [kg]
θ ₁ : angle of the first joint 202 (first joint angle) [rad]
θ ₂ : angle of the second joint 204 (second joint angle) [rad]
θ ₃ : angle of the third joint 206 (third joint angle) [rad]
x ₁₀ : The horizontal position of the center of gravity of the first link 201 (first link center of gravity horizontal position) with the third joint 206 as a reference and the left direction in FIG. 2 being positive (m)
y ₁₀ : Vertical position of the center of gravity of the first link 201 (first link center of gravity vertical position) with the third joint 206 as a reference and the upward direction in FIG. 2 being positive [m]
x ₂₀ : Horizontal position of the center of gravity of the second link 203 (second link center of gravity horizontal position) with the third joint 206 as a reference and the left direction in FIG. 2 being positive (m)
y ₂₀ : The vertical position of the center of gravity of the second link 203 (second link center of gravity vertical position) with the third joint 206 as a reference and the upward direction in FIG. 2 being positive [m]
x ₃₀ : horizontal position of the center of gravity of the third link 205 (third link center of gravity horizontal position) with the third joint 206 as a reference and the left direction in FIG. 2 being positive (m)
y ₃₀ : Vertical position of the center of gravity of the third link 205 (third link center of gravity vertical position) with the upper direction in FIG. 2 as positive with the third joint 206 as a reference [m]
J ₁₀ : moment of inertia of the first joint 202 [kg · m ² ]
J ₂₀ : moment of inertia of second joint 204 [kg · m ² ]
J ₃₀ : moment of inertia of the third joint 206 [kg · m ² ]

The first link centroid horizontal position _{x 10,} the first link centroid vertical position _{y 10,} the second link centroid horizontal position _{x 20,} the second link centroid vertical position _{y 20,} the third link centroid horizontal position _{x 30,} third link centroid vertical When the position y ₃₀ is rewritten using the first joint angle θ ₁ , the second joint angle θ ₂ , and the third joint angle θ ₃ , they are expressed as [Equation 10] to [Equation 15].

However, the meaning of the symbols is as follows.
l ₁₀ : distance [m] between the center of gravity of the first link 201 and the first joint 202
l ₂₀ : distance [m] between the center of gravity of the second link 203 and the second joint 204
l ₃₀ : distance [m] between the center of gravity of the third link 205 and the third joint 206
l ₂ : length of the second link 203 [m]
l ₃ : length of the third link 205 [m]

Similarly, when the fingertip vertical position y ₁ of the first link 201 is rewritten using the first joint angle θ ₁ , the second joint angle θ ₂ , and the third joint angle θ ₃ , [Expression 16] is expressed.

ただし、ｌ_１は第１リンク２０１の長さ[ｍ]である。

Here, l ₁ is the length [m] of the first link 201.

  The equation of motion of the finger mechanism 122 is expressed by the Euler-Lagrange equation of [Equation 17] to [Equation 19].

Here, Q ₁ , Q ₂ , and Q ₃ are generalization forces, and are expressed by [Equation 20] to [Equation 22] when the fingertip is in contact with the key 207.

Here, the meanings of the symbols are as follows.
T ₁ : Torque with positive counterclockwise rotation generated by the joint motor 121 of the first joint 202 (first joint motor torque) [N · m]
T ₂ : Torque with the counterclockwise direction generated by the joint motor 121 of the second joint 204 as positive (second joint motor torque) [N · m]
T ₃ : Torque with the counterclockwise direction generated by the joint motor 121 of the third joint 206 as positive (third joint motor torque) [N · m]
T _d1 : Torque disturbance with positive counterclockwise rotation applied to the first joint 202 (first joint torque disturbance) [N · m]
T _d2 : Torque disturbance with positive counterclockwise rotation applied to the second joint 204 (second joint torque disturbance) [N · m]
T _d3 : Torque disturbance with the counterclockwise rotation applied to the third joint 206 as positive (third joint torque disturbance) [N · m]

  Substituting [Equation 8] to [Equation 16] and [Equation 20] to [Equation 22] into [Equation 17] to [Equation 19], the equation of motion of the finger mechanism 122 becomes [Equation 23] to [Equation 25]. Can be derived.

On the other hand, if the finger is not in contact with the key 207, the generalized force _{Q 1} is in place in the number 20, is represented as [Expression 26].

  When the fingertip is not in contact with the key 207, [Equation 23] is replaced with [Equation 27] by [Equation 26].

  The equation of motion of the finger mechanism 122 when the key is touched can be rewritten as [Equation 28] from [Equation 23] to [Equation 25].

However, the meaning of the symbols is as follows.
J: inertia matrix f _c : vector composed of nonlinear terms from [Equation 23] to [Equation 25] (nonlinear force at the time of key contact)
q: Generalized coordinates T: Joint motor torque d: Torque disturbance

  Similarly, the equation of motion of the finger mechanism 122 when the key is not touched is rewritten as [Equation 29] from [Equation 24], [Equation 25], and [Equation 27].

However, f _nc is a vector (non-key non-contact non-linear force) composed of non-linear terms of [ _Equation 24], [ _Equation 25], and [Equation 27].

  The key contact joint state estimator 131 estimates the generalized coordinate q and the first and second-order time differential values (joint state at key contact) at the time of key contact so as not to include noise according to [Equation 30]. To do.

However, the meaning of the symbols is as follows.
L: estimator gain q ^: generalized coordinates estimate f c _^: key contact time nonlinear force estimate

  Similarly, the joint state estimator 132 at the time of non-contact with the key calculates the generalized coordinates q at the time of non-contact with the key and the first and second-order time differential values (joint state at the time of non-contact of the key) as noise. It is estimated not to include.

Here, f _nc ^ is a non-contact non-linear force estimation value.

  The estimation error equation of the key contact joint state estimator 131 at the time of key contact is derived as [Equation 32] by subtracting [Equation 30] from [Equation 28].

Where q ^~ is a generalized coordinate estimation error.
Here, if each element of the estimator gain L is set to be a sufficiently large negative real number, [Expression 33] is established, and the joint state at the time of key contact is estimated.

  On the other hand, the estimation error equation of the joint non-key joint state estimator 132 when the key is touched is derived as [Equation 34] by subtracting [Equation 31] from [Equation 28].

When the key is touched, the time average value of the nonlinear term f _c −f _c ^ in [Equation 32] is always smaller than the time average value of the nonlinear term f _c −f _nc ^ in [Equation 34]. towards the generalized coordinates estimation error q time average value of ^- calculated from the estimation error equation of key contact during articulation state estimator 131 [number 32] is the estimation error equation number 34 key noncontact during articulation state estimator 132 ] Smaller than the value calculated from the above.

  Similarly, the estimation error equation of the joint non-key joint state estimator 132 when the key is not touched is derived as [Equation 35] by subtracting [Equation 31] from [Equation 29].

  Here, if each element of the estimator gain L is set to be a sufficiently large negative real number, [Equation 33] is established, and the joint state at the time of non-key contact is estimated.

For the same reason as at the time of key contact, the key in the non-contact time, the direction of the key non-contact time estimated generalized coordinates estimated and calculated from the error equation error q time average ^value between the joint state estimator 132, the key It is smaller than the value calculated from the estimation error equation of the joint state estimator 131 at the time of contact.

Key contact determiner 133 on the basis of the principles described above, among the key contact during articulation state estimator 131 and the key non-contact during articulation state estimator 132, the time average latter generalized coordinates estimation error q ^~ the former output If it is smaller than that, it is determined that the key is touching, and otherwise, it is determined that the key is not touching.

  When the key contact determiner 133 determines that the key is being touched, the generalized coordinate estimated value q ^ converges to the generalized coordinate q. Therefore, the torque disturbance estimator 134 uses [Equation 36] from [Equation 28]. It can be seen that the estimated torque disturbance value can be calculated.

ただし、ｄ＾はトルク外乱推定値である。

However, d ^ is a torque disturbance estimated value.

  Using [Equation 23] and the estimated torque disturbance value d ^, the stiffness estimator 135 calculates the estimated stiffness value k ^ by [Equation 37].

However, the meaning of the symbols is as follows.
T _d1 ^: Estimated value of torque disturbance in which counterclockwise rotation applied to the first joint 202 is positive (first joint torque disturbance estimated value) [N · m]
θ1 ^: first joint angle estimated value [rad]
θ2 ^: second joint angle estimated value [rad]
θ3 ^: third joint angle estimated value [rad]

  The key bottom arrival determiner 136 calculates the time average of the key contact joint state estimation error output by the key contact joint state estimator 131 while the key contact determiner 133 determines that the key contact is in progress, and It is determined that the key bottom has been reached when both of the time averages of the joint non-contact joint state estimation errors output by the joint non-key joint state estimator 132 diverge.

In FIG. 2, when the reaction force from the key 207 and the motor torque generated by the first joint 202, the second joint 204, and the third joint 206 are statically balanced, the control stiffness at each joint is equivalent to the first joint equivalent. Assuming that the control stiffness k ₁₀ , the second joint equivalent control stiffness k ₂₀ , and the third joint equivalent control stiffness k ₃₀ , [Equation 38] to [Equation 40] are established.

However, the meaning of the symbols is as follows.
δ _y : small amount of vertical displacement of key 207 [m]
δ _θ1 : Minute displacement [rad] of the first joint angle θ1
_δθ2 : Minute displacement [rad] of the second joint angle θ2.
_δθ3 : Minute displacement [rad] of the third joint angle θ3

In the [number 38] [Expression 40], [delta] _y and _δ θ1, δ θ2, δ θ3 so to offset a small amount is obtained [Formula 43] From Equation 41].

The control stiffness calculator 137 includes a first joint control stiffness k ₁ that is the control stiffness of the first joint 202, a second joint control stiffness k ₂ that is the control stiffness of the second joint 204, and the control stiffness of the third joint 206. And the third joint control stiffness k ₃ are determined based on [Equation 44].

However, the meaning of the symbols is as follows.
c ₁ : Parameters of the first joint control rigidity k 1 c ₂ : Parameters of the second joint control rigidity k 2 c ₃ : Parameters of the third joint control rigidity k 3 k _1n : First joint control rigidity k _{2n at} non-contact k _2n : Non-contact Second joint control stiffness at time k _3n : Third joint control stiffness when key bottom is not reached

The control stiffness calculator 137 sets the control gain of the controller 138 so that the control stiffness determined by [Equation 44] is obtained. Thus, until the finger mechanism 122 presses the key 207, the control stiffness for each of the first joint 202, the second joint 204, and the third joint 206 is equal to the stiffness of the contacted body (the spring constant of the spring 208) k. 202, the second joint 204, and the third joint 206 are larger than the value equivalent to the rotational rigidity around the first joint 201, and the tip (fingertip) of the first link 201, the first joint angle θ ₁ , the second joint angle θ ₂ , The three joint angles θ ₃ can be moved to a position suitable for pressing the key 207 quickly and accurately.

  After the key contact, the control stiffness for each of the first joint 202 and the second joint 204 is equal to the value obtained by equivalently converting the stiffness k of the contacted body into the rotational stiffness around the first joint 202 and the second joint 204, Since the control stiffness for the three joints 206 is larger than the value obtained by equivalently converting the stiffness k of the contacted object to the rotational stiffness around the third joint 206, the minimum necessary to prevent the key 207 from being damaged mainly by the operation of the third joint 206 only. The key 207 can be pressed with a limited force.

  When the key bottom is further reached, the control stiffness for each of the first joint 202, the second joint 204, and the third joint 206 is rotated around the first joint 202, the second joint 204, and the third joint 206. The first joint 202, the second joint 204, and the third joint 206 can be relaxed while leaving only the minimum force necessary to hold the key 207 on the key bottom, which is equal to the value equivalently converted to rigidity.

At the time of key contact, the first joint motor torque T ₁ , the second joint motor torque T ₂ , and the third joint motor torque T ₃ , where the control stiffness at each joint is [Equation 44], are calculated from [Equation 23] to [Equation 25]. ] Can be derived from [Equation 45] to [Equation 47].

However, the meaning of the symbols is as follows.
d ₁ : Control attenuation in the first joint 202 (first joint control attenuation)
d ₂ : Control attenuation in the second joint 204 (second joint control attenuation)
d ₃ : Control attenuation in the third joint 206 (third joint control attenuation)
T _d2 ^: Estimated value of torque disturbance with positive counterclockwise rotation applied to the second joint (second joint torque disturbance estimated value) [N · m]
T _d3 ^: Estimated value of torque disturbance with positive counterclockwise rotation applied to the third joint (third joint torque disturbance estimated value) [N · m]
Here, the control attenuation is attenuation obtained by adding attenuation by a mechanical mechanism of the joint and attenuation of a joint motor, a reduction gear, and the like included in the joint.

On the other hand, when the key is not touched, the second joint motor torque T ₂ and the third joint motor torque T ₃ are the same as [Equation 46] and [Equation 47], and the first joint motor torque T ₁ is [Equation 27]. [Expression 48].

  As described above, since the rigidity of the contacted body is estimated, it is possible to flexibly cope with the contacted bodies having different hardnesses with a simple mechanism. In particular, when the control device of this embodiment is applied to a robot hand that performs key operations, etc., when a service robot operates a keyboard of a personal computer or performs a key operation of home appliances, the key can be quickly and accurately not damaged. When the service robot plays a musical instrument, it is possible to perform a sophisticated performance accurately and quickly without damaging the keyboard or buttons.

  As described above, according to the present embodiment, the service robot can quickly and accurately perform key operations and performances without damaging the keys of household appliances, the keyboards and buttons of musical instruments.

<Simulation results>
This simulation simulates the case where the robot hand of the service robot operates the keys of the personal computer. The numerical values used for the simulation are as follows.
l ₁ = 20 × 10 ⁻³ [m]
l ₂ = 20 × 10 ⁻³ [m]
l ₃ = 20 × 10 ⁻³ [m]
l ₁₀ = 10 × 10 ⁻³ [m]
l ₂₀ = 10 × 10 ⁻³ [m]
l ₃₀ = 10 × 10 ⁻³ [m]
m ₁ = 30 × 10 ⁻³ [kg]
m ₂ = 30 × 10 ⁻³ [kg]
m ₃ = 30 × 10 ⁻³ [kg]
J ₁₀ = 1.42 × 10 ⁻⁶ [kg · m ² ]
J ₂₀ = 1.42 × 10 ⁻⁶ [kg · m ² ]
J ₃₀ = 1.42 × 10 ⁻⁶ [kg · m ² ]
m = 1.9 × 10 ⁻³ [kg]
k = 140 [N / m]
y ₀ = 3 × 10 ⁻³ [m]
g = 9.8 [m / s ² ]
T _d1 = 0 [N · m]
T _d2 = 0 [N · m]
T _d3 = 0 [N · m]
c ₁ = 3
c ₂ = 3
c ₃ = 3
d ₁ = 2 × 10 ⁻¹ [N · m · s / rad]
d ₂ = 6 × 10 ⁻¹ [N · m · s / rad]
d ₃ = 9 × 10 ⁻¹ [N · m · s / rad]
k _1n = 1 × 10 ⁵ [N · m / rad]
k _2n = 1 × 10 ⁵ [N · m / rad]
k _3n = 1 × 10 ⁵ [N · m / rad]

However, the key mass m and the stiffness k of the spring (elastic body) are values of a standard personal computer keyboard, and are values of the first joint control attenuation d ₁ , the second joint control attenuation d ₂ , and the third joint control attenuation d ₃ . Was set according to the magnitude of the moment of inertia at the tip of each joint.

  3 is a time change of the first joint angle in this simulation, FIG. 4 is a time change of the second joint angle in this simulation, FIG. 5 is a time change of the third joint angle in this simulation, and FIG. 6 is a control stiffness in this simulation. Is a time change.

3 to 5, the broken line indicates a joint angle command, and the solid line indicates an actual joint angle. In FIG. 6, the solid line indicates the first joint control rigidity k ₁ , the broken line indicates the second joint control rigidity k ₂ , and the alternate long and short dash line indicates the third joint control rigidity k ₃ .

In FIG. 3 and FIG. 4 2.5 [s], the tip (fingertip) of the first joint 202 is in contact with the key 207, and the determination result that the key contact determination unit 133 is “key contact” is the joint angle command generator 110. The joint angle command generator 110 decelerates the first joint angle command θ _1r and the second joint angle command θ _2r . The first joint angle command θ _1r and the second joint angle command θ _2r are converged to an angle larger than the first joint angle θ ₁ and the second joint angle θ _{2 at the} time of key contact. This is to keep the key 207 in close contact with the surface. In FIG. 6, the first joint control stiffness k ₁ and the second joint control stiffness k ₂ calculated by the control stiffness calculator 137 using [Equation 38] are necessary for following the joint angle command before the key contact. It can be seen that when the key contact is determined at 2.5 [s] and set to a value sufficiently high to secure the control band, the control rigidity is suppressed to the minimum.

When the key contact is determined at 2.5 [s] in FIG. 5, the third joint angle command is decelerated once, and the operation shifts to the operation of pressing the key 207. When the key bottom is reached at 5.5 [s], the key 207 cannot be pushed any further, so the third joint angle θ ₃ is saturated at 0.31 [rad]. At this time, the key bottom arrival determination unit 136 outputs the determination result that the key bottom has been reached because the absolute value of the joint state estimated value at the time of key contact diverges as described above. The joint angle command generator 110 decelerates the third joint angle command θ _3r when the determination result “having reached the key bottom” is input. The reason why the third joint angle command θ _3r converges to a value larger than the third joint angle θ ₃ when the key bottom is reached is to hold the key 207 firmly on the key bottom. The control stiffness calculator 137 sets the third joint control stiffness k ₃ in order to ensure a control band necessary for causing the third joint angle θ ₃ to follow the third joint angle command θ _3r before reaching the key bottom. The third joint control stiffness k ₃ is set to a minimum value necessary to hold the key 207 on the key bottom after the key bottom is reached at 5.5 [s] after being set to a sufficiently large value. I understand that.

  According to the robot hand disclosed in Patent Document 1, a fast keystroke operation cannot be performed when a finger with low rigidity is used, and a fast keystroke operation can be performed when a finger with high rigidity is used. In addition, there is a problem that the key 207 can be damaged because it cannot be changed appropriately. On the other hand, according to the present embodiment, as described above, the control rigidity of each joint is appropriately changed according to the key rigidity in the three cases before the key contact, when the key is pressed, and after the key bottom is reached. Fast and accurate typing without hurting

  In addition, when the present invention is applied to the performance of a keyboard instrument such as a piano, it is possible to perform an accurate and fast performance without damaging the keyboard by appropriately changing the control rigidity of each joint according to the key rigidity as described above. It becomes possible to express delicate tonality.

  As described above, according to the present embodiment, the service robot can quickly and accurately perform key operations and performances without damaging the keys of household appliances, the keyboards and buttons of musical instruments.

The embodiment of the control device for the robot hand according to the present invention has been described above. However, the present invention is not limited to the above configuration, and can be changed without departing from the technical idea of the present invention.
The control device of the present embodiment is applied to control a robot hand that operates keys such as home appliances and musical instruments, but may be applied to control a robot hand that grips a gripping target. In this case, the key bottom arrival determination device 136 may be omitted.
Although the finger mechanism of this embodiment is configured to include the first link 201, the second link 203, and the third link 205, the number of links and joints is not limited.
Although the spring is used as the elastic body of the contacted body of the present embodiment, a restoring member such as rubber may be used.

DESCRIPTION OF SYMBOLS 110 Joint angle command generator 120 Control object 121 Joint motor 122 Finger mechanism 123 Joint angle detector 130 Controller 131 Joint state estimator at the time of key contact 132 Joint state estimator at the time of non-contact 133 Key contact determination unit 134 Torque disturbance estimator 135 rigidity estimator 136 key bottom arrival determination unit 137 control rigidity calculator 138 controller 201 first link 202 first joint 203 second link 204 second joint 205 third link 206 third joint 207 key 208 spring 701 first finger First link 702 First finger MP joint 703 First finger second link 704 First finger IP joint 705 Second finger first link 706 Second finger MP joint 707 Second finger second link 708 Second finger PIP joint 709 First Second finger third link 710 Second finger DIP joint 711 Third finger first link 712 Third finger MP joint 71 3 3rd finger 2nd link 714 3rd finger PIP joint 715 3rd finger 3rd link 716 3rd finger DIP joint 717 4th finger 1st link 718 4th finger MP joint 719 4th finger 2nd link 720 4th finger PIP joint 721 4th finger 3rd link 722 4th finger DIP joint 723 Base

Claims (8)

In a control device for a robot hand provided with a finger having a link and a joint of the link,
Based on a joint angle detection value that is a detected angle value of the joint, a joint state estimator at the time of contact that estimates a joint angle, a joint speed, and a joint acceleration when the finger contacts the contacted body. When,
A non-contact joint state estimator that estimates the state of the joint when the finger is not in contact with the contacted body;
Based on the joint angle detection value, the contact joint state estimation value output by the contact joint state estimator, and the non-contact joint state estimation value output by the non-contact joint state estimator, A contact determination device for determining whether or not a finger has contacted the contacted object;
A torque disturbance estimator for estimating a torque disturbance applied to the joint based on the joint state estimation value at the time of contact when the determination result output by the contact determination unit is a contact state;
When the contact determination result is a contact state, a stiffness estimator that estimates the stiffness of the contacted body based on the contact joint state estimated value and the torque disturbance estimated value output by the torque disturbance estimator; ,
When the contact determination result is a contact state, based on the joint angle detection value and the rigidity estimation value, the joint control rigidity is set to a minimum value at which the finger can maintain the pressed state of the contacted body. Operate,
When the contact determination result is a non-contact state, the control rigidity of the joint is calculated to be larger than the joint control rigidity when the finger is in contact with the contacted body. An arithmetic unit;
A controller that controls a joint motor that drives the joint based on a control stiffness calculation value output by the control stiffness calculator;
A robot hand control device comprising: The fingers of the robot hand include a first joint, a second joint, and a third joint,
The contacted body includes a key of a household appliance or a keyboard or button of a musical instrument and an elastic body,
The control device further includes an arrival determination device that determines whether or not the contacted body has reached the pushing-in bottom based on the contact state estimated value when the contact determination result is a contact state,
When the contact determination result is a non-contact state, the control stiffness calculator is larger than a value obtained by equivalently converting the stiffness estimation value into rotational stiffness around the first joint and the second joint, and the first stiffness of the finger Calculate the control stiffness of the joint and the second joint,
When the contact determination result is a contact state, the control rigidity of the first joint and the second joint is set to a value substantially equal to a value obtained by equivalently converting the rigidity estimated value to the rotational rigidity around the first joint and the second joint. And when the arrival determination result output by the arrival determination device is in a state where it does not reach the push-in bottom, the rigidity estimated value is set to a value greater than the value obtained by equivalent conversion to the rotational rigidity around the third joint, When the control rigidity of the third joint is calculated, and the arrival determination result reaches the push-in bottom, the rigidity estimation value is equal to a value equivalent to a value obtained by equivalently converting the rotation rigidity around the third joint. The robot hand control device according to claim 1, wherein the control stiffness of the three joints is calculated.   The contact determination unit is configured such that a time average value of an absolute value of a contact joint angle estimation error obtained by subtracting a contact joint angle estimation value which is a component of the contact joint state estimated value from the joint angle is calculated from the joint angle. If the absolute value of the non-contact joint angle estimation error, which is the component of the non-contact joint state estimated value subtracted from the non-contact joint angle estimated value, is smaller than the time average value, it is determined that the contact state is present. The robot hand control device according to claim 1, wherein the robot hand is determined to be in a non-contact state. 4. The robot control apparatus according to claim 1, wherein the torque disturbance estimator calculates the torque disturbance estimated value by [Equation 49]. 5.

However, the meaning of the symbols are as _{follows, d ^, q ^, f} c ^, T is a vector, J is a matrix.
d ^: Estimated value of torque disturbance J: Inertia matrix f _c ^: Estimated value of non-linear force during contact (estimated value of non-linear force during contact)
q ^: Estimated value of joint state at the time of contact (generalized coordinate estimated value)
T: Torque of the joint motor 5. The robot hand control device according to claim 2, wherein the rigidity estimator calculates the rigidity estimated value by [Equation 50]. 6.

However, the meaning of the symbols is as follows.
k ^: Estimated stiffness of contacted object [N / m]
m ₁ : mass of the first joint [kg]
J ₁₀ : Moment of inertia of the first joint [kg · m ² ]
θ ₁ : first joint angle [rad]
θ ₂ : second joint angle [rad]
θ ₃ : third joint angle [rad]
m: Mass of keys, etc. [kg]
g: Gravitational acceleration [m / s ² ],
l ₁ : length of the first link [m]
l ₂ : length of the second link [m]
l ₃ : length of the third link [m]
l ₁₀ : distance [m] between the center of gravity of the first link and the first joint
l ₂₀ : distance [m] between the center of gravity of the second link and the second joint
l ₃₀ : distance [m] between the center of gravity of the third link and the third joint
T ₁ : Torque with positive counterclockwise rotation generated by the joint motor of the first joint (first joint motor torque) [N · m]
T _d1 ^: Estimated value of torque disturbance with positive counterclockwise rotation applied to the first joint (first joint torque disturbance estimated value) [N · m]
y ₀ : natural length of the elastic body that supports the contacted body   The arrival determination unit includes a time average value of a contact joint angle estimation error obtained by subtracting a contact joint angle estimation value that is a component of the contact joint state estimated value from the joint angle, and the non-contact time from the joint angle. When both the time average value of the non-contact joint angle estimation error obtained by subtracting the non-contact joint angle estimation value, which is a component of the joint state estimation value, diverges, the finger has reached the bottom of the contacted object. The robot hand control device according to any one of claims 2 to 5, wherein it is determined that it has not been reached otherwise. 7. The robot hand control device according to claim 2, wherein the control stiffness computing unit computes control stiffness computation values of the first joint, the second joint, and the third joint according to [Equation 51]. 7. .

However, the meaning of the symbols is as follows.
k ₁ : Control stiffness of the first joint (first joint control stiffness) [N · m / rad]
k ₂ : Control stiffness of the second joint (second joint control stiffness) [N · m / rad]
k ₃ : control stiffness of the third joint (third joint control stiffness) [N · m / rad]
k _1n : First joint control rigidity at the time of non-contact [N · m / rad]
k _2n : second joint control stiffness at non-contact [N · m / rad]
k _3n : Third joint control rigidity when the indentation bottom is not reached [N · m / rad]
k ₁₀ : Value obtained by equivalently converting the rigidity of the contacted object around the first joint (first joint equivalent control rigidity) [N · m / rad]
k ₂₀ : a value obtained by equivalently converting the rigidity of the contacted object around the second joint (second joint equivalent control rigidity) [N · m / rad]
k ₃₀ : Value obtained by equivalently converting the rigidity of the contacted body around the third joint (third joint equivalent control rigidity) [N · m / rad]
k ^: Key stiffness estimated value [N / m]
k: Key stiffness c ₁ : Parameter of first control stiffness c ₂ : Parameter of second control stiffness c ₃ : Parameter of third control stiffness θ ₁ : Angle detection value of first joint [rad]
θ ₂ : second joint angle detection value [rad]
θ ₃ : third joint angle detection value [rad] The joint motor that drives the first joint generates a first joint motor torque T ₁ represented by [Equation 52],
The joint motor that drives the second joint generates a second joint motor torque T ₂ represented by [Equation 53],
The third joint motor for driving the joint, the control device of the robot hand according to any one of claims 2 to 7 for generating a third joint motor torque T ₃ expressed by Equation 54].

However, the meaning of the symbols is as follows.
k ₁ : Control stiffness of the first joint [N · m / rad]
k ₂ : Control stiffness of the second joint [N · m / rad]
k ₃ : Control stiffness of the third joint [N · m / rad]
d ₁ : Control attenuation in the first joint [N · m · s / rad]
d ₂ : Control attenuation in the second joint [N · m · s / rad]
d ₃ : Control attenuation in the third joint [N · m · s / rad]
l ₁₀ : Distance between the center of gravity of the first link and the first joint [m]
l ₂₀ : distance between the center of gravity of the second link and the second joint [m]
l ₃₀ : Distance between the center of gravity of the third link and the third joint [m]
l ₁ : length of the first link [m]
l ₂ : length of the second link [m]
l ₃ : length of the third link [m]
m ₁ : mass of the first joint [kg]
θ ₁ : first joint angle [rad]
θ ₁ ^: Estimated angle of the first joint [rad]
m ₂ : mass of the second joint [kg]
θ ₂ : second joint angle [rad]
θ ₂ ^: second joint angle estimate [rad]
m ₃ : third joint mass [kg]
θ ₃ : third joint angle [rad]
θ ₃ ^: Estimated angle of the third joint [rad]
m: Mass of keys, etc. [kg]
g: Gravity acceleration [m / s ² ]
T _d1 ^: Estimated value of torque disturbance with positive counterclockwise rotation applied to the first joint [N · m]
T _d2 ^: Estimated value of torque disturbance with the counterclockwise rotation applied to the second joint being positive [N · m]
T _d3 ^: Estimated value of torque disturbance with positive counterclockwise rotation applied to the third joint [N · m]
θ _1r : First joint angle command [rad]
θ _2r : Second joint angle command [rad]
θ _3r : Angle command for third joint [rad]

Images