JP2015155126A - ロボットシステムのツール座標系補正方法、およびロボットシステム - Google Patents

Abstract

【課題】水平方向ズレの補正のみならず傾きズレの補正も含めたロボットのツール座標系の芯ズレ補正方法を提供する。
【解決手段】力覚センサ３とツール４を搭載したロボットアーム１のツール座標系の芯ズレ補正において、マスターワーク５をマスターワーク６に対して力覚センサ３を用いた力覚制御により嵌合させる。この嵌合動作中の少なくとも２つの状態におけるメカニカルインタフェース座標系の原点の位置データに基づきツール座標系の傾きズレを補正する。また、嵌合させたマスターワーク５をマスターワーク６に対して力覚センサ３を用いた力覚制御で水平の２方向に押し付け、その際マスターワーク５にかかる力ならびにトルクの検出結果に基づきツール座標系の水平方向ズレを補正する。
【選択図】図１

Description

本発明は、ツールが先端に装着されたロボットアームと、前記ツールに把持されたワークに作用する力およびトルクを検出する力覚センサ、およびロボットシステムに関する。

近年、小型で複雑な構造を有する工業製品の組み立てに対して、ロボットシステムを利用した自動化の要求が高まってきている。これらの製品の組み立てには、複雑な動作で正確な力覚制御を伴う組み立てが必要になることがある。

このような需要に鑑み、従来より力覚制御ロボットシステムが知られている。この種の装置では、多軸（数軸、たとえば６軸など）、多関節のロボットアームの先端に特定の操作や作業を行わせるエンドエフェクタが装着される。このエンドエフェクタには、塗装用のスプレーガン、溶接ガン、ナット締め具など種々のツールが含まれる。また、ワーク（部品）などの対象物を把持する把持部を構成するツールはロボットハンドなどと呼ばれる。

特に、ロボットハンド（以下ハンドともいう）が用いられる場合、このツールは力覚センサを介してロボットアーム先端に装着される。この力覚センサはワークを介してツールに加わる力ないしはトルクを検出し、その検出量はロボットアーム、ないしはツールの動作（位置や姿勢、速度等）を制御するのに用いられる。このような制御を力覚制御という。また、ハンド等のツールの装着部位（たとえばロボットアーム先端のフランジ面など）は、特にメカニカルインタフェースなどと呼ばれる。

以上のようなロボットシステムにおいて、ロボットアーム、ツール（エンドエフェクタ）などの各部の位置や動きはコンピュータ、メモリなどを用いて構成された制御装置を介して制御される。その場合、上記各部の位置や動きは、ベース座標系、メカニカルインタフェース座標系、ツール座標系（エンドエフェクタ座標系）などの座標系を基準にして制御される。このうちベース座標系はロボットシステム全体の基準となるもので、ロボットのベースの設置面などを基準に配置される。メカニカルインタフェース座標系は、上記のエンドエフェクタ（ツール）装着面を基準とした座標系である。

また、ツール座標系（エンドエフェクタ座標系）はツールの駆動制御に用いられる座標系で、特にツール各部の位置や姿勢を制御するために用いられる。ツール座標系は、メカニカルインタフェースから見たエンドエフェクタ先端の位置や姿勢を表すデータに基づいてエンドエフェクタ（ツール）先端の所定位置に設定される。通常、ツール座標系はエンドエフェクタ（ツール）の構造や、先端に設置される爪などの設計寸法に応じた所定位置に設定される。

この種のロボットシステムでは、複数のワーク（部品）同士特定の結合関係で結合させることができ、たとえば嵌合関係によってワーク同士を結合する場合には次のような制御が行なわれる。

例えば、特定の位相を持ったそれぞれ凸、凹の断面形状を有する円筒部品同士を精密に嵌合させる場合、まず、ロボットアーム先端部を回転させることで円筒部品同士の位相を一致させ、その後、嵌合操作を行う。具体的には、まずロボットアーム先端に設置したエンドエフェクタで把持した円筒部品の中心軸と嵌合先の円筒部品の中心軸が一致する位置へと把持した円筒部品を移動させる。次に、把持した円筒部品の位相と嵌合先の円筒部品の位相が一致するように、ツール座標系（エンドエフェクタ座標系）の特定の軸を中心としてロボットアーム先端部を回転させ、その後、力覚制御による精密嵌合を行う。

しかしながら、エンドエフェクタ等の各部品の寸法公差や設置精度等の影響により、ハンドで把持した円筒部品の中心軸と設計寸法を基に設定したツール座標系の回転中心軸が一致していない場合がある。このようにツール座標系の回転中心軸にずれが生じたまま部品の操作を行うと、各ワークへの負荷や嵌合に要するタクトが増大し、各ワークの削れ、タクトの遅延等のトラブルを生じるおそれがある。

また、ハンドで把持した部品の中心軸とツール座標系の回転中心軸が傾きズレを生じた状態は、ハンドの爪や操作される部品（あるいは治具でも同様）の工作精度や形状、相性等の特性に応じて発生しうる。また、このツール座標系の傾きズレは治具（マスターワーク）を用いたプログラミング後に発生していれば、同じ形状のワークに対する操作を行う時にも同じ傾向で発生するものと考えられる。

上記のようなトラブルを防止するためには、ツール座標系の回転中心軸と把持した円筒部品の中心軸とが一致するように、ツール座標系の回転中心軸を補正する必要がある。前記補正を実現には、把持した円筒部品の中心軸の傾きと一致するようにツール座標系の回転中心軸の傾きのみを補正することと、把持した円筒部品の中心軸からツール座標系の回転中心軸までの距離のズレを補正することが必要である。

以下では、ツール（エンドエフェクタ）で把持した円筒部品の中心軸の傾きと一致するようにツール座標系の回転中心軸の傾きのみを補正することを「傾きズレ補正」と呼ぶこととする。また、把持した円筒部品の中心軸からツール座標系の回転中心軸までの距離のズレのみを補正することを「水平方向ズレ補正」と呼ぶ。さらに、把持した円筒部品の中心軸と一致するようにツール座標系の回転中心軸の「傾きズレ補正」と「水平方向ズレ補正」の両方を行うことを「芯ズレ補正」と呼ぶこととする。

上記のような芯ズレ補正を、例えばマスターワーク（治具）を用いたロボットアームのプログラミングの段階で行い、補正（校正）量として記憶させることができれば好ましい。そして、記憶させた補正（校正）量をマスターワーク（治具）に対応するワークを実際に操作する際に作用させることにより、上記のようなトラブルを回避することができると考えられる。

さて、従来より、ツール座標系の芯ズレ補正に関する従来技術として、画像処理機器を用いた方法（下記の特許文献１）や微小変位計を用いた方法（下記の特許文献２）などが提案されている。

