JP7310963B2 - ロボットハンド、ロボットハンド制御方法及びプログラム - Google Patents

Description

本発明は、ロボットハンド、ロボットハンド制御方法、及びロボットハンド制御プログラムに関する。

ロボットハンドでワークなどの把持物体を把持して、把持物体を搬送したり、組付先の物体に組み付けたりする場合における、ロボットハンドの制御に関する技術が提案されている。

例えば、３指の多関節機構のロボットハンドとカメラとを用いて、物体の位置や角度などの状態を推定し、物体の状態に基づいてロボットハンドを制御する技術が提案されている（非特許文献１）。

また、指関節、指関節を駆動するアクチュエータ、及び指関節に支持され、アクチュエータの駆動力下で動作するリンクをそれぞれに有する複数の指機構と、複数の指機構のそれぞれのアクチュエータを、互いに独立して制御可能な動作制御部と、複数の指機構のそれぞれの指関節の作動位置を検出する位置検出部と、複数の指機構の各々に設けられ、指機構に加わる力によりリンクに生ずるひずみを検出するひずみ検出部とを備える把握型ハンドが提案されている。この把握型ハンドの動作制御部は、位置検出部が検出した指関節の作動位置と、ひずみ検出部が検出したリンクのひずみとに基づき、複数の指機構のアクチュエータを協調制御して、複数の指機構による把握力を調整する。

特開２００６－１０２９２０号公報

Yutaro Matsui, Yuji Yamakawa, and Masatoshi Ishikawa, " Cooperative Operation between a Human and a Robot based on Real-time Measurement of Location and Posture of Target Object by High-speed Vision", 2017 IEEE Conference on Control Technology and Applications (CCTA), August 27-30, 2017.

ロボットハンドで把持した把持物体を組付先物体に組み付ける従来の手法には、以下のような問題がある。なお、以下の図２４～図２７では、把持物体であるリングを、組付先物体であるシャフトに組み付ける場合を例に説明する。

例えば、図２４に示すように、２本指の産業用グリッパと手首力覚センサとを組み合わせた方式（以下、「第１の方式」という）では、グリッパ及びロボットの硬さ及び重さが把持物体にダイレクトに伝わるため、高速組付時の接触力が衝撃的となり、把持物体が破壊又は損傷するおそれがある。そのため、組付作業を高速化することができない。

第１の方式のグリッパ及びロボットの硬さの問題を回避できる方式として、図２５に示すように、産業用グリッパとリストコンプライアンサとを組み合わせた方式（以下、「第２の方式という」）がある。第１の方式よりは、組付作業の高速化を図れるが、グリッパの重さの問題が残るため限界がある。そもそも、把持物体と組付先物体との位置合わせは、把持物体が組付先物体から受ける接触力による受動的動作に頼るため、対応できる物体が限定される。例えば、図２５の例では、把持物体が組付先物体から受ける接触力の方向（図２５中の実線矢印）に対する、接触力による受動動作の方向（図２５中の破線矢印）は、位置合わせ動作の方向に対応していない。

第２の方式のグリッパの重さが残る問題、及び物体が限定される問題を回避できる方式として、図２６に示すように、柔軟要素付ハンドと視覚センサとを組み合わせた方式（以下、「第３の方式」という）がある。図２６の例では、ハンドの関節が柔軟要素である。第３の方式では、把持物体と組付先物体との相対的な位置姿勢関係を視覚センサで取得するため、オクルージョンの問題から組付可能な条件が限定される。例えば、図２６に示すように、組付作業領域がケース内にある場合など、ケース外の視覚センサからは、把持物体及び組付先物体が見えない場合がある。また、組付作業領域の上部の視覚センサでは、把持物体や組付先物体の角度が正確に取得できず、把持物体と組付先物体との相対的な位置姿勢関係を推定するに足る情報が得られない場合がある。

第３の方式のオクルージョン問題を回避できる方式として、柔軟要素付ハンドと指力覚センサとを組み合わせた方式（以下、「第４の方式」という）がある。この方式は、非特許文献１に記載の技術のように、指先が球形であることが多く、図２７に示すように、高速組付時の把持物体と組付先物体との接触力で、把持物体と指先との間にヒステリシス的な滑り、すなわち、接触力がゼロになっても滑った状態が元に戻らず、ハンドが元の把持姿勢に復帰しないような滑りが生じる。この現象は、把持物体と組付先物体との相対的な位置姿勢の推定結果を劣化させるため、組付そのものが困難となる。

本発明は、上記の点に鑑みてなされたものであり、把持物体と物体との衝撃を緩和しつつ、高速に作業を行うことができるロボットハンド、ロボットハンド制御方法及びプログラムを提供することを目的とする。

