(57)【要約】【課題】ロボットのアームでワークを保持する際、ワークの状態に関わらず保持を安定化する。【解決手段】ワークであるコネクタ７０を把持するための第１の把持指２４ａ、第２の把持指２４ｂの各々に第１のコンプライアンスモデル、第２のコンプライアンスモデルを設定する。さらに、第１の把持指２４ａと第２の把持指２４ｂとが、第３仮想バネ１２０ｃと第３仮想ダンパ１２２ｃを含む仮想スタビライザ１２６で連結されていると仮定し、第１の把持指２４ａと第２の把持指２４ｂの間に第３のコンプライアンスモデルを設定する。第１〜第３のコンプライアンスモデルから連立運動方程式を構築し、第１の把持指２４ａないし第２の把持指２４ｂの移動先の目標座標（目標位置）を求める。【選択図】図４
2015196208.tif 000002
【特許請求の範囲】
【請求項１】
互いに対向可能な第１の把持指及び第２の把持指でワークを保持するワーク保持方法において、
前記第１の把持指及び前記第２の把持指の各々に第１のコンプライアンスモデル、第２のコンプライアンスモデルを設定し、且つ前記第１の把持指と前記第２の把持指の間に第３のコンプライアンスモデルを設定し、前記第１のコンプライアンスモデル、前記第２のコンプライアンスモデル及び前記第３のコンプライアンスモデルに基づいて、前記第１の把持指又は前記第２の把持指の移動先の目標座標を変数として含む連立運動方程式を構築する工程と、
前記第３のコンプライアンスモデルに基づく拘束条件下で前記第１の把持指と前記第２の把持指が相互に連動する前記目標座標を算出する工程と、
前記第１の把持指又は前記第２の把持指を前記目標座標に移動させる工程と、
を有することを特徴とするワーク保持方法。
【請求項２】
請求項１記載の保持方法において、前記ワークはケーブルが接続されたコネクタであることを特徴とするワーク保持方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、互いに対向可能な第１の把持指及び第２の把持指でワークを保持するワーク保持方法に関する。
【背景技術】
【０００２】
例えば、自動車あるいはその関連部品等の生産ラインでは、多数のロボットが用いられている。この種の生産ラインにおける生産効率を向上させるためには、ロボットで作業可能であると考えられる作業の可及的大部分をロボットに行わせることが望ましい。例えば、ハンド部に２個の把持指を有するロボットを用い、電気機器のコネクタ挿入孔にコネクタを挿入する工程を自動化することが考えられる。例えば、特許文献１では、ロボットのアーム部の先端に設けられたハンド部でコネクタを把持し、コンプライアンス制御を用いてコネクタを固定部に挿入することが提案されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平４−１８９４９５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
特許文献１の第５図等に示されるように、コネクタは、２個の長辺部と短辺部を有し、一方の短辺部に端子部が設けられ、且つ他方の短辺部にケーブルが接続された長尺体であることが一般的である。このような形状のコネクタを２個の把持指で把持するときには、把持指の長手方向と、コネクタの２個の長辺部とが直交するような姿勢が望ましい。コネクタを安定的に保持して、前記端子部をコネクタ挿入孔に確実に挿入し得るようにするためである。
【０００５】
しかしながら、コネクタ（ワーク）の状態によっては、ケーブルの反力等の外乱によってコネクタに回転方向の力が作用することがある。このような事態が生じると、保持が不安定となる一因となる。
【０００６】
本発明は上記した問題を解決するためになされたもので、上記したようなワーク（例えば、コネクタ）の位置・姿勢のずれを吸収するコンプライアンス制御を実現しつつ、外力による望ましくない過剰な姿勢変化を抑制するワーク保持方法を提供することを目的とする。
【課題を解決するための手段】
【０００７】
前記の目的を達成するために、本発明は、互いに対向可能な第１の把持指及び第２の把持指でワークを保持するワーク保持方法において、
前記第１の把持指及び前記第２の把持指の各々に第１のコンプライアンスモデル、第２のコンプライアンスモデルを設定し、且つ前記第１の把持指と前記第２の把持指の間に第３のコンプライアンスモデルを設定し、前記第１のコンプライアンスモデル、前記第２のコンプライアンスモデル及び前記第３のコンプライアンスモデルに基づいて、前記第１の把持指又は前記第２の把持指の移動先の目標座標を変数として含む連立運動方程式を構築する工程と、
前記第３のコンプライアンスモデルに基づく拘束条件下で前記第１の把持指と前記第２の把持指が相互に連動する前記目標座標を算出する工程と、
前記第１の把持指又は前記第２の把持指を前記目標座標に移動させる工程と、
を有することを特徴とする。
【０００８】
すなわち、本発明においては、第１の把持指と第２の把持指とが仮想的な連結機構（例えば、仮想スタビライザ）によって連結されていると仮定し、両把持指の間に第３のコンプライアンスモデルを設定する。そして、第１〜第３のコンプライアンスモデルに基づいて構築される連立運動方程式を解くことにより、第１の把持指ないし第２の把持指の移動先の目標座標（目標位置）が求められる。
【０００９】
第１の把持指ないし第２の把持指は、この目標座標（目標位置）となるように移動される。その結果、ワークの状態に関わらず、該ワークが安定して保持されるようになる。
【００１０】
なお、ワークとしては、例えば、ケーブルが接続されたコネクタを挙げることができる。
【発明の効果】
【００１１】
本発明によれば、第１の把持指と第２の把持指との間に仮想的な連結機構（例えば、仮想スタビライザ）が存在しているとの仮定に基づき、両把持指の間に第３のコンプライアンスモデルを設定するようにしている。この場合、第１〜第３のコンプライアンスモデルに基づいて連立運動方程式が構築される。該連立運動方程式を解くことにより、第１の把持指ないし第２の把持指の移動先の目標座標（目標位置）が求められる。
【００１２】
第１の把持指ないし第２の把持指は、このようにして求められた目標座標（目標位置）となるように移動される。その結果、ワークを安定して保持することができるようになる。
【図面の簡単な説明】
【００１３】
【図１】本発明の実施の形態に係るコネクタ挿入装置の概略構成図である。
【図２】図１に示したコネクタ挿入装置におけるハンド部の斜視図である。
【図３】コネクタ挿入装置の制御系統を示すブロック図である。
【図４】前記ハンド部のコンプライアンスモデルを示すモデル図である。
【図５】図５Ａ〜図５Ｃは、第１の把持指と第２の把持指との間に仮想スタビライザに関するコンプライアンスモデルを構築していない従来技術において、コネクタにトルクが加わったときの第１の把持指及び第２の把持指の変位をフローとして示した要部概略斜視図である。
【図６】図６Ａ及び図６Ｂは、第１の把持指と第２の把持指との間に仮想スタビライザに関するコンプライアンスモデルを構築した実施の形態において、コネクタが回転することを抑制しながらコネクタを移動させるフローを示した要部概略斜視図である。
【図７】図７Ａ〜図７Ｄは、コネクタがコネクタ挿入孔に挿入されるまでの過程を示した要部斜視図である。
【図８】図８Ａ〜図８Ｃは、コネクタがコネクタ挿入孔に挿入されるまでの把持指の動作を示した要部平面図である。
【発明を実施するための形態】
【００１４】
以下、本発明に係るワーク保持方法につき、それを実施するための装置との関係で好適な実施の形態を挙げ、添付の図面を参照しながら詳細に説明する。なお、本実施の形態においては、コネクタをワークとして把持する場合を例示するものとする。
【００１５】
図１は、本実施の形態に係るワーク保持方法を実施するためのコネクタ挿入装置１０の概略構成図である。コネクタ挿入装置１０は、図７Ａ等に示す電気機器のコネクタ挿入孔７５にコネクタ７０を挿入するための装置である。このコネクタ挿入装置１０は、ロボット１２と、このロボット１２を制御する制御部１４とを備える。
【００１６】
ロボット１２は、複数の駆動軸を有する多関節ロボットである。具体的には、ロボット１２は、ベース台１６と、鉛直な駆動軸を中心としてベース台１６に回動可能に支持された旋回部１８と、旋回部１８に回動可能に支持された多関節アーム２０と、多関節アーム２０の先端に設けられたハンド部２２とを有する。
【００１７】
ロボット１２は、複数のアーム２０ａ〜２０ｃを有する多関節アーム２０の動作により、３次元空間上でハンド部２２の位置及び姿勢を変化させる。ロボット１２は、制御部１４の制御により、ロボット１２に設けられた各駆動軸が数値制御され、３次元空間上でのハンド部２２の位置及び姿勢が制御されるようになっている。
【００１８】
図２は、ハンド部２２の斜視図である。ハンド部２２は、コネクタ７０（図７Ａ参照）を把持可能な把持機構として構成される。ハンド部２２は、第１緩衝アーム２３ａ、第２緩衝アーム２３ｂの各先端に設けられた第１の把持指２４ａ、第２の把持指２４ｂと、第１の把持指２４ａ、第２の把持指２４ｂを移動させる２つの移動機構２６ａ、２６ｂと、２つの移動機構２６ａ、２６ｂが取り付けられたベース部２８とを有する。本実施の形態において、第１の把持指２４ａ、第２の把持指２４ｂは、平面状の把持面２５ａ、２５ｂを有する棒状の長尺体として構成され、互いに平行に配置される。
【００１９】
第１の把持指２４ａ、第２の把持指２４ｂの先端には、それぞれ、図２及び図３に示すように、第１指用力覚センサ２７ａ、第２指用力覚センサ２７ｂが設けられる。
【００２０】
第１指用力覚センサ２７ａ、第２指用力覚センサ２７ｂによる検出情報は、制御部１４を構成する目標位置算出部１０８に信号として送信される。なお、制御部１４は、前記目標位置算出部１０８の他、サーボモータ制御部１１０を含んで構成される。
【００２１】
第１の把持指２４ａ、第２の把持指２４ｂ（図２参照）は互いに対向可能である。第１の把持指２４ａ、第２の把持指２４ｂは、動作位置によって、それらの把持面２５ａ、２５ｂ同士が対向する位置となる場合（図２中のＸ方向、Ｙ方向及びＺ方向の位置が一致する場合）もあるが、把持面２５ａ、２５ｂ同士が対向する位置とならない場合もある。
【００２２】
第１の把持指２４ａを移動させるための移動機構２６ａは、ベース部２８に対してＹ方向（ベース部２８の延在方向）にスライド可能な第１移動部３０ａと、第１移動部３０ａに対してＺ方向にスライド可能な第２移動部３２ａと、第２移動部３２ａに対してＸ方向に移動可能な第３移動部３４ａとを有する。第１の把持指２４ａは、第１指用力覚センサ２７ａを介して、第３移動部３４ａの先端に取り付けられる。従って、第１の把持指２４ａは、Ｘ方向、Ｙ方向及びＺ方向の直交３軸の動作が可能である。
【００２３】
第１移動部３０ａは、第１駆動部３６の作用によりベース部２８に対してＹ方向にスライド可能である。第１駆動部３６は、サーボモータ４０ａと、ボールネジ及びナットからなるボールネジ機構３８ａとを有する。サーボモータ４０ａは、制御部１４を構成するサーボモータ制御部１１０（図３参照）により駆動制御され、これにより、第１の把持指２４ａのＹ方向の位置が制御される。
【００２４】
第２移動部３２ａは、第２駆動部４２の作用により第１移動部３０ａに対してＺ方向にスライド可能である。第２駆動部４２は、サーボモータ４４ａと、ボールネジ及びナットからなるボールネジ機構４６ａとを有する。サーボモータ４４ａもサーボモータ制御部１１０（図３参照）によって駆動制御され、これにより、第１の把持指２４ａのＺ方向の位置が制御される。
【００２５】
第３移動部３４ａは、第３駆動部４８の作用により第２移動部３２ａに対してＸ方向にスライド可能である。第３駆動部４８は、サーボモータ５０ａと、ボールネジ及びナットからなボールネジ機構５２ａとを有する。サーボモータ５０ａもサーボモータ制御部１１０（図３参照）によって駆動制御され、これにより、第１の把持指２４ａのＸ方向の位置が制御される。
【００２６】
第２の把持指２４ｂを移動させる移動機構２６ｂは、第１駆動部５４の作用によりベース部２８に対してＹ方向にスライド可能な第１移動部３０ｂと、第２駆動部５６の作用により第１移動部３０ｂに対してＺ方向にスライド可能な第２移動部３２ｂと、第３駆動部５８の作用により第２移動部３２ｂに対してＸ方向に移動可能な第３移動部３４ｂとを有する。第２の把持指２４ｂは、第２指用力覚センサ２７ｂを介して、第３移動部３４ｂの先端に取り付けられる。従って、他方の第２の把持指２４ｂは、Ｘ方向、Ｙ方向及びＺ方向の直交３軸の動作が可能である。
【００２７】
他方の移動機構２６ｂにおける第１移動部３０ｂ、第２移動部３２ｂ及び第３移動部３４ｂは、それぞれ、上述した一方の移動機構２６ａにおける第１移動部３０ａ、第２移動部３２ａ及び第３移動部３４ａと同様に構成される。
【００２８】
また、他方の移動機構２６ｂにおける第１駆動部５４、第２駆動部５６及び第３駆動部５８は、それぞれ、上述した一方の移動機構２６ａにおける第１駆動部３６、第２駆動部４２及び第３駆動部４８と同様に構成される。すなわち、第１駆動部５４は、サーボモータ４０ｂとボールネジ機構３８ｂを有する。また、第２駆動部５６はサーボモータ４４ｂとボールネジ機構４６ｂを有し、第３駆動部５８はサーボモータ５０ｂとボールネジ機構５２ｂを有する。サーボモータ４０ｂ、４４ｂ、５０ｂは、制御部１４を構成するサーボモータ制御部１１０（図３参照）によって駆動制御され、これにより、第２の把持指２４ｂのＸ方向、Ｚ方向の位置が制御される。
【００２９】
上記のように構成される各移動機構２６ａ、２６ｂにより、第１の把持指２４ａ、第２の把持指２４ｂを個別に移動させることができる。すなわち、制御部１４の制御作用下に、各移動機構２６ａ、２６ｂの作用下に第１の把持指２４ａ、第２の把持指２４ｂをＸ方向、Ｙ方向又はＺ方向に移動させることにより、ベース部２８に対する第１の把持指２４ａ、第２の把持指２４ｂの位置を個別に調整することが可能である。また、サーボモータ制御部１１０では、各エンコーダからの出力値を微分することで、第１の把持指２４ａ、第２の把持指２４ｂの速度を算出する。この値に基づき、速度制限を行うことができる。
【００３０】
ベース部２８には、ブラケット６０を介してカメラ６２が取り付けられる。カメラ６２は、第１の把持指２４ａ、第２の把持指２４ｂ側を指向しており、第１の把持指２４ａ、第２の把持指２４ｂ及びその周辺を撮像可能な視野を有する。カメラ６２により撮像される画像情報は、制御部１４に送信され、制御部１４においてパターンマッチングを行い、把持部により把持されたコネクタ７０の位置及び姿勢を認識することが可能である。
【００３１】
なお、本実施の形態におけるハンド部２２では、直交３軸の機構で構成された移動機構２６ａ、２６ｂにより第１の把持指２４ａ、第２の把持指２４ｂを個別に移動させる構成となっているが、ハンド部２２の構成はこれに限られない。例えば、ハンド部２２において、直線動作と回転動作が可能な移動機構を第１の把持指２４ａ、第２の把持指２４ｂ毎に設け、それらの移動機構により第１の把持指２４ａ、第２の把持指２４ｂを個別に移動させる構成としてもよい。
【００３２】
一方の移動機構２６ａにおける第１駆動部３６、第２駆動部４２及び第３駆動部４８は、それぞれボールネジ機構３８ａ、４６ａ、５２ａにより構成されるものに限られない。従って、第１駆動部３６、第２駆動部４２及び第３駆動部４８は、それぞれ、ボールネジ機構以外の形態のアクチュエータ、例えば、ラックアンドピニオン、シリンダ装置等により構成されてもよい。同様に、他方の移動機構２６ｂにおける第１駆動部５４、第２駆動部５６及び第３駆動部５８も、ボールネジ機構以外の形態のアクチュエータ、例えば、ラックアンドピニオン、シリンダ装置等により構成されてもよい。
【００３３】
図１に示す制御部１４は、ロボット１２の多関節アーム２０の先端位置を数値制御するだけでなく、コネクタ７０をコネクタ挿入孔７５に挿入する際に、ハンド部２２の動作をコンプライアンス制御することが可能である。すなわち、コネクタ挿入装置１０は、コンプライアンス制御を用いて、ハンド部２２により把持したコネクタ７０を、コネクタ挿入孔７５に挿入することができる。
【００３４】
図４は、ハンド部２２のコンプライアンスモデルを示すモデル図である。周知の通り、コンプライアンス制御は、バネ、ダンパ、慣性パラメータを仮想的に設定したモデルの運動をロボットの動作として実現するものである。
【００３５】
本実施の形態では、第１の把持指２４ａ、第２の把持指２４ｂの各々に、第１のコンプライアンスモデル、第２のコンプライアンスモデルが設定される。すなわち、第１の把持指２４ａに関する第１のコンプライアンスモデルは、第１仮想バネ１２０ａ及び第１仮想ダンパ１２２ａを含む。なお、図４には、当該コンプライアンスモデルにおいて、第１仮想バネ１２０ａの釣合位置及び第１仮想ダンパ１２２ａの基部に該当する移動物体を第１仮想台車１２４ａとして併せて示している。
【００３６】
一方、第２の把持指２４ｂに関する第２のコンプライアンスモデルは、第２仮想バネ１２０ｂ及び第２仮想ダンパ１２２ｂを含む。図４には、上記と同様に第２仮想バネ１２０ｂ及び第２仮想ダンパ１２２ｂの基部に該当する移動物体を第２仮想台車１２４ｂとして併せて示している。
【００３７】
第１仮想バネ１２０ａと第２仮想バネ１２０ｂ、第１仮想ダンパ１２２ａと第２仮想ダンパ１２２ｂは、それぞれ等価であるとする。すなわち、第１仮想バネ１２０ａ及び第２仮想バネ１２０ｂのバネ定数は等しく、第１仮想ダンパ１２２ａ及び第２仮想ダンパ１２２ｂの減衰係数は等しいとする。そこで、以下、第１仮想バネ１２０ａ及び第２仮想バネ１２０ｂのバネ定数をＫ、仮想質量をｍ、第１仮想ダンパ１２２ａ及び第２仮想ダンパ１２２ｂの減衰係数をＣとする。
【００３８】
第１の把持指２４ａのコンプライアンス原点（釣合位置）をＰblc1、移動先の目標位置（目標座標）をＰd1、目標加速度をａ1、目標速度をＶ1、第１仮想台車１２４ａの速度をＶblc1、第１の把持指２４ａに作用する外力をＦe1とするとき、第１の把持指２４ａに関する運動方程式は、下記の式（１）として構築される。
ｍａ1＝Ｆe1−Ｋ（Ｐd1−Ｐblc1）−Ｃ（Ｖ1−Ｖblc1）  …（１）
【００３９】
なお、外力Ｆe1は、第１指用力覚センサ２７ａによって検出される。また、コンプライアンス原点（釣合位置）Ｐblc1は、コンプライアンス制御がないとしたときの第１の把持指２４ａの目標位置であり、本実施の形態においては、第１の把持指２４ａの目標位置近傍に設定することができる。
【００４０】
さらに、目標加速度ａ1、目標速度Ｖ1、及び第１仮想台車１２４ａの速度Ｖblc1は、後述する通り、離散化によりコンプライアンス原点（釣合位置）であるＰblc1、移動先の目標位置（目標座標）Ｐd1、サンプル時刻ｔN、制御周期Ｔにより表すことができる。
【００４１】
一方、第２の把持指２４ｂのコンプライアンス原点（釣合位置）をＰblc2、移動先の目標位置（目標座標）をＰd2、目標加速度をａ2、目標速度をＶ2、第２仮想台車１２４ｂの速度をＶblc2、第２の把持指２４ｂに作用する外力をＦe2とするとき、第２の把持指２４ｂに関する運動方程式は、下記の式（２）として構築される。
ｍａ2＝Ｆe2−Ｋ（Ｐd2−Ｐblc2）−Ｃ（Ｖ2−Ｖblc2）  …（２）
【００４２】
上記と同様に、外力Ｆe2は第２指用力覚センサ２７ｂによって検出される。また、コンプライアンス原点（釣合位置）Ｐblc2は、は、コンプライアンス制御がないとしたときの第２の把持指２４ｂの目標位置であり、本実施の形態においては、第２の把持指２４ｂの目標位置近傍に設定することができる。
【００４３】
さらに、目標加速度ａ2、目標速度Ｖ2、及び第２仮想台車１２４ｂの速度Ｖblc2は、後述する通り、離散化によりコンプライアンス原点（釣合位置）であるＰblc2、移動先の目標位置（目標座標）Ｐd2、サンプル時刻ｔN、制御周期Ｔにより表すことができる。
【００４４】
ここで、本実施の形態においては、第１の把持指２４ａと第２の把持指２４ｂの間に第３のコンプライアンスモデルを設定する。第３のコンプライアンスモデルは、第３仮想バネ１２０ｃ及び第３仮想ダンパ１２２ｃを含む仮想スタビライザ１２６である。すなわち、この場合、第１の把持指２４ａと第２の把持指２４ｂとが、第３仮想バネ１２０ｃと第３仮想ダンパ１２２ｃを含む仮想スタビライザ１２６で連結されていると仮定する。第３仮想バネ１２０ｃのバネ定数はＫs、第３仮想ダンパ１２２ｃの減衰係数はＣsで表される。
【００４５】
そして、仮想スタビライザ１２６の釣合距離をＤsとするとき、第１の把持指２４ａの運動方程式は、式（１）から、Ｋs（Ｐd1−Ｐd2＋Ｄs）を差し引き、さらに、Ｃs（Ｖ1−Ｖ2）を差し引くことで補正される。同様に、第２の把持指２４ｂの運動方程式は、式（２）から、Ｋs（Ｐd2−Ｐd1−Ｄs）を差し引き、さらに、Ｃs（Ｖ2−Ｖ1）を差し引くことで補正される。すなわち、下記の連立方程式が成立する。
【数１】
2015196208.tif 000003
【００４６】
次に、目標加速度ａ1、ａ2、目標速度Ｖ1、Ｖ2、仮想台車の速度Ｖblcに対して、下記の式（３）〜（５）にて離散化を行う。
【数２】
2015196208.tif 000004
【００４７】
式（３）〜（５）中のｔNはサンプリング時刻、Ｔはサンプリング周期、Ｎはサンプリング回数である。サンプリング周期Ｔは、所望の制御性能に応じ、制御解析の技術知識に基づいて設定する。
【００４８】
このように、本実施の形態においては、第１の把持指２４ａ、第２の把持指２４ｂの各々にコンプライアンスモデル（第１のコンプライアンスモデル、第２のコンプライアンスモデル）を設定するのみならず、第１の把持指２４ａと第２の把持指２４ｂとの間にも、コンプライアンスモデル（第３のコンプライアンスモデル）を設定するようにしている。すなわち、第１の把持指２４ａと第２の把持指２４ｂの目標位置が相互に連動しあう拘束条件の下に算出される関係式を構築することで、仮想スタビライザ１２６が存在するとしている。これにより、第１の把持指２４ａ及び第２の把持指２４ｂによるコネクタ７０の把持が一層安定となる。
【００４９】
なお、図３に示すように、第１の把持指２４ａ、第２の把持指２４ｂのコンプライアンス原点（釣合位置）Ｐblc1、Ｐblc2、目標加速度ａ1、ａ2、目標速度Ｖ1、Ｖ2、第１仮想台車１２４ａ、第２仮想台車１２４ｂの速度Ｖblc1、Ｖblc2、第１の把持指２４ａ、第２の把持指２４ｂに作用する外力Ｆe1、Ｆe2の値は、目標位置算出部１０８に記憶される。
【００５０】
本実施の形態に係るコネクタ挿入装置１０は、基本的には以上のように構成されるものであり、以下、その作用及び効果について、本発明の実施の形態に係るコネクタ挿入方法との関係で説明する。
【００５１】
対象となるコネクタ７０は、図７Ａ等に示すように、コネクタ７０（オスコネクタ）の基端部にケーブル７２の一端が接続されたものであり、電子機器に設けられた接続部材７４（メスコネクタ）のコネクタ挿入孔７５（接続ポート）に当該コネクタ７０を挿入する。ケーブル７２は、図示しない他の電子機器から延出している。以下の説明において、コネクタ挿入装置１０の動作は、特に言及しない場合でも、制御部１４の制御作用下に行われるものとする。
【００５２】
コネクタ７０を接続部材７４のコネクタ挿入孔７５に挿入するために、先ず、制御部１４の制御作用下に、ロボット１２の多関節アーム２０を動作させて、カメラ６２がコネクタ７０を撮像できる位置（コネクタ認識可能位置）までハンド部２２を移動させる。そして、ハンド部２２がコネクタ認識可能位置まで移動したら、カメラ６２によりコネクタ７０を撮像するとともに、パターンマッチングを行うことによりコネクタ７０の位置及び姿勢を認識する。具体的には、制御部１４には、対象となるコネクタ７０の３次元形状モデル（３Ｄデータ）が記憶されており、撮像したコネクタ７０の画像情報と３Ｄデータとを照合することにより、コネクタ７０の位置及び姿勢を認識する。
【００５３】
コネクタ７０の位置及び姿勢を認識したら、制御部１４の制御作用下に、ハンド部２２を把持位置まで移動するとともに、各移動機構２６ａ、２６ｂを動作させて、図７Ａに示すように、第１の把持指２４ａ、第２の把持指２４ｂによりコネクタ７０を把持する（把持工程）。この場合、第１の把持指２４ａ、第２の把持指２４ｂを互いに対向するように位置させ、コネクタ７０の互いに反対側の側面７０ａ、７０ｂ（一方の側面７０ａと他方の側面７０ｂ）にそれぞれ第１の把持指２４ａ、第２の把持指２４ｂを当接させて、コネクタ７０を把持する。
【００５４】
なお、第１の把持指２４ａ、第２の把持指２４ｂによりコネクタ７０を把持した後、再びコネクタ７０の位置及び姿勢をパターンマッチングにより認識し、把持した際のコネクタ７０の位置ずれが発生していないかどうかを確認してもよい。
【００５５】
このとき、制御部１４は、目標位置算出部１０８に送られた情報、すなわち、コネクタ７０を把持するために、コネクタ７０の位置や形状に基づき算出される第１の把持指２４ａ、第２の把持指２４ｂのコンプライアンス原点（釣合位置）Ｐblc1、Ｐblc2、第１仮想台車１２４ａ、第２仮想台車１２４ｂの速度Ｖblc1、Ｖblc2、第１の把持指２４ａ、第２の把持指２４ｂに作用する外力Ｆe1、Ｆe2に基づき、上記の連立運動方程式を解く。
【００５６】
連立運動方程式において、未知数は、第１の把持指２４ａの、例えば、Ｘ軸に沿う方向の移動先の目標位置（目標座標）Ｐd1、第２の把持指２４ｂの移動先の目標位置（目標座標）Ｐd2の２個である。従って、連立運動方程式をＰd1、Ｐd2について離散化式に基づき解くことにより、各時刻でのＰd1、Ｐd2の値が順次求められる。
【００５７】
すなわち、目標位置算出部１０８により、離散化式による逐次演算の複数回サンプリングを行い、各サンプル時刻での第１の把持指２４ａ、第２の把持指２４ｂを如何なる程度移動させるべきかの情報が得られる。
【００５８】
目標位置算出部１０８は、この情報を、サーボモータ制御部１１０に送る。該サーボモータ制御部１１０は、第１の把持指２４ａ又は第２の把持指２４ｂの移動先が各サンプル時刻毎のＰd1、Ｐd2となるように、例えば、サーボモータ４４ａ又はサーボモータ４４ｂを駆動させる。その結果、第１の把持指２４ａ又は第２の把持指２４ｂがコネクタ７０を把持する位置まで移動する。
【００５９】
以上は、Ｚ軸に関してのコンプライアンス制御を例示しているが、Ｘ軸及びＹ軸に対しても同様に、コンプライアンス制御が営まれる。各軸のコンプライアンス制御は、同時に行われる。
【００６０】
このように、第１〜第３のコンプライアンスモデルを設定することにより、コネクタ７０を、第１の把持指２４ａと第２の把持指２４ｂが連動しあう条件下に把持することができる。
【００６１】
図５Ａ〜図５Ｃには、第１の把持指２４ａ、第２の把持指２４ｂの各々にのみコンプライアンスモデルを構築した従来技術において、把持しているコネクタ７０を、左方に平行移動させる際のフローを示している。すなわち、先ず、コネクタ７０を把持した第１の把持指２４ａ、第２の把持指２４ｂは、図５Ａ中の矢印Ｌに示すように、左方に平行移動しようとする。
【００６２】
この場合、ケーブル７２による反力が生じる。その結果、図５Ｂに示すように、コネクタ７０に、矢印Ｍに示す方向の回転モーメントが生じる。このため、第１の把持指２４ａ、第２の把持指２４ｂに、矢印Ｎ１、Ｎ２方向の力が作用する。モーメントが過剰な場合、コネクタ７０が回転することに倣って第１の把持指２４ａ、第２の把持指２４ｂが大きく変位する。
【００６３】
その結果、図５Ｃに示すように、第１の把持指２４ａ、第２の把持指２４ｂの相対距離が大きくなる。このため、コネクタ７０の保持が不安定となる。
【００６４】
図６Ａ及び図６Ｂには、本実施の形態に係るワーク保持方法を採用したコネクタ挿入装置１０のハンド部２２によって、把持しているコネクタ７０を、左方に平行移動させる際のフローを示している。
【００６５】
ここで、本実施の形態では、第１の把持指２４ａと第２の把持指２４ｂが仮想スタビライザ１２６で転結されたと仮定して、第１の把持指２４ａと第２の把持指２４ｂの間にも第３のコンプライアンスモデルを構築するようにしている。このため、図６Ａに示すようにコネクタ７０を把持した場合、第１の把持指２４ａ、第２の把持指２４ｂに対し、その変位を抑制しようとする力が作用する。
【００６６】
従って、コネクタ７０が回転することが防止される。このため、図６Ｂに示すように、第１の把持指２４ａ、第２の把持指２４ｂの相対距離が小さくなり、この状態が保たれる。その結果、コネクタ７０が安定して第１の把持指２４ａ、第２の把持指２４ｂに保持される。すなわち、保持が安定化し、且つ第１の把持指２４ａに関するコンプライアンス制御、及び第２の把持指２４ｂに関するコンプライアンス制御とのバランスを採ることができる。
【００６７】
第１の把持指２４ａ、第２の把持指２４ｂによりコネクタ７０を把持した後、次に、カメラ６２により撮像している画像情報から、制御部１４に記憶されている、接続部材７４の開口側部分７６の３次元形状モデル（３Ｄデータ）を用いてパターンマッチングを行い、接続部材７４の開口側部分７６の位置及び姿勢を認識する。そして、コネクタ７０の位置及び姿勢と、接続部材７４の開口側部分７６の位置及び姿勢とから、コネクタ７０と接続部材７４（コネクタ挿入孔７５）との相対位置を算出する。
【００６８】
次に、算出された相対位置に基づき、制御部１４の制御作用下に、コネクタ７０の先端面７１が接続部材７４のコネクタ挿入孔７５に対向する位置に来るように、コネクタ７０を把持したハンド部２２の位置及び姿勢を変化させる。これにより、図７Ｂに示すように、接続部材７４のコネクタ挿入孔７５に対向する位置にコネクタ７０を配置する。すなわち、コネクタ７０の先端面７１を、接続部材７４のコネクタ挿入孔７５の真正面に位置させる。
【００６９】
次に、制御部１４の制御作用下に、図７Ｃに示すように、コネクタ７０を把持した第１の把持指２４ａ、第２の把持指２４ｂを接続部材７４側に向かって移動させることにより、コネクタ７０をコネクタ挿入孔７５の途中位置まで挿入する（挿入工程）。すなわち、ここでは、先ず、コネクタ７０の先端部７０ｃだけをコネクタ挿入孔７５の入口部分に挿入する。
【００７０】
このとき、制御部１４は、目標位置算出部１０８において、コネクタ７０の先端部７０ｃのみをコネクタ挿入孔７５の入口部分に挿入するべく、コネクタ挿入孔７５の位置から算出される第１の把持指２４ａ、第２の把持指２４ｂが向かうべき位置である新たなコンプライアンス原点（釣合位置）Ｐblc1、Ｐblc2を用いて、前述の把持工程と同様に第１〜第３のコンプライアンスモデルを設定し、コンプライアンス制御による目標位置算出を行う。
【００７１】
次に、先端部７０ｃがコネクタ挿入孔７５に挿入されたコネクタ７０を一方の第１の把持指２４ａにより押圧して、コネクタ７０をコネクタ挿入孔７５の内側面７８（一方の第１の把持指２４ａとは反対側の内側面７８）に押し付ける（押付工程）。これにより、コネクタ７０と当該内側面７８との間の摩擦力が大きくなり、先端部７０ｃだけがコネクタ挿入孔７５に挿入されたコネクタ７０が、一時的に接続部材７４に対して固定される。
【００７２】
この工程においても、押し付けに適したコンプライアンス原点（釣合位置）Ｐblc1、Ｐblc2を用いることにより、同様のコンプライアンス制御を行うことができる。
【００７３】
次に、図７Ｄに示すように、コネクタ７０をコネクタ挿入孔７５の内側面７８に押し当てた状態を維持しつつ、他方の第２の把持指２４ｂをコネクタ７０から離間させる。図８Ａは、図７Ｄにおいて、コネクタ７０及び接続部材７４を矢印Ａの方向から見た概略図である。このとき、一方の第１の把持指２４ａによりコネクタ７０が内側面７８に押し付けられてコネクタ７０が固定された状態となっていることから、他方の第２の把持指２４ｂをコネクタ７０から離間させても、ケーブル７２による拘束力に起因してコネクタ７０が接続部材７４から外れる（抜け出る）ことはない。すなわち、ケーブル７２がコネクタ７０を基端方向に引っ張るように作用しても、一方の第１の把持指２４ａでコネクタ７０を内側面７８に押し付けることによる固定力の方が、ケーブル７２による引っ張り力よりも大きいため、コネクタ７０が接続部材７４から外れることが防止される。
【００７４】
次に、図８Ａに示すように、コネクタ７０をコネクタ挿入孔７５の内側面７８に押し当てた状態で、他方の第２の把持指２４ｂを、コネクタ７０の基端部側（コネクタ挿入孔７５に挿入される側に対して反対側）に移動させる（移動工程）。これにより、他方の第２の把持指２４ｂをコネクタ７０の基端部（基端面７０ｄ）に当接させる。なお、他方の第２の把持指２４ｂをコネクタ７０から離間させる動作と、他方の第２の把持指２４ｂをコネクタ７０の基端部側に移動させる動作とを、連続した一つの動作としてもよい。
【００７５】
この工程においても、移動に適したコンプライアンス原点（釣合位置）Ｐblc1、Ｐblc2を用いることにより、同様のコンプライアンス制御を行うことができる。なお、この工程以降は、第１の把持指２４ａ、第２の把持指２４ｂ毎に独立した動作となるため、第１の把持指２４ａ、第２の把持指２４ｂが連動しないように仮想スタビライザ１２６の機能を停止させる。具体的には、第３仮想バネ１２０ｃのバネ定数Ｋｓと、第３仮想ダンパ１２２ｃの減衰係数Ｃｓに０を代入する。
【００７６】
次に、図８Ｃに示すように、他方の第２の把持指２４ｂによりコネクタ７０を押して、コネクタ７０をコネクタ挿入孔７５にさらに押し込む（押込工程）。これにより、コネクタ７０をコネクタ挿入孔７５の規定の挿入深さまで挿入する。なお、他方の第２の把持指２４ｂをコネクタ７０の基端部に当接させた後には、一方の第１の把持指２４ａによるコネクタ７０に対する押圧力を減少させてもよく、あるいは一方の第１の把持指２４ａをコネクタ７０から離間させてもよい。規定の挿入深さまで挿入されたコネクタ７０は、図示しないロック部（例えば、爪部等）の作用により接続部材７４に対して固定され、装着状態となる。
【００７７】
この工程においても、押し込みに適したコンプライアンス原点（釣合位置）Ｐblc1、Ｐblc2を用いることにより、同様のコンプライアンス制御を行うことができる。
【００７８】
以上説明したように、本実施の形態に係るコネクタ挿入方法及びコネクタ挿入装置１０によれば、コネクタ挿入孔７５にコネクタ７０を途中まで挿入した状態で、一方の第１の把持指２４ａによりコネクタ７０をコネクタ挿入孔７５の内側面７８に押し付けることにより、コネクタ７０が一時的に固定される。このため、コネクタ７０に接続されたケーブル７２の拘束力によって、コネクタ挿入孔７５とは反対方向にコネクタ７０を引っ張る力が作用した場合でも、コネクタ７０がコネクタ挿入孔７５から抜け出ることがない。そして、他方の第２の把持指２４ｂによりコネクタ挿入孔７５とは反対側からコネクタ７０を押して、コネクタ７０を押し込むため、コネクタ７０をコネクタ挿入孔７５の所定位置まで確実に挿入することができる。
【００７９】
その際に、第１〜第３のコンプライアンスモデルを設定したコンプライアンス制御を行うことにより、コネクタ７０の姿勢を、第１の把持指２４ａと第２の把持指２４ｂが連動しあう条件下に把持することができる。このため、外力による第１の把持指２４ａ、第２の把持指２４ｂの相対変位が抑制される。その結果、コネクタ７０を安定して把持することが可能となり、また、コネクタ挿入孔７５に確実に挿入することが可能となる。
【００８０】
上記において、本発明について好適な実施の形態を挙げて説明したが、本発明は前記実施の形態に限定されるものではなく、本発明の要旨を逸脱しない範囲において、種々の改変が可能なことは言うまでもない。
【００８１】
例えば、この実施の形態では、コネクタ挿入に至るまでの各工程毎にコンプライアンス制御を行うようにしているが、コンプライアンス原点（釣合位置）が各工程における目標位置に連動して自動的に更新されるようにすることも可能である。この場合、各工程毎の制御ではなく、把持工程から押込工程に至る全工程において、連動したコンプライアンス制御によって目標位置を算出することが可能である。
【符号の説明】
【００８２】
１０…コネクタ挿入装置                １２…ロボット
１４…制御部                          ２２…ハンド部
２３ａ、２３ｂ…緩衝アーム            ２４ａ、２４ｂ…把持指
２７ａ、２７ｂ…指用力覚センサ
３８ａ、３８ｂ、４６ａ、４６ｂ、５２ａ、５２ｂ…ボールネジ機構
４０ａ、４０ｂ、４４ａ、４４ｂ、５０ａ、５０ｂ…サーボモータ
７０…コネクタ                        ７５…コネクタ挿入孔
７８…内側面                          １０８…目標位置算出部
１１０…サーボモータ制御部
１２０ａ、１２０ｂ、１２０ｃ…仮想バネ
１２２ａ、１２２ｂ、１２２ｃ…仮想ダンパ
１２４ａ、１２４ｂ…仮想台車
１２６…仮想スタビライザ
【図１】
2015196208.tif 000005
【図２】
2015196208.tif 000006
【図３】
2015196208.tif 000007
【図４】
2015196208.tif 000008
【図５】
2015196208.tif 000009
【図６】
2015196208.tif 000010
【図７】
2015196208.tif 000011
【図８】
2015196208.tif 000012
【手続補正書】
【提出日】平成26年5月30日(2014.5.30)
【手続補正１】
【補正対象書類名】明細書
【補正対象項目名】００４５
【補正方法】変更
【補正の内容】
【００４５】
そして、仮想スタビライザ１２６の釣合距離をＤsとするとき、第１の把持指２４ａの運動方程式は、式（１）から、Ｋs（Ｐd1−Ｐd2＋Ｄs）を差し引き、さらに、Ｃs（Ｖ1−Ｖ2）を差し引くことで補正される。同様に、第２の把持指２４ｂの運動方程式は、式（２）から、Ｋs（Ｐd2−Ｐd1−Ｄs）を差し引き、さらに、Ｃs（Ｖ2−Ｖ1）を差し引くことで補正される。すなわち、下記の連立方程式が成立する。
【数１】
2015196208.tif 000013