特許４２８９６１９号公報 特開平０１−５８４９０号公報

特許文献１では、画像処理機器で検出可能なマーカーを施したワークをロボットのエンドエフェクタに把持させ、アーム移動に伴なうマーカー位置のズレを画像処理機器により検出し、アームの先端軸に対するエンドエフェクタの把持点のズレを算出している。また、特許文献２では、２つの微小変位計と円柱治具とを用いて、ロボットアームの先端軸を回転させて微小変位計の信号を読み取り、読み取った情報からロボットアームの先端軸を校正している。

しかしながら、特許文献１、特許文献２のいずれの方法においても、ツール座標系の水平方向ズレ補正のみが可能であり、ツール座標系の傾きズレ補正はできなかった。したがって、これら従来技術では水平方向ズレ補正と傾きズレ補正の双方を含んだ芯ズレ補正は不可能であった。

本発明の課題は上述の問題点に鑑み、上述の水平方向ズレ補正と傾きズレ補正の双方を含むツール座標系の芯ズレ補正を行えるようにすることにある。

上記課題を解決するため、本発明においては、ツールが先端に装着されたロボットアームと、前記ツールに把持されたワークに作用する力およびトルクを検出する力覚センサと、前記ロボットアームの動作を制御する制御装置を備えたロボットシステムにより、相互に係合する凸部または凹部の一方および他方をそれぞれ有し所定の最終結合位置関係で結合される第１のマスターワークおよび第２のマスターワークを用いて、第１のマスターワークを前記ツールに把持させ、第１および第２のマスターワークの前記凸部および凹部を係合させて第１および第２のマスターワークを結合する際に、前記ツールの駆動制御に用いられるツール座標系を補正するロボットシステムのツール座標系補正方法において、前記制御装置が、前記第１のマスターワークを前記ツールに把持させ、前記力覚センサの検出量を用いて前記ロボットアームの動作を制御しつつ第１のマスターワークの第２のマスターワークに対する係合を開始させ、この係合の動作中における異なる少なくとも２つの状態における前記ツールが装着されたメカニカルインタフェース座標系の原点の少なくとも２つの異なる位置に基づき第１のマスターワークの中心軸の位置を求め、前記第１のマスターワークの中心軸の位置に基づき前記ツール座標系の傾きズレを算出し、算出された傾きズレに基づいてツール座標系の傾きズレを補正する第１のステップと、第１および第２のマスターワークを係合させた後、前記力覚センサの検出量を用いて前記ロボットアームの動作を制御しつつ第１のマスターワークを第２のマスターワークに対して第１および第２のマスターワークの係合方向に交差する水平の２方向にそれぞれ押し付け、その際の前記力覚センサの検出量に基づき前記ツール座標系の水平方向ズレを算出し、算出された水平方向ズレに基づきツール座標系の水平方向ズレを補正する第２のステップと、から成るツール座標系の芯ズレ補正制御を行う構成を採用した。

また、ツールが先端に装着されたロボットアームと、前記ツールに把持されたワークに作用する力およびトルクを検出する力覚センサと、前記ロボットアームの動作を制御する制御装置を備え、相互に係合する凸部または凹部の一方および他方をそれぞれ有し所定の最終結合位置関係で結合される第１のマスターワークおよび第２のマスターワークを用いて、第１のマスターワークを前記ツールに把持させ、第１および第２のマスターワークの前記凸部および凹部を係合させて第１および第２のマスターワークを結合する際に、前記ツールの駆動制御に用いられるツール座標系を補正するロボットシステムにおいて、前記制御装置が、前記第１のマスターワークを前記ツールに把持させ、前記力覚センサの検出量を用いて前記ロボットアームの動作を制御しつつ第１のマスターワークの第２のマスターワークに対する係合を開始させ、この係合の動作中における異なる少なくとも２つの状態における前記ツールが装着されたメカニカルインタフェース座標系の原点の少なくとも２つの異なる位置に基づき第１のマスターワークの中心軸の位置を求め、前記第１のマスターワークの中心軸の位置に基づき前記ツール座標系の傾きズレを算出し、算出された傾きズレに基づいてツール座標系の傾きズレを補正する第１のステップと、第１および第２のマスターワークを係合させた後、前記力覚センサの検出量を用いて前記ロボットアームの動作を制御しつつ第１のマスターワークを第２のマスターワークに対して第１および第２のマスターワークの係合方向に交差する水平の２方向にそれぞれ押し付け、その際の前記力覚センサの検出量に基づき前記ツール座標系の水平方向ズレを算出し、算出された水平方向ズレに基づきツール座標系の水平方向ズレを補正する第２のステップと、から成るツール座標系の芯ズレ補正制御を行う構成を採用した。

上記構成によれば、ツール座標系の水平方向ズレの補正だけでなく、傾きズレの補正も可能である。本発明によれば、従来のようにツール座標系の水平方向ズレの補正のみを行った場合と比較して、円筒部品同士の嵌合時の円筒部品への負荷や嵌合に要するタクトを減少させることができる、という優れた効果がある。

本発明の実施形態に係るロボットシステムの全体構成を模式的に示す斜視図である。 本発明の実施形態に係る制御装置のブロック図である。 本発明の実施形態に係るロボットシステムで用いられる座標系の概要を示した説明図である。 本発明の実施形態に係る芯ズレ補正方法を示すフローチャート図である。 傾きズレ算出動作開始前の状態を模式的に示した説明図である。 傾きズレ算出動作時にマスターワークが中途まで挿入された状態を模式的に示した説明図である。 傾きズレ算出動作時にマスターワークの挿入が完了した状態を模式的に示した説明図である。 水平方向ズレ算出動作開始前の状態を模式的に示した説明図である。 水平方向ズレ算出動作時にマスターワークをX方向に押し付けた際の状態を模式的に示した説明図である。 水平方向ズレ算出動作時にマスターワークをY方向に押し付けた際の状態を模式的に示した説明図である。

以下、本発明の実施形態に係るロボットのツール座標系の芯ズレ補正方法について添付図面を用いて説明する。

まず、本発明の実施形態に係るロボットシステムの全体構成について、図１を用いて説明する。図１は本発明の実施形態に係るロボットシステムの全体構成を模式的に示した斜視図である。

本実施形態に係るロボットシステムは、図１に示すように６軸垂直多関節のロボットアーム１と、ロボットアーム１を制御する制御装置２、力覚センサ３、ワークを把持可能なツール４とを備えている。

図１において、不図示の架台上に固定されたロボットアーム１は、その各関節を各関節軸まわりにそれぞれ回転駆動する６つのアクチュエータ（不図示）を備えている。そして、制御装置２を介してこれら６つのアクチュエータのそれぞれを選択的に駆動することによりツール４を任意の３次元位置に移動させることができる。