上記目的を達成するために、本発明に係るロボットハンドは、把持物体との接触部分に前記把持物体のズレ防止機構と、外力によって異方向に３自由度で可動する機構とを有するハンドと、前記ハンドに外力が加わったときに、前記外力が加わる前の力学的に平衡な状態からの前記ハンドの変位量を検出するセンサと、前記センサにより検出された変位量に基づいて、前記把持物体を物体に挿入する際の前記把持物体の位置及び姿勢の変位量を推定する推定部と、前記推定部により推定された前記把持物体の位置及び姿勢の変位量に基づいて、前記ハンドを制御する制御部と、を含んで構成されている。

本発明に係るロボットハンドによれば、把持物体との接触部分に把持物体のズレ防止機構と、外力によって異方向に３自由度で可動する機構とを有するハンドに外力が加わったときに、外力が加わる前の力学的に平衡な状態からのハンドの変位量をセンサが検出する。そして、推定部が、センサにより検出された変位量に基づいて、把持物体を物体に挿入する際の把持物体の位置及び姿勢の変位量を推定し、制御部が、推定部により推定された把持物体の位置及び姿勢の変位量に基づいて、ハンドを制御する。これにより、把持物体と先物体との衝撃を緩和しつつ、高速に作業を行うことができる。

また、前記推定部は、推定した前記把持物体の位置及び姿勢の変位量に基づいて、前記把持物体と前記物体との相対的な位置及び姿勢の誤差を推定し、前記制御部は、前記推定部により推定された前記把持物体の位置及び姿勢の変位量と、前記相対的な位置及び姿勢の誤差と、予め定めた動作計画とに基づいて、前記ハンドを制御することができる。これにより、例えば、把持物体と物体との相対的な位置及び姿勢の誤差をゼロにするようにハンドを制御するなど、簡易な制御を実現することができる。

また、前記推定部は、前記把持物体と前記物体とが、水平面であるｘ－ｙ平面における位置誤差が補償されることにより回転誤差も補償される関係である場合、前記把持物体の姿勢の変位量のｘ成分及びｙ成分から、ｘ－ｙ平面における前記把持物体と前記物体との相対的な位置の誤差を推定することができる。これにより、例えば、ｘ－ｙ平面における把持物体と先物体との相対的な位置及び姿勢の誤差をゼロにするようにハンドを制御するなど、簡易な制御を実現することができる。

また、前記制御部は、前記推定部により推定された前記把持物体の位置及び姿勢の変位量と、予め定めた動作計画とに基づいて、前記ハンドを制御することができる。これにより、例えば、把持物体の位置及び姿勢が所定の状態になるようにハンドを制御するなど、簡易な制御を実現することができる。

また、前記ズレ防止機構は、前記把持物体との接触部分を、前記把持物体の上部と接する面と前記把持物体の側面と接する面とを有する切り欠き形状とすることができる。

また、前記ハンドは、前記把持物体との接触部分に吸着パッドを有していてもよい。この場合、吸着パッドそのものを前記ズレ防止機構として用いてもよい。

また、前記ハンドを、指部全体を回転させるための回転機構を有する複数の指部を備えた挟持ハンドとしたり、関節の回転角度により指先部分の位置及び姿勢を制御可能な複数の指部としたりすることができる。

また、本発明に係るロボットハンド制御方法は、把持物体との接触部分に前記把持物体のズレ防止機構と、外力によって異方向に３自由度で可動する機構とを有するハンドと、前記ハンドに外力が加わったときに、前記外力が加わる前の状態からの前記ハンドの変位量を検出するセンサと、推定部と、制御部とを備えたロボットハンドにおいて、前記推定部が、前記センサにより検出された変位量に基づいて、前記把持物体を物体に挿入する際の前記把持物体の位置及び姿勢の変位量を推定し、前記制御部が、前記推定部により推定された前記把持物体の位置及び姿勢の変位量に基づいて、前記ハンドを制御する方法である。

また、本発明に係るロボットハンド制御プログラムは、コンピュータを、上記のロボットハンドを構成する推定部及び制御部として機能させるためのプログラムである。

本発明に係るロボットハンド、方法、及びプログラムによれば、ハンドに外力が加わったときの、力学的に平衡な状態からの変位量から推定される接触時の把持物体の位置及び姿勢の変位量に基づいて、ハンドを制御することにより、把持物体と物体との衝撃を緩和しつつ、高速に作業を行うことができる。

本実施形態に係るロボットハンドの構成を示す概略図である。 組付作業の一例を説明するための図である。 ハンドのズレ防止機構の一例を示す図である。 ハンドのズレ防止機構の他の例を示す図である。 コントローラのハードウェア構成を示すブロック図である。 コントローラの機能構成を示すブロック図である。 推定部による推定の概要を説明するための図である。 ｉ番目の指の機構図の一例である。 リングの位置変化量を説明するための図である。 ２点接触時の位置誤差方向とリングの姿勢変位方向とのベクトルの関係を示す図である。 相対的位置姿勢誤差を説明するための図である。 相対的位置姿勢誤差を説明するための図である。 ｅの推定値と真値との比較結果を示す図である。 ロボットハンド制御処理の流れを示すフローチャートである。 リング挿入動作計画の一例を示すシーケンス図である。 コネクタ挿入の組付動作を示す図である。 コネクタ挿入動作計画の一例を示すシーケンス図である。 コネクタ挿入の組付動作を説明するための図である。 ペグ挿入の組付動作を示す図である。 ハンドの他の構成例を示す図である。 ハンドの他の構成例を説明するための図である。 ハンドの他の構成例を示す図である。 ハンドの他の構成例を示す図である。 従来方式の問題点を説明するための図である。 従来方式の問題点を説明するための図である。 従来方式の問題点を説明するための図である。 従来方式の問題点を説明するための図である。

