JP2015196208A - ワーク保持方法 - Google Patents

Abstract

【課題】ロボットのアームでワークを保持する際、ワークの状態に関わらず保持を安定化する。
【解決手段】ワークであるコネクタ７０を把持するための第１の把持指２４ａ、第２の把持指２４ｂの各々に第１のコンプライアンスモデル、第２のコンプライアンスモデルを設定する。さらに、第１の把持指２４ａと第２の把持指２４ｂとが、第３仮想バネ１２０ｃと第３仮想ダンパ１２２ｃを含む仮想スタビライザ１２６で連結されていると仮定し、第１の把持指２４ａと第２の把持指２４ｂの間に第３のコンプライアンスモデルを設定する。第１〜第３のコンプライアンスモデルから連立運動方程式を構築し、第１の把持指２４ａないし第２の把持指２４ｂの移動先の目標座標（目標位置）を求める。
【選択図】図４

Description

本発明は、互いに対向可能な第１の把持指及び第２の把持指でワークを保持するワーク保持方法に関する。

例えば、自動車あるいはその関連部品等の生産ラインでは、多数のロボットが用いられている。この種の生産ラインにおける生産効率を向上させるためには、ロボットで作業可能であると考えられる作業の可及的大部分をロボットに行わせることが望ましい。例えば、ハンド部に２個の把持指を有するロボットを用い、電気機器のコネクタ挿入孔にコネクタを挿入する工程を自動化することが考えられる。例えば、特許文献１では、ロボットのアーム部の先端に設けられたハンド部でコネクタを把持し、コンプライアンス制御を用いてコネクタを固定部に挿入することが提案されている。

特開平４−１８９４９５号公報

特許文献１の第５図等に示されるように、コネクタは、２個の長辺部と短辺部を有し、一方の短辺部に端子部が設けられ、且つ他方の短辺部にケーブルが接続された長尺体であることが一般的である。このような形状のコネクタを２個の把持指で把持するときには、把持指の長手方向と、コネクタの２個の長辺部とが直交するような姿勢が望ましい。コネクタを安定的に保持して、前記端子部をコネクタ挿入孔に確実に挿入し得るようにするためである。

しかしながら、コネクタ（ワーク）の状態によっては、ケーブルの反力等の外乱によってコネクタに回転方向の力が作用することがある。このような事態が生じると、保持が不安定となる一因となる。

本発明は上記した問題を解決するためになされたもので、上記したようなワーク（例えば、コネクタ）の位置・姿勢のずれを吸収するコンプライアンス制御を実現しつつ、外力による望ましくない過剰な姿勢変化を抑制するワーク保持方法を提供することを目的とする。

前記の目的を達成するために、本発明は、互いに対向可能な第１の把持指及び第２の把持指でワークを保持するワーク保持方法において、
前記第１の把持指及び前記第２の把持指の各々に第１のコンプライアンスモデル、第２のコンプライアンスモデルを設定し、且つ前記第１の把持指と前記第２の把持指の間に第３のコンプライアンスモデルを設定し、前記第１のコンプライアンスモデル、前記第２のコンプライアンスモデル及び前記第３のコンプライアンスモデルに基づいて、前記第１の把持指又は前記第２の把持指の移動先の目標座標を変数として含む連立運動方程式を構築する工程と、
前記第３のコンプライアンスモデルに基づく拘束条件下で前記第１の把持指と前記第２の把持指が相互に連動する前記目標座標を算出する工程と、
前記第１の把持指又は前記第２の把持指を前記目標座標に移動させる工程と、
を有することを特徴とする。

すなわち、本発明においては、第１の把持指と第２の把持指とが仮想的な連結機構（例えば、仮想スタビライザ）によって連結されていると仮定し、両把持指の間に第３のコンプライアンスモデルを設定する。そして、第１〜第３のコンプライアンスモデルに基づいて構築される連立運動方程式を解くことにより、第１の把持指ないし第２の把持指の移動先の目標座標（目標位置）が求められる。