ツール４はワークを把持可能な３つの爪１１、１２、１３と、これらの爪１１〜１３を駆動する不図示のアクチュエータを備えている。ツール４はロボットアーム１先端に装着されている。ツール４は、ツール４が把持したワークに作用する力およびトルクを検出する力覚センサを介してロボットアーム先端部に装着されている。ツール４の爪１１、１２、１３は、アクチュエータの駆動によりロボットアーム１の先端軸の中心に向かって移動自在に構成されており、把持中心となる先端軸J６の中心に対して接離することで開閉し、ワーク等を挟み込んで把持する。たとえば、ツール４の３つの爪１１〜１３を先端軸J６の中心に向かって移動させることによりワーク等を把持し、また、先端軸J６の中心から離反させることにより解放する。

なお、ロボットアーム１の各関節を回転駆動するアクチュエータとしてはサーボモータやステッピングモータを用いることができる。また、ツール４の爪１１〜１３を駆動するアクチュエータとしてはステッピングモータ等を用いることができる。ロボットアーム１の各関節およびツール４の爪１１〜１３の現在位置や姿勢の検出にはロータリーエンコーダ等のセンサデバイスを用いることができる。

力覚センサ３は、ツール４の３つの爪１１、１２、１３のそれぞれにかかる３軸方向の力及び３軸モーメントを検出可能なセンサデバイスから構成されている。力覚センサ３には抵抗線ひずみゲージ、圧電素子、磁歪素子などを用いた公知のデバイスを用いることができる。

図１の構成において、制御装置２の制御に基づきロボットアーム１の各関節の駆動によりツール４を移動させ、また所望の位置でアクチュエータを駆動することにより爪１１〜１３を開閉させてワーク等を把持する等の操作を行わせることができる。

また、その際、力覚センサ３でツール４の爪１１〜１３のそれぞれに生じる反力（応力）を検出しつつその検出量を操作量にフィードバックすることにより、爪１１〜１３やワークの破損や変形等を防止しつつワークなどの対象物を操作することができる。

実際の製造現場などにおいて、ロボットアーム１を用いるに先立ち、マスターワーク（治具）およびティーチングペンダント２５を用いてロボットアーム１の動作をプログラミングすることができる。

図１に示したマスターワーク５およびマスターワーク６は、それぞれ本発明の第１のマスターワークおよび第２のマスターワークを構成する。マスターワーク５、６は、作業対象のワーク（部品）をロボットアーム１で取り扱う時の動作をプログラミングする時などに用いられる一対のワークである。マスターワーク５および６は実際に製造現場などで用いられるワークと同一の形状、サイズに、好ましくは実際のワークよりも高い寸法精度でそれぞれ形成される。

マスターワーク５および６は、相互に係合する凸部または凹部の一方および他方をそれぞれ有し、ロボットアーム１とツール４によって操作され、所定の最終結合位置関係で結合される。本実施例において例示するこの結合操作は、マスターワーク６に対してマスターワーク５を嵌合させる動作である。

本実施例のマスターワーク５はほぼ円筒形状をしており、マスターワーク６にはマスターワーク５を嵌合できる穴１６が穿たれている。マスターワーク６は不図示の架台上に固定される。マスターワーク５は、ロボットアーム１に装着されたツール４の爪１１〜１３により把持される。そして、その後、後述の図５〜図７に示すような順序でマスターワーク６の穴１６内に嵌合される。本実施例では、このマスターワーク５、６を用いたティーチング時に後述のツール座標系の芯ズレ補正処理を行う。そして、マスターワーク５、６に対応するワークを実際に操作する際に作用させることができる補正（校正）量をRAM２３などの記憶手段に格納する。

図２は、ロボットアーム１を制御する制御装置２の構造を示したブロック図である。制御装置２は、CPU２１、ROM２２、RAM２３などから成るコンピュータ本体に、ロボットアーム１、力覚センサ３、ツール４、ティーチングペンダント２５がバス２６を介して接続されて構成されている。なお、上記各ブロックを接続するためのインタフェースについては、各ブロックの入出力仕様にそれぞれ適した公知のインタフェース手段を用いればよく、図２ではその図示を省略している。

図１、図２のロボットシステムにおいて、ロボットアーム、ツール（エンドエフェクタ）などの各部の位置、姿勢は制御装置２により制御される。その場合、上記各部の位置や動きは、ベース座標系、メカニカルインタフェース座標系、ツール座標系（エンドエフェクタ座標系）などの座標系を基準にして制御される。このうちベース座標系はロボットシステム全体の基準となるもので、ロボットのベースの設置面などを基準に配置される。メカニカルインタフェース座標系は、上記のエンドエフェクタ（ツール）装着面を基準とした座標系である。

また、ツール座標系（エンドエフェクタ座標系）はツールの駆動制御に用いられる座標系で、特にツール各部の位置や姿勢を制御するために用いられる。ツール座標系は、メカニカルインタフェースから見たエンドエフェクタ先端の位置や姿勢を表すデータに基づいてエンドエフェクタ（ツール）先端の所定位置に設定される。通常、ツール座標系はエンドエフェクタ（ツール）の構造や、先端に設置される爪などの設計寸法に応じた所定位置に設定される。

CPU２１は、ROM２２やRAM２３に記憶された各種プログラムやティーチングペンダント２５から入力される設定等に基づいて、ロボットアーム１やツール４を制御する。例えば、CPU２１は、ROM２２やRAM２３に記憶された後述のツール座標系芯ズレ補正プログラムに従って、ツール座標系の芯ズレ補正を実行させる。ROM２２には各種プログラムや制御データ等が格納されており、RAM２３はCPU２１の作業領域として利用される。

また、ROM２２には、(E)EPROMなどのプログラマブルROMの領域も含まれる。以下の実施例では、後述の補正（校正）演算で得られたデータ等はこのプログラマブルROMの領域に記憶させるものとして説明する。ただし補正（校正）演算データ等は必ずしもROM２２に記憶させる必要はなく、システム要件その他の事情に応じてRAM２３や他の外部記憶装置（不図示）に格納してもよい。

図１、図２の構成において、マスターワーク５、６およびティーチングペンダント２５を用い、ロボットアーム１にマスターワーク５を操作させることにより、後の製造現場等におけるワークに対する操作をプログラミングすることができる。プログラムされた動作は、後に実際にワークを扱う場合に利用できるよう、所定の記録形式によって制御装置２のROM２２（あるいはRAM２３や他の外部記憶装置等）に格納される。本実施例におけるワークの操作は、例えば図５〜図７に示すように、マスターワーク６に対してマスターワーク５を結合（以下では嵌合）する操作である。