以下、本発明の実施形態の一例を、図面を参照しつつ説明する。なお、各図面において同一又は等価な構成要素及び部分には同一の参照符号を付与している。また、図面の寸法及び比率は、説明の都合上誇張されており、実際の比率とは異なる場合がある。

図１に示すように、本実施形態に係るロボットハンド１００は、アーム１０と、アーム１０の先端に取り付けられたハンド１２と、アーム１０及びハンド１２の関節の角度を検出する変位センサ１４と、アーム１０及びハンド１２の動作を制御するコントローラ１６とを含んで構成される。ロボットハンド１００は、ハンド１２で把持された把持物体２０を組付先物体２２に組み付ける動作を実行する。本実施形態では、図２に示すように、把持物体２０の一例であるリングを、組付先物体２２の一例であるシャフトに挿入する場合を例に説明する。なお、ロボットハンド１００は、本発明のロボットハンドの一例である。

アーム１０は、例えば、３次元空間における動作に必要な６自由度の構成を備えた垂直多関節型の機構である。なお、アーム１０の自由度は、６自由度に冗長自由度を加えた７自由度としてもよい。アーム１０は、コントローラ１６からの指示に応じて、ハンド１２の位置及び姿勢を制御する。

ハンド１２は、把持物体２０との接触部分に把持物体２０のズレ防止機構を有すると共に、外力によって異方向に３自由度で可動する機構を有する。本実施形態では、ハンド１２は、３指の多関節機構である。ハンド１２は、関節で連結された機構を有し、関節の角度を制御することで物体の把持を行う。

関節は、電動モータ等の関節駆動用アクチュエータと、マイコン等の関節駆動用アクチュエータ制御器とを備える。関節駆動用アクチュエータ制御器は、後述する変位センサ１４で検出された値に基づいて、ハンド１２の指先で物体の把持が行えるように関節駆動用アクチュエータのトルク値を決定する。関節駆動用アクチュエータは、関節駆動用アクチュエータ制御器で決定されたトルク値に基づいて、関節にトルクを与える。

また、関節は、組付時に発生する外力によってバックドライブ可能な高いバックドライバビリティを有し、関節駆動用アクチュエータなどによって力学的平衡点を作ることができる。バックドライバビリティとは、アクチュエータや動力伝達機構において、その出力節に外力を加えたときに、外力によってその出力節が動くことであり、高いバックドライバビリティとは、より小さな外力でも出力節が動くことである。また、力学的平衡点とは、力によって質点系の変位が時間的に変化する力学系において、力が一定な条件下で、時間的変化が起きずに静止状態となったときの変位の値である。

また、ハンド１２の指先は、把持物体２０との接触部分であり、ズレ防止機構として、図３に示すように、把持物体２０の上部と接する面Ｓ１と把持物体２０の側面と接する面Ｓ２とを有する切り欠き形状を備える。これにより、把持物体２０に対し上向き又は横向きの接触力が加わった際に、把持物体２０と指先との間にヒステリシス的な滑りが生じなくなる。結果、ハンド１２を構成する指の関節の力学的平衡点からの変位が、把持物体２０が外部（組付先物体２２）から受ける接触力由来であることが保証される。

なお、指先の形状は、図３に示すように、面Ｓ２が谷状の場合に限定されない。例えば、図４に示すように、面Ｓ２は椀状であってもよい。把持物体２０の形状を考慮して、把持安定性の高い形状を採用すればよい。

変位センサ１４は、例えば、エンコーダなど、関節の現在角度を検出可能なセンサである。本実施形態においては、変位センサ１４は、関節の力学的平衡点からの変位量、すなわち、把持物体２０に接触力がかかっていないときの角度を、接触力がかかっているときの角度から引いた値をコントローラ１６に出力する。なお、図１では、変位センサ１４を１か所のみ図示しているが、変位センサ１４は各関節に設けられている。

コントローラ１６は、コンピュータ等の情報処理装置により実現される。図５に、コントローラ１６のハードウェア構成を示す。図５に示すように、コントローラ１６は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）３２、メモリ３４、記憶装置３６、入力装置３８、出力装置４０、光ディスク駆動装置４２、及び通信インタフェース（Ｉ／Ｆ）４４を有する。各構成は、バスを介して相互に通信可能に接続されている。