第１の把持指ないし第２の把持指は、この目標座標（目標位置）となるように移動される。その結果、ワークの状態に関わらず、該ワークが安定して保持されるようになる。

なお、ワークとしては、例えば、ケーブルが接続されたコネクタを挙げることができる。

本発明によれば、第１の把持指と第２の把持指との間に仮想的な連結機構（例えば、仮想スタビライザ）が存在しているとの仮定に基づき、両把持指の間に第３のコンプライアンスモデルを設定するようにしている。この場合、第１〜第３のコンプライアンスモデルに基づいて連立運動方程式が構築される。該連立運動方程式を解くことにより、第１の把持指ないし第２の把持指の移動先の目標座標（目標位置）が求められる。

第１の把持指ないし第２の把持指は、このようにして求められた目標座標（目標位置）となるように移動される。その結果、ワークを安定して保持することができるようになる。

本発明の実施の形態に係るコネクタ挿入装置の概略構成図である。 図１に示したコネクタ挿入装置におけるハンド部の斜視図である。 コネクタ挿入装置の制御系統を示すブロック図である。 前記ハンド部のコンプライアンスモデルを示すモデル図である。 図５Ａ〜図５Ｃは、第１の把持指と第２の把持指との間に仮想スタビライザに関するコンプライアンスモデルを構築していない従来技術において、コネクタにトルクが加わったときの第１の把持指及び第２の把持指の変位をフローとして示した要部概略斜視図である。 図６Ａ及び図６Ｂは、第１の把持指と第２の把持指との間に仮想スタビライザに関するコンプライアンスモデルを構築した実施の形態において、コネクタが回転することを抑制しながらコネクタを移動させるフローを示した要部概略斜視図である。 図７Ａ〜図７Ｄは、コネクタがコネクタ挿入孔に挿入されるまでの過程を示した要部斜視図である。 図８Ａ〜図８Ｃは、コネクタがコネクタ挿入孔に挿入されるまでの把持指の動作を示した要部平面図である。

以下、本発明に係るワーク保持方法につき、それを実施するための装置との関係で好適な実施の形態を挙げ、添付の図面を参照しながら詳細に説明する。なお、本実施の形態においては、コネクタをワークとして把持する場合を例示するものとする。

図１は、本実施の形態に係るワーク保持方法を実施するためのコネクタ挿入装置１０の概略構成図である。コネクタ挿入装置１０は、図７Ａ等に示す電気機器のコネクタ挿入孔７５にコネクタ７０を挿入するための装置である。このコネクタ挿入装置１０は、ロボット１２と、このロボット１２を制御する制御部１４とを備える。

ロボット１２は、複数の駆動軸を有する多関節ロボットである。具体的には、ロボット１２は、ベース台１６と、鉛直な駆動軸を中心としてベース台１６に回動可能に支持された旋回部１８と、旋回部１８に回動可能に支持された多関節アーム２０と、多関節アーム２０の先端に設けられたハンド部２２とを有する。

ロボット１２は、複数のアーム２０ａ〜２０ｃを有する多関節アーム２０の動作により、３次元空間上でハンド部２２の位置及び姿勢を変化させる。ロボット１２は、制御部１４の制御により、ロボット１２に設けられた各駆動軸が数値制御され、３次元空間上でのハンド部２２の位置及び姿勢が制御されるようになっている。

図２は、ハンド部２２の斜視図である。ハンド部２２は、コネクタ７０（図７Ａ参照）を把持可能な把持機構として構成される。ハンド部２２は、第１緩衝アーム２３ａ、第２緩衝アーム２３ｂの各先端に設けられた第１の把持指２４ａ、第２の把持指２４ｂと、第１の把持指２４ａ、第２の把持指２４ｂを移動させる２つの移動機構２６ａ、２６ｂと、２つの移動機構２６ａ、２６ｂが取り付けられたベース部２８とを有する。本実施の形態において、第１の把持指２４ａ、第２の把持指２４ｂは、平面状の把持面２５ａ、２５ｂを有する棒状の長尺体として構成され、互いに平行に配置される。