そして、本実施例では、ロボットアーム１の嵌合操作をプログラミングする際、制御装置２で図４に示すような制御を行うことにより、マスターワーク５、６（ないしワーク）の嵌合操作制御に作用させるべき必要な補正（校正）量を取得する。この制御では、まずマスターワーク５、６が準備される（図４のステップＳ１）。ここではマスターワーク６が不図示の架台上等に固定されることに、ツール４のハンド爪１１〜１３でマスターワーク５を把持する。

その後、ティーチングペンダント２５等を用いてロボットアーム１を操作してマスターワーク５をマスターワーク６の穴１６の上空に移動（ステップＳ２）し、マスターワーク５を穴１６に嵌合させる動作（ステップＳ３〜Ｓ４）を開始させる。

前述のように、実際にマスターワーク５をマスターワーク６に係合させる前の段階、図５のようにツールでマスターワーク５を把持した時点で、マスターワーク５の中心軸とツール座標系の回転中心軸が傾きズレを起こしている場合がある。このツール座標系の「傾きズレ」は前述のようにハンド爪１１〜１３やマスターワーク５（ないしは実際に現場で用いられるワーク）の精度、形状、相性といった特定の特性に応じて生じる。

本実施例では、マスターワーク５をマスターワーク６の穴１６に接触させた後、力覚センサ３を用いた後述の力覚制御を介してマスターワーク５の姿勢を制御しつつ、図６に示すようにマスターワーク５をマスターワーク６の穴１６に挿入していく。このマスターワーク５、６の係合の動作中、例えば係合開始後のある状態において、メカニカルインタフェース座標系（後述）の原点の位置（図６の^BP₁）を制御装置２のROM２２等に記憶させる（ステップＳ３）。

この力覚制御を介した嵌合操作は、図７に示すような最終結合状態に到達する（ステップＳ４）。先に記憶させた原点の位置^BP₁に加え、本実施例ではさらにそれとは異なる状態、例えば図７の最終結合状態において、メカニカルインタフェース座標系の原点の位置（図７の^BP₂）を制御装置２のROM２２等に記憶させる（ステップＳ５）。

このように、本実施例では、この嵌合操作中における異なる少なくとも２つの状態においてツール４が装着されたメカニカルインタフェース座標系の原点の少なくとも２つの異なる位置を制御装置２のROM２２等に記憶させる。記憶させるメカニカルインタフェース座標系の原点の位置は例えば図６の^BP₁と図７の^BP₂である。このマスターワーク５、６の嵌合操作中のメカニカルインタフェース座標系の原点の２つの異なる位置は上記のツール座標系の「傾きズレ」補正するのに必要な補正（校正）量を反映している。従って、ROM２２等に格納したこれらメカニカルインタフェース座標系の原点の２つの異なる位置に基づき、後述のような演算式を用いてツール座標系のマスターワーク５の中心軸との傾きズレ量を算出することができる（ステップＳ６）。

すなわち、本実施例では力覚センサ３を用いた力覚制御を作用させつつ行う係合操作中において少なくとも２つの異なる状態において、ツール４が装着されたメカニカルインタフェース座標系の原点の異なる位置（^BP₁、^BP₂）を記憶する。そして、ツール座標系のマスターワーク５の中心軸との傾きズレ量を必要な補正（校正）量として算出することができる（ステップＳ７）。

本実施例では、上記のようにマスターワーク５と６を垂直方向に所期の位置関係で嵌合させた後、さらにマスターワーク５の中心軸からツール座標系の回転中心軸までの距離のズレの補正、すなわち水平方向ズレ補正を行う（図４ステップＳ８〜Ｓ１１）。

まず、マスターワーク５と６の垂直方向の嵌合が完了した状態で、マスターワーク６に対しマスターワーク５を傾きズレ補正後のツール座標系のＸ軸（後述のX_TCP1）方向に一定力で押し付けるように力覚センサ３を用いて力覚制御を行う。その際、Ｘ軸（後述のX_TCP1）方向の力とＺ軸（後述のZ_TCP1）周りのトルクを力覚センサ３を用いて検出する（図４のステップＳ８）。また、マスターワーク６に対しマスターワーク５を傾きズレ補正後のツール座標系のＹ軸（Y_TCP1）方向に一定力で押し付けるようにロボットアーム１の力覚制御を行う。その際のＹ軸（後述のY_TCP1）方向の力とＺ軸（後述のZ_TCP1軸）周りのトルクを力覚センサ３を用いて検出する（ステップＳ９）。

さらに、後述のような演算式を用い、ステップＳ８およびＳ９で得られたＸ軸方向の力とＺ軸周りのトルク、およびＹ軸方向の力とＺ軸周りのトルクを用いてツール座標系の水平方向ズレ量を算出する（ステップＳ１０）。そして、得られた水平方向ズレ量からツール座標系の水平方向ズレを補正するのに必要な補正（校正）量を求め、ROM２２に格納する（ステップＳ１１）。

以上、本実施例におけるロボットアーム１のプログラミングおよびその際に行われるツール座標系の回転中心軸の傾きズレ補正（図４ステップＳ１〜Ｓ７）およびツール座標系水平方向ズレ補正（同Ｓ８〜Ｓ１１）を含む芯ズレ補正処理の概略を説明した。以下では、上記の補正処理における演算につき詳細に説明する。

ここで図３に本実施例のロボットシステムの座標系の概要を示す。図３において、ベース座標系（ロボットアーム１のベース底面に設定した座標系）、およびメカニカルインタフェース座標系をそれぞれΣ_B、Σ_MIと表す。

ベース座標系Σ_Bはロボットアーム１のベース底面に設定した座標系、メカニカルインタフェース座標系Σ_MIはロボットアーム１先端のメカニカルインタフェースに設定した座標系でフランジ座標系ともいう。

また、後述の芯ズレ補正処理を作用させる前のツール座標系、および芯ズレ補正後のツール座標系をそれぞれΣ_TCP、Σ_TCP2と表し、力覚センサ座標系（力覚センサに設定した座標系）をΣ_FSと表す。

また、以下では各座標系のX軸、Y軸、Z軸、X軸方向の単位ベクトル、Y軸方向の単位ベクトル、Z軸方向の単位ベクトルを、X，Y，Z，n，o，aに添え字を加えた形式でそれぞれ表す。例えば、芯ズレ補正前のツール座標系Σ_TCPのZ軸はZ_TCPと表し、メカニカルインタフェース座標系Σ_MIから見た芯ズレ補正前のツール座標系Σ_TCPのZ_TCP軸方向の単位ベクトルは、^MIa_TCPと表すものとする。

次に、本実施形態のロボットのツール座標系の芯ズレ補正処理において、芯ズレ具合を示すパラメータについて説明する。