記憶装置３６には、ロボットハンド制御処理を実行するためのロボットハンド制御プログラムが格納されている。ＣＰＵ３２は、中央演算処理ユニットであり、各種プログラムを実行したり、各構成を制御したりする。すなわち、ＣＰＵ３２は、記憶装置３６からプログラムを読み出し、メモリ３４を作業領域としてプログラムを実行する。ＣＰＵ３２は、記憶装置３６に記憶されているプログラムに従って、上記各構成の制御及び各種の演算処理を行う。

メモリ３４は、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）により構成され、作業領域として一時的にプログラム及びデータを記憶する。記憶装置３６は、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、及びＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）又はＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）により構成され、オペレーティングシステムを含む各種プログラム、及び各種データを格納する。

入力装置３８は、例えば、キーボードやマウス等の、各種の入力を行うための装置である。出力装置４０は、例えば、ディスプレイやプリンタ等の、各種の情報を出力するための装置である。出力装置４０として、タッチパネルディスプレイを採用することにより、入力装置３８として機能させてもよい。光ディスク駆動装置４２は、各種の記録媒体（ＣＤ－ＲＯＭ又はブルーレイディスクなど）に記憶されたデータの読み込みや、記録媒体に対するデータの書き込み等を行う。

通信Ｉ／Ｆ４４は、他の機器と通信するためのインタフェースであり、例えば、イーサネット（登録商標）、ＦＤＤＩ、Ｗｉ－Ｆｉ（登録商標）等の規格が用いられる。

次に、コントローラ１６の機能構成について説明する。図６に示すように、コントローラ１６は、機能構成として、準備部５０と、推定部５２と、制御部５４とを含む。各機能構成は、ＣＰＵ３２が記憶装置３６に記憶されたロボットハンド制御プログラムを読み出し、メモリ３４に展開して実行することにより実現される。

準備部５０は、組付動作前の準備を行う。具体的には、準備部５０は、組付動作に必要な情報の取得、動作計画（詳細は後述）の受け付け、ハンド１２による把持物体２０の把持、及び組付作業領域付近までのハンド１２を移動させるための制御等を行う。

推定部５２は、変位センサ１４により検出された変位量に基づいて、把持物体２０を組付先物体２２に組み付ける際に、把持物体２０が組付先物体２２から受ける接触力により生じる把持物体２０の位置及び姿勢の変位量（以下、「位置及姿勢変位量」という）を推定する。また、推定部５２は、推定した把持物体２０の位置姿勢変位量に基づいて、把持物体２０と組付先物体２２との相対的な位置及び姿勢の誤差（以下、「相対的位置姿勢誤差」という）を推定する。

図７を参照して、推定部５２による推定の概要を説明する。

推定部５２は、変位センサ１４で検出された、ハンド１２の関節の力学的平衡点からの変位量（Δｑ）に基づいて、把持物体２０が組付先物体２２から受ける接触力によって生じる把持物体２０の位置姿勢変位量（Δｐ，Δθ）を推定する。推定部５２は、推定した把持物体２０の位置姿勢変位量（Δｐ，Δθ）から、把持物体２０と組付先物体２２との相対的位置姿勢誤差（Δｅ）を推定する。Δｅの定義は、把持物体２０及び組付先物体２２と、把持物体２０を組付先物体２２へ組み付けるための動作計画によって異なる。

動作計画とは、組付などの作業を確実（かつ、場合によっては高速に）完了させるための、センサ値を用いた状態遷移である。各状態で実行する、センサ値に基づく状態の推定と、推定結果に基づくロボットの制御との組をスキルという。すなわち、動作計画は、動作生成のみを行う単なるモーションプラン以外のスキルの目標状態を達成するプロセスを示す。なお、動作計画は動作戦略とも呼ばれる。

把持物体２０であるリングを組付先物体２２であるシャフトに挿入する場合を例に、推定部５２による推定をより詳細に説明する。

ハンド１２の運動学に基づいて、各指の関節角度ｑ_ｉから、各指先の並進位置ｐ_ｉを算出する。図８は、ｉ番目の指の機構図の一例である。指先の並進位置ｐ_ｉは、関節角ｑ_ｉ＝［ｑ_ｉＩＰ，ｑ_ｉＭＰ，ｑ_ｉＴＭ］^Ｔ、指に関する順運動学をＦＫ_{ｆｉｎｇｅｒ}とすると、ｐ_ｉ＝ＦＫ_{ｆｉｎｇｅｒ}（ｑ_ｉ）により算出する。次に、各指先の無負荷時の並進位置を初期値ｐ_ｉ０とし、各指先の並進変位Δｐ_ｉを、Δｐ_ｉ＝ｐ_ｉ０－ｐ_ｉにより算出する。そして、下記（１）式に示すように、各指先のｐ_ｉを平均することによって、初期位置姿勢からのリングの位置変位量Δｐが求まる。