第１の把持指２４ａ、第２の把持指２４ｂの先端には、それぞれ、図２及び図３に示すように、第１指用力覚センサ２７ａ、第２指用力覚センサ２７ｂが設けられる。

第１指用力覚センサ２７ａ、第２指用力覚センサ２７ｂによる検出情報は、制御部１４を構成する目標位置算出部１０８に信号として送信される。なお、制御部１４は、前記目標位置算出部１０８の他、サーボモータ制御部１１０を含んで構成される。

第１の把持指２４ａ、第２の把持指２４ｂ（図２参照）は互いに対向可能である。第１の把持指２４ａ、第２の把持指２４ｂは、動作位置によって、それらの把持面２５ａ、２５ｂ同士が対向する位置となる場合（図２中のＸ方向、Ｙ方向及びＺ方向の位置が一致する場合）もあるが、把持面２５ａ、２５ｂ同士が対向する位置とならない場合もある。

第１の把持指２４ａを移動させるための移動機構２６ａは、ベース部２８に対してＹ方向（ベース部２８の延在方向）にスライド可能な第１移動部３０ａと、第１移動部３０ａに対してＺ方向にスライド可能な第２移動部３２ａと、第２移動部３２ａに対してＸ方向に移動可能な第３移動部３４ａとを有する。第１の把持指２４ａは、第１指用力覚センサ２７ａを介して、第３移動部３４ａの先端に取り付けられる。従って、第１の把持指２４ａは、Ｘ方向、Ｙ方向及びＺ方向の直交３軸の動作が可能である。

第１移動部３０ａは、第１駆動部３６の作用によりベース部２８に対してＹ方向にスライド可能である。第１駆動部３６は、サーボモータ４０ａと、ボールネジ及びナットからなるボールネジ機構３８ａとを有する。サーボモータ４０ａは、制御部１４を構成するサーボモータ制御部１１０（図３参照）により駆動制御され、これにより、第１の把持指２４ａのＹ方向の位置が制御される。

第２移動部３２ａは、第２駆動部４２の作用により第１移動部３０ａに対してＺ方向にスライド可能である。第２駆動部４２は、サーボモータ４４ａと、ボールネジ及びナットからなるボールネジ機構４６ａとを有する。サーボモータ４４ａもサーボモータ制御部１１０（図３参照）によって駆動制御され、これにより、第１の把持指２４ａのＺ方向の位置が制御される。

第３移動部３４ａは、第３駆動部４８の作用により第２移動部３２ａに対してＸ方向にスライド可能である。第３駆動部４８は、サーボモータ５０ａと、ボールネジ及びナットからなボールネジ機構５２ａとを有する。サーボモータ５０ａもサーボモータ制御部１１０（図３参照）によって駆動制御され、これにより、第１の把持指２４ａのＸ方向の位置が制御される。

第２の把持指２４ｂを移動させる移動機構２６ｂは、第１駆動部５４の作用によりベース部２８に対してＹ方向にスライド可能な第１移動部３０ｂと、第２駆動部５６の作用により第１移動部３０ｂに対してＺ方向にスライド可能な第２移動部３２ｂと、第３駆動部５８の作用により第２移動部３２ｂに対してＸ方向に移動可能な第３移動部３４ｂとを有する。第２の把持指２４ｂは、第２指用力覚センサ２７ｂを介して、第３移動部３４ｂの先端に取り付けられる。従って、他方の第２の把持指２４ｂは、Ｘ方向、Ｙ方向及びＺ方向の直交３軸の動作が可能である。

他方の移動機構２６ｂにおける第１移動部３０ｂ、第２移動部３２ｂ及び第３移動部３４ｂは、それぞれ、上述した一方の移動機構２６ａにおける第１移動部３０ａ、第２移動部３２ａ及び第３移動部３４ａと同様に構成される。