メカニカルインタフェース座標系Σ_MIから芯ズレ補正後のツール座標系Σ_TCP2へ座標変換を行うための回転行列を^MIR_TCP2、位置ベクトルを^MIq_TCP2と表す。メカニカルインタフェース座標系Σ_MIから芯ズレ補正後のツール座標系Σ_TCP2への同次変換行列^MIT_TCP2は数１のように表される。

ただし、同次変換行列^MIT_TCP2は４×４行列、回転行列^MIR_TCP2は３×３行列、位置ベクトル^MIq_TCP2は３×１行列である。

回転行列^MIR_TCP2は、メカニカルインタフェース座標系Σ_MIをZ_MI、Y’_MI軸周りにそれぞれ角度φ、θだけ、この順番で回転させる行列である。Y’_MI軸とは、メカニカルインタフェース座標系Σ_MIをZ_MI軸周りに角度φだけ回転させて得られる座標系Σ_MI’のY軸である。ここでメカニカルインタフェース座標系Σ_MIをZ_MI軸周りに角度φだけ回転させる行列をRot(φ,Z)、メカニカルインタフェース座標系Σ_MI’をY’_MI軸周りに角度θだけ回転させる行列をRot(θ,Y) とする。これらの行列Rot(φ,z)、Rot(θ,y)はそれぞれ数２、数３のように表される。

これらの行列Rot(φ,z)とRot(θ,y)を用いると、数１の回転行列^MIR_TCP2は数４のように表される。

また、位置ベクトル^MIq_TCP2を芯ズレ補正後のツール座標系Σ_TCP2のX_TCP2Y_TCP2平面へ射影したベクトルを^MIq’_TCP2（図３）とすると、位置ベクトル^MIq_TCP2は数５のように表される。

ただし、kは定数、^MIa_TCP2はメカニカルインタフェース座標系Σ_MIから見た芯ズレ補正後のZ_TCP2軸方向の単位ベクトルである。

ツール４の３つの爪１１、１２、１３により把持されたマスターワーク５の中心軸周りにツール４を回転させるようにツール座標系の芯ズレ補正を行うには、マスターワーク５の中心軸と一致するようにツール座標系Σ_TCPのZ_TCP軸を補正する必要がある。そのためには芯ズレ補正後のツール座標系Σ_TCP2のZ_TCP2軸がマスターワーク５の中心軸と一致するような回転行列^MIR_TCP2の角度θ、φ、ならびに射影ベクトル^MIq’_TCP2を求めればよい。

本実施例では、この回転行列^MIR_TCP2の角度θ、φを求め、ツール座標系の傾きズレを補正する演算処理は「傾きズレ補正ステップ」と呼ぶ。また、射影ベクトル^MIq’_TCP2を求め、ツール座標系の水平方向のズレを補正する演算処理を「水平方向ズレ補正ステップ」と呼ぶ。

以上の事項を前提として、マスターワーク５、６を用いたロボットアーム１のツール座標系Σ_TCPの芯ズレ補正方法について、図１から図３に加え、図４から図１０を再度参照しながら以下詳細に説明する。

[1] 傾きズレ補正ステップ（図４ステップＳ１〜Ｓ７）
ツール座標系の傾きズレ補正ステップ（図４Ｓ１〜Ｓ７）につき、図５から図７を参照しながら説明する。

まず、マスターワーク５をツール４の爪１１、１２、１３で把持し、マスターワーク６は不図示の架台上に固定する（ステップS１）。マスターワーク６を配置する位置は、ロボットアーム１の可動範囲内であればよい。

次に、ロボットアーム１をマスターワーク６の穴１６の上方に移動させた後、力覚制御によるマスターワーク５とマスターワーク６との嵌合を開始する（ステップS２）。図５は傾きズレ算出動作開始前の状態を模式的に示しており、この時のロボットアーム１の動作方向は芯ズレ補正前のツール座標系Σ_TCPのZ_TCP軸方向とする。

この動作開始時点においてマスターワーク５の中心軸とマスターワーク６の中心軸は通常一致していないため、Z_TCP軸方向への移動開始後のある時点でマスターワーク５および６が接触し、それによって発生する６方向力が力覚センサ３により検出される。本実施例では、この時力覚センサ３で検出した６方向力に基づき力覚制御を行い、マスターワーク５とマスターワーク６の中心軸が一致するようロボットアーム１の姿勢を制御する。

本実施例の傾きズレ補正で用いる力覚制御方式は、マスターワーク５とマスターワーク６の結合（嵌合）操作を制御できるものであれば、いかなる力覚制御方式を用いても構わない。ここでは例えば力覚制御方式として例えばダンピング制御を適用する。このダンピング制御はロボットアーム１のX, Y, Z各軸方向の速度とそれぞれの軸周りのR_X，R_Y，R_Z軸の回転角速度の目標値V_refを６方向の力目標値F_refと力覚センサにて検知した力F_extの差分に応じて修正する制御である（数６）。

ただし、Vは６×６の実際のロボットアーム１の速度を示す行列、V_refは６×６の目標速度行列、Dはダンピング値としての６×６の粘性行列、F_extは力覚センサにて検知した外力行列、F_refは目標力行列である。本実施例では、芯ズレ補正前のツール座標系Σ_TCPの原点O_TCPを数６に従って制御する。また、力覚センサにて検知した外力F_extは、芯ズレ補正前のツール座標系Σ_TCPの原点O_TCPが作用点となるように、力覚センサ座標系Σ_FSから芯ズレ補正前のツール座標系Σ_TCPへと座標変換を行っている。

本実施例におけるダンピング制御では、目標速度をZ_TCP軸方向のみに与え、その他の軸は外力が検知されない限り静止させる。またZ_TCP軸周りの角度は固定とする。すなわち、本実施例における数６を各軸成分に分けて記述すると、数７のように表される。

ただし、上式においてV_XTCP，V_YTCP，V_ZTCPはそれぞれX_TCP，Y_TCP，Z_TCP軸方向の実際の速度、ω_RXTCP，ω_RYTCP，ω_RZTCPはそれぞれR_XTCP，R_YTCP，R_ZTCP軸方向の角速度である。また、D_XTCP，D_YTCP，D_ZTCP，D_RXTCP，D_RYTCPはそれぞれX_TCP，Y_TCP，Z_TCP，R_XTCP，R_YTCP軸方向のダンピング値である。また、F_ZTCPrefはZ_TCP軸方向の目標力である。また、F_XTCPext，F_YTCPext，F_ZTCPext，T_RXTCPext，T_RYTCPextは、それぞれ力覚センサ３で検出したX_TCP，Y_TCP，Z_TCP，R_XTCP，R_YTCP軸方向の外力である。

以上のように、本実施例の傾きズレ補正で用いる力覚制御では、力覚センサの検出量を用いて前記ロボットアームの動作を制御するに際して力覚センサの検出量に基づき所定のダンピング値を作用させ前記ロボットアームの速度を決定するダンピング制御を行う。