リングの位置変位量Δｐは、図９に示すように、シャフトからの接触力がないときのリング中心点（ｄ１）の位置と、シャフトからの接触力があるときのリング中心点（ｄ２）の位置との差（ΔＸ，ΔＹ，ΔＺ）である。

次に、下記（２）式に示すように、各指の変位の１次モーメントｐ_ｉ×Δｐ_ｉを平均することによって、初期位置姿勢からのリングの姿勢変位量Δθが求まる。

なお、角度に相当する量を得るためには、半径で割る必要があるが、シャフトとの相対位置誤差補償を制御ベースで行う場合、後述の通り角度の絶対精度は不要であるため、ここでは省略する。

リングのシャフトへの挿入では、ｘ－ｙ平面における位置誤差が補償されれば、リングとシャフトとの幾何学的関係から受動的に回転誤差も補償される。よって、リングとシャフトとの位置誤差方向ｅが推定できれば十分である。リングとシャフトとの２点接触における幾何学的拘束条件に基づいて、リングの姿勢変位量Δθからｅを算出する。図１０は、２点接触時の位置誤差方向のベクトルとリングの姿勢変位方向のベクトルとの関係を示す図である。図１０に示すＴｏｐ Ｖｉｅｗ（ｘ－ｙ平面）において、２つのベクトルが直交している。この直交関係を利用して、下記（３）式により、Δθからｅを推定する。

ｅは、図１１に示すように、シャフトの中心点（ｄ３）からリング中心点（ｄ４）へのベクトルであり、図１２に示すように、ｘ－ｙ平面における２次元の情報であるので、（３）式では次元を合わせるため、ｅ_ｚ＝０としている。

図１３に、推定部５２によるｅの推定値と真値との比較結果を示す。ロボット動作にフィードバックして組付作業の制御を行うには十分な推定精度であるといえる。上述したように、ハンド１２の指先にズレ防止機構を設けたことで、把持物体２０に対し上向き又は横向きの接触力が加わった際に、把持物体２０と指先との間にヒステリシス的な滑りが生じなくなり、ハンド１２を構成する指の関節の力学的平衡点からの変位が、把持物体２０が組付先物体２２から受ける接触力由来であることが保証されるため、上記のような推定を精度良く行うことができる。

制御部５４は、推定部５２により推定された把持物体２０の位置姿勢変位量と、把持物体２０と組付先物体２２との相対的位置姿勢誤差と、予め定めた動作計画とに基づいて、把持物体２０を組付先物体２２に組み付けるように、アーム１０及びハンド１２を制御する。具体的には、制御部５４は、位置姿勢変位量（Δｐ，Δθ）を制約条件に用いて、相対位置姿勢誤差（Δｅ）をゼロにするようにハンド１２を制御する。

次に、本実施形態に係るロボットハンド１００の作用について説明する。

図１４は、コントローラ１６のＣＰＵ３２により実行されるロボットハンド制御処理の流れを示すフローチャートである。ＣＰＵ３２が記憶装置３６からロボットハンド制御プログラムを読み出して、メモリ３４に展開して実行することにより、ＣＰＵ３２がコントローラ１６の各機能構成として機能し、図１４に示すロボットハンド制御処理が実行される。

まず、ステップＳ１２で、準備部５０が、把持物体２０及び組付先物体２２の種類、寸法パラメータ、位置等の情報を取得する。この情報は、予め入力されているＣＡＤデータから取得したり、ユーザにより入力された情報を取得したりすればよい。

次に、ステップＳ１４で、準備部５０が、把持物体２０の把持姿勢、及び把持物体２０を組付先物体２２に組み付けるための動作計画をユーザから受け付け、決定する。

次に、ステップＳ１６で、準備部５０が、ハンド１２を把持物体２０に接近させ、上記ステップＳ１４で決定した把持姿勢で把持物体２０をハンド１２で把持させる。

次に、ステップＳ１８で、準備部５０が、把持物体２０がハンド１２の指先以外と接触していない状態になる位置まで持ち上げる。すなわち、力学的平衡点を作る。

次に、ステップＳ２０で、準備部５０が、各関節の変位センサ１４から、各関節の角度ｑ_ｉ０を取得し、ｐ_ｉ０＝ＦＫｆｉｎｇｅｒ（ｑ_ｉ０）により、力学的平衡点を示す初期値ｐ_ｉ０算出し、所定の記憶領域に記憶する。

次に、ステップＳ２２で、準備部５０が、アーム１０及びハンド１２を制御して、上記ステップＳ１２で取得した組付先物体２２の位置に基づいて、ハンド１２で把持された把持物体２０を組付先物体２２に接近させる。

次に、ステップＳ３０で、推定部５２及び制御部５４により、上記ステップＳ１４で決定された動作計画が実行される。

図１５を参照して、リングをシャフトに挿入するリング挿入動作計画について説明する。リング挿入動作計画（Ｓ３０）が開始すると、状態判定により、リングのＺ位置が閾値Ｔｈ以下となったか否かが監視される（Ｓ３０２）。閾値Ｔｈは、リングがシャフトに挿入された状態のＺ位置の値として定めておくことができる。また、リング挿入動作が開始されると、リング軸合わせスキル（Ｓ３０４）が呼び出される。