また、他方の移動機構２６ｂにおける第１駆動部５４、第２駆動部５６及び第３駆動部５８は、それぞれ、上述した一方の移動機構２６ａにおける第１駆動部３６、第２駆動部４２及び第３駆動部４８と同様に構成される。すなわち、第１駆動部５４は、サーボモータ４０ｂとボールネジ機構３８ｂを有する。また、第２駆動部５６はサーボモータ４４ｂとボールネジ機構４６ｂを有し、第３駆動部５８はサーボモータ５０ｂとボールネジ機構５２ｂを有する。サーボモータ４０ｂ、４４ｂ、５０ｂは、制御部１４を構成するサーボモータ制御部１１０（図３参照）によって駆動制御され、これにより、第２の把持指２４ｂのＸ方向、Ｚ方向の位置が制御される。

上記のように構成される各移動機構２６ａ、２６ｂにより、第１の把持指２４ａ、第２の把持指２４ｂを個別に移動させることができる。すなわち、制御部１４の制御作用下に、各移動機構２６ａ、２６ｂの作用下に第１の把持指２４ａ、第２の把持指２４ｂをＸ方向、Ｙ方向又はＺ方向に移動させることにより、ベース部２８に対する第１の把持指２４ａ、第２の把持指２４ｂの位置を個別に調整することが可能である。また、サーボモータ制御部１１０では、各エンコーダからの出力値を微分することで、第１の把持指２４ａ、第２の把持指２４ｂの速度を算出する。この値に基づき、速度制限を行うことができる。

ベース部２８には、ブラケット６０を介してカメラ６２が取り付けられる。カメラ６２は、第１の把持指２４ａ、第２の把持指２４ｂ側を指向しており、第１の把持指２４ａ、第２の把持指２４ｂ及びその周辺を撮像可能な視野を有する。カメラ６２により撮像される画像情報は、制御部１４に送信され、制御部１４においてパターンマッチングを行い、把持部により把持されたコネクタ７０の位置及び姿勢を認識することが可能である。

なお、本実施の形態におけるハンド部２２では、直交３軸の機構で構成された移動機構２６ａ、２６ｂにより第１の把持指２４ａ、第２の把持指２４ｂを個別に移動させる構成となっているが、ハンド部２２の構成はこれに限られない。例えば、ハンド部２２において、直線動作と回転動作が可能な移動機構を第１の把持指２４ａ、第２の把持指２４ｂ毎に設け、それらの移動機構により第１の把持指２４ａ、第２の把持指２４ｂを個別に移動させる構成としてもよい。

一方の移動機構２６ａにおける第１駆動部３６、第２駆動部４２及び第３駆動部４８は、それぞれボールネジ機構３８ａ、４６ａ、５２ａにより構成されるものに限られない。従って、第１駆動部３６、第２駆動部４２及び第３駆動部４８は、それぞれ、ボールネジ機構以外の形態のアクチュエータ、例えば、ラックアンドピニオン、シリンダ装置等により構成されてもよい。同様に、他方の移動機構２６ｂにおける第１駆動部５４、第２駆動部５６及び第３駆動部５８も、ボールネジ機構以外の形態のアクチュエータ、例えば、ラックアンドピニオン、シリンダ装置等により構成されてもよい。

図１に示す制御部１４は、ロボット１２の多関節アーム２０の先端位置を数値制御するだけでなく、コネクタ７０をコネクタ挿入孔７５に挿入する際に、ハンド部２２の動作をコンプライアンス制御することが可能である。すなわち、コネクタ挿入装置１０は、コンプライアンス制御を用いて、ハンド部２２により把持したコネクタ７０を、コネクタ挿入孔７５に挿入することができる。

図４は、ハンド部２２のコンプライアンスモデルを示すモデル図である。周知の通り、コンプライアンス制御は、バネ、ダンパ、慣性パラメータを仮想的に設定したモデルの運動をロボットの動作として実現するものである。