マスターワーク５、６が接触した後、上記の力覚制御を介してマスターワーク５、６の嵌合を進行させる。そして、図６のようにある程度までマスターワーク５が挿入された状態におけるベース座標系Σ_Bから見たメカニカルインタフェース座標系Σ_MIの原点O_MIの位置・姿勢を^BP₁としてROM２２に記憶する（ステップS３）。この時、原点の位置^BP₁を取得するタイミングは、マスターワーク５の中心軸とマスターワーク６の中心軸が一致した状態である必要があり、マスターワーク６に対してマスターワーク５をマスターワーク５の直径と同程度の深さ以上挿入させた状態が望ましい。

続いて、マスターワーク５の進入をさらに進行させ、マスターワーク５の先端がマスターワーク６の穴１６の底部に到達したら嵌合動作を終了させる（ステップS４）。この時点におけるベース座標系Σ_Bから見たメカニカルインタフェース座標系Σ_MIの原点O_MIの位置・姿勢を^BP₂としてROM２２に記憶する（ステップS５）。図７はマスターワーク５の挿入が完了した状態を模式的に示した説明図である。

以上のようにして取得した^BP₁と^BP₂を用いると、ベース座標系Σ_Bから見たマスターワーク５の中心軸を示すベクトルは^BP₂-^BP₁と表される。

次に、メカニカルインタフェース座標系Σ_MIのZ_MI軸に対するマスターワーク５の中心軸との傾きズレ量を算出する（ステップS６）。前述の通り、傾きズレ補正を行うためには、芯ズレ補正後のツール座標系Σ_TCP1のZ_TCP1軸がマスターワーク５の中心軸と一致するような回転行列^MIR_TCP1の角度θ、φを求めればよい。

ここで、傾きズレのみを補正したツール座標系Σ_TCP2を定義する。傾きズレのみを補正したツール座標系Σ_TCP1は、芯ズレ補正後のツール座標系Σ_TCP2と同一の向きであり、座標系Σ_TCP1の原点^BO_TCP1はメカニカルインタフェース座標系Σ_MIの原点^BO_MIと同一であるものとする。すなわち、メカニカルインタフェース座標系Σ_MIから傾きズレのみ補正したツール座標系Σ_TCP1への同次変換行列^MIT_TCP1、ベース座標系Σ_Bからツール座標系Σ_TCP1への位置ベクトル^Bq_TCP1はそれぞれ数８、数９のように表される。

数８における回転行列^MIR_TCP1は、その定義よりメカニカルインタフェース座標系Σ_MIから芯ズレ補正後のツール座標系Σ_TCP2への回転行列^MIR_TCP2と同一である（数１０）。

また、ベース座標系Σ_Bから見た傾きズレのみを補正したツール座標系Σ_TCP1’のZ_TCP1’軸方向の単位ベクトル^Ba_TCP1は、数１１のように表される。

ただし、数１１におけるベース座標系Σ_Bからメカニカルインタフェース座標系Σ_MIへの同次変換行列^BT_MIは既知であるとする（数１２）。

また、傾きズレのみを補正したツール座標系Σ_TCP1から見たZ_TCP1軸方向の単位ベクトル^TCP1a_TCP1は、その定義より数１３のように表される。

また、ベース座標系Σ_Bから見たZ_TCP1軸方向の単位ベクトル^Ba_TCP1は、先に求めた^BP₂-^BP₁と同一の向きであるため、数１４のように表される。

数８、数１２、数１４より、数１１は数１５のように表すことができる。

さらに、数２、数３、数９、数１０、数１３より、数１５は数１６のように表すことができる。

この数１６を解くことによりツール座標系の傾きズレ量として角度φ、θを得られる。

次に、先で求めたツール座標系の角度ズレ量φ、θを用いて、ツール座標系の角度ズレの補正（校正）量を記録する（ステップS７）。すなわち、以上のようにして求めた角度ズレ量φ、θを制御装置２のROM２２の所定領域に書き込み、ツール座標系の角度ズレ補正を完了する。なお、この時、角度ズレを補正して得られたツール座標系は先に定義したΣ_TCP1である。

以上のように、本実施例では、マスターワーク５をツール４に把持させ、力覚センサ３の検出量を用いてマスターワーク５の位置を制御しつつマスターワーク５のマスターワーク６に対する係合を開始させる。この係合の動作中における異なる少なくとも２つの状態におけるツール４が装着されたメカニカルインタフェース座標系の原点の少なくとも２つの異なる位置に基づきマスターワーク５の中心軸の位置を求める。そして、マスターワーク５の中心軸の位置に基づきツール座標系の傾きズレを算出し、算出された傾きズレに基づいてツール座標系の傾きズレを補正する処理を第１の第１のステップとして実行する。

[2] 水平方向ズレ補正ステップ（図４ステップＳ８〜Ｓ１１）
次に、ツール座標系の水平方向ズレ補正ステップについて、図８から図１０を参照しながら説明する。

図８は、本発明の実施形態に係る水平方向ズレ算出動作開始前のマスターワーク５、マスターワーク６の状態を傾きズレ補正後のツール座標系Σ_TCP1のZ_TCP1軸方向から見た図である。

まず、マスターワーク５の嵌合が完了した状態で、マスターワーク６に対しマスターワーク５を傾きズレ補正後のツール座標系Σ_TCP1のX_TCP1軸方向に一定力で押し付けるようにロボットアーム１の力覚制御を行う。そして、その際のX_TCP1軸方向の力とZ_TCP1軸周りのトルクを力覚センサ３を用いて検出する（ステップS８）。図９は、X_TCP1軸方向のみに一定力で押し付けた際のマスターワーク５、マスターワーク６の状態を傾きズレ補正後のツール座標系Σ_TCP1のZ_TCP1軸方向から見た図である。

本実施例の水平方向ズレ補正（Ｘ軸方向）で用いる力覚制御方式は、マスターワーク６に対しマスターワーク５を一定力で押し付ける操作が可能であれば、いかなる力覚制御方式でも構わない。ここでは例えば力覚制御方式としてダンピング制御を用いる。本実施例の水平方向ズレ補正（Ｘ軸方向）におけるロボットアーム１のX_TCP1，Y_TCP1，Z_TCP1軸方向の速度と、それぞれの軸周りのR_XTCP1，R_YTCP1，R_ZTCP1軸周りの回転角速度に関するダンピング制御式を数１７に示す。