リング軸合わせスキルでは、推定部５２による推定と、制御部５４によるハンド１２の制御が行われる。具体的には、推定部５２が、上記（１）式～（３）式により、シャフト中心から見たリング中心の方向ベクトル（ｅ）、及びシャフトからの接触力でリングが動く変位のＺ成分（ΔＺ）を推定する。そして、制御部５４が、Ｘ、Ｙ、Ｚの各方向に、以下のような速度で把持物体２０が移動するように、ハンド１２の各関節にトルクを与え、ハンド１２の動作を制御する。

Ｚ方向速度：＝目標速度－Ｇｚ・ΔＺ （Ｇｚ：定数ゲイン）
ＸＹ方向速度：＝Ｇｘｙ・（－ｅ） （Ｇｘｙ：定数ゲイン）

シャフト中心とリング中心とが一致すると、リングがシャフトに挿入され、状態判定でＺ≦Ｔｈと判定され、リング軸合わせスキルを終了すると共に、リング挿入動作計画が完了する。

以上説明したように、本実施形態に係るロボットハンドによれば、ハンドは、把持物体との接触部分に把持物体のズレ防止機構と、外力によって異方向に３自由度で可動する機構とを有する。また、ハンドは、外力が加わったときに、外力が加わる前の力学的に平衡な状態から変位する。すなわち、バックドライバビリティを有する。そして、センサがその変位量を検出し、検出された変位量に基づいて、把持物体の位置姿勢変位量及び把持物体と組付先物体との相対的位置姿勢誤差を推定し、推定した情報に基づいて、把持物体を組付先物体に組み付けるように、ハンドを制御する。

このように、ハンドのバックドライバビリティにより、把持物体と組付先物体との接触時の衝撃を緩和することができると共に、接触時に把持物体に変位を発生させることができる。また、指先にズレ防止機構を備えたことで、接触時のヒステリシス的なズレを防止でき、上記の変位量が、把持物体が組付先物体から受ける接触力由来のものであることが保証される。これにより、上記の変位量を用いて、組付作業を精度良く制御することができるため、把持物体と組付先物体との衝撃を緩和しつつ、クリアランスが小さい物体同士の組み付けであっても、高速に組付作業を行うことができる。

また、推定部による推定は、ロボット動作にフィーバックすることが前提であるので、図１３に示したような単調増加性さえあればよい。すなわち、推定の絶対精度は不要であるため、高価なセンサを設ける必要はない。また、推定の絶対精度が不要であることから、高速に推定することができ、例えば速度比例制御のような単純なフィードバック制御則で組付動作を実現することができる。このため、制御のための調整パラメータが減り、調整も簡単になる。

なお、上記実施形態では、把持物体がリング、組付先物体がシャフトで、リングをシャフトに挿入する組付動作について説明したが、本実施形態は、他の組付動作にも適用することができる。

例えば、図１６に示すように、オスコネクタをメスコネクタに挿入する組付動作に本実施形態を適用することができる。このような直方体の把持物体を３指ハンドで把持する場合、図１６に示すように、１つの指で長辺部分を、残り２つの指でその長辺に対向する角を把持すると安定すると共に、後述する動作計画を実現できる。

この場合の動作計画の一例を図１７に示す。図１７に示すように、コネクタ挿入動作計画（Ｓ３１）が開始されると、状態判定（Ｓ３１２）により、ΔＺが閾値Ｔｈｚ以上となったか否かが監視されると共に、Ｚ面アライメントスキル（Ｓ３１４）が呼び出される。

Ｚ面アライメントスキルでは、推定部５２が、上記（１）式により、外部からの接触力で把持物体２０が動く変位（ΔＸ，ΔＹ，ΔＺ）を推定する。そして、制御部５４が、Ｘ、Ｙ、Ｚの各方向に、以下のような速度で把持物体２０が移動するように、ハンド１２の各関節にトルクを与え、ハンド１２の動作を制御する。

Ｘ方向速度：＝－Ｇｘ・ΔＸ
Ｙ方向速度：＝－Ｇｙ・ΔＹ
Ｚ方向速度：＝目標速度－Ｇｚ・ΔＺ （Ｇｘ、Ｇｙ、Ｇｚ：定数ゲイン）

これは、図１８の上段に示すように、オスコネクタをＸＹ方向に意図的にずらした位置で、Ｚ方向に押し当てる動作である。ハンド１２の指先から図１８中の点Ａにかかる力がモーメントとなり、オスコネクタは線Ｂを回転軸としてメスコネクタの内側に入り込む形で微小に姿勢変位する。

ΔＺ≧閾値Ｔｈｚとなると、状態判定（Ｓ３１２）が完了し、次の状態判定（Ｓ３１６）が開始して、ΔＸが閾値Ｔｈｘ以上となったか否かが監視されると共に、Ｘ面アライメントスキル（Ｓ３１８）が呼び出される。

