ロボットハンドの制御装置
【課題】簡易な構成で硬さの異なる被接触体に対応することができるロボットハンドの制御装置を提供する。
【解決手段】本発明の一形態に係るロボットハンドの制御装置は、指が被接触体に接触した時における関節の状態を推定する接触時関節状態推定器１３１と、指が被接触体に接触していない時における関節の状態を推定する非接触時関節状態推定器１３２と、指が被接触体に接触したか否かを判定する接触判定器１３３と、関節に印加されるトルク外乱を推定するトルク外乱推定器１３４と、被接触体の剛性を推定する剛性推定器１３５と、接触判定結果が接触状態の場合、関節の制御剛性を、指が被接触体の押し込み状態を維持できる最小値に演算する制御剛性演算器１３７と、関節モータ１２１を制御する制御器１３８と、を備える。
【解決手段】本発明の一形態に係るロボットハンドの制御装置は、指が被接触体に接触した時における関節の状態を推定する接触時関節状態推定器１３１と、指が被接触体に接触していない時における関節の状態を推定する非接触時関節状態推定器１３２と、指が被接触体に接触したか否かを判定する接触判定器１３３と、関節に印加されるトルク外乱を推定するトルク外乱推定器１３４と、被接触体の剛性を推定する剛性推定器１３５と、接触判定結果が接触状態の場合、関節の制御剛性を、指が被接触体の押し込み状態を維持できる最小値に演算する制御剛性演算器１３７と、関節モータ１２１を制御する制御器１３８と、を備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットハンドの制御装置に関する。
【背景技術】
【０００２】
一般的なロボットハンドは、複数の指と、当該指が根元部分において動作可能（回転可能）に連結されたベースと、を備える。当該指は、複数のリンクと、隣り合う２つのリンクを動作可能に連結する関節と、関節を駆動する関節モータと、を備える。このような構成により、ロボットハンドは人間の手の機能の一部を模倣可能な構成とされている。
【０００３】
図７はロボットハンドの概略図を示す。図７に示すロボットハンドは、第１指〜第４指がベース７２３に動作可能に連結されている。
詳細には、第１指は、第１指第１リンク７０１と、第１指ＭＰ（metacarpophalangeal）関節７０２と、第１指第２リンク７０３と、第１指ＩＰ（interphalangeal）関節７０４と、を備える。
【０００４】
第２指は、第２指第１リンク７０５と、第２指ＭＰ関節７０６と、第２指第２リンク７０７と、第２指ＰＩＰ（proximal interphalangeal）関節７０８と、第２指第３リンク７０９と、第２指ＤＩＰ（distal interphalangeal）関節７１０と、を備える。
【０００５】
第３指は、第３指第１リンク７１１と、第３指ＭＰ関節７１２と、第３指第２リンク７１３と、第３指ＰＩＰ関節７１４と、第３指第３リンク７１５と、第３指ＤＩＰ関節７１６と、を備える。
【０００６】
第４指は、第４指第１リンク７１７と、第４指ＭＰ関節７１８と、第４指第２リンク７１９と、第４指ＰＩＰ関節７２０と、第４指第３リンク７２１と、第４指ＤＩＰ関節７２２と、を備える。
【０００７】
これらの指の関節はトーションばねを備える。隣り合った２つの関節は連動リンク部材（不図示）により連動して動作するようになっている。連動リンク部材を、関節モータ（不図示）で回転させることにより、各指の曲げ伸ばしを実施する。このとき、指の曲げ伸ばし動作中に指が被接触体に接触し反力を受けると、上述のトーションばねがその反力を吸収する。
【０００８】
このようなロボットアームの構成を利用し、特許文献１のロボットハンドは、複数の指それぞれの関節の剛性を変えることで、柔らかい被接触体を扱う際には関節の剛性の低い指を用い、硬い被接触体を扱う際には関節の剛性の高い指を用いる構成とされている。
【０００９】
また、特許文献２にも、硬さの異なる被接触体を良好に把持することができるように構成されたロボットハンドが開示されている。特許文献２のロボットハンドは、指の先端に設けられた探触子の検出情報に基づいて、被接触体への接触検出、及び滑り検出を行い、被接触体が重力方向の相対的移動を生じないように、被接触体を最小限度の把持力で把持する構成とされている。
【００１０】
ちなみに、特許文献３、４には、被接触体を良好に把持できるように構成されたロボットハンドが開示されている。特許文献３のロボットハンドは、指リンクを剛性が低い被覆部材で被覆し、更にその上から摩擦係数が高い被覆部材で被覆した構成とされている。特許文献４のロボットハンドは、力検出手段により検出されたロボットハンドに加わる外力が増加した時に把持力を増加させる構成とされている。
【先行技術文献】
【特許文献】
【００１１】
【特許文献１】特開２００９−１２５８８８号公報
【特許文献２】特開２０１０−１４９２６２号公報
【特許文献３】特開２００５−８８０９６号公報
【特許文献４】特開平１０−１０００８９号公報
【発明の概要】
【発明が解決しようとする課題】
【００１２】
特許文献１のロボットハンドは、関節の剛性が予め定められているので、硬さの異なる被接触体に柔軟に対応することができない。
特許文献２のロボットハンドは、当該ロボットハンドの先端に探触子を設ける必要があり、構成が煩雑になる。
【００１３】
本発明の目的は、このような問題を解決するためになされたものであり、簡易な構成で硬さの異なる被接触体に対応することができるロボットハンドの制御装置を提供することである。
【課題を解決するための手段】
【００１４】
本発明の一形態に係るロボットハンドの制御装置は、リンクと、前記リンクの関節と、を有する指を備えたロボットハンドの制御装置において、前記関節の角度検出値である関節角度検出値に基づいて、前記指が被接触体に接触した時における関節角度、関節速度、関節加速度である前記関節の状態を推定する接触時関節状態推定器と、前記指が前記被接触体に接触していない時における前記関節の状態を推定する非接触時関節状態推定器と、前記関節角度検出値と、前記接触時関節状態推定器が出力する接触時関節状態推定値と、前記非接触時関節状態推定器が出力する非接触時関節状態推定値と、に基づいて、前記指が前記被接触体に接触したか否かを判定する接触判定器と、前記接触判定器が出力する判定結果が接触状態の場合、前記接触時関節状態推定値に基づいて、前記関節に印加されるトルク外乱を推定するトルク外乱推定器と、前記接触判定結果が接触状態の場合、前記接触時関節状態推定値と、前記トルク外乱推定器が出力するトルク外乱推定値と、に基づいて、前記被接触体の剛性を推定する剛性推定器と、前記接触判定結果が接触状態の場合、前記関節角度検出値と、前記剛性推定値と、に基づいて、前記関節の制御剛性を、前記指が前記被接触体の押し込み状態を維持できる最小値に演算し、前記接触判定結果が非接触状態の場合、前記関節の制御剛性を、予め推定された前記指が前記被接触体に接触した状態である場合における前記関節の制御剛性より大きな値に演算する制御剛性演算器と、前記制御剛性演算器が出力する制御剛性演算値に基づいて、前記関節を駆動する関節モータを制御する制御器と、を備える。
【００１５】
前記ロボットハンドの指は、第１関節、第２関節、第３関節を備え、前記被接触体は、家電のキー又は楽器の鍵盤若しくはボタンと弾性体とを備え、前記制御装置は、前記接触判定結果が接触状態の場合、前記接触時関節状態推定値に基づいて、前記被接触体が押し込み底に到達したか否かを判定する到達判定器をさらに備え、前記制御剛性演算器は、前記接触判定結果が非接触状態の場合、前記剛性推定値を前記第１関節及び前記第２関節回りの回転剛性に等価変換した値よりも大きく、前記指の第１関節及び第２関節の制御剛性を演算し、前記接触判定結果が接触状態の場合、前記剛性推定値を前記第１関節及び前記第２関節回りの回転剛性に等価変換した値と略等しい値に、前記第１関節及び第２関節の制御剛性を演算し、かつ前記到達判定器が出力する到達判定結果が前記押し込み底に非到達状態の場合、前記剛性推定値を前記第３関節回りの回転剛性に等価変換した値よりも大きな値に、前記第３関節の制御剛性を演算し、前記到達判定結果が前記押し込み底に到達状態の場合、前記剛性推定値を前記第３関節回りの回転剛性に等価変換した値と等しい値に、前記第３関節の制御剛性を演算すること、が好ましい。
【００１６】
前記接触判定器は、前記関節角度から前記接触時関節状態推定値の成分である接触時関節角度推定値を減算した接触時関節角度推定誤差の絶対値の時間平均値が、前記関節角度から前記非接触時関節状態推定値の成分である非接触時関節角度推定値を減算した非接触時関節角度推定誤差の絶対値の時間平均値より小さい場合、接触状態であると判定し、それ以外の場合は非接触状態であると判定すること、が好ましい。
【００１７】
前記トルク外乱推定器は、前記トルク外乱推定値を［数１］により算出すること、が好ましい。
【数１】
ただし、記号の意味は以下の通りであり、ｄ＾、ｑ＾、ｆｃ＾、Ｔはベクトル、Ｊは行列である。
ｄ＾：トルク外乱推定値
Ｊ：慣性行列
ｆｃ＾：接触時の非線形力の推定値（接触時非線形力推定値）
ｑ＾：接触時の関節の状態の推定値（一般化座標推定値）
Ｔ：関節モータのトルク
【００１８】
前記剛性推定器は、前記剛性推定値を［数２］により算出すること、が好ましい。
【数２】
ただし、記号の意味は以下の通りである。
ｋ＾：被接触体の剛性推定値[Ｎ／ｍ]
ｍ１：第１関節の質量[ｋｇ]
Ｊ１０：第１関節の慣性モーメント[ｋｇ・ｍ２]
θ１：第１関節の角度[ｒａｄ]
θ２：第２関節の角度[ｒａｄ]
θ３：第３関節の角度[ｒａｄ]
ｍ：キー等の質量[ｋｇ]
ｇ：重力加速度[ｍ／ｓ２]、
ｌ１：第１リンクの長さ[ｍ]
ｌ２：第２リンクの長さ[ｍ]
ｌ３：第３リンクの長さ[ｍ]
ｌ１０：第１リンクの重心と第１関節との距離[ｍ]
ｌ２０：第２リンクの重心と第２関節との距離[ｍ]
ｌ３０：第３リンクの重心と第３関節との距離[ｍ]
Ｔ１：第１関節の関節モータが発生する反時計回りを正としたトルク（第１関節モータトルク）[Ｎ・ｍ]
Ｔｄ１＾：第１関節に加わる反時計回りを正としたトルク外乱の推定値（第１関節トルク外乱推定値）[Ｎ・ｍ]
ｙ０：被接触体を支持する弾性体の自然長
【００１９】
前記到達判定器は、前記関節角度から前記接触時関節状態推定値の成分である接触時関節角度推定値を減算した接触時関節角度推定誤差の時間平均値と、前記関節角度から前記非接触時関節状態推定値の成分である非接触時関節角度推定値を減算した非接触時関節角度推定誤差の時間平均値との両方が発散した場合、前記指が前記被接触体の押し込み底に到達したと判定し、それ以外の時は到達していないと判定すること、が好ましい。
【００２０】
前記制御剛性演算器は、前記第１関節、第２関節及び第３関節の制御剛性演算値を［数３］により演算すること、が好ましい。
【数３】
ただし、記号の意味は以下の通りである。
ｋ１：第１関節の制御剛性（第１関節制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ２：第２関節の制御剛性（第２関節制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ３：第３関節の制御剛性（第３関節制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ１ｎ：非接触時の第１関節制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ２ｎ：非接触時の第２関節制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ３ｎ：押し込み底未到達時の第３関節制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ１０：第１関節回りにおいて被接触体の剛性を等価変換した値（第１関節等価制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ２０：第２関節回りにおいて被接触体の剛性を等価変換した値（第２関節等価制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ３０：第３関節回りにおいて被接触体の剛性を等価変換した値（第３関節等価制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ＾：キー剛性推定値[Ｎ／ｍ]
ｋ：キー剛性
ｃ１：第１制御剛性のパラメータ
ｃ２：第２制御剛性のパラメータ
ｃ３：第３制御剛性のパラメータ
θ１：第１関節の角度検出値［ｒａｄ］
θ２：第２関節の角度検出値［ｒａｄ］
θ３：第３関節の角度検出値［ｒａｄ］
【００２１】
前記第１関節を駆動する関節モータは、［数４］で表される第１関節モータトルクＴ１を発生し、前記第２関節を駆動する関節モータは、［数５］で表される第２関節モータトルクＴ２を発生し、前記第３関節を駆動する関節モータは、［数６］で表される第３関節モータトルクＴ３を発生すること、が好ましい。
【数４】
【数５】
【数６】
ただし、記号の意味は以下の通りである。
ｋ１：第１関節の制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ２：第２関節の制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ３：第３関節の制御剛性[Ｎ・ｍ／ｒａｄ]
ｄ１：第１関節における制御減衰[Ｎ・ｍ・ｓ／ｒａｄ]
ｄ２：第２関節における制御減衰[Ｎ・ｍ・ｓ／ｒａｄ]
ｄ３：第３関節における制御減衰[Ｎ・ｍ・ｓ／ｒａｄ]
ｌ１０：第１リンクの重心と第１関節の距離[ｍ]
ｌ２０：第２リンクの重心と第２関節の距離[ｍ]
ｌ３０：第３リンクの重心と第３関節の距離[ｍ]
ｌ１：第１リンクの長さ[ｍ]
ｌ２：第２リンクの長さ[ｍ]
ｌ３：第３リンクの長さ[ｍ]
ｍ１：第１関節の質量[ｋｇ]
θ１：第１関節の角度[ｒａｄ]
θ１＾：第１関節の角度推定値[ｒａｄ]
ｍ２：第２関節の質量[ｋｇ]
θ２：第２関節の角度[ｒａｄ]
θ２＾：第２関節の角度推定値[ｒａｄ]
ｍ３：第３関節の質量[ｋｇ]
θ３：第３関節の角度[ｒａｄ]
θ３＾：第３関節の角度推定値[ｒａｄ]
ｍ：キー等の質量[ｋｇ]
ｇ：重力加速度[ｍ／ｓ２]
Ｔｄ１＾：第１関節に加わる反時計回りを正としたトルク外乱の推定値[Ｎ・ｍ]
Ｔｄ２＾：第２関節に加わる反時計回りを正としたトルク外乱の推定値[Ｎ・ｍ]
Ｔｄ３＾：第３関節に加わる反時計回りを正としたトルク外乱の推定値[Ｎ・ｍ]
θ１ｒ：第１関節の角度指令[ｒａｄ]
θ２ｒ：第２関節の角度指令[ｒａｄ]
θ３ｒ：第３関節の角度指令[ｒａｄ]
【発明の効果】
【００２２】
以上、説明したように、本発明によると、簡易な構成で硬さの異なる被接触体に対応することができるロボットハンドの制御装置を提供することができる。
【図面の簡単な説明】
【００２３】
【図１】本発明の実施形態に係るロボットハンドの制御装置及びその周辺装置を示す機能ブロック図である。
【図２】本発明の実施形態に係るロボットハンドにおける、指機構を示すフリーボディー図である。
【図３】本発明のシミュレーションにおける第１関節の角度の時間変化を示す図である。
【図４】本発明のシミュレーションにおける第２関節の角度の時間変化を示す図である。
【図５】本発明のシミュレーションにおける第３関節の角度の時間変化を示す図である。
【図６】本発明のシミュレーションにおける制御剛性の時間変化を示す図である。
【図７】ロボットハンドを示す概略図である。
【発明を実施するための形態】
【００２４】
以下、本発明を実施するための最良の形態について、添付図面を参照しながら説明する。但し、本発明が以下の実施形態に限定される訳ではない。また、説明を明確にするため、以下の記載及び図面は、適宜、簡略化されている。
【００２５】
本発明の実施形態に係るロボットハンドの制御装置（以下、単に制御装置と云う場合がある。）は、例えば家電のキー等を操作する場合又はピアノの鍵盤やトランペットのボタン等を操作するロボットハンドを制御するために良好に適用される。但し、当該制御装置は、家電のキー操作やピアノの鍵盤操作やトランペットのボタン操作等のように、押圧動作を伴う被接触体への接触に限らず、被接触体を把持する場合等にも良好に適用できる。ちなみに、本実施形態の被接触体は、家電のキー等又は楽器の鍵盤若しくはボタン等と、これらを接触前の位置に復帰させる弾性体と、を備えたものである。
【００２６】
先ず、当該制御装置によって制御されるロボットハンドの構成を説明する。図２は、ロボットハンドの指機構を示すフリーボディー図である。
指機構は、第１リンク２０１と、第２リンク２０３と、第３リンク２０５と、を備える。第１リンク２０１と第２リンク２０３とは、第１関節２０２を介して動作可能に連結されている。第２リンク２０３と第３リンク２０５とは、第２関節２０４を介して動作可能に連結されている。第３リンク２０５は、第３関節２０６を介して動作可能に例えばベースに連結されている。
【００２７】
第１関節２０２、第２関節２０４、第３関節２０６それぞれは関節モータ（不図示）を備え、当該関節モータの回転駆動により第１関節角度θ１、第２関節角度θ２、第３関節角度θ３を変化させてキー２０７を操作する。
【００２８】
ここで、本実施形態の指機構は、図２に示すように、パソコンのキーボードのキー、電化製品の操作部に設置されたボタン、鍵盤楽器の鍵盤等であるキー２０７を操作する。キー２０７は、バネ２０８によって支持されている。
【００２９】
次に、当該指機構を制御する制御装置及びその周辺装置の構成を説明する。図１は、制御装置及びその周辺装置を示す機能ブロック図である。図１に示すように、制御装置１３０の周辺装置として、関節角度指令発生器１１０と、制御対象１２０と、を備える。
【００３０】
関節角度指令発生器１１０は、制御装置１３０から入力されるキー接触判定値とキー底到達判定値とに基づいて、指機構１２２がキー２０７を押す際の第１関節角度θ１、第２関節角度θ２、第３関節角度θ３の目標値である関節角度指令を、第１リンク２０１がキー２０７と接触する前、接触して押している間、及び押したキー２０７がキー底に到達した後に対して生成し出力する。ここで、キー接触判定値とは、キー２０７が第１リンク２０１と接触しているか否かを判定した結果である。キー底到達判定値とは、押されたキー２０７がキー底に到達したか否かを判定した結果である。
【００３１】
制御対象１２０は、一般的なロボットハンドである。制御対象１２０は、指のメカ機構である指機構１２２と、制御装置１３０から入力されるモータ電流に基づいて、第１関節２０２、第２関節２０４、第３関節２０６それぞれを駆動する関節モータ１２１と、第１関節角度θ１、第２関節角度θ２、第３関節角度θ３を検出し関節角度検出値として出力する関節角度検出器１２３と、を備える。
【００３２】
制御装置１３０は、キー接触時関節状態推定器１３１と、キー非接触時関節状態推定器１３２と、キー接触判定器１３３と、トルク外乱推定器１３４と、剛性推定器１３５と、キー底到達判定器１３６と、制御剛性演算器１３７と、制御器１３８と、を備える。
【００３３】
キー接触時関節状態推定器１３１は、制御対象１２０の関節角度検出器１２３から入力される関節角度検出値（第１関節角度θ１、第２関節角度θ２、第３関節角度θ３）等を用いて導出した、第１リンク２０１がキー２０７に接触している間における運動方程式に基づいて、関節角度、関節速度、関節加速度（即ち、関節の状態）を推定しキー接触時関節状態推定値として出力する。
【００３４】
キー非接触時関節状態推定器１３２は、制御対象１２０の関節角度検出器１２３から入力される関節角度検出値等を用いて導出した、第１リンク２０１がキー２０７に接触していない間における運動方程式に基づいて、関節角度、関節速度、関節加速度を推定しキー非接触時関節状態推定値として出力する。
【００３５】
キー接触判定器１３３は、制御対象１２０の関節角度検出器１２３から入力される関節角度検出値と、キー接触時関節状態推定器１３１から入力されるキー接触時関節状態推定値と、キー非接触時関節状態推定器１３２から入力されるキー非接触時関節状態推定値と、に基づいて、キー接触時における第１関節角度推定誤差θ１ｃ〜（キー接触時における一般化座標推定誤差ｑ１〜＝ｑ１−ｑ１＾の第１成分）と、キー非接触時における第１関節角度推定誤差θ１ｎｃ〜（キー非接触時における一般化座標推定誤差ｑ１〜＝ｑ１−ｑ１＾の第１成分）と、を算出する。キー接触判定器１３３は、キー接触時における第１関節角度推定誤差θ１ｃ〜の絶対値がキー非接触時における第１関節角度推定誤差θ１ｎｃ〜の絶対値より小さい場合（ａｂｓ（θ１ｃ〜）＜ａｂｓ（θ１ｎｃ〜））、第１リンク２０１はキー２０７に接触していると判定し、それ以外の場合は接触していないと判定し、その判定結果をキー接触判定結果として出力する。
【００３６】
トルク外乱推定器１３４は、キー接触判定器１３３から入力されるキー接触判定結果が、第１リンク２０１がキー２０７に接触しているとの判定結果のとき、キー接触時関節状態推定器１３１から入力されるキー接触時関節状態推定値に基づいて、第１関節２０２、第２関節２０４、第３関節２０６それぞれに掛かるトルク外乱を推定し、第１関節トルク外乱推定値Ｔｄ１＾、第２関節トルク外乱推定値Ｔｄ２＾、第３関節トルク外乱推定値Ｔｄ３＾として出力する。一方、トルク外乱推定器１３４は、キー接触判定器１３３から入力されるキー接触判定結果が、第１リンク２０１がキー２０７に接触していないとの判定結果のとき、何も出力しない。
【００３７】
剛性推定器１３５は、第１リンク２０１がキー２０７に接触している場合のみに、トルク外乱推定器１３４から入力されるトルク外乱推定値（第１関節トルク外乱推定値Ｔｄ１＾、第２関節トルク外乱推定値Ｔｄ２＾、第３関節トルク外乱推定値Ｔｄ３＾）と、キー接触時関節状態推定器１３１から入力されるキー接触時関節状態推定値と、に基づいて、被接触体の剛性（本実施形態では、ばね２０８のばね定数）を推定し、剛性推定値ｋ＾として出力する。
【００３８】
キー底到達判定器１３６は、接触判定器１３３から入力されるキー接触判定結果が、第１リンク２０１がキー２０７に接触しているとの判定結果のとき、キー接触時関節状態推定器１３１から入力されるキー接触時関節状態推定値の絶対値が発散したら、キー２０７がキー底に到達したと判定し、それ以外の場合はキー底に到達していないと判定し、その判定結果をキー底到達判定結果として出力する。
【００３９】
制御剛性演算器１３７は、関節角度検出器１２３から入力される関節角度検出値と、キー接触判定器１３３から入力されるキー接触判定結果と、剛性推定器１３５から入力される剛性推定値ｋ＾と、キー底到達判定器１３６から入力されるキー底到達判定結果と、に基づいて、第１リンク２０１がキー２０７に接触しているか否か、キー２０７に接触している場合はキー底に到達しているか否かを判定し、この判定結果に応じた、第１関節２０２、第２関節２０４、第３関節２０６それぞれの制御剛性を、剛性推定値ｋ＾と関節角度検出値とに基づいて演算し、制御剛性演算値として出力する。ここで、制御剛性とは、関節のメカ機構による剛性と、関節が備える関節モータや減速機等の剛性と、を加えた剛性のことである。
【００４０】
制御器１３８は、関節角度姿勢発生器１１０から入力される関節角度指令である、第１関節角度θ１、第２関節角度θ２、第３関節角度θ３の目標値と、制御剛性演算器１３７から入力される制御剛性演算値と、を実現するべく、第１関節角度θ１、第２関節角度θ２、第３関節角度θ３それぞれを制御する制御ゲインを設定し、その制御ゲインに基づいて算出されたモータ電流を出力する。
【００４１】
なお、制御装置１３０は、電気・電子・プログラマブル電子制御系として実現する。例えば、制御装置１３０は、演算処理等を行うＣＰＵ（Central Processing Unit）、ＣＰＵによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）等からなるマイクロコンピュータを中心にして、ハードウェア構成されている。また、ＡＳＩＣ（Application Specific Integrated Circuit）等のハードウェアにより、制御装置１３０の各器を実現しても良い。
【００４２】
次に、制御装置１３０を構成する各機能ブロックの詳細な動作原理を導出して説明する。
先ず、図２を用いて被接触体及び指機構１２２の運動方程式を導出する。図２より被接触体の運動方程式は［数７］と導出できる。
【００４３】
【数７】
【００４４】
ただし、記号の意味は以下の通りである。
ｙ０：図２の上方向を正としたばね２０８の自然長[ｍ]
ｙ１：第３関節２０６を基準とし、図２の上方向を正とした第１リンク２０１の先端（指先）の垂直位置（指先垂直位置）[ｍ]
ｆ：図２の上方向を正とした打鍵力[Ｎ]
ｍ：キー質量[ｋｇ]
ｇ：重力加速度[ｍ／ｓ２]
ｋ：ばね２０８の剛性[Ｎ／ｍ]
【００４５】
次に、指機構１２２の運動方程式を解析力学の手法に基づいて導出する。指機構１２２の運動エネルギーＫ及びポテンシャルエネルギーＵは、［数８］及び［数９］と導出できる。
【００４６】
【数８】
【００４７】
【数９】
【００４８】
ただし、記号の意味は以下の通りである。
ｍ１：第１関節２０２の質量（第１関節質量）[ｋｇ]
ｍ２：第２関節２０４の質量（第２関節質量）[ｋｇ]
ｍ３：第３関節２０６の質量（第３関節質量）[ｋｇ]
θ１：第１関節２０２の角度（第１関節角度）[ｒａｄ]
θ２：第２関節２０４の角度（第２関節角度）[ｒａｄ]
θ３：第３関節２０６の角度（第３関節角度）[ｒａｄ]
ｘ１０：第３関節２０６を基準とし、図２の左方向を正とした第１リンク２０１の重心の水平位置（第１リンク重心水平位置）[ｍ]
ｙ１０：第３関節２０６を基準とし、図２の上方向を正とした第１リンク２０１の重心の垂直位置（第１リンク重心垂直位置）[ｍ]
ｘ２０：第３関節２０６を基準とし、図２の左方向を正とした第２リンク２０３の重心の水平位置（第２リンク重心水平位置）[ｍ]
ｙ２０：第３関節２０６を基準とし、図２の上方向を正とした第２リンク２０３の重心の垂直位置（第２リンク重心垂直位置）[ｍ]
ｘ３０：第３関節２０６を基準とし、図２の左方向を正とした第３リンク２０５の重心の水平位置（第３リンク重心水平位置）[ｍ]
ｙ３０：第３関節２０６を基準とし、図２の上方向を正とした第３リンク２０５の重心の垂直位置（第３リンク重心垂直位置）[ｍ]
Ｊ１０：第１関節２０２の慣性モーメント[ｋｇ・ｍ２]
Ｊ２０：第２関節２０４の慣性モーメント[ｋｇ・ｍ２]
Ｊ３０：第３関節２０６の慣性モーメント[ｋｇ・ｍ２]
【００４９】
第１リンク重心水平位置ｘ１０、第１リンク重心垂直位置ｙ１０、第２リンク重心水平位置ｘ２０、第２リンク重心垂直位置ｙ２０、第３リンク重心水平位置ｘ３０、第３リンク重心垂直位置ｙ３０を、第１関節角度θ１、第２関節角度θ２、第３関節角度θ３を用いて書き換えると［数１０］から［数１５］と表される。
【００５０】
【数１０】
【００５１】
【数１１】
【００５２】
【数１２】
【００５３】
【数１３】
【００５４】
【数１４】
【００５５】
【数１５】
【００５６】
ただし、記号の意味は以下の通りである。
ｌ１０：第１リンク２０１の重心と第１関節２０２との距離[ｍ]
ｌ２０：第２リンク２０３の重心と第２関節２０４との距離[ｍ]
ｌ３０：第３リンク２０５の重心と第３関節２０６との距離[ｍ]
ｌ２：第２リンク２０３の長さ[ｍ]
ｌ３：第３リンク２０５の長さ[ｍ]
【００５７】
同様に、第１リンク２０１の指先垂直位置ｙ１を、第１関節角度θ１、第２関節角度θ２、第３関節角度θ３を用いて書き換えると［数１６］と表される。
【００５８】
【数１６】
ただし、ｌ１は第１リンク２０１の長さ[ｍ]である。
【００５９】
指機構１２２の運動方程式は、［数１７］から［数１９］のオイラーラグランジ方程式で表される。
【００６０】
【数１７】
【００６１】
【数１８】
【００６２】
【数１９】
【００６３】
ここで、Ｑ１、Ｑ２、Ｑ３は一般化力であり、指先がキー２０７に接触している場合、［数２０］から［数２２］で表される。
【００６４】
【数２０】
【００６５】
【数２１】
【００６６】
【数２２】
【００６７】
ここで、記号の意味は以下のようになる。
Ｔ１：第１関節２０２の関節モータ１２１が発生する反時計回りを正としたトルク（第１関節モータトルク）[Ｎ・ｍ]
Ｔ２：第２関節２０４の関節モータ１２１が発生する反時計回りを正としたトルク（第２関節モータトルク）[Ｎ・ｍ]
Ｔ３：第３関節２０６の関節モータ１２１が発生する反時計回りを正としたトルク（第３関節モータトルク）[Ｎ・ｍ]
Ｔｄ１：第１関節２０２に加わる反時計回りを正としたトルク外乱（第１関節トルク外乱）[Ｎ・ｍ]
Ｔｄ２：第２関節２０４に加わる反時計回りを正としたトルク外乱（第２関節トルク外乱）[Ｎ・ｍ]
Ｔｄ３：第３関節２０６に加わる反時計回りを正としたトルク外乱（第３関節トルク外乱）[Ｎ・ｍ]
【００６８】
［数８］から［数１６］及び［数２０］から［数２２］を、［数１７］から［数１９］に代入すると、指機構１２２の運動方程式は［数２３］から［数２５］と導出できる。
【００６９】
【数２３】
【００７０】
【数２４】
【００７１】
【数２５】
【００７２】
一方、指先がキー２０７に接触していない場合、一般化力Ｑ１は［数２０］に代えて、［数２６］と表される。
【００７３】
【数２６】
【００７４】
指先がキー２０７に接触していない場合、［数２６］により、［数２３］は［数２７］に置き換えられる。
【００７５】
【数２７】
【００７６】
キー接触時における指機構１２２の運動方程式は、［数２３］から［数２５］より［数２８］と書き換えられる。
【００７７】
【数２８】
【００７８】
ただし、記号の意味は以下のようになる。
Ｊ：慣性行列
ｆｃ：［数２３］から［数２５］の非線形項から構成されるベクトル（キー接触時非線形力）
ｑ：一般化座標
Ｔ：関節モータトルク
ｄ：トルク外乱
【００７９】
同様にキー非接触時の指機構１２２の運動方程式は、［数２４］、［数２５］、及び［数２７］より［数２９］と書き換えられる。
【００８０】
【数２９】
【００８１】
ただし、ｆｎｃは［数２４］、［数２５］、及び［数２７］の非線形項から構成されるベクトル（キー非接触時非線形力）である。
【００８２】
キー接触時関節状態推定器１３１は、［数３０］によりキー接触時における一般化座標ｑとその１階及び２階時間微分値（キー接触時関節状態）とを、ノイズを含まないように推定する。
【００８３】
【数３０】
【００８４】
ただし、記号の意味は以下のようになる。
Ｌ：推定器ゲイン
ｑ＾：一般化座標推定値
ｆｃ＾：キー接触時非線形力推定値
【００８５】
同様に、キー非接触時関節状態推定器１３２は、［数３１］によりキー非接触時における一般化座標ｑとその１階及び２階時間微分値（キー非接触時関節状態）とを、ノイズを含まないように推定する。
【００８６】
【数３１】
【００８７】
ただし、ｆｎｃ＾はキー非接触時非線形力推定値である。
【００８８】
キー接触時におけるキー接触時関節状態推定器１３１の推定誤差方程式は、［数２８］から［数３０］を減算することにより［数３２］と導出される。
【００８９】
【数３２】
【００９０】
ただし、ｑ~は一般化座標推定誤差である。
ここで、推定器ゲインＬの各要素が十分に大きな負の実数となるように設定すると、［数３３］が成立し、キー接触時関節状態が推定される。
【００９１】
【数３３】
【００９２】
一方、キー接触時におけるキー非接触時関節状態推定器１３２の推定誤差方程式は、［数２８］から［数３１］を減算することにより［数３４］と導出される。
【００９３】
【数３４】
【００９４】
キー接触時において、［数３２］の非線形項ｆｃ−ｆｃ＾の時間平均値は、［数３４］の非線形項ｆｃ−ｆｎｃ＾の時間平均値よりも常に小さい値をとるため、キー接触時関節状態推定器１３１の推定誤差方程式［数３２］より算出した一般化座標推定誤差ｑ〜の時間平均値の方が、キー非接触時関節状態推定器１３２の推定誤差方程式［数３４］より算出した値より小さい。
【００９５】
同様に、キー非接触時におけるキー非接触時関節状態推定器１３２の推定誤差方程式は、［数２９］から［数３１］を減算することにより［数３５］と導出される。
【００９６】
【数３５】
【００９７】
ここで、推定器ゲインＬの各要素が十分に大きな負の実数となるように設定すると、［数３３］が成立し、キー非接触時関節状態が推定される。
【００９８】
キー接触時の場合と同様の理由から、キー非接触時においては、キー非接触時関節状態推定器１３２の推定誤差方程式より算出した一般化座標推定誤差ｑ〜の時間平均値の方が、キー接触時関節状態推定器１３１の推定誤差方程式より算出した値より小さい。
【００９９】
上述の原理に基づいてキー接触判定器１３３は、キー接触時関節状態推定器１３１及びキー非接触時関節状態推定器１３２のうち、前者が出力する一般化座標推定誤差ｑ〜の時間平均が後者よりも小さい場合はキー接触時であると判定し、それ以外の場合はキー非接触時であると判定する。
【０１００】
キー接触判定器１３３がキー接触時であると判定した場合、一般化座標推定値ｑ＾は一般化座標ｑに収束するので、［数２８］よりトルク外乱推定器１３４は［数３６］を用いてトルク外乱推定値を算出できることが分かる。
【０１０１】
【数３６】
ただし、ｄ＾はトルク外乱推定値である。
【０１０２】
［数２３］とトルク外乱推定値ｄ＾を用いて剛性推定器１３５は、剛性推定値ｋ＾を［数３７］により算出する。
【０１０３】
【数３７】
【０１０４】
ただし、記号の意味は以下のようになる。
Ｔｄ１＾：第１関節２０２に加わる反時計回りを正としたトルク外乱の推定値（第１関節トルク外乱推定値）[Ｎ・ｍ]
θ１＾：第１関節角度推定値[ｒａｄ]
θ２＾：第２関節角度推定値[ｒａｄ]
θ３＾：第３関節角度推定値[ｒａｄ]
【０１０５】
キー底到達判定器１３６は、キー接触判定器１３３がキー接触時であると判定している間に、キー接触時関節状態推定器１３１が出力するキー接触時関節状態推定誤差の時間平均、及びキー非接触時関節状態推定器１３２が出力するキー非接触時関節状態推定誤差の時間平均の両方が発散することをもって、キー底に到達したと判定する。
【０１０６】
図２において、キー２０７からの反力と第１関節２０２、第２関節２０４、第３関節２０６の発生するモータトルクが静的に釣り合っている場合、各関節における制御剛性それぞれを第１関節等価制御剛性ｋ１０、第２関節等価制御剛性ｋ２０、第３関節等価制御剛性ｋ３０とすると、［数３８］から［数４０］が成立する。
【０１０７】
【数３８】
【０１０８】
【数３９】
【０１０９】
【数４０】
【０１１０】
ただし、記号の意味は次の通りである。
δｙ：キー２０７の垂直方向微小変位量[ｍ]
δθ１：第１関節角度θ１の微小変位量[ｒａｄ]
δθ２：第２関節角度θ２の微小変位量[ｒａｄ]
δθ３：第３関節角度θ３の微小変位量[ｒａｄ]
【０１１１】
［数３８］から［数４０］において、δｙとδθ１、δθ２、δθ３は微小量であり相殺するので、［数４１］から［数４３］が得られる。
【０１１２】
【数４１】
【０１１３】
【数４２】
【０１１４】
【数４３】
【０１１５】
制御剛性演算器１３７は、第１関節２０２の制御剛性である第１関節制御剛性ｋ１と、第２関節２０４の制御剛性である第２関節制御剛性ｋ２と、第３関節２０６の制御剛性である第３関節制御剛性ｋ３と、をそれぞれ［数４４］に基づいて決定する。
【０１１６】
【数４４】
【０１１７】
ただし、記号の意味は以下の通りである。
ｃ１：第１関節制御剛性ｋ１のパラメータ
ｃ２：第２関節制御剛性ｋ２のパラメータ
ｃ３：第３関節制御剛性ｋ３のパラメータ
ｋ１ｎ：非接触時の第１関節制御剛性
ｋ２ｎ：非接触時の第２関節制御剛性
ｋ３ｎ：キー底未到達時の第３関節制御剛性
【０１１８】
制御剛性演算器１３７は、［数４４］で決定した制御剛性となるように、制御器１３８の制御ゲインを設定する。これにより、指機構１２２がキー２０７を押すまでは、第１関節２０２、第２関節２０４、第３関節２０６それぞれに対する制御剛性が被接触体の剛性（ばね２０８のばね定数）ｋを第１関節２０２、第２関節２０４、第３関節２０６回りの回転剛性に等価変換した値より大きいため、第１リンク２０１の先端（指先）と、第１関節角度θ１、第２関節角度θ２、第３関節角度θ３を素早く正確にキー２０７を押すのに適した位置に移動させることができる。
【０１１９】
また、キー接触後は、第１関節２０２、第２関節２０４それぞれに対する制御剛性が被接触体の剛性ｋを第１関節２０２、第２関節２０４回りの回転剛性に等価変換した値と等しく、第３関節２０６に対する制御剛性が被接触体の剛性ｋを第３関節２０６回りの回転剛性に等価変換した値より大きいため、主に第３関節２０６のみの動作によりキー２０７を傷つけないように必要最小限の力でキー２０７を押すことができる。
【０１２０】
さらにキー底に到達したら、第１関節２０２、第２関節２０４、第３関節２０６それぞれに対する制御剛性が被接触体の剛性ｋを第１関節２０２、第２関節２０４、第３関節２０６回りの回転剛性に等価変換した値と等しく、キー２０７をキー底に保持するために必要最小限の力のみを残して第１関節２０２、第２関節２０４、第３関節２０６をリラックスさせることができる。
【０１２１】
キー接触時において、各関節における制御剛性を［数４４］とする第１関節モータトルクＴ１、第２関節モータトルクＴ２、第３関節モータトルクＴ３は、［数２３］から［数２５］より、［数４５］から［数４７］と導出できる。
【０１２２】
【数４５】
【０１２３】
【数４６】
【０１２４】
【数４７】
【０１２５】
ただし、記号の意味は以下の通りである。
ｄ１：第１関節２０２における制御減衰（第１関節制御減衰）
ｄ２：第２関節２０４における制御減衰（第２関節制御減衰）
ｄ３：第３関節２０６における制御減衰（第３関節制御減衰）
Ｔｄ２＾：第２関節に加わる反時計回りを正としたトルク外乱の推定値（第２関節トルク外乱推定値）[Ｎ・ｍ]
Ｔｄ３＾：第３関節に加わる反時計回りを正としたトルク外乱の推定値（第３関節トルク外乱推定値）[Ｎ・ｍ]
ここで、制御減衰とは、関節のメカ機構による減衰と、関節が備える関節モータや減速機等の減衰と、を加えた減衰のことである。
【０１２６】
一方、キー非接触時において、第２関節モータトルクＴ２、第３関節モータトルクＴ３は［数４６］、［数４７］と同じであり、第１関節モータトルクＴ１は［数２７］より［数４８］と表される。
【０１２７】
【数４８】
【０１２８】
上述のように、被接触体の剛性を推定するので、簡易な機構で硬さの異なる被接触体に柔軟に対応することができる。特に、本実施形態の制御装置を、キー操作等を行うロボットハンドに適用すると、サービスロボットがパソコンのキーボード操作する場合や、家電のキー操作をする場合に、キーを傷つけることなく早く正確にキーを押すことが可能となり、サービスロボットが楽器を演奏する場合は、鍵盤やボタンを傷つけることなく正確に早く洗練された演奏をすることが可能となる。
【０１２９】
このように、本実施形態によるとサービスロボットが家電のキー、楽器の鍵盤やボタンを傷つけることなく、早く正確に洗練されたキー操作や演奏をすることができる。
【０１３０】
＜シミュレーション結果＞
本シミュレーションは、サービスロボットのロボットハンドがパソコンのキー操作をする場合を模擬している。シミュレーションに用いた数値は以下のとおりである。
ｌ１＝２０×１０−３[ｍ]
ｌ２＝２０×１０−３[ｍ]
ｌ３＝２０×１０−３[ｍ]
ｌ１０＝１０×１０−３[ｍ]
ｌ２０＝１０×１０−３[ｍ]
ｌ３０＝１０×１０−３[ｍ]
ｍ１＝３０×１０−３[ｋｇ]
ｍ２＝３０×１０−３[ｋｇ]
ｍ３＝３０×１０−３[ｋｇ]
Ｊ１０＝１．４２×１０−６[ｋｇ・ｍ２]
Ｊ２０＝１．４２×１０−６[ｋｇ・ｍ２]
Ｊ３０＝１．４２×１０−６[ｋｇ・ｍ２]
ｍ＝１．９×１０−３[ｋｇ]
ｋ＝１４０[Ｎ／ｍ]
ｙ０＝３×１０−３[ｍ]
ｇ＝９．８[ｍ／ｓ２]
Ｔｄ１＝０[Ｎ・ｍ]
Ｔｄ２＝０[Ｎ・ｍ]
Ｔｄ３＝０[Ｎ・ｍ]
ｃ１＝３
ｃ２＝３
ｃ３＝３
ｄ１＝２×１０−１[Ｎ・ｍ・ｓ／ｒａｄ]
ｄ２＝６×１０−１[Ｎ・ｍ・ｓ／ｒａｄ]
ｄ３＝９×１０−１[Ｎ・ｍ・ｓ／ｒａｄ]
ｋ１ｎ＝１×１０５[Ｎ・ｍ／ｒａｄ]
ｋ２ｎ＝１×１０５[Ｎ・ｍ／ｒａｄ]
ｋ３ｎ＝１×１０５[Ｎ・ｍ／ｒａｄ]
【０１３１】
ただし、キー質量ｍ及びばね（弾性体）の剛性ｋは標準的なパソコンのキーボードの値とし、第１関節制御減衰ｄ１、第２関節制御減衰ｄ２、第３関節制御減衰ｄ３の値は、各関節より先端の部分の慣性モーメントの大きさに合わせて設定した。
【０１３２】
図３は本シミュレーションにおける第１関節角度の時間変化、図４は本シミュレーションにおける第２関節角度の時間変化、図５は本シミュレーションにおける第３関節角度の時間変化、図６は本シミュレーションにおける制御剛性の時間変化である。
【０１３３】
図３から図５において、破線は関節角度指令、実線は実際の関節角度を示す。また、図６において、実線は第１関節制御剛性ｋ１、破線は第２関節制御剛性ｋ２、一点鎖線は第３関節制御剛性ｋ３を示す。
【０１３４】
図３と図４の２．５[ｓ]において第１関節２０２の先端（指先）がキー２０７に接触し、キー接触判定器１３３が「キー接触した」という判定結果を関節角度指令発生器１１０に出力し、関節角度指令発生器１１０は第１関節角度指令θ１ｒ及び第２関節角度指令θ２ｒとを減速させている。第１関節角度指令θ１ｒや第２関節角度指令θ２ｒを、キー接触時の第１関節角度θ１や第２関節角度θ２より大きい角度に収束させるのは、１度キー接触した指先をキー２０７の表面に密着した状態に維持するためである。図６において、制御剛性演算器１３７が［数３８］を用いて算出する第１関節制御剛性ｋ１及び第２関節制御剛性ｋ２は、キー接触前は関節角度指令に追従するのに必要な制御帯域を確保するのに十分高い値に設定され、２．５[ｓ]でキー接触が判定されると、最小限の制御剛性に抑えられていることが分かる。
【０１３５】
図５の２．５[ｓ]においてキー接触が判定されると、第３関節角度指令は１度減速し、キー２０７を押し込む動作に移行する。５．５[ｓ]においてキー底に到達すると、キー２０７をそれ以上押し込むことが出来なくなるので、第３関節角度θ３は０．３１［ｒａｄ］で飽和している。この時、キー底到達判定器１３６は、上述のようにキー接触時関節状態推定値の絶対値が発散するので、キー底に到達したという判定結果を出力する。関節角度指令発生器１１０は、「キー底に到達した」という判定結果が入力されると、第３関節角度指令θ３ｒを減速させる。第３関節角度指令θ３ｒがキー底到達時の第３関節角度θ３より大きな値に収束しているのは、キー２０７をキー底にしっかり保持するためである。制御剛性演算器１３７は、キー底到達前において、第３関節角度θ３を第３関節角度指令θ３ｒに追従させるのに必要な制御帯域を確保するために、第３関節制御剛性ｋ３を十分に大きな値に設定し、５．５[ｓ]においてキー底に到達した以降において、キー２０７をキー底に保持するのに必要最小限の値に第３関節制御剛性ｋ３を設定していることが分かる。
【０１３６】
特許文献１のロボットハンドによると、剛性の低い指を使うと早い打鍵動作ができず、剛性の高い指を使うと早い打鍵動作はできるが、キー底到達後にキー２０７を押し込む力をキー剛性に合わせて適切に変化させることができず、キー２０７を傷つけてしまう問題がある。一方、本実施形態によると、上述のようにキー接触前、キー押し込み時、キー底到達後の３つの場合において、キー剛性に応じて適切に各関節の制御剛性を時間変化させるので、キー２０７を傷つけることなく早く正確なタイピングができる。
【０１３７】
また、ピアノなど鍵盤楽器の演奏に対して本発明を適用すると、上述のようにキー剛性に応じて適切に各関節の制御剛性を時間変化させることにより、鍵盤を傷つけることなく、正確に早い演奏が可能となり、繊細なトナリティーの表現が可能となる。
【０１３８】
このように、本実施形態によるとサービスロボットが家電のキー、楽器の鍵盤やボタンを傷つけることなく、早く正確に洗練されたキー操作や演奏をすることができる。
【０１３９】
以上、本発明に係るロボットハンドの制御装置の実施形態を説明したが、上記の構成に限らず、本発明の技術的思想を逸脱しない範囲で、変更することが可能である。
本実施形態の制御装置は、家電や楽器等のキーを操作するロボットハンドを制御するために適用したが、把持対象物を把持するロボットハンドを制御するために適用しても良い。この場合、キー底到達判定器１３６を省略しても良い。
本実施形態の指機構は、第１リンク２０１、第２リンク２０３、第３リンク２０５を備えた構成とされているが、リンクや関節の個数は限定されない。
本実施形態の被接触体の弾性体としてバネを用いたが、ゴム等の復元部材を用いても良い。
【符号の説明】
【０１４０】
１１０ 関節角度指令発生器
１２０ 制御対象
１２１ 関節モータ
１２２ 指機構
１２３ 関節角度検出器
１３０ 制御装置
１３１ キー接触時関節状態推定器
１３２ キー非接触時関節状態推定器
１３３ キー接触判定器
１３４ トルク外乱推定器
１３５ 剛性推定器
１３６ キー底到達判定器
１３７ 制御剛性演算器
１３８ 制御器
２０１ 第１リンク
２０２ 第１関節
２０３ 第２リンク
２０４ 第２関節
２０５ 第３リンク
２０６ 第３関節
２０７ キー
２０８ ばね
７０１ 第１指第１リンク
７０２ 第１指ＭＰ関節
７０３ 第１指第２リンク
７０４ 第１指ＩＰ関節
７０５ 第２指第１リンク
７０６ 第２指ＭＰ関節
７０７ 第２指第２リンク
７０８ 第２指ＰＩＰ関節
７０９ 第２指第３リンク
７１０ 第２指ＤＩＰ関節
７１１ 第３指第１リンク
７１２ 第３指ＭＰ関節
７１３ 第３指第２リンク
７１４ 第３指ＰＩＰ関節
７１５ 第３指第３リンク
７１６ 第３指ＤＩＰ関節
７１７ 第４指第１リンク
７１８ 第４指ＭＰ関節
７１９ 第４指第２リンク
７２０ 第４指ＰＩＰ関節
７２１ 第４指第３リンク
７２２ 第４指ＤＩＰ関節
７２３ ベース
【技術分野】
【０００１】
本発明は、ロボットハンドの制御装置に関する。
【背景技術】
【０００２】
一般的なロボットハンドは、複数の指と、当該指が根元部分において動作可能（回転可能）に連結されたベースと、を備える。当該指は、複数のリンクと、隣り合う２つのリンクを動作可能に連結する関節と、関節を駆動する関節モータと、を備える。このような構成により、ロボットハンドは人間の手の機能の一部を模倣可能な構成とされている。
【０００３】
図７はロボットハンドの概略図を示す。図７に示すロボットハンドは、第１指〜第４指がベース７２３に動作可能に連結されている。
詳細には、第１指は、第１指第１リンク７０１と、第１指ＭＰ（metacarpophalangeal）関節７０２と、第１指第２リンク７０３と、第１指ＩＰ（interphalangeal）関節７０４と、を備える。
【０００４】
第２指は、第２指第１リンク７０５と、第２指ＭＰ関節７０６と、第２指第２リンク７０７と、第２指ＰＩＰ（proximal interphalangeal）関節７０８と、第２指第３リンク７０９と、第２指ＤＩＰ（distal interphalangeal）関節７１０と、を備える。
【０００５】
第３指は、第３指第１リンク７１１と、第３指ＭＰ関節７１２と、第３指第２リンク７１３と、第３指ＰＩＰ関節７１４と、第３指第３リンク７１５と、第３指ＤＩＰ関節７１６と、を備える。
【０００６】
第４指は、第４指第１リンク７１７と、第４指ＭＰ関節７１８と、第４指第２リンク７１９と、第４指ＰＩＰ関節７２０と、第４指第３リンク７２１と、第４指ＤＩＰ関節７２２と、を備える。
【０００７】
これらの指の関節はトーションばねを備える。隣り合った２つの関節は連動リンク部材（不図示）により連動して動作するようになっている。連動リンク部材を、関節モータ（不図示）で回転させることにより、各指の曲げ伸ばしを実施する。このとき、指の曲げ伸ばし動作中に指が被接触体に接触し反力を受けると、上述のトーションばねがその反力を吸収する。
【０００８】
このようなロボットアームの構成を利用し、特許文献１のロボットハンドは、複数の指それぞれの関節の剛性を変えることで、柔らかい被接触体を扱う際には関節の剛性の低い指を用い、硬い被接触体を扱う際には関節の剛性の高い指を用いる構成とされている。
【０００９】
また、特許文献２にも、硬さの異なる被接触体を良好に把持することができるように構成されたロボットハンドが開示されている。特許文献２のロボットハンドは、指の先端に設けられた探触子の検出情報に基づいて、被接触体への接触検出、及び滑り検出を行い、被接触体が重力方向の相対的移動を生じないように、被接触体を最小限度の把持力で把持する構成とされている。
【００１０】
ちなみに、特許文献３、４には、被接触体を良好に把持できるように構成されたロボットハンドが開示されている。特許文献３のロボットハンドは、指リンクを剛性が低い被覆部材で被覆し、更にその上から摩擦係数が高い被覆部材で被覆した構成とされている。特許文献４のロボットハンドは、力検出手段により検出されたロボットハンドに加わる外力が増加した時に把持力を増加させる構成とされている。
【先行技術文献】
【特許文献】
【００１１】
【特許文献１】特開２００９−１２５８８８号公報
【特許文献２】特開２０１０−１４９２６２号公報
【特許文献３】特開２００５−８８０９６号公報
【特許文献４】特開平１０−１０００８９号公報
【発明の概要】
【発明が解決しようとする課題】
【００１２】
特許文献１のロボットハンドは、関節の剛性が予め定められているので、硬さの異なる被接触体に柔軟に対応することができない。
特許文献２のロボットハンドは、当該ロボットハンドの先端に探触子を設ける必要があり、構成が煩雑になる。
【００１３】
本発明の目的は、このような問題を解決するためになされたものであり、簡易な構成で硬さの異なる被接触体に対応することができるロボットハンドの制御装置を提供することである。
【課題を解決するための手段】
【００１４】
本発明の一形態に係るロボットハンドの制御装置は、リンクと、前記リンクの関節と、を有する指を備えたロボットハンドの制御装置において、前記関節の角度検出値である関節角度検出値に基づいて、前記指が被接触体に接触した時における関節角度、関節速度、関節加速度である前記関節の状態を推定する接触時関節状態推定器と、前記指が前記被接触体に接触していない時における前記関節の状態を推定する非接触時関節状態推定器と、前記関節角度検出値と、前記接触時関節状態推定器が出力する接触時関節状態推定値と、前記非接触時関節状態推定器が出力する非接触時関節状態推定値と、に基づいて、前記指が前記被接触体に接触したか否かを判定する接触判定器と、前記接触判定器が出力する判定結果が接触状態の場合、前記接触時関節状態推定値に基づいて、前記関節に印加されるトルク外乱を推定するトルク外乱推定器と、前記接触判定結果が接触状態の場合、前記接触時関節状態推定値と、前記トルク外乱推定器が出力するトルク外乱推定値と、に基づいて、前記被接触体の剛性を推定する剛性推定器と、前記接触判定結果が接触状態の場合、前記関節角度検出値と、前記剛性推定値と、に基づいて、前記関節の制御剛性を、前記指が前記被接触体の押し込み状態を維持できる最小値に演算し、前記接触判定結果が非接触状態の場合、前記関節の制御剛性を、予め推定された前記指が前記被接触体に接触した状態である場合における前記関節の制御剛性より大きな値に演算する制御剛性演算器と、前記制御剛性演算器が出力する制御剛性演算値に基づいて、前記関節を駆動する関節モータを制御する制御器と、を備える。
【００１５】
前記ロボットハンドの指は、第１関節、第２関節、第３関節を備え、前記被接触体は、家電のキー又は楽器の鍵盤若しくはボタンと弾性体とを備え、前記制御装置は、前記接触判定結果が接触状態の場合、前記接触時関節状態推定値に基づいて、前記被接触体が押し込み底に到達したか否かを判定する到達判定器をさらに備え、前記制御剛性演算器は、前記接触判定結果が非接触状態の場合、前記剛性推定値を前記第１関節及び前記第２関節回りの回転剛性に等価変換した値よりも大きく、前記指の第１関節及び第２関節の制御剛性を演算し、前記接触判定結果が接触状態の場合、前記剛性推定値を前記第１関節及び前記第２関節回りの回転剛性に等価変換した値と略等しい値に、前記第１関節及び第２関節の制御剛性を演算し、かつ前記到達判定器が出力する到達判定結果が前記押し込み底に非到達状態の場合、前記剛性推定値を前記第３関節回りの回転剛性に等価変換した値よりも大きな値に、前記第３関節の制御剛性を演算し、前記到達判定結果が前記押し込み底に到達状態の場合、前記剛性推定値を前記第３関節回りの回転剛性に等価変換した値と等しい値に、前記第３関節の制御剛性を演算すること、が好ましい。
【００１６】
前記接触判定器は、前記関節角度から前記接触時関節状態推定値の成分である接触時関節角度推定値を減算した接触時関節角度推定誤差の絶対値の時間平均値が、前記関節角度から前記非接触時関節状態推定値の成分である非接触時関節角度推定値を減算した非接触時関節角度推定誤差の絶対値の時間平均値より小さい場合、接触状態であると判定し、それ以外の場合は非接触状態であると判定すること、が好ましい。
【００１７】
前記トルク外乱推定器は、前記トルク外乱推定値を［数１］により算出すること、が好ましい。
【数１】
ただし、記号の意味は以下の通りであり、ｄ＾、ｑ＾、ｆｃ＾、Ｔはベクトル、Ｊは行列である。
ｄ＾：トルク外乱推定値
Ｊ：慣性行列
ｆｃ＾：接触時の非線形力の推定値（接触時非線形力推定値）
ｑ＾：接触時の関節の状態の推定値（一般化座標推定値）
Ｔ：関節モータのトルク
【００１８】
前記剛性推定器は、前記剛性推定値を［数２］により算出すること、が好ましい。
【数２】
ただし、記号の意味は以下の通りである。
ｋ＾：被接触体の剛性推定値[Ｎ／ｍ]
ｍ１：第１関節の質量[ｋｇ]
Ｊ１０：第１関節の慣性モーメント[ｋｇ・ｍ２]
θ１：第１関節の角度[ｒａｄ]
θ２：第２関節の角度[ｒａｄ]
θ３：第３関節の角度[ｒａｄ]
ｍ：キー等の質量[ｋｇ]
ｇ：重力加速度[ｍ／ｓ２]、
ｌ１：第１リンクの長さ[ｍ]
ｌ２：第２リンクの長さ[ｍ]
ｌ３：第３リンクの長さ[ｍ]
ｌ１０：第１リンクの重心と第１関節との距離[ｍ]
ｌ２０：第２リンクの重心と第２関節との距離[ｍ]
ｌ３０：第３リンクの重心と第３関節との距離[ｍ]
Ｔ１：第１関節の関節モータが発生する反時計回りを正としたトルク（第１関節モータトルク）[Ｎ・ｍ]
Ｔｄ１＾：第１関節に加わる反時計回りを正としたトルク外乱の推定値（第１関節トルク外乱推定値）[Ｎ・ｍ]
ｙ０：被接触体を支持する弾性体の自然長
【００１９】
前記到達判定器は、前記関節角度から前記接触時関節状態推定値の成分である接触時関節角度推定値を減算した接触時関節角度推定誤差の時間平均値と、前記関節角度から前記非接触時関節状態推定値の成分である非接触時関節角度推定値を減算した非接触時関節角度推定誤差の時間平均値との両方が発散した場合、前記指が前記被接触体の押し込み底に到達したと判定し、それ以外の時は到達していないと判定すること、が好ましい。
【００２０】
前記制御剛性演算器は、前記第１関節、第２関節及び第３関節の制御剛性演算値を［数３］により演算すること、が好ましい。
【数３】
ただし、記号の意味は以下の通りである。
ｋ１：第１関節の制御剛性（第１関節制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ２：第２関節の制御剛性（第２関節制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ３：第３関節の制御剛性（第３関節制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ１ｎ：非接触時の第１関節制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ２ｎ：非接触時の第２関節制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ３ｎ：押し込み底未到達時の第３関節制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ１０：第１関節回りにおいて被接触体の剛性を等価変換した値（第１関節等価制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ２０：第２関節回りにおいて被接触体の剛性を等価変換した値（第２関節等価制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ３０：第３関節回りにおいて被接触体の剛性を等価変換した値（第３関節等価制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ＾：キー剛性推定値[Ｎ／ｍ]
ｋ：キー剛性
ｃ１：第１制御剛性のパラメータ
ｃ２：第２制御剛性のパラメータ
ｃ３：第３制御剛性のパラメータ
θ１：第１関節の角度検出値［ｒａｄ］
θ２：第２関節の角度検出値［ｒａｄ］
θ３：第３関節の角度検出値［ｒａｄ］
【００２１】
前記第１関節を駆動する関節モータは、［数４］で表される第１関節モータトルクＴ１を発生し、前記第２関節を駆動する関節モータは、［数５］で表される第２関節モータトルクＴ２を発生し、前記第３関節を駆動する関節モータは、［数６］で表される第３関節モータトルクＴ３を発生すること、が好ましい。
【数４】
【数５】
【数６】
ただし、記号の意味は以下の通りである。
ｋ１：第１関節の制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ２：第２関節の制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ３：第３関節の制御剛性[Ｎ・ｍ／ｒａｄ]
ｄ１：第１関節における制御減衰[Ｎ・ｍ・ｓ／ｒａｄ]
ｄ２：第２関節における制御減衰[Ｎ・ｍ・ｓ／ｒａｄ]
ｄ３：第３関節における制御減衰[Ｎ・ｍ・ｓ／ｒａｄ]
ｌ１０：第１リンクの重心と第１関節の距離[ｍ]
ｌ２０：第２リンクの重心と第２関節の距離[ｍ]
ｌ３０：第３リンクの重心と第３関節の距離[ｍ]
ｌ１：第１リンクの長さ[ｍ]
ｌ２：第２リンクの長さ[ｍ]
ｌ３：第３リンクの長さ[ｍ]
ｍ１：第１関節の質量[ｋｇ]
θ１：第１関節の角度[ｒａｄ]
θ１＾：第１関節の角度推定値[ｒａｄ]
ｍ２：第２関節の質量[ｋｇ]
θ２：第２関節の角度[ｒａｄ]
θ２＾：第２関節の角度推定値[ｒａｄ]
ｍ３：第３関節の質量[ｋｇ]
θ３：第３関節の角度[ｒａｄ]
θ３＾：第３関節の角度推定値[ｒａｄ]
ｍ：キー等の質量[ｋｇ]
ｇ：重力加速度[ｍ／ｓ２]
Ｔｄ１＾：第１関節に加わる反時計回りを正としたトルク外乱の推定値[Ｎ・ｍ]
Ｔｄ２＾：第２関節に加わる反時計回りを正としたトルク外乱の推定値[Ｎ・ｍ]
Ｔｄ３＾：第３関節に加わる反時計回りを正としたトルク外乱の推定値[Ｎ・ｍ]
θ１ｒ：第１関節の角度指令[ｒａｄ]
θ２ｒ：第２関節の角度指令[ｒａｄ]
θ３ｒ：第３関節の角度指令[ｒａｄ]
【発明の効果】
【００２２】
以上、説明したように、本発明によると、簡易な構成で硬さの異なる被接触体に対応することができるロボットハンドの制御装置を提供することができる。
【図面の簡単な説明】
【００２３】
【図１】本発明の実施形態に係るロボットハンドの制御装置及びその周辺装置を示す機能ブロック図である。
【図２】本発明の実施形態に係るロボットハンドにおける、指機構を示すフリーボディー図である。
【図３】本発明のシミュレーションにおける第１関節の角度の時間変化を示す図である。
【図４】本発明のシミュレーションにおける第２関節の角度の時間変化を示す図である。
【図５】本発明のシミュレーションにおける第３関節の角度の時間変化を示す図である。
【図６】本発明のシミュレーションにおける制御剛性の時間変化を示す図である。
【図７】ロボットハンドを示す概略図である。
【発明を実施するための形態】
【００２４】
以下、本発明を実施するための最良の形態について、添付図面を参照しながら説明する。但し、本発明が以下の実施形態に限定される訳ではない。また、説明を明確にするため、以下の記載及び図面は、適宜、簡略化されている。
【００２５】
本発明の実施形態に係るロボットハンドの制御装置（以下、単に制御装置と云う場合がある。）は、例えば家電のキー等を操作する場合又はピアノの鍵盤やトランペットのボタン等を操作するロボットハンドを制御するために良好に適用される。但し、当該制御装置は、家電のキー操作やピアノの鍵盤操作やトランペットのボタン操作等のように、押圧動作を伴う被接触体への接触に限らず、被接触体を把持する場合等にも良好に適用できる。ちなみに、本実施形態の被接触体は、家電のキー等又は楽器の鍵盤若しくはボタン等と、これらを接触前の位置に復帰させる弾性体と、を備えたものである。
【００２６】
先ず、当該制御装置によって制御されるロボットハンドの構成を説明する。図２は、ロボットハンドの指機構を示すフリーボディー図である。
指機構は、第１リンク２０１と、第２リンク２０３と、第３リンク２０５と、を備える。第１リンク２０１と第２リンク２０３とは、第１関節２０２を介して動作可能に連結されている。第２リンク２０３と第３リンク２０５とは、第２関節２０４を介して動作可能に連結されている。第３リンク２０５は、第３関節２０６を介して動作可能に例えばベースに連結されている。
【００２７】
第１関節２０２、第２関節２０４、第３関節２０６それぞれは関節モータ（不図示）を備え、当該関節モータの回転駆動により第１関節角度θ１、第２関節角度θ２、第３関節角度θ３を変化させてキー２０７を操作する。
【００２８】
ここで、本実施形態の指機構は、図２に示すように、パソコンのキーボードのキー、電化製品の操作部に設置されたボタン、鍵盤楽器の鍵盤等であるキー２０７を操作する。キー２０７は、バネ２０８によって支持されている。
【００２９】
次に、当該指機構を制御する制御装置及びその周辺装置の構成を説明する。図１は、制御装置及びその周辺装置を示す機能ブロック図である。図１に示すように、制御装置１３０の周辺装置として、関節角度指令発生器１１０と、制御対象１２０と、を備える。
【００３０】
関節角度指令発生器１１０は、制御装置１３０から入力されるキー接触判定値とキー底到達判定値とに基づいて、指機構１２２がキー２０７を押す際の第１関節角度θ１、第２関節角度θ２、第３関節角度θ３の目標値である関節角度指令を、第１リンク２０１がキー２０７と接触する前、接触して押している間、及び押したキー２０７がキー底に到達した後に対して生成し出力する。ここで、キー接触判定値とは、キー２０７が第１リンク２０１と接触しているか否かを判定した結果である。キー底到達判定値とは、押されたキー２０７がキー底に到達したか否かを判定した結果である。
【００３１】
制御対象１２０は、一般的なロボットハンドである。制御対象１２０は、指のメカ機構である指機構１２２と、制御装置１３０から入力されるモータ電流に基づいて、第１関節２０２、第２関節２０４、第３関節２０６それぞれを駆動する関節モータ１２１と、第１関節角度θ１、第２関節角度θ２、第３関節角度θ３を検出し関節角度検出値として出力する関節角度検出器１２３と、を備える。
【００３２】
制御装置１３０は、キー接触時関節状態推定器１３１と、キー非接触時関節状態推定器１３２と、キー接触判定器１３３と、トルク外乱推定器１３４と、剛性推定器１３５と、キー底到達判定器１３６と、制御剛性演算器１３７と、制御器１３８と、を備える。
【００３３】
キー接触時関節状態推定器１３１は、制御対象１２０の関節角度検出器１２３から入力される関節角度検出値（第１関節角度θ１、第２関節角度θ２、第３関節角度θ３）等を用いて導出した、第１リンク２０１がキー２０７に接触している間における運動方程式に基づいて、関節角度、関節速度、関節加速度（即ち、関節の状態）を推定しキー接触時関節状態推定値として出力する。
【００３４】
キー非接触時関節状態推定器１３２は、制御対象１２０の関節角度検出器１２３から入力される関節角度検出値等を用いて導出した、第１リンク２０１がキー２０７に接触していない間における運動方程式に基づいて、関節角度、関節速度、関節加速度を推定しキー非接触時関節状態推定値として出力する。
【００３５】
キー接触判定器１３３は、制御対象１２０の関節角度検出器１２３から入力される関節角度検出値と、キー接触時関節状態推定器１３１から入力されるキー接触時関節状態推定値と、キー非接触時関節状態推定器１３２から入力されるキー非接触時関節状態推定値と、に基づいて、キー接触時における第１関節角度推定誤差θ１ｃ〜（キー接触時における一般化座標推定誤差ｑ１〜＝ｑ１−ｑ１＾の第１成分）と、キー非接触時における第１関節角度推定誤差θ１ｎｃ〜（キー非接触時における一般化座標推定誤差ｑ１〜＝ｑ１−ｑ１＾の第１成分）と、を算出する。キー接触判定器１３３は、キー接触時における第１関節角度推定誤差θ１ｃ〜の絶対値がキー非接触時における第１関節角度推定誤差θ１ｎｃ〜の絶対値より小さい場合（ａｂｓ（θ１ｃ〜）＜ａｂｓ（θ１ｎｃ〜））、第１リンク２０１はキー２０７に接触していると判定し、それ以外の場合は接触していないと判定し、その判定結果をキー接触判定結果として出力する。
【００３６】
トルク外乱推定器１３４は、キー接触判定器１３３から入力されるキー接触判定結果が、第１リンク２０１がキー２０７に接触しているとの判定結果のとき、キー接触時関節状態推定器１３１から入力されるキー接触時関節状態推定値に基づいて、第１関節２０２、第２関節２０４、第３関節２０６それぞれに掛かるトルク外乱を推定し、第１関節トルク外乱推定値Ｔｄ１＾、第２関節トルク外乱推定値Ｔｄ２＾、第３関節トルク外乱推定値Ｔｄ３＾として出力する。一方、トルク外乱推定器１３４は、キー接触判定器１３３から入力されるキー接触判定結果が、第１リンク２０１がキー２０７に接触していないとの判定結果のとき、何も出力しない。
【００３７】
剛性推定器１３５は、第１リンク２０１がキー２０７に接触している場合のみに、トルク外乱推定器１３４から入力されるトルク外乱推定値（第１関節トルク外乱推定値Ｔｄ１＾、第２関節トルク外乱推定値Ｔｄ２＾、第３関節トルク外乱推定値Ｔｄ３＾）と、キー接触時関節状態推定器１３１から入力されるキー接触時関節状態推定値と、に基づいて、被接触体の剛性（本実施形態では、ばね２０８のばね定数）を推定し、剛性推定値ｋ＾として出力する。
【００３８】
キー底到達判定器１３６は、接触判定器１３３から入力されるキー接触判定結果が、第１リンク２０１がキー２０７に接触しているとの判定結果のとき、キー接触時関節状態推定器１３１から入力されるキー接触時関節状態推定値の絶対値が発散したら、キー２０７がキー底に到達したと判定し、それ以外の場合はキー底に到達していないと判定し、その判定結果をキー底到達判定結果として出力する。
【００３９】
制御剛性演算器１３７は、関節角度検出器１２３から入力される関節角度検出値と、キー接触判定器１３３から入力されるキー接触判定結果と、剛性推定器１３５から入力される剛性推定値ｋ＾と、キー底到達判定器１３６から入力されるキー底到達判定結果と、に基づいて、第１リンク２０１がキー２０７に接触しているか否か、キー２０７に接触している場合はキー底に到達しているか否かを判定し、この判定結果に応じた、第１関節２０２、第２関節２０４、第３関節２０６それぞれの制御剛性を、剛性推定値ｋ＾と関節角度検出値とに基づいて演算し、制御剛性演算値として出力する。ここで、制御剛性とは、関節のメカ機構による剛性と、関節が備える関節モータや減速機等の剛性と、を加えた剛性のことである。
【００４０】
制御器１３８は、関節角度姿勢発生器１１０から入力される関節角度指令である、第１関節角度θ１、第２関節角度θ２、第３関節角度θ３の目標値と、制御剛性演算器１３７から入力される制御剛性演算値と、を実現するべく、第１関節角度θ１、第２関節角度θ２、第３関節角度θ３それぞれを制御する制御ゲインを設定し、その制御ゲインに基づいて算出されたモータ電流を出力する。
【００４１】
なお、制御装置１３０は、電気・電子・プログラマブル電子制御系として実現する。例えば、制御装置１３０は、演算処理等を行うＣＰＵ（Central Processing Unit）、ＣＰＵによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）等からなるマイクロコンピュータを中心にして、ハードウェア構成されている。また、ＡＳＩＣ（Application Specific Integrated Circuit）等のハードウェアにより、制御装置１３０の各器を実現しても良い。
【００４２】
次に、制御装置１３０を構成する各機能ブロックの詳細な動作原理を導出して説明する。
先ず、図２を用いて被接触体及び指機構１２２の運動方程式を導出する。図２より被接触体の運動方程式は［数７］と導出できる。
【００４３】
【数７】
【００４４】
ただし、記号の意味は以下の通りである。
ｙ０：図２の上方向を正としたばね２０８の自然長[ｍ]
ｙ１：第３関節２０６を基準とし、図２の上方向を正とした第１リンク２０１の先端（指先）の垂直位置（指先垂直位置）[ｍ]
ｆ：図２の上方向を正とした打鍵力[Ｎ]
ｍ：キー質量[ｋｇ]
ｇ：重力加速度[ｍ／ｓ２]
ｋ：ばね２０８の剛性[Ｎ／ｍ]
【００４５】
次に、指機構１２２の運動方程式を解析力学の手法に基づいて導出する。指機構１２２の運動エネルギーＫ及びポテンシャルエネルギーＵは、［数８］及び［数９］と導出できる。
【００４６】
【数８】
【００４７】
【数９】
【００４８】
ただし、記号の意味は以下の通りである。
ｍ１：第１関節２０２の質量（第１関節質量）[ｋｇ]
ｍ２：第２関節２０４の質量（第２関節質量）[ｋｇ]
ｍ３：第３関節２０６の質量（第３関節質量）[ｋｇ]
θ１：第１関節２０２の角度（第１関節角度）[ｒａｄ]
θ２：第２関節２０４の角度（第２関節角度）[ｒａｄ]
θ３：第３関節２０６の角度（第３関節角度）[ｒａｄ]
ｘ１０：第３関節２０６を基準とし、図２の左方向を正とした第１リンク２０１の重心の水平位置（第１リンク重心水平位置）[ｍ]
ｙ１０：第３関節２０６を基準とし、図２の上方向を正とした第１リンク２０１の重心の垂直位置（第１リンク重心垂直位置）[ｍ]
ｘ２０：第３関節２０６を基準とし、図２の左方向を正とした第２リンク２０３の重心の水平位置（第２リンク重心水平位置）[ｍ]
ｙ２０：第３関節２０６を基準とし、図２の上方向を正とした第２リンク２０３の重心の垂直位置（第２リンク重心垂直位置）[ｍ]
ｘ３０：第３関節２０６を基準とし、図２の左方向を正とした第３リンク２０５の重心の水平位置（第３リンク重心水平位置）[ｍ]
ｙ３０：第３関節２０６を基準とし、図２の上方向を正とした第３リンク２０５の重心の垂直位置（第３リンク重心垂直位置）[ｍ]
Ｊ１０：第１関節２０２の慣性モーメント[ｋｇ・ｍ２]
Ｊ２０：第２関節２０４の慣性モーメント[ｋｇ・ｍ２]
Ｊ３０：第３関節２０６の慣性モーメント[ｋｇ・ｍ２]
【００４９】
第１リンク重心水平位置ｘ１０、第１リンク重心垂直位置ｙ１０、第２リンク重心水平位置ｘ２０、第２リンク重心垂直位置ｙ２０、第３リンク重心水平位置ｘ３０、第３リンク重心垂直位置ｙ３０を、第１関節角度θ１、第２関節角度θ２、第３関節角度θ３を用いて書き換えると［数１０］から［数１５］と表される。
【００５０】
【数１０】
【００５１】
【数１１】
【００５２】
【数１２】
【００５３】
【数１３】
【００５４】
【数１４】
【００５５】
【数１５】
【００５６】
ただし、記号の意味は以下の通りである。
ｌ１０：第１リンク２０１の重心と第１関節２０２との距離[ｍ]
ｌ２０：第２リンク２０３の重心と第２関節２０４との距離[ｍ]
ｌ３０：第３リンク２０５の重心と第３関節２０６との距離[ｍ]
ｌ２：第２リンク２０３の長さ[ｍ]
ｌ３：第３リンク２０５の長さ[ｍ]
【００５７】
同様に、第１リンク２０１の指先垂直位置ｙ１を、第１関節角度θ１、第２関節角度θ２、第３関節角度θ３を用いて書き換えると［数１６］と表される。
【００５８】
【数１６】
ただし、ｌ１は第１リンク２０１の長さ[ｍ]である。
【００５９】
指機構１２２の運動方程式は、［数１７］から［数１９］のオイラーラグランジ方程式で表される。
【００６０】
【数１７】
【００６１】
【数１８】
【００６２】
【数１９】
【００６３】
ここで、Ｑ１、Ｑ２、Ｑ３は一般化力であり、指先がキー２０７に接触している場合、［数２０］から［数２２］で表される。
【００６４】
【数２０】
【００６５】
【数２１】
【００６６】
【数２２】
【００６７】
ここで、記号の意味は以下のようになる。
Ｔ１：第１関節２０２の関節モータ１２１が発生する反時計回りを正としたトルク（第１関節モータトルク）[Ｎ・ｍ]
Ｔ２：第２関節２０４の関節モータ１２１が発生する反時計回りを正としたトルク（第２関節モータトルク）[Ｎ・ｍ]
Ｔ３：第３関節２０６の関節モータ１２１が発生する反時計回りを正としたトルク（第３関節モータトルク）[Ｎ・ｍ]
Ｔｄ１：第１関節２０２に加わる反時計回りを正としたトルク外乱（第１関節トルク外乱）[Ｎ・ｍ]
Ｔｄ２：第２関節２０４に加わる反時計回りを正としたトルク外乱（第２関節トルク外乱）[Ｎ・ｍ]
Ｔｄ３：第３関節２０６に加わる反時計回りを正としたトルク外乱（第３関節トルク外乱）[Ｎ・ｍ]
【００６８】
［数８］から［数１６］及び［数２０］から［数２２］を、［数１７］から［数１９］に代入すると、指機構１２２の運動方程式は［数２３］から［数２５］と導出できる。
【００６９】
【数２３】
【００７０】
【数２４】
【００７１】
【数２５】
【００７２】
一方、指先がキー２０７に接触していない場合、一般化力Ｑ１は［数２０］に代えて、［数２６］と表される。
【００７３】
【数２６】
【００７４】
指先がキー２０７に接触していない場合、［数２６］により、［数２３］は［数２７］に置き換えられる。
【００７５】
【数２７】
【００７６】
キー接触時における指機構１２２の運動方程式は、［数２３］から［数２５］より［数２８］と書き換えられる。
【００７７】
【数２８】
【００７８】
ただし、記号の意味は以下のようになる。
Ｊ：慣性行列
ｆｃ：［数２３］から［数２５］の非線形項から構成されるベクトル（キー接触時非線形力）
ｑ：一般化座標
Ｔ：関節モータトルク
ｄ：トルク外乱
【００７９】
同様にキー非接触時の指機構１２２の運動方程式は、［数２４］、［数２５］、及び［数２７］より［数２９］と書き換えられる。
【００８０】
【数２９】
【００８１】
ただし、ｆｎｃは［数２４］、［数２５］、及び［数２７］の非線形項から構成されるベクトル（キー非接触時非線形力）である。
【００８２】
キー接触時関節状態推定器１３１は、［数３０］によりキー接触時における一般化座標ｑとその１階及び２階時間微分値（キー接触時関節状態）とを、ノイズを含まないように推定する。
【００８３】
【数３０】
【００８４】
ただし、記号の意味は以下のようになる。
Ｌ：推定器ゲイン
ｑ＾：一般化座標推定値
ｆｃ＾：キー接触時非線形力推定値
【００８５】
同様に、キー非接触時関節状態推定器１３２は、［数３１］によりキー非接触時における一般化座標ｑとその１階及び２階時間微分値（キー非接触時関節状態）とを、ノイズを含まないように推定する。
【００８６】
【数３１】
【００８７】
ただし、ｆｎｃ＾はキー非接触時非線形力推定値である。
【００８８】
キー接触時におけるキー接触時関節状態推定器１３１の推定誤差方程式は、［数２８］から［数３０］を減算することにより［数３２］と導出される。
【００８９】
【数３２】
【００９０】
ただし、ｑ~は一般化座標推定誤差である。
ここで、推定器ゲインＬの各要素が十分に大きな負の実数となるように設定すると、［数３３］が成立し、キー接触時関節状態が推定される。
【００９１】
【数３３】
【００９２】
一方、キー接触時におけるキー非接触時関節状態推定器１３２の推定誤差方程式は、［数２８］から［数３１］を減算することにより［数３４］と導出される。
【００９３】
【数３４】
【００９４】
キー接触時において、［数３２］の非線形項ｆｃ−ｆｃ＾の時間平均値は、［数３４］の非線形項ｆｃ−ｆｎｃ＾の時間平均値よりも常に小さい値をとるため、キー接触時関節状態推定器１３１の推定誤差方程式［数３２］より算出した一般化座標推定誤差ｑ〜の時間平均値の方が、キー非接触時関節状態推定器１３２の推定誤差方程式［数３４］より算出した値より小さい。
【００９５】
同様に、キー非接触時におけるキー非接触時関節状態推定器１３２の推定誤差方程式は、［数２９］から［数３１］を減算することにより［数３５］と導出される。
【００９６】
【数３５】
【００９７】
ここで、推定器ゲインＬの各要素が十分に大きな負の実数となるように設定すると、［数３３］が成立し、キー非接触時関節状態が推定される。
【００９８】
キー接触時の場合と同様の理由から、キー非接触時においては、キー非接触時関節状態推定器１３２の推定誤差方程式より算出した一般化座標推定誤差ｑ〜の時間平均値の方が、キー接触時関節状態推定器１３１の推定誤差方程式より算出した値より小さい。
【００９９】
上述の原理に基づいてキー接触判定器１３３は、キー接触時関節状態推定器１３１及びキー非接触時関節状態推定器１３２のうち、前者が出力する一般化座標推定誤差ｑ〜の時間平均が後者よりも小さい場合はキー接触時であると判定し、それ以外の場合はキー非接触時であると判定する。
【０１００】
キー接触判定器１３３がキー接触時であると判定した場合、一般化座標推定値ｑ＾は一般化座標ｑに収束するので、［数２８］よりトルク外乱推定器１３４は［数３６］を用いてトルク外乱推定値を算出できることが分かる。
【０１０１】
【数３６】
ただし、ｄ＾はトルク外乱推定値である。
【０１０２】
［数２３］とトルク外乱推定値ｄ＾を用いて剛性推定器１３５は、剛性推定値ｋ＾を［数３７］により算出する。
【０１０３】
【数３７】
【０１０４】
ただし、記号の意味は以下のようになる。
Ｔｄ１＾：第１関節２０２に加わる反時計回りを正としたトルク外乱の推定値（第１関節トルク外乱推定値）[Ｎ・ｍ]
θ１＾：第１関節角度推定値[ｒａｄ]
θ２＾：第２関節角度推定値[ｒａｄ]
θ３＾：第３関節角度推定値[ｒａｄ]
【０１０５】
キー底到達判定器１３６は、キー接触判定器１３３がキー接触時であると判定している間に、キー接触時関節状態推定器１３１が出力するキー接触時関節状態推定誤差の時間平均、及びキー非接触時関節状態推定器１３２が出力するキー非接触時関節状態推定誤差の時間平均の両方が発散することをもって、キー底に到達したと判定する。
【０１０６】
図２において、キー２０７からの反力と第１関節２０２、第２関節２０４、第３関節２０６の発生するモータトルクが静的に釣り合っている場合、各関節における制御剛性それぞれを第１関節等価制御剛性ｋ１０、第２関節等価制御剛性ｋ２０、第３関節等価制御剛性ｋ３０とすると、［数３８］から［数４０］が成立する。
【０１０７】
【数３８】
【０１０８】
【数３９】
【０１０９】
【数４０】
【０１１０】
ただし、記号の意味は次の通りである。
δｙ：キー２０７の垂直方向微小変位量[ｍ]
δθ１：第１関節角度θ１の微小変位量[ｒａｄ]
δθ２：第２関節角度θ２の微小変位量[ｒａｄ]
δθ３：第３関節角度θ３の微小変位量[ｒａｄ]
【０１１１】
［数３８］から［数４０］において、δｙとδθ１、δθ２、δθ３は微小量であり相殺するので、［数４１］から［数４３］が得られる。
【０１１２】
【数４１】
【０１１３】
【数４２】
【０１１４】
【数４３】
【０１１５】
制御剛性演算器１３７は、第１関節２０２の制御剛性である第１関節制御剛性ｋ１と、第２関節２０４の制御剛性である第２関節制御剛性ｋ２と、第３関節２０６の制御剛性である第３関節制御剛性ｋ３と、をそれぞれ［数４４］に基づいて決定する。
【０１１６】
【数４４】
【０１１７】
ただし、記号の意味は以下の通りである。
ｃ１：第１関節制御剛性ｋ１のパラメータ
ｃ２：第２関節制御剛性ｋ２のパラメータ
ｃ３：第３関節制御剛性ｋ３のパラメータ
ｋ１ｎ：非接触時の第１関節制御剛性
ｋ２ｎ：非接触時の第２関節制御剛性
ｋ３ｎ：キー底未到達時の第３関節制御剛性
【０１１８】
制御剛性演算器１３７は、［数４４］で決定した制御剛性となるように、制御器１３８の制御ゲインを設定する。これにより、指機構１２２がキー２０７を押すまでは、第１関節２０２、第２関節２０４、第３関節２０６それぞれに対する制御剛性が被接触体の剛性（ばね２０８のばね定数）ｋを第１関節２０２、第２関節２０４、第３関節２０６回りの回転剛性に等価変換した値より大きいため、第１リンク２０１の先端（指先）と、第１関節角度θ１、第２関節角度θ２、第３関節角度θ３を素早く正確にキー２０７を押すのに適した位置に移動させることができる。
【０１１９】
また、キー接触後は、第１関節２０２、第２関節２０４それぞれに対する制御剛性が被接触体の剛性ｋを第１関節２０２、第２関節２０４回りの回転剛性に等価変換した値と等しく、第３関節２０６に対する制御剛性が被接触体の剛性ｋを第３関節２０６回りの回転剛性に等価変換した値より大きいため、主に第３関節２０６のみの動作によりキー２０７を傷つけないように必要最小限の力でキー２０７を押すことができる。
【０１２０】
さらにキー底に到達したら、第１関節２０２、第２関節２０４、第３関節２０６それぞれに対する制御剛性が被接触体の剛性ｋを第１関節２０２、第２関節２０４、第３関節２０６回りの回転剛性に等価変換した値と等しく、キー２０７をキー底に保持するために必要最小限の力のみを残して第１関節２０２、第２関節２０４、第３関節２０６をリラックスさせることができる。
【０１２１】
キー接触時において、各関節における制御剛性を［数４４］とする第１関節モータトルクＴ１、第２関節モータトルクＴ２、第３関節モータトルクＴ３は、［数２３］から［数２５］より、［数４５］から［数４７］と導出できる。
【０１２２】
【数４５】
【０１２３】
【数４６】
【０１２４】
【数４７】
【０１２５】
ただし、記号の意味は以下の通りである。
ｄ１：第１関節２０２における制御減衰（第１関節制御減衰）
ｄ２：第２関節２０４における制御減衰（第２関節制御減衰）
ｄ３：第３関節２０６における制御減衰（第３関節制御減衰）
Ｔｄ２＾：第２関節に加わる反時計回りを正としたトルク外乱の推定値（第２関節トルク外乱推定値）[Ｎ・ｍ]
Ｔｄ３＾：第３関節に加わる反時計回りを正としたトルク外乱の推定値（第３関節トルク外乱推定値）[Ｎ・ｍ]
ここで、制御減衰とは、関節のメカ機構による減衰と、関節が備える関節モータや減速機等の減衰と、を加えた減衰のことである。
【０１２６】
一方、キー非接触時において、第２関節モータトルクＴ２、第３関節モータトルクＴ３は［数４６］、［数４７］と同じであり、第１関節モータトルクＴ１は［数２７］より［数４８］と表される。
【０１２７】
【数４８】
【０１２８】
上述のように、被接触体の剛性を推定するので、簡易な機構で硬さの異なる被接触体に柔軟に対応することができる。特に、本実施形態の制御装置を、キー操作等を行うロボットハンドに適用すると、サービスロボットがパソコンのキーボード操作する場合や、家電のキー操作をする場合に、キーを傷つけることなく早く正確にキーを押すことが可能となり、サービスロボットが楽器を演奏する場合は、鍵盤やボタンを傷つけることなく正確に早く洗練された演奏をすることが可能となる。
【０１２９】
このように、本実施形態によるとサービスロボットが家電のキー、楽器の鍵盤やボタンを傷つけることなく、早く正確に洗練されたキー操作や演奏をすることができる。
【０１３０】
＜シミュレーション結果＞
本シミュレーションは、サービスロボットのロボットハンドがパソコンのキー操作をする場合を模擬している。シミュレーションに用いた数値は以下のとおりである。
ｌ１＝２０×１０−３[ｍ]
ｌ２＝２０×１０−３[ｍ]
ｌ３＝２０×１０−３[ｍ]
ｌ１０＝１０×１０−３[ｍ]
ｌ２０＝１０×１０−３[ｍ]
ｌ３０＝１０×１０−３[ｍ]
ｍ１＝３０×１０−３[ｋｇ]
ｍ２＝３０×１０−３[ｋｇ]
ｍ３＝３０×１０−３[ｋｇ]
Ｊ１０＝１．４２×１０−６[ｋｇ・ｍ２]
Ｊ２０＝１．４２×１０−６[ｋｇ・ｍ２]
Ｊ３０＝１．４２×１０−６[ｋｇ・ｍ２]
ｍ＝１．９×１０−３[ｋｇ]
ｋ＝１４０[Ｎ／ｍ]
ｙ０＝３×１０−３[ｍ]
ｇ＝９．８[ｍ／ｓ２]
Ｔｄ１＝０[Ｎ・ｍ]
Ｔｄ２＝０[Ｎ・ｍ]
Ｔｄ３＝０[Ｎ・ｍ]
ｃ１＝３
ｃ２＝３
ｃ３＝３
ｄ１＝２×１０−１[Ｎ・ｍ・ｓ／ｒａｄ]
ｄ２＝６×１０−１[Ｎ・ｍ・ｓ／ｒａｄ]
ｄ３＝９×１０−１[Ｎ・ｍ・ｓ／ｒａｄ]
ｋ１ｎ＝１×１０５[Ｎ・ｍ／ｒａｄ]
ｋ２ｎ＝１×１０５[Ｎ・ｍ／ｒａｄ]
ｋ３ｎ＝１×１０５[Ｎ・ｍ／ｒａｄ]
【０１３１】
ただし、キー質量ｍ及びばね（弾性体）の剛性ｋは標準的なパソコンのキーボードの値とし、第１関節制御減衰ｄ１、第２関節制御減衰ｄ２、第３関節制御減衰ｄ３の値は、各関節より先端の部分の慣性モーメントの大きさに合わせて設定した。
【０１３２】
図３は本シミュレーションにおける第１関節角度の時間変化、図４は本シミュレーションにおける第２関節角度の時間変化、図５は本シミュレーションにおける第３関節角度の時間変化、図６は本シミュレーションにおける制御剛性の時間変化である。
【０１３３】
図３から図５において、破線は関節角度指令、実線は実際の関節角度を示す。また、図６において、実線は第１関節制御剛性ｋ１、破線は第２関節制御剛性ｋ２、一点鎖線は第３関節制御剛性ｋ３を示す。
【０１３４】
図３と図４の２．５[ｓ]において第１関節２０２の先端（指先）がキー２０７に接触し、キー接触判定器１３３が「キー接触した」という判定結果を関節角度指令発生器１１０に出力し、関節角度指令発生器１１０は第１関節角度指令θ１ｒ及び第２関節角度指令θ２ｒとを減速させている。第１関節角度指令θ１ｒや第２関節角度指令θ２ｒを、キー接触時の第１関節角度θ１や第２関節角度θ２より大きい角度に収束させるのは、１度キー接触した指先をキー２０７の表面に密着した状態に維持するためである。図６において、制御剛性演算器１３７が［数３８］を用いて算出する第１関節制御剛性ｋ１及び第２関節制御剛性ｋ２は、キー接触前は関節角度指令に追従するのに必要な制御帯域を確保するのに十分高い値に設定され、２．５[ｓ]でキー接触が判定されると、最小限の制御剛性に抑えられていることが分かる。
【０１３５】
図５の２．５[ｓ]においてキー接触が判定されると、第３関節角度指令は１度減速し、キー２０７を押し込む動作に移行する。５．５[ｓ]においてキー底に到達すると、キー２０７をそれ以上押し込むことが出来なくなるので、第３関節角度θ３は０．３１［ｒａｄ］で飽和している。この時、キー底到達判定器１３６は、上述のようにキー接触時関節状態推定値の絶対値が発散するので、キー底に到達したという判定結果を出力する。関節角度指令発生器１１０は、「キー底に到達した」という判定結果が入力されると、第３関節角度指令θ３ｒを減速させる。第３関節角度指令θ３ｒがキー底到達時の第３関節角度θ３より大きな値に収束しているのは、キー２０７をキー底にしっかり保持するためである。制御剛性演算器１３７は、キー底到達前において、第３関節角度θ３を第３関節角度指令θ３ｒに追従させるのに必要な制御帯域を確保するために、第３関節制御剛性ｋ３を十分に大きな値に設定し、５．５[ｓ]においてキー底に到達した以降において、キー２０７をキー底に保持するのに必要最小限の値に第３関節制御剛性ｋ３を設定していることが分かる。
【０１３６】
特許文献１のロボットハンドによると、剛性の低い指を使うと早い打鍵動作ができず、剛性の高い指を使うと早い打鍵動作はできるが、キー底到達後にキー２０７を押し込む力をキー剛性に合わせて適切に変化させることができず、キー２０７を傷つけてしまう問題がある。一方、本実施形態によると、上述のようにキー接触前、キー押し込み時、キー底到達後の３つの場合において、キー剛性に応じて適切に各関節の制御剛性を時間変化させるので、キー２０７を傷つけることなく早く正確なタイピングができる。
【０１３７】
また、ピアノなど鍵盤楽器の演奏に対して本発明を適用すると、上述のようにキー剛性に応じて適切に各関節の制御剛性を時間変化させることにより、鍵盤を傷つけることなく、正確に早い演奏が可能となり、繊細なトナリティーの表現が可能となる。
【０１３８】
このように、本実施形態によるとサービスロボットが家電のキー、楽器の鍵盤やボタンを傷つけることなく、早く正確に洗練されたキー操作や演奏をすることができる。
【０１３９】
以上、本発明に係るロボットハンドの制御装置の実施形態を説明したが、上記の構成に限らず、本発明の技術的思想を逸脱しない範囲で、変更することが可能である。
本実施形態の制御装置は、家電や楽器等のキーを操作するロボットハンドを制御するために適用したが、把持対象物を把持するロボットハンドを制御するために適用しても良い。この場合、キー底到達判定器１３６を省略しても良い。
本実施形態の指機構は、第１リンク２０１、第２リンク２０３、第３リンク２０５を備えた構成とされているが、リンクや関節の個数は限定されない。
本実施形態の被接触体の弾性体としてバネを用いたが、ゴム等の復元部材を用いても良い。
【符号の説明】
【０１４０】
１１０ 関節角度指令発生器
１２０ 制御対象
１２１ 関節モータ
１２２ 指機構
１２３ 関節角度検出器
１３０ 制御装置
１３１ キー接触時関節状態推定器
１３２ キー非接触時関節状態推定器
１３３ キー接触判定器
１３４ トルク外乱推定器
１３５ 剛性推定器
１３６ キー底到達判定器
１３７ 制御剛性演算器
１３８ 制御器
２０１ 第１リンク
２０２ 第１関節
２０３ 第２リンク
２０４ 第２関節
２０５ 第３リンク
２０６ 第３関節
２０７ キー
２０８ ばね
７０１ 第１指第１リンク
７０２ 第１指ＭＰ関節
７０３ 第１指第２リンク
７０４ 第１指ＩＰ関節
７０５ 第２指第１リンク
７０６ 第２指ＭＰ関節
７０７ 第２指第２リンク
７０８ 第２指ＰＩＰ関節
７０９ 第２指第３リンク
７１０ 第２指ＤＩＰ関節
７１１ 第３指第１リンク
７１２ 第３指ＭＰ関節
７１３ 第３指第２リンク
７１４ 第３指ＰＩＰ関節
７１５ 第３指第３リンク
７１６ 第３指ＤＩＰ関節
７１７ 第４指第１リンク
７１８ 第４指ＭＰ関節
７１９ 第４指第２リンク
７２０ 第４指ＰＩＰ関節
７２１ 第４指第３リンク
７２２ 第４指ＤＩＰ関節
７２３ ベース
【特許請求の範囲】
【請求項１】
リンクと、前記リンクの関節と、を有する指を備えたロボットハンドの制御装置において、
前記関節の角度検出値である関節角度検出値に基づいて、前記指が被接触体に接触した時における関節角度、関節速度、関節加速度である前記関節の状態を推定する接触時関節状態推定器と、
前記指が前記被接触体に接触していない時における前記関節の状態を推定する非接触時関節状態推定器と、
前記関節角度検出値と、前記接触時関節状態推定器が出力する接触時関節状態推定値と、前記非接触時関節状態推定器が出力する非接触時関節状態推定値と、に基づいて、前記指が前記被接触体に接触したか否かを判定する接触判定器と、
前記接触判定器が出力する判定結果が接触状態の場合、前記接触時関節状態推定値に基づいて、前記関節に印加されるトルク外乱を推定するトルク外乱推定器と、
前記接触判定結果が接触状態の場合、前記接触時関節状態推定値と、前記トルク外乱推定器が出力するトルク外乱推定値と、に基づいて、前記被接触体の剛性を推定する剛性推定器と、
前記接触判定結果が接触状態の場合、前記関節角度検出値と、前記剛性推定値と、に基づいて、前記関節の制御剛性を、前記指が前記被接触体の押し込み状態を維持できる最小値に演算し、
前記接触判定結果が非接触状態の場合、前記関節の制御剛性を、予め推定された前記指が前記被接触体に接触した状態である場合における前記関節の制御剛性より大きな値に演算する制御剛性演算器と、
前記制御剛性演算器が出力する制御剛性演算値に基づいて、前記関節を駆動する関節モータを制御する制御器と、
を備えるロボットハンドの制御装置。
【請求項２】
前記ロボットハンドの指は、第１関節、第２関節、第３関節を備え、
前記被接触体は、家電のキー又は楽器の鍵盤若しくはボタンと弾性体とを備え、
前記制御装置は、前記接触判定結果が接触状態の場合、前記接触時関節状態推定値に基づいて、前記被接触体が押し込み底に到達したか否かを判定する到達判定器をさらに備え、
前記制御剛性演算器は、前記接触判定結果が非接触状態の場合、前記剛性推定値を前記第１関節及び前記第２関節回りの回転剛性に等価変換した値よりも大きく、前記指の第１関節及び第２関節の制御剛性を演算し、
前記接触判定結果が接触状態の場合、前記剛性推定値を前記第１関節及び前記第２関節回りの回転剛性に等価変換した値と略等しい値に、前記第１関節及び第２関節の制御剛性を演算し、かつ前記到達判定器が出力する到達判定結果が前記押し込み底に非到達状態の場合、前記剛性推定値を前記第３関節回りの回転剛性に等価変換した値よりも大きな値に、前記第３関節の制御剛性を演算し、前記到達判定結果が前記押し込み底に到達状態の場合、前記剛性推定値を前記第３関節回りの回転剛性に等価変換した値と等しい値に、前記第３関節の制御剛性を演算する請求項１に記載のロボットハンドの制御装置。
【請求項３】
前記接触判定器は、前記関節角度から前記接触時関節状態推定値の成分である接触時関節角度推定値を減算した接触時関節角度推定誤差の絶対値の時間平均値が、前記関節角度から前記非接触時関節状態推定値の成分である非接触時関節角度推定値を減算した非接触時関節角度推定誤差の絶対値の時間平均値より小さい場合、接触状態であると判定し、それ以外の場合は非接触状態であると判定する請求項１又は２に記載のロボットハンドの制御装置。
【請求項４】
前記トルク外乱推定器は、前記トルク外乱推定値を［数４９］により算出する請求項１乃至３のいずれか１項に記載のロボットの制御装置。
【数４９】
ただし、記号の意味は以下の通りであり、ｄ＾、ｑ＾、ｆｃ＾、Ｔはベクトル、Ｊは行列である。
ｄ＾：トルク外乱推定値
Ｊ：慣性行列
ｆｃ＾：接触時の非線形力の推定値（接触時非線形力推定値）
ｑ＾：接触時の関節の状態の推定値（一般化座標推定値）
Ｔ：関節モータのトルク
【請求項５】
前記剛性推定器は、前記剛性推定値を［数５０］により算出する請求項２乃至４のいずれか１項に記載のロボットハンドの制御装置。
【数５０】
ただし、記号の意味は以下の通りである。
ｋ＾：被接触体の剛性推定値[Ｎ／ｍ]
ｍ１：第１関節の質量[ｋｇ]
Ｊ１０：第１関節の慣性モーメント[ｋｇ・ｍ２]
θ１：第１関節の角度[ｒａｄ]
θ２：第２関節の角度[ｒａｄ]
θ３：第３関節の角度[ｒａｄ]
ｍ：キー等の質量[ｋｇ]
ｇ：重力加速度[ｍ／ｓ２]、
ｌ１：第１リンクの長さ[ｍ]
ｌ２：第２リンクの長さ[ｍ]
ｌ３：第３リンクの長さ[ｍ]
ｌ１０：第１リンクの重心と第１関節との距離[ｍ]
ｌ２０：第２リンクの重心と第２関節との距離[ｍ]
ｌ３０：第３リンクの重心と第３関節との距離[ｍ]
Ｔ１：第１関節の関節モータが発生する反時計回りを正としたトルク（第１関節モータトルク）[Ｎ・ｍ]
Ｔｄ１＾：第１関節に加わる反時計回りを正としたトルク外乱の推定値（第１関節トルク外乱推定値）[Ｎ・ｍ]
ｙ０：被接触体を支持する弾性体の自然長
【請求項６】
前記到達判定器は、前記関節角度から前記接触時関節状態推定値の成分である接触時関節角度推定値を減算した接触時関節角度推定誤差の時間平均値と、前記関節角度から前記非接触時関節状態推定値の成分である非接触時関節角度推定値を減算した非接触時関節角度推定誤差の時間平均値との両方が発散した場合、前記指が前記被接触体の押し込み底に到達したと判定し、それ以外の時は到達していないと判定する請求項２乃至５のいずれか１項に記載のロボットハンドの制御装置。
【請求項７】
前記制御剛性演算器は、前記第１関節、第２関節及び第３関節の制御剛性演算値を［数５１］により演算する請求項２乃至６のいずれか１項に記載のロボットハンドの制御装置。
【数５１】
ただし、記号の意味は以下の通りである。
ｋ１：第１関節の制御剛性（第１関節制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ２：第２関節の制御剛性（第２関節制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ３：第３関節の制御剛性（第３関節制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ１ｎ：非接触時の第１関節制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ２ｎ：非接触時の第２関節制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ３ｎ：押し込み底未到達時の第３関節制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ１０：第１関節回りにおいて被接触体の剛性を等価変換した値（第１関節等価制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ２０：第２関節回りにおいて被接触体の剛性を等価変換した値（第２関節等価制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ３０：第３関節回りにおいて被接触体の剛性を等価変換した値（第３関節等価制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ＾：キー剛性推定値[Ｎ／ｍ]
ｋ：キー剛性
ｃ１：第１制御剛性のパラメータ
ｃ２：第２制御剛性のパラメータ
ｃ３：第３制御剛性のパラメータ
θ１：第１関節の角度検出値［ｒａｄ］
θ２：第２関節の角度検出値［ｒａｄ］
θ３：第３関節の角度検出値［ｒａｄ］
【請求項８】
前記第１関節を駆動する関節モータは、［数５２］で表される第１関節モータトルクＴ１を発生し、
前記第２関節を駆動する関節モータは、［数５３］で表される第２関節モータトルクＴ２を発生し、
前記第３関節を駆動する関節モータは、［数５４］で表される第３関節モータトルクＴ３を発生する請求項２乃至７のいずれか１項に記載のロボットハンドの制御装置。
【数５２】
【数５３】
【数５４】
ただし、記号の意味は以下の通りである。
ｋ１：第１関節の制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ２：第２関節の制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ３：第３関節の制御剛性[Ｎ・ｍ／ｒａｄ]
ｄ１：第１関節における制御減衰[Ｎ・ｍ・ｓ／ｒａｄ]
ｄ２：第２関節における制御減衰[Ｎ・ｍ・ｓ／ｒａｄ]
ｄ３：第３関節における制御減衰[Ｎ・ｍ・ｓ／ｒａｄ]
ｌ１０：第１リンクの重心と第１関節の距離[ｍ]
ｌ２０：第２リンクの重心と第２関節の距離[ｍ]
ｌ３０：第３リンクの重心と第３関節の距離[ｍ]
ｌ１：第１リンクの長さ[ｍ]
ｌ２：第２リンクの長さ[ｍ]
ｌ３：第３リンクの長さ[ｍ]
ｍ１：第１関節の質量[ｋｇ]
θ１：第１関節の角度[ｒａｄ]
θ１＾：第１関節の角度推定値[ｒａｄ]
ｍ２：第２関節の質量[ｋｇ]
θ２：第２関節の角度[ｒａｄ]
θ２＾：第２関節の角度推定値[ｒａｄ]
ｍ３：第３関節の質量[ｋｇ]
θ３：第３関節の角度[ｒａｄ]
θ３＾：第３関節の角度推定値[ｒａｄ]
ｍ：キー等の質量[ｋｇ]
ｇ：重力加速度[ｍ／ｓ２]
Ｔｄ１＾：第１関節に加わる反時計回りを正としたトルク外乱の推定値[Ｎ・ｍ]
Ｔｄ２＾：第２関節に加わる反時計回りを正としたトルク外乱の推定値[Ｎ・ｍ]
Ｔｄ３＾：第３関節に加わる反時計回りを正としたトルク外乱の推定値[Ｎ・ｍ]
θ１ｒ：第１関節の角度指令[ｒａｄ]
θ２ｒ：第２関節の角度指令[ｒａｄ]
θ３ｒ：第３関節の角度指令[ｒａｄ]
【請求項１】
リンクと、前記リンクの関節と、を有する指を備えたロボットハンドの制御装置において、
前記関節の角度検出値である関節角度検出値に基づいて、前記指が被接触体に接触した時における関節角度、関節速度、関節加速度である前記関節の状態を推定する接触時関節状態推定器と、
前記指が前記被接触体に接触していない時における前記関節の状態を推定する非接触時関節状態推定器と、
前記関節角度検出値と、前記接触時関節状態推定器が出力する接触時関節状態推定値と、前記非接触時関節状態推定器が出力する非接触時関節状態推定値と、に基づいて、前記指が前記被接触体に接触したか否かを判定する接触判定器と、
前記接触判定器が出力する判定結果が接触状態の場合、前記接触時関節状態推定値に基づいて、前記関節に印加されるトルク外乱を推定するトルク外乱推定器と、
前記接触判定結果が接触状態の場合、前記接触時関節状態推定値と、前記トルク外乱推定器が出力するトルク外乱推定値と、に基づいて、前記被接触体の剛性を推定する剛性推定器と、
前記接触判定結果が接触状態の場合、前記関節角度検出値と、前記剛性推定値と、に基づいて、前記関節の制御剛性を、前記指が前記被接触体の押し込み状態を維持できる最小値に演算し、
前記接触判定結果が非接触状態の場合、前記関節の制御剛性を、予め推定された前記指が前記被接触体に接触した状態である場合における前記関節の制御剛性より大きな値に演算する制御剛性演算器と、
前記制御剛性演算器が出力する制御剛性演算値に基づいて、前記関節を駆動する関節モータを制御する制御器と、
を備えるロボットハンドの制御装置。
【請求項２】
前記ロボットハンドの指は、第１関節、第２関節、第３関節を備え、
前記被接触体は、家電のキー又は楽器の鍵盤若しくはボタンと弾性体とを備え、
前記制御装置は、前記接触判定結果が接触状態の場合、前記接触時関節状態推定値に基づいて、前記被接触体が押し込み底に到達したか否かを判定する到達判定器をさらに備え、
前記制御剛性演算器は、前記接触判定結果が非接触状態の場合、前記剛性推定値を前記第１関節及び前記第２関節回りの回転剛性に等価変換した値よりも大きく、前記指の第１関節及び第２関節の制御剛性を演算し、
前記接触判定結果が接触状態の場合、前記剛性推定値を前記第１関節及び前記第２関節回りの回転剛性に等価変換した値と略等しい値に、前記第１関節及び第２関節の制御剛性を演算し、かつ前記到達判定器が出力する到達判定結果が前記押し込み底に非到達状態の場合、前記剛性推定値を前記第３関節回りの回転剛性に等価変換した値よりも大きな値に、前記第３関節の制御剛性を演算し、前記到達判定結果が前記押し込み底に到達状態の場合、前記剛性推定値を前記第３関節回りの回転剛性に等価変換した値と等しい値に、前記第３関節の制御剛性を演算する請求項１に記載のロボットハンドの制御装置。
【請求項３】
前記接触判定器は、前記関節角度から前記接触時関節状態推定値の成分である接触時関節角度推定値を減算した接触時関節角度推定誤差の絶対値の時間平均値が、前記関節角度から前記非接触時関節状態推定値の成分である非接触時関節角度推定値を減算した非接触時関節角度推定誤差の絶対値の時間平均値より小さい場合、接触状態であると判定し、それ以外の場合は非接触状態であると判定する請求項１又は２に記載のロボットハンドの制御装置。
【請求項４】
前記トルク外乱推定器は、前記トルク外乱推定値を［数４９］により算出する請求項１乃至３のいずれか１項に記載のロボットの制御装置。
【数４９】
ただし、記号の意味は以下の通りであり、ｄ＾、ｑ＾、ｆｃ＾、Ｔはベクトル、Ｊは行列である。
ｄ＾：トルク外乱推定値
Ｊ：慣性行列
ｆｃ＾：接触時の非線形力の推定値（接触時非線形力推定値）
ｑ＾：接触時の関節の状態の推定値（一般化座標推定値）
Ｔ：関節モータのトルク
【請求項５】
前記剛性推定器は、前記剛性推定値を［数５０］により算出する請求項２乃至４のいずれか１項に記載のロボットハンドの制御装置。
【数５０】
ただし、記号の意味は以下の通りである。
ｋ＾：被接触体の剛性推定値[Ｎ／ｍ]
ｍ１：第１関節の質量[ｋｇ]
Ｊ１０：第１関節の慣性モーメント[ｋｇ・ｍ２]
θ１：第１関節の角度[ｒａｄ]
θ２：第２関節の角度[ｒａｄ]
θ３：第３関節の角度[ｒａｄ]
ｍ：キー等の質量[ｋｇ]
ｇ：重力加速度[ｍ／ｓ２]、
ｌ１：第１リンクの長さ[ｍ]
ｌ２：第２リンクの長さ[ｍ]
ｌ３：第３リンクの長さ[ｍ]
ｌ１０：第１リンクの重心と第１関節との距離[ｍ]
ｌ２０：第２リンクの重心と第２関節との距離[ｍ]
ｌ３０：第３リンクの重心と第３関節との距離[ｍ]
Ｔ１：第１関節の関節モータが発生する反時計回りを正としたトルク（第１関節モータトルク）[Ｎ・ｍ]
Ｔｄ１＾：第１関節に加わる反時計回りを正としたトルク外乱の推定値（第１関節トルク外乱推定値）[Ｎ・ｍ]
ｙ０：被接触体を支持する弾性体の自然長
【請求項６】
前記到達判定器は、前記関節角度から前記接触時関節状態推定値の成分である接触時関節角度推定値を減算した接触時関節角度推定誤差の時間平均値と、前記関節角度から前記非接触時関節状態推定値の成分である非接触時関節角度推定値を減算した非接触時関節角度推定誤差の時間平均値との両方が発散した場合、前記指が前記被接触体の押し込み底に到達したと判定し、それ以外の時は到達していないと判定する請求項２乃至５のいずれか１項に記載のロボットハンドの制御装置。
【請求項７】
前記制御剛性演算器は、前記第１関節、第２関節及び第３関節の制御剛性演算値を［数５１］により演算する請求項２乃至６のいずれか１項に記載のロボットハンドの制御装置。
【数５１】
ただし、記号の意味は以下の通りである。
ｋ１：第１関節の制御剛性（第１関節制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ２：第２関節の制御剛性（第２関節制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ３：第３関節の制御剛性（第３関節制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ１ｎ：非接触時の第１関節制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ２ｎ：非接触時の第２関節制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ３ｎ：押し込み底未到達時の第３関節制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ１０：第１関節回りにおいて被接触体の剛性を等価変換した値（第１関節等価制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ２０：第２関節回りにおいて被接触体の剛性を等価変換した値（第２関節等価制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ３０：第３関節回りにおいて被接触体の剛性を等価変換した値（第３関節等価制御剛性）[Ｎ・ｍ／ｒａｄ]
ｋ＾：キー剛性推定値[Ｎ／ｍ]
ｋ：キー剛性
ｃ１：第１制御剛性のパラメータ
ｃ２：第２制御剛性のパラメータ
ｃ３：第３制御剛性のパラメータ
θ１：第１関節の角度検出値［ｒａｄ］
θ２：第２関節の角度検出値［ｒａｄ］
θ３：第３関節の角度検出値［ｒａｄ］
【請求項８】
前記第１関節を駆動する関節モータは、［数５２］で表される第１関節モータトルクＴ１を発生し、
前記第２関節を駆動する関節モータは、［数５３］で表される第２関節モータトルクＴ２を発生し、
前記第３関節を駆動する関節モータは、［数５４］で表される第３関節モータトルクＴ３を発生する請求項２乃至７のいずれか１項に記載のロボットハンドの制御装置。
【数５２】
【数５３】
【数５４】
ただし、記号の意味は以下の通りである。
ｋ１：第１関節の制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ２：第２関節の制御剛性[Ｎ・ｍ／ｒａｄ]
ｋ３：第３関節の制御剛性[Ｎ・ｍ／ｒａｄ]
ｄ１：第１関節における制御減衰[Ｎ・ｍ・ｓ／ｒａｄ]
ｄ２：第２関節における制御減衰[Ｎ・ｍ・ｓ／ｒａｄ]
ｄ３：第３関節における制御減衰[Ｎ・ｍ・ｓ／ｒａｄ]
ｌ１０：第１リンクの重心と第１関節の距離[ｍ]
ｌ２０：第２リンクの重心と第２関節の距離[ｍ]
ｌ３０：第３リンクの重心と第３関節の距離[ｍ]
ｌ１：第１リンクの長さ[ｍ]
ｌ２：第２リンクの長さ[ｍ]
ｌ３：第３リンクの長さ[ｍ]
ｍ１：第１関節の質量[ｋｇ]
θ１：第１関節の角度[ｒａｄ]
θ１＾：第１関節の角度推定値[ｒａｄ]
ｍ２：第２関節の質量[ｋｇ]
θ２：第２関節の角度[ｒａｄ]
θ２＾：第２関節の角度推定値[ｒａｄ]
ｍ３：第３関節の質量[ｋｇ]
θ３：第３関節の角度[ｒａｄ]
θ３＾：第３関節の角度推定値[ｒａｄ]
ｍ：キー等の質量[ｋｇ]
ｇ：重力加速度[ｍ／ｓ２]
Ｔｄ１＾：第１関節に加わる反時計回りを正としたトルク外乱の推定値[Ｎ・ｍ]
Ｔｄ２＾：第２関節に加わる反時計回りを正としたトルク外乱の推定値[Ｎ・ｍ]
Ｔｄ３＾：第３関節に加わる反時計回りを正としたトルク外乱の推定値[Ｎ・ｍ]
θ１ｒ：第１関節の角度指令[ｒａｄ]
θ２ｒ：第２関節の角度指令[ｒａｄ]
θ３ｒ：第３関節の角度指令[ｒａｄ]
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【公開番号】特開２０１２−１７９６７９（Ｐ２０１２−１７９６７９Ａ）
【公開日】平成２４年９月２０日（２０１２．９．２０）
【国際特許分類】
【出願番号】特願２０１１−４４０９１（Ｐ２０１１−４４０９１）
【出願日】平成２３年３月１日（２０１１．３．１）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２４年９月２０日（２０１２．９．２０）
【国際特許分類】
【出願日】平成２３年３月１日（２０１１．３．１）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]