ただし、V_XTCP1，V_YTCP1，V_ZTCP1はそれぞれX_TCP1，Y_TCP1，Z_TCP1軸方向の実際の速度である。また、ω_RXTCP1，ω_RYTCP1，ω_RZTCP1はそれぞれR_XTCP1，R_YTCP1，R_ZTCP1軸方向の角速度である。また、D_XTCP1，D_YTCP1はそれぞれX_TCP1，Y_TCP1軸方向のダンピング値である。また、F_XTCP1refはX_TCP1軸方向の目標力、F_XTCP1ext，F_YTCP1extはそれぞれ力覚センサ３で検出したX_TCP1，Y_TCP1軸方向の外力である。

ここで、マスターワーク６に対しマスターワーク５をX_TCP1軸方向に一定力で押し付けた際に力覚センサ３で検出したX_TCP1軸方向の力とZ_TCP1軸周りのトルクをそれぞれF’_XTCP1，M’_ZTCP1とする。又メカニカルインタフェース座標系Σ_MIから芯ズレ補正後のツール座標系Σ_TCP2への位置ベクトル^MIq_TCP2をX_TCP2Y_TCP2平面へ射影したベクトル^MIq’_TCP2とX_TCP1軸方向の単位ベクトル^TCP1n_TCP1のなす角をΨとする。その場合、F’_XTCP1とM’_ZTCP1の関係は数１８のように表される。

以上のように、本実施例の水平方向ズレ補正（Ｘ軸方向）で用いる力覚制御では、力覚センサの検出量を用いて前記ロボットアームの動作を制御する。その際、力覚センサの検出量に基づき所定のダンピング値を作用させ前記ロボットアームの速度を決定するダンピング制御を行っている。

次に、マスターワーク６に対しマスターワーク５を角度ズレ補正後のツール座標系Σ_TCP1のY_TCP1軸方向に一定力で押し付けるようにロボットアーム１の力覚制御を行う。そして、その際のY_TCP1軸方向の力とZ_TCP1軸周りのトルクを力覚センサ３を用いて検出する（ステップS９）。図１０は、Y_TCP1軸方向のみに一定力で押し付けた際のマスターワーク５、マスターワーク６の状態を傾きズレ補正後のツール座標系Σ_TCP1のZ_TCP1軸方向から見た図である。

本実施例の水平方向ズレ補正（Ｙ軸方向）で用いる力覚制御方式は、マスターワーク６に対しマスターワーク５を一定力で押し付ける操作が可能であれば、いかなる力覚制御方式でも構わない。ここでは例えば力覚制御方式としてダンピング制御を用いる。本実施例の水平方向ズレ補正（Ｙ軸方向）におけるロボットアーム１のX_TCP1，Y_TCP1，Z_TCP1軸方向の速度と、それぞれの軸周りのR_XTCP1，R_YTCP1，R_ZTCP1軸の回転角速度に関するダンピング制御式を数１９に示す。

ただし数１９において、F_YTCP1refはY_TCP1軸方向の目標力、他の文字式は数１７の場合と同様である。

ここで、マスターワーク６に対しマスターワーク５をY_TCP1軸方向のみに一定力で押し付けた際に力覚センサ３にて検出したY_TCP1軸方向の力とZ_TCP1軸周りのトルクをそれぞれF’_YTCP1，M’’_ZTCP1とする。その場合、F’_XTCP1とM’_ZTCP1の関係は、数２０のように表される。

以上のように、本実施例の水平方向ズレ補正（Ｙ軸方向）で用いる力覚制御では、力覚センサの検出量を用いて前記ロボットアームの動作を制御する。その際、力覚センサの検出量に基づき所定のダンピング値を作用させ前記ロボットアームの速度を決定するダンピング制御を行っている。

次に、ツール座標系の水平方向ズレ量を算出する（ステップS１０）。前述の通り、水平方向ズレ補正を行うためには、射影ベクトル^MIq’_TCP2を求めればよい。この時、傾きズレ補正後のツール座標系Σ_TCP1の向きは芯ズレ補正後のツール座標系Σ_TCP2と同一である。従って、X_TCP2Y_TCP2平面へ射影したベクトル^MIq’_TCP2のX_TCP2軸方向、Y_TCP2軸方向成分は、数１８、数２０よりそれぞれ数２１、数２２のように表される。

次に、数２１、数２２のように求められたツール座標系の水平方向ズレ量の補正（校正）量を記録する(S１１)。すなわち、数２１、数２２により求めたツール座標系の水平方向データを制御装置２のROM２２の所定領域に書き込む。このとき傾きズレ補正ならびに水平方向ズレ補正を行ったツール座標系、すなわち芯ズレ補正後のツール座標系は先に定義したΣ_TCP2である。

以上のように、本実施例では、マスターワーク５、６を係合させた後、力覚センサ３の検出量を用いてマスターワーク５の位置を制御しつつ、マスターワーク５をマスターワーク６に対してこれら２部材の係合方向に交差する水平の２方向にそれぞれ押し付ける。その際、マスターワーク５に作用する力およびトルクを力覚センサ３によって検出する。そして力覚センサ３のこの検出量に基づきツール座標系の水平方向ズレを算出し、算出された水平方向ズレに基づきツール座標系の水平方向ズレを補正する第２のステップとして実行する。

以上のように、本実施例のツール座標系の芯ズレ補正は、ツール座標系の傾きズレ補正（図５Ｓ１〜Ｓ７）、および水平方向ズレ補正（同Ｓ８〜Ｓ１１）から構成されている。これら第１および第２のステップ、即ち傾きズレ補正処理（図５Ｓ１〜Ｓ７）および水平方向ズレ補正ステップ（同Ｓ８〜Ｓ１１）は、芯ズレ補正プログラムとして例えばROM２２に格納しておくことができる。

ROM２２に格納した芯ズレ補正プログラムは、例えば、マスターワーク５および６を用い、作業者がティーチングペンダント２５を介してロボットアーム１を操作しつつワークの操作をプログラミングする場合に校正処理として作用させることができる。例えばツール４の３つの爪１１、１２、１３により把持したマスターワーク５をマスターワーク６の穴１６の上空に移動させた後、ROM２２に格納した上述の芯ズレ補正プログラムを実行することにより傾きズレ補正と水平方向ズレ補正を行うことができる。このとき得られたツール座標軸の傾きズレ補正量と水平方向ズレ補正量はROM２２（のプログラマブルROM領域やRAM２３等）に記憶させておくことができ、実際のワーク組付時に作用させることができる。これにより、ワークの位相を補正して嵌合などの結合操作を行う際に、ワークへの負荷や結合操作に要するタクトタイムの増大などの問題を回避できる。

以上説明したように本実施例のツール座標系の芯ズレ補正によれば、ツール座標系の水平方向ズレの補正だけでなく、傾きズレの補正も可能である。そのため、ツール座標系の水平方向ズレの補正のみを行った場合と比較して、例えばツール座標系の回転中心軸周りにツール（エンドエフェクタ）を回転させた際に、ツールで把持したマスターワーク（治具）ないしワークの位置ズレを減少させることができる。これにより、把持したマスターワークないしワークの位相を補正して嵌合などの結合操作を行う際に、マスターワークないしワークへの負荷や結合操作に要するタクトタイムの増大などの問題を回避することができる。