本実施の形態では、第１の把持指２４ａ、第２の把持指２４ｂの各々に、第１のコンプライアンスモデル、第２のコンプライアンスモデルが設定される。すなわち、第１の把持指２４ａに関する第１のコンプライアンスモデルは、第１仮想バネ１２０ａ及び第１仮想ダンパ１２２ａを含む。なお、図４には、当該コンプライアンスモデルにおいて、第１仮想バネ１２０ａの釣合位置及び第１仮想ダンパ１２２ａの基部に該当する移動物体を第１仮想台車１２４ａとして併せて示している。

一方、第２の把持指２４ｂに関する第２のコンプライアンスモデルは、第２仮想バネ１２０ｂ及び第２仮想ダンパ１２２ｂを含む。図４には、上記と同様に第２仮想バネ１２０ｂ及び第２仮想ダンパ１２２ｂの基部に該当する移動物体を第２仮想台車１２４ｂとして併せて示している。

第１仮想バネ１２０ａと第２仮想バネ１２０ｂ、第１仮想ダンパ１２２ａと第２仮想ダンパ１２２ｂは、それぞれ等価であるとする。すなわち、第１仮想バネ１２０ａ及び第２仮想バネ１２０ｂのバネ定数は等しく、第１仮想ダンパ１２２ａ及び第２仮想ダンパ１２２ｂの減衰係数は等しいとする。そこで、以下、第１仮想バネ１２０ａ及び第２仮想バネ１２０ｂのバネ定数をＫ、仮想質量をｍ、第１仮想ダンパ１２２ａ及び第２仮想ダンパ１２２ｂの減衰係数をＣとする。

第１の把持指２４ａのコンプライアンス原点（釣合位置）をＰ_blc1、移動先の目標位置（目標座標）をＰ_d1、目標加速度をａ₁、目標速度をＶ₁、第１仮想台車１２４ａの速度をＶ_blc1、第１の把持指２４ａに作用する外力をＦ_e1とするとき、第１の把持指２４ａに関する運動方程式は、下記の式（１）として構築される。
ｍａ₁＝Ｆ_e1−Ｋ（Ｐ_d1−Ｐ_blc1）−Ｃ（Ｖ₁−Ｖ_blc1） …（１）

なお、外力Ｆ_e1は、第１指用力覚センサ２７ａによって検出される。また、コンプライアンス原点（釣合位置）Ｐ_blc1は、コンプライアンス制御がないとしたときの第１の把持指２４ａの目標位置であり、本実施の形態においては、第１の把持指２４ａの目標位置近傍に設定することができる。

さらに、目標加速度ａ₁、目標速度Ｖ₁、及び第１仮想台車１２４ａの速度Ｖ_blc1は、後述する通り、離散化によりコンプライアンス原点（釣合位置）であるＰ_blc1、移動先の目標位置（目標座標）Ｐ_d1、サンプル時刻ｔ_N、制御周期Ｔにより表すことができる。

一方、第２の把持指２４ｂのコンプライアンス原点（釣合位置）をＰ_blc2、移動先の目標位置（目標座標）をＰ_d2、目標加速度をａ₂、目標速度をＶ₂、第２仮想台車１２４ｂの速度をＶ_blc2、第２の把持指２４ｂに作用する外力をＦ_e2とするとき、第２の把持指２４ｂに関する運動方程式は、下記の式（２）として構築される。
ｍａ₂＝Ｆ_e2−Ｋ（Ｐ_d2−Ｐ_blc2）−Ｃ（Ｖ₂−Ｖ_blc2） …（２）

上記と同様に、外力Ｆ_e2は第２指用力覚センサ２７ｂによって検出される。また、コンプライアンス原点（釣合位置）Ｐ_blc2は、は、コンプライアンス制御がないとしたときの第２の把持指２４ｂの目標位置であり、本実施の形態においては、第２の把持指２４ｂの目標位置近傍に設定することができる。