Ｘ面アライメントスキルでは、Ｚ面アライメントスキル（Ｓ３１４）と同様に、推定部５２が（ΔＸ，ΔＹ，ΔＺ）を推定する。そして、制御部５４が、以下のようにハンド１２の動作を制御する。

Ｘ方向速度：＝目標速度－Ｇｘ・ΔＸ
Ｙ方向速度：＝－Ｇｙ・ΔＹ
Ｚ方向速度：＝－Ｇｚ・ΔＺ

これは、図１８の中段に示すように、Ｚ方向への押し付けは継続しながら、Ｘ方向にずらし、Ｘ方向にある程度の接触力が検知されたら止める動作である。図１８の例では、線Ｃの部分が接触している。

ΔＸ≧閾値Ｔｈｘとなると、状態判定（Ｓ３１６）が完了し、次の状態判定（Ｓ３２０）が開始して、ΔＹが閾値Ｔｈｙ以上となったか否かが監視されると共に、Ｙ面アライメントスキル（Ｓ３２２）が呼び出される。

Ｙ面アライメントスキルでは、Ｚ面アライメントスキル（Ｓ３１４）と同様に、推定部５２が（ΔＸ，ΔＹ，ΔＺ）を推定する。そして、制御部５４が、以下のようにハンド１２の動作を制御する。

Ｘ方向速度：＝－Ｇｘ・ΔＸ
Ｙ方向速度：＝目標速度－Ｇｙ・ΔＹ
Ｚ方向速度：＝－Ｇｚ・ΔＺ

これは、図１８の下段に示すように、Ｚ方向及びＸ方向の押し付けは継続しながら、Ｙ方向にずらし、Ｙ方向にある程度の接触力が検知されたら止める動作である。図１８の例では、線Ｄの部分が接触している。

ΔＹ≧閾値Ｔｈｙとなると、状態判定（Ｓ３２０）が完了し、次の状態判定（Ｓ３２４）が開始して、ΔＺが閾値Ｔｈｚ１以上、かつＺが閾値Ｔｈｚ２以上となったか否かが監視されると共に、Ｚ面アライメントスキル（Ｓ３２６）が呼び出される。なお、Ｓ３２４の状態判定では、ΔＺの時系列波形の特徴から状態を判定してもよい。Ｚ面アライメントスキル（Ｓ３２６）は、Ｚ面アライメントスキル（Ｓ３１４）と同様である。これは、Ｘ方向及びＹ方向の押し付けは接触力が微小状態になるように継続しながら、Ｚ方向に押し入れ、Ｚ方向にある程度押し込んだら止める動作である。

また、他の例として、図１９に示すように、ペグを穴へ挿入する組付動作に本実施形態を適用することができる。このような円柱状の把持物体を３指ハンドで把持する場合、図１９に示すように、円周に沿って１２０°毎に指を配置するように把持すると安定すると共に、後述する動作計画を実現できる。

この場合の動作計画では、上記のコネクタ挿入動作計画のＺ面アライメントと同様のスキルにより、ペグをＺ方向に押し付ける。そして、Ｚ方向への押し付けは継続しながら、Ｘ方向及びＹ方向に単振動状の動きを生成し、ペグと穴との軸が一致するよう、単振動状の動きの中心として、推定したペグの位置変位量の平均を採用する。ペグの位置変位量は、リングのΔｐと同じ手法で推定することができる。

なお、コネクタ挿入及びペグ挿入の例では、把持物体２０と組付先物体２２との相対位置姿勢誤差は用いないため、推定部５２は、把持物体２０の位置変位量のみ推定すればよい。

また、上記実施形態では、ハンド１２を多関節多指型の構成とし、把持物体２０のズレ防止機構として、把持物体２０との接触部分であるハンド１２の指先の形状を図３又は図４に示すような形状とする場合について説明したが、これに限定されない。

例えば、図２０に示すように、把持物体との接触部分に把持物体のズレ防止機構と、外力によって異方向に３自由度で可動する機構を有するハンドとして、吸着パッドを用いる構成としてもよい。この場合、図２１に示すように、吸着パッドの変位量として、押込量と、ねじれ量×２の３自由度分の変位量を検出可能な変位センサ１４を設ければよい。

また、例えば、図２２に示すように、指先に変位センサ１４付きの吸着パッドを備えた挟持ハンドとしてもよい。この場合、挟持ハンドを構成する複数（図２２の例では２つ）の指部の各々は、指部全体を回転させるための回転機構を有することが望ましい。また、他の例として、図２３に示すように、多関節多指型のハンドの指先に、変位センサ１４付きの吸着パッドを備えてもよい。

いずれの構成においても、変位センサ１４で検出される変位量を用いて、上記実施形態と同様に推定及び制御を行うことができる。