本発明の芯ズレ補正制御を構成するツール座標系の傾き補正、および水平方向ズレ補正の各ステップは、ロボットシステムを制御する制御装置の主制御部を構成するコンピュータ（CPU）に実行させることができる。本発明の芯ズレ補正制御を構成する上記の各ステップはロボットシステムを制御する制御装置の主制御部を構成するコンピュータ（CPU）のツール座標系補正プログラムとして実装することができる。このツール座標系補正プログラムを記録したROM、各種のプログラマブルROMなどのコンピュータ読み取り可能な記録媒体はロボットシステムを制御する制御装置の主制御部に組み込みで実装することができる。また、修理、補修、アップグレードなどを目的として、種々のコンピュータ読み取り可能な記録媒体に記録して特定のロボットシステムを対象として供給することができる。上述のコンピュータ読み取り可能な記録媒体には、各種形式の光ディスク、フラッシュメモリ、SSDなどの半導体ディスク、HDDなどの磁気ディスクなど任意の形式のデータメディアが含まれる。

以上、本発明の実施形態について説明したが、本発明は上述の実施形態にのみ限定されるものではない。また、本発明の効果は上記実施形態に記載されたものに限定されるものではない。

例えば、本実施形態においては、６軸垂直多関節のロボットアーム１を用いて説明したが、本発明はこのような構成に限定されるものではない。例えば本発明は先端軸を有する多軸多関節（軸数や関節数は任意）のロボットアームを有する任意のロボットシステムに好適に実施することができる。

また、本実施形態においては、ロボットアーム１に装着して用いるエンドエフェクタとして、３つの爪１１、１２、１３を有するツール４を用いて説明したが、本発明はこのような構成に限定されるものではない。本発明は例えば２つの爪、あるいは４つの爪等の複数爪を有するロボットハンドをツール（エンドエフェクタ）として用いるロボットシステムなどにおいても実施可能であるのはいうまでもない。

１…ロボットアーム；２…制御装置；３…力覚センサ；４…ツール（エンドエフェクタ）；５、６…マスターワーク；２１…CPU；２２…ROM；２３…RAM；２５…ティーチングペンダント；２６…バス

Claims (5)

1. ツールが先端に装着されたロボットアームと、前記ツールに把持されたワークに作用する力およびトルクを検出する力覚センサと、前記ロボットアームの動作を制御する制御装置を備えたロボットシステムにより、相互に係合する凸部または凹部の一方および他方をそれぞれ有し所定の最終結合位置関係で結合される第１のマスターワークおよび第２のマスターワークを用いて、第１のマスターワークを前記ツールに把持させ、第１および第２のマスターワークの前記凸部および凹部を係合させて第１および第２のマスターワークを結合する際に、前記ツールの駆動制御に用いられるツール座標系を補正するロボットシステムのツール座標系補正方法において、
   前記制御装置が、
   前記第１のマスターワークを前記ツールに把持させ、前記力覚センサの検出量を用いて前記ロボットアームの動作を制御しつつ第１のマスターワークの第２のマスターワークに対する係合を開始させ、この係合の動作中における異なる少なくとも２つの状態における前記ツールが装着されたメカニカルインタフェース座標系の原点の少なくとも２つの異なる位置に基づき第１のマスターワークの中心軸の位置を求め、前記第１のマスターワークの中心軸の位置に基づき前記ツール座標系の傾きズレを算出し、算出された傾きズレに基づいてツール座標系の傾きズレを補正する第１のステップと、
   第１および第２のマスターワークを係合させた後、前記力覚センサの検出量を用いて前記ロボットアームの動作を制御しつつ第１のマスターワークを第２のマスターワークに対して第１および第２のマスターワークの係合方向に交差する水平の２方向にそれぞれ押し付け、その際の前記力覚センサの検出量に基づき前記ツール座標系の水平方向ズレを算出し、算出された水平方向ズレに基づきツール座標系の水平方向ズレを補正する第２のステップと、
   から成るツール座標系の芯ズレ補正制御を行うことを特徴とするロボットシステムのツール座標系補正方法。
2. 前記第１および第２のステップにおいて力覚センサの検出量を用いて前記ロボットアームの動作を制御するに際して、力覚センサの検出量に基づき所定のダンピング値を作用させ前記ロボットアームの速度を決定するダンピング制御を行うことを特徴とする請求項１に記載のロボットシステムのツール座標系補正方法。
3. 請求項１または２に記載の各ステップをコンピュータに実行させるためのロボットシステムのツール座標系補正プログラム。
4. 請求項３に記載のロボットシステムのツール座標系補正プログラムを記録したコンピュータ読み取り可能な記録媒体。
5. ツールが先端に装着されたロボットアームと、前記ツールに把持されたワークに作用する力およびトルクを検出する力覚センサと、前記ロボットアームの動作を制御する制御装置を備え、相互に係合する凸部または凹部の一方および他方をそれぞれ有し所定の最終結合位置関係で結合される第１のマスターワークおよび第２のマスターワークを用いて、第１のマスターワークを前記ツールに把持させ、第１および第２のマスターワークの前記凸部および凹部を係合させて第１および第２のマスターワークを結合する際に、前記ツールの駆動制御に用いられるツール座標系を補正するロボットシステムにおいて、
   前記制御装置が、
   前記第１のマスターワークを前記ツールに把持させ、前記力覚センサの検出量を用いて前記ロボットアームの動作を制御しつつ第１のマスターワークの第２のマスターワークに対する係合を開始させ、この係合の動作中における異なる少なくとも２つの状態における前記ツールが装着されたメカニカルインタフェース座標系の原点の少なくとも２つの異なる位置に基づき第１のマスターワークの中心軸の位置を求め、前記第１のマスターワークの中心軸の位置に基づき前記ツール座標系の傾きズレを算出し、算出された傾きズレに基づいてツール座標系の傾きズレを補正する第１のステップと、
   第１および第２のマスターワークを係合させた後、前記力覚センサの検出量を用いて前記ロボットアームの動作を制御しつつ第１のマスターワークを第２のマスターワークに対して第１および第２のマスターワークの係合方向に交差する水平の２方向にそれぞれ押し付け、その際の前記力覚センサの検出量に基づき前記ツール座標系の水平方向ズレを算出し、算出された水平方向ズレに基づきツール座標系の水平方向ズレを補正する第２のステップと、
   から成るツール座標系の芯ズレ補正制御を行うことを特徴とするロボットシステム。

Images