さらに、目標加速度ａ₂、目標速度Ｖ₂、及び第２仮想台車１２４ｂの速度Ｖ_blc2は、後述する通り、離散化によりコンプライアンス原点（釣合位置）であるＰ_blc2、移動先の目標位置（目標座標）Ｐ_d2、サンプル時刻ｔ_N、制御周期Ｔにより表すことができる。

ここで、本実施の形態においては、第１の把持指２４ａと第２の把持指２４ｂの間に第３のコンプライアンスモデルを設定する。第３のコンプライアンスモデルは、第３仮想バネ１２０ｃ及び第３仮想ダンパ１２２ｃを含む仮想スタビライザ１２６である。すなわち、この場合、第１の把持指２４ａと第２の把持指２４ｂとが、第３仮想バネ１２０ｃと第３仮想ダンパ１２２ｃを含む仮想スタビライザ１２６で連結されていると仮定する。第３仮想バネ１２０ｃのバネ定数はＫ_s、第３仮想ダンパ１２２ｃの減衰係数はＣ_sで表される。

そして、仮想スタビライザ１２６の釣合距離をＤ_sとするとき、第１の把持指２４ａの運動方程式は、式（１）から、Ｋ_s（Ｐ_d1−Ｐ_d2＋Ｄ_s）を差し引き、さらに、Ｃ_s（Ｖ₁−Ｖ₂）を差し引くことで補正される。同様に、第２の把持指２４ｂの運動方程式は、式（２）から、Ｋ_s（Ｐ_d2−Ｐ_d1−Ｄ_s）を差し引き、さらに、Ｃ_s（Ｖ₂−Ｖ₁）を差し引くことで補正される。すなわち、下記の連立方程式が成立する。

次に、目標加速度ａ₁、ａ₂、目標速度Ｖ₁、Ｖ₂、仮想台車の速度Ｖ_blcに対して、下記の式（３）〜（５）にて離散化を行う。

式（３）〜（５）中のｔ_Nはサンプリング時刻、Ｔはサンプリング周期、Ｎはサンプリング回数である。サンプリング周期Ｔは、所望の制御性能に応じ、制御解析の技術知識に基づいて設定する。

このように、本実施の形態においては、第１の把持指２４ａ、第２の把持指２４ｂの各々にコンプライアンスモデル（第１のコンプライアンスモデル、第２のコンプライアンスモデル）を設定するのみならず、第１の把持指２４ａと第２の把持指２４ｂとの間にも、コンプライアンスモデル（第３のコンプライアンスモデル）を設定するようにしている。すなわち、第１の把持指２４ａと第２の把持指２４ｂの目標位置が相互に連動しあう拘束条件の下に算出される関係式を構築することで、仮想スタビライザ１２６が存在するとしている。これにより、第１の把持指２４ａ及び第２の把持指２４ｂによるコネクタ７０の把持が一層安定となる。

なお、図３に示すように、第１の把持指２４ａ、第２の把持指２４ｂのコンプライアンス原点（釣合位置）Ｐ_blc1、Ｐ_blc2、目標加速度ａ₁、ａ₂、目標速度Ｖ₁、Ｖ₂、第１仮想台車１２４ａ、第２仮想台車１２４ｂの速度Ｖ_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を用いることにより、同様のコンプライアンス制御を行うことができる。

以上説明したように、本実施の形態に係るコネクタ挿入方法及びコネクタ挿入装置１０によれば、コネクタ挿入孔７５にコネクタ７０を途中まで挿入した状態で、一方の第１の把持指２４ａによりコネクタ７０をコネクタ挿入孔７５の内側面７８に押し付けることにより、コネクタ７０が一時的に固定される。このため、コネクタ７０に接続されたケーブル７２の拘束力によって、コネクタ挿入孔７５とは反対方向にコネクタ７０を引っ張る力が作用した場合でも、コネクタ７０がコネクタ挿入孔７５から抜け出ることがない。そして、他方の第２の把持指２４ｂによりコネクタ挿入孔７５とは反対側からコネクタ７０を押して、コネクタ７０を押し込むため、コネクタ７０をコネクタ挿入孔７５の所定位置まで確実に挿入することができる。