また、上記実施形態でＣＰＵがソフトウェア（プログラム）を読み込んで実行したロボットハンド制御処理を、ＣＰＵ以外の各種のプロセッサが実行してもよい。この場合のプロセッサとしては、ＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）等の製造後に回路構成を変更可能なＰＬＤ（Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｄｅｖｉｃｅ）、及びＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）等の特定の処理を実行させるために専用に設計された回路構成を有するプロセッサである専用電気回路等が例示される。また、ロボットハンド制御処理を、これらの各種のプロセッサのうちの１つで実行してもよいし、同種又は異種の２つ以上のプロセッサの組み合わせ（例えば、複数のＦＰＧＡ、及びＣＰＵとＦＰＧＡとの組み合わせ等）で実行してもよい。また、これらの各種のプロセッサのハードウェア的な構造は、より具体的には、半導体素子等の回路素子を組み合わせた電気回路である。

また、上記各実施形態では、ロボットハンド制御プログラムが記憶装置に予め記憶（インストール）されている態様を説明したが、これに限定されない。プログラムは、ＣＤ－ＲＯＭ（Ｃｏｍｐａｃｔ Ｄｉｓｃ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＤＶＤ－ＲＯＭ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、及びＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）メモリ等の記録媒体に記録された形態で提供されてもよい。また、プログラムは、ネットワークを介して外部装置からダウンロードされる形態としてもよい。

１０ アーム
１２ ハンド
１４ 変位センサ
１６ コントローラ
２０ 把持物体
２２ 組付先物体
３２ ＣＰＵ
３４ メモリ
３６ 記憶装置
３８ 入力装置
４０ 出力装置
４２ 光ディスク駆動装置
４４ 通信Ｉ／Ｆ
５０ 準備部
５２ 推定部
５４ 制御部

Claims (10)

1. 把持物体との接触部分に前記把持物体のズレ防止機構と、外力によって異方向に３自由度で可動する機構とを有するハンドと、
   前記ハンドに外力が加わったときに、前記外力が加わる前の力学的に平衡な状態からの前記ハンドの変位量を検出するセンサと、
   前記センサにより検出された変位量に基づいて、前記把持物体を物体に挿入する際の前記把持物体の位置及び姿勢の変位量を推定する推定部と、
   前記推定部により推定された前記把持物体の位置及び姿勢の変位量に基づいて、前記ハンドを制御する制御部と、
   を含むロボットハンド。
2. 前記推定部は、推定した前記把持物体の位置及び姿勢の変位量に基づいて、前記把持物体と前記物体との相対的な位置及び姿勢の誤差を推定し、
   前記制御部は、前記推定部により推定された前記把持物体の位置及び姿勢の変位量と、前記相対的な位置及び姿勢の誤差と、予め定めた動作計画とに基づいて、前記ハンドを制御する
   請求項１に記載のロボットハンド。
3. 前記推定部は、前記把持物体と前記物体とが、水平面であるｘ－ｙ平面における位置誤差が補償されることにより回転誤差も補償される関係である場合、前記把持物体の姿勢の変位量のｘ成分及びｙ成分から、ｘ－ｙ平面における前記把持物体と前記物体との相対的な位置の誤差を推定する請求項２に記載のロボットハンド。
4. 前記制御部は、前記推定部により推定された前記把持物体の位置及び姿勢の変位量と、予め定めた動作計画とに基づいて、前記ハンドを制御する請求項１に記載のロボットハンド。
5. 前記ズレ防止機構は、前記把持物体との接触部分を、前記把持物体の上部と接する面と前記把持物体の側面と接する面とを有する切り欠き形状としたことである請求項１～請求項４のいずれか１項に記載のロボットハンド。
6. 前記ハンドは、前記把持物体との接触部分に吸着パッドを有する請求項１～請求項４のいずれか１項に記載のロボットハンド。
7. 前記ハンドを、指部全体を回転させるための回転機構を有する複数の指部を備えた挟持ハンドとした請求項１～請求項６のいずれか１項に記載のロボットハンド。
8. 前記ハンドを、関節の回転角度により指先部分の位置及び姿勢を制御可能な複数の指部とした請求項１～請求項６のいずれか１項に記載のロボットハンド。
9. 把持物体との接触部分に前記把持物体のズレ防止機構と、外力によって異方向に３自由度で可動する機構とを有するハンドと、前記ハンドに外力が加わったときに、前記外力が加わる前の状態からの前記ハンドの変位量を検出するセンサと、推定部と、制御部とを備えたロボットハンドにおいて、
   前記推定部が、前記センサにより検出された変位量に基づいて、前記把持物体を物体に挿入する際の前記把持物体の位置及び姿勢の変位量を推定し、
   前記制御部が、前記推定部により推定された前記把持物体の位置及び姿勢の変位量に基づいて、前記ハンドを制御する
   ロボットハンド制御方法。
10. コンピュータを、請求項１～請求項８のいずれか１項に記載のロボットハンドを構成する推定部及び制御部として機能させるためのロボットハンド制御プログラム。