その際に、第１〜第３のコンプライアンスモデルを設定したコンプライアンス制御を行うことにより、コネクタ７０の姿勢を、第１の把持指２４ａと第２の把持指２４ｂが連動しあう条件下に把持することができる。このため、外力による第１の把持指２４ａ、第２の把持指２４ｂの相対変位が抑制される。その結果、コネクタ７０を安定して把持することが可能となり、また、コネクタ挿入孔７５に確実に挿入することが可能となる。

上記において、本発明について好適な実施の形態を挙げて説明したが、本発明は前記実施の形態に限定されるものではなく、本発明の要旨を逸脱しない範囲において、種々の改変が可能なことは言うまでもない。

例えば、この実施の形態では、コネクタ挿入に至るまでの各工程毎にコンプライアンス制御を行うようにしているが、コンプライアンス原点（釣合位置）が各工程における目標位置に連動して自動的に更新されるようにすることも可能である。この場合、各工程毎の制御ではなく、把持工程から押込工程に至る全工程において、連動したコンプライアンス制御によって目標位置を算出することが可能である。

１０…コネクタ挿入装置 １２…ロボット
１４…制御部 ２２…ハンド部
２３ａ、２３ｂ…緩衝アーム ２４ａ、２４ｂ…把持指
２７ａ、２７ｂ…指用力覚センサ
３８ａ、３８ｂ、４６ａ、４６ｂ、５２ａ、５２ｂ…ボールネジ機構
４０ａ、４０ｂ、４４ａ、４４ｂ、５０ａ、５０ｂ…サーボモータ
７０…コネクタ ７５…コネクタ挿入孔
７８…内側面 １０８…目標位置算出部
１１０…サーボモータ制御部
１２０ａ、１２０ｂ、１２０ｃ…仮想バネ
１２２ａ、１２２ｂ、１２２ｃ…仮想ダンパ
１２４ａ、１２４ｂ…仮想台車
１２６…仮想スタビライザ

そして、仮想スタビライザ１２６の釣合距離をＤ_sとするとき、第１の把持指２４ａの運動方程式は、式（１）から、Ｋ_s（Ｐ_d1−Ｐ_d2＋Ｄ_s）を差し引き、さらに、Ｃ_s（Ｖ₁−Ｖ₂）を差し引くことで補正される。同様に、第２の把持指２４ｂの運動方程式は、式（２）から、Ｋ_s（Ｐ_d2−Ｐ_d1−Ｄ_s）を差し引き、さらに、Ｃ_s（Ｖ₂−Ｖ₁）を差し引くことで補正される。すなわち、下記の連立方程式が成立する。

Claims (2)

1. 互いに対向可能な第１の把持指及び第２の把持指でワークを保持するワーク保持方法において、
   前記第１の把持指及び前記第２の把持指の各々に第１のコンプライアンスモデル、第２のコンプライアンスモデルを設定し、且つ前記第１の把持指と前記第２の把持指の間に第３のコンプライアンスモデルを設定し、前記第１のコンプライアンスモデル、前記第２のコンプライアンスモデル及び前記第３のコンプライアンスモデルに基づいて、前記第１の把持指又は前記第２の把持指の移動先の目標座標を変数として含む連立運動方程式を構築する工程と、
   前記第３のコンプライアンスモデルに基づく拘束条件下で前記第１の把持指と前記第２の把持指が相互に連動する前記目標座標を算出する工程と、
   前記第１の把持指又は前記第２の把持指を前記目標座標に移動させる工程と、
   を有することを特徴とするワーク保持方法。
2. 請求項１記載の保持方法において、前記ワークはケーブルが接続されたコネクタであることを特徴とするワーク保持方法。

Images