(57)【特許請求の範囲】
【請求項１】
目標相対位置を入力し、
作業を遂行する複数のマニピュレータにそれぞれ設けられたエンドエフェクタの絶対座標位置を計算し、
前記各エンドエフェクタの絶対座標位置から相対座標位置を計算し、
計算された前記相対座標位置と入力された前記目標相対位置とを比較することによって、前記エンドエフェクタが、前記目標相対位置に到達したか否かを判断し、
前記目標相対位置に到達していないと判断された場合に、一つのエンドエフェクタに対向する別のエンドエフェクタの相対ヤコビアンを計算し、
前記目標相対位置、前記相対座標位置、および、前記相対ヤコビアンを用いて前記複数のマニピュレータの関節トルクを計算し、
前記相対座標位置が、前記目標相対位置に近づくように、
前記関節トルクによって前記複数のマニピュレータの協調作業を制御する、
ことを含むロボットの協調作業制御方法。
【請求項２】
前記複数のエンドエフェクタの絶対座標位置を計算することは、
前記複数のマニピュレータの関節角を測定し、前記測定された関節角の関数で前記複数のエンドエフェクタの現在の絶対座標位置を計算することである請求項１に記載のロボットの協調作業制御方法。
【請求項３】
前記複数のエンドエフェクタが予め入力された目標相対位置に到達したかを判断することをさらに含み、
前記複数のエンドエフェクタが前記目標相対位置に到達した場合、前記複数のマニピュレータの協調作業を終了する請求項１に記載のロボットの協調作業制御方法。
【請求項４】
前記目標相対位置は、前記複数のエンドエフェクタのうち何れか一つのエンドエフェクタの絶対座標位置に対する他のエンドエフェクタの軌跡を追跡して入力する請求項３に記載のロボットの協調作業制御方法。
【請求項５】
前記相対座標位置は、前記複数のエンドエフェクタのうち何れか一つのエンドエフェクタの絶対座標位置から相対エンドエフェクタの絶対座標位置を引き算した値である請求項１に記載のロボットの協調作業制御方法。
【請求項６】
インピーダンス制御が実現されるように、前記相対ヤコビアンと前記相対座標位置を使用して前記複数のマニピュレータの関節トルクを計算すること、をさらに含む請求項１に記載のロボットの協調作業制御方法。
【請求項７】
前記エンドエフェクタの一つに対向するエンドエフェクタの相対ヤコビアンを計算することにおいて、１個の相対ヤコビアンが計算される請求項１に記載のロボットの協調作業制御方法。
【請求項８】
目標相対位置を入力し、
作業を遂行する第１及び第２マニピュレータにそれぞれ設けられた第１及び第２エンドエフェクタの絶対座標位置を計算し、
前記第１及び第２エンドエフェクタのうち何れか一つの絶対座標位置から他のエンドエフェクタの相対座標位置を計算し、
計算された前記相対座標位置と入力された前記目標相対位置とを比較することによって、前記第１及び第２エンドエフェクタが、前記目標相対位置に到達したか否かを判断し、
前記目標相対位置に到達していないと判断された場合に、
一方のエンドエフェクタに対向する他方のエンドエフェクタの相対ヤコビアンを計算し、
前記目標相対位置、前記相対座標位置および、前記相対ヤコビアンを用いて前記第１及び第２マニピュレータの関節トルクを計算し、
前記相対座標位置が、前記目標相対位置に近づくように、
前記関節トルクによって前記第１及び第２マニピュレータの協調作業を制御する、
ことを含むロボットの協調作業制御方法。
【請求項９】
前記第１及び第２エンドエフェクタの絶対座標位置を計算することは、
前記第１及び第２マニピュレータの関節角（θＡ，θＢ）を測定し、前記測定された関節角（θＡ，θＢ）の関数［ｆ（θＡ，θＢ）］で前記第１及び第２エンドエフェクタの現在の絶対座標位置（ＸＡ，ＸＢ）を計算することである請求項８に記載のロボットの協調作業制御方法。
【請求項１０】
前記関節トルクを計算することは、インピーダンス制御が実現されるように、前記第１及び第２エンドエフェクタの現在の絶対座標位置（ＸＡ，ＸＢ）から計算された相対座標位置に従って相対ヤコビアン行列を用いて前記関節トルクを求めることである請求項９に記載のロボットの協調作業制御方法。
【請求項１１】
前記第１及び第２マニピュレータの関節トルクを計算する前記インピーダンス制御の式は、下記のように表す請求項１０に記載のロボットの協調作業制御方法。
（τＡ，τＢ）＝ＪｒｅｌＫｒｅｌ（Ｘｄｒｅｌ−Ｘｒｅｌ）
ここで、Ｊｒｅｌは相対ヤコビアンで、Ｘｄｒｅｌは目標相対位置で、Ｘｒｅｌは相対位置で、Ｋｒｅｌは相対座標系に対して予め入力された剛性係数で、（τＡ，τＢ）は前記第１及び第２マニピュレータの関節トルクである。
【請求項１２】
作業を遂行する複数のマニピュレータと；
前記複数のマニピュレータにそれぞれ設けられた複数のエンドエフェクタと；
目標相対位置を入力するための目標位置入力部と；
前記各エンドエフェクタの絶対座標位置を計算する現在位置計算部と；
前記各エンドエフェクタの絶対座標位置から相対座標位置を計算する相対位置計算部と；
計算された前記相対座標位置と入力された前記目標相対位置とを比較することによって、前記エンドエフェクタが、前記目標相対位置に到達したか否かを判断する位置比較部と；
前記目標相対位置に到達していないと判断された場合に、一つのエンドエフェクタに対向する別のエンドエフェクタの相対ヤコビアンを計算する相対ヤコビアン生成部と；
前記目標相対位置、前記相対座標位置、および、前記相対ヤコビアンを用いて前記複数のマニピュレータの関節トルクを計算し、前記相対座標位置が、前記目標相対位置に近づくように、前記関節トルクによって前記複数のマニピュレータの協調作業を制御する制御部と；
を含むロボット。
【請求項１３】
前記相対位置計算部は、一つのエンドエフェクタの相対座標位置を、複数のエンドエフェクタのうち前記エンドエフェクタに対向する別のエンドエフェクタの絶対座標位置から計算する請求項１２に記載のロボット。
【請求項１４】
前記相対座標位置は、前記エンドエフェクタに対向するエンドエフェクタの絶対座標位置から前記エンドエフェクタの絶対座標位置を引き算した値である請求項１３に記載のロボット。
【請求項１５】
インピーダンス制御が実現されるように、前記相対ヤコビアンと前記相対座標位置を使用して前記複数のマニピュレータの関節トルクを計算する請求項１２に記載のロボット。
【請求項１６】
相対ヤコビアン生成部は、前記エンドエフェクタの一つに対向するエンドエフェクタの１個の相対ヤコビアンを計算する請求項１２に記載のロボット。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット及びその協調作業制御方法に関するもので、より詳細には、複数のロボットマニピュレータがインピーダンス制御を通して協調作業を遂行するロボット及びその協調作業制御方法に関するものである。
【背景技術】
【０００２】
一般的に、ロボットは、電気的または磁気的な作用を用いて人間の動作と類似した運動を行う機械装置である。最近、ロボットは、制御技術の発達によって多様な分野で活用されており、その例としては、家庭での家事手伝いロボット、公共場所用サービスロボット、生産現場での搬送ロボット、作業者支援ロボットなどがある。このようなロボットは、電気的・機械的なメカニズムによって腕や手の動作と類似した運動を行えるように設けられたマニピュレータを用いて作業を遂行する。
【０００３】
現在使用されているほとんどのロボットマニピュレータは、多数個のリンクが互いに連結されて構成される。各リンクの部位を関節というが、ロボットマニピュレータの運動特性は、このような各リンクと各関節との間の幾何学的な関係によって決定される。この幾何学的な関係を数学的に表現したものが機構学（Ｋｉｎｅｍａｔｉｃｓ）であり、ほとんどのロボットマニピュレータは、このような機構学的な特性を有し、作業を遂行するための方向にロボット先端（ｅｎｄ−ｅｆｆｅｃｔｏｒ；以下、エンドエフェクタという。）を移動させる。
【０００４】
このようなロボットマニピュレータを用いて作業を遂行する例を図１に示した。図１に示すように、瓶の蓋を回して開ける作業を遂行するためには、少なくとも２台のロボットマニピュレータ１，３が必要であり、この場合、２台のロボットマニピュレータ１，３が互いに協調作業を遂行しなければならない。２台のロボットマニピュレータ１，３が協調作業を遂行するために、まず、制御部５は、二つのエンドエフェクタ１−１，３−１の軌跡を計算して各ロボットマニピュレータ１，３の絶対位置を計算し、二つのエンドエフェクタ１，３の位置と方向ベクトルによって関節トルクを計算し、それぞれのロボットマニピュレータ１，３を制御する。このように２台のロボットマニピュレータ１，３を用いて協調作業を遂行する一例は、特許文献１に開示されている。
【特許文献１】大韓民国登録特許公報第０１６０６９３号
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、従来のロボットマニピュレータを用いた協調作業方法は、ロボットの軌跡を計算するにおいて、各絶対座標系に対する二つのエンドエフェクタ１−１，３−１の位置と方向が要求されるので、複雑な協調作業を遂行する場合、使用者が一々各エンドエフェクタ１−１，３−１の位置と方向を考慮すべきであるという計算上の不便さがあり、一つのロボットマニピュレータ１が物体を把持して位置を固定した状態で他の一つのロボットマニピュレータ３が協調作業を遂行する場合、関節角の制限によって作業空間における制約を受けるという問題点がある。
【０００６】
本発明は、上記のような従来の問題点を解決するためのもので、その目的は、一つのエンドエフェクタに対する他のエンドエフェクタの相対ヤコビアンを用いるインピーダンス制御を通して作業空間の制約なしに協調作業を遂行することができるロボット及びその協調作業制御方法を提供することにある。
【課題を解決するための手段】
【０００７】
上記の目的を達成するために、本発明のロボットの協調作業制御方法は、作業を遂行する複数のマニピュレータにそれぞれ設けられたエンドエフェクタの絶対座標位置を計算し、前記各エンドエフェクタの絶対座標位置から相対座標位置を計算し、前記相対座標位置を用いて前記複数のマニピュレータの関節トルクを計算し、前記関節トルクによって前記複数のマニピュレータの協調作業を制御することを含む。
【０００８】
前記複数のエンドエフェクタの絶対座標位置を計算することは、前記複数のマニピュレータの関節角を測定し、前記測定された関節角の関数で前記複数のエンドエフェクタの現在の絶対座標位置を計算することであることを特徴とする。
【０００９】
また、本発明のロボットの協調作業制御方法は、前記複数のエンドエフェクタの絶対座標位置が予め入力された目標位置に到達したかを判断することをさらに含み、前記複数のエンドエフェクタの絶対座標位置が前記目標位置に到達した場合、前記複数のマニピュレータの協調作業を終了することを特徴とする。
【００１０】
前記目標位置は、前記複数のエンドエフェクタのうち何れか一つのエンドエフェクタの絶対座標位置に対する他のエンドエフェクタの軌跡を追跡して入力することを特徴とする。
【００１１】
前記相対座標位置を計算することは、前記各エンドエフェクタの絶対座標位置から相対エンドエフェクタの相対的な座標位置を計算することであることを特徴とする。
【００１２】
前記相対的な座標位置は、前記複数のエンドエフェクタのうち何れか一つのエンドエフェクタの絶対座標位置から相対エンドエフェクタの絶対座標位置を引き算した値であることを特徴とする。
【００１３】
また、本発明のロボットの協調作業制御方法は、前記複数のエンドエフェクタのうち何れか一つのエンドエフェクタに対する相対エンドエフェクタの相対ヤコビアンを求めることをさらに含み、前記相対ヤコビアンを求めることは、前記相対座標位置によってヤコビアン行列を用いてインピーダンス制御入力を求めることであることを特徴とする。
【００１４】
前記インピーダンス制御は、前記相対ヤコビアンと前記相対座標位置を用いて前記複数のマニピュレータの関節トルクを計算することであることを特徴とする。
【００１５】
そして、本発明のロボットの協調作業制御方法は、作業を遂行する第１及び第２マニピュレータにそれぞれ設けられた第１及び第２エンドエフェクタの絶対座標位置を計算し、前記第１及び第２エンドエフェクタのうち何れか一つの絶対座標位置から他のエンドエフェクタの相対座標位置を計算し、前記相対座標位置を用いて前記第１及び第２マニピュレータの関節トルクを計算し、前記関節トルクによって前記第１及び第２マニピュレータの協調作業を制御することを含む。
【００１６】
前記第１及び第２エンドエフェクタの絶対座標位置を計算することは、前記第１及び第２マニピュレータの関節角（θＡ，θＢ）を測定し、前記測定された関節角（θＡ，θＢ）の関数［ｆ（θＡ，θＢ）］で前記第１及び第２エンドエフェクタの現在の絶対座標位置（ＸＡ，ＸＢ）を計算することであることを特徴とする。
【００１７】
また、本発明のロボットの協調作業制御方法は、前記第１及び第２エンドエフェクタのうち何れか一つのエンドエフェクタに対する相対エンドエフェクタの相対ヤコビアンを求めることをさらに含み、前記相対ヤコビアンを求めることは、前記第１及び第２エンドエフェクタの現在の絶対座標位置（ＸＡ，ＸＢ）から計算された相対座標位置によってヤコビアン行列を用いてインピーダンス制御入力を求めることであることを特徴とする。
【００１８】
前記第１及び第２マニピュレータの関節トルクを計算する前記インピーダンス制御式は、下記のように表すことを特徴とする。
【００１９】
（τＡ，τＢ）＝ＪｒｅｌＫｒｅｌ（Ｘｄｒｅｌ−Ｘｒｅｌ）
ここで、Ｊｒｅｌは相対ヤコビアンで、Ｘｄｒｅｌは目標位置で、Ｘｒｅｌは相対位置で、Ｋｒｅｌは相対座標系に対して予め入力された剛性係数で、（τＡ，τＢ）は前記第１及び第２マニピュレータの関節トルクである。
【００２０】
そして、本発明のロボットは、作業を遂行する複数のマニピュレータと、前記複数のマニピュレータにそれぞれ設けられた複数のエンドエフェクタと、前記各エンドエフェクタの絶対座標位置から相対座標位置を計算する相対位置計算部と、前記相対座標位置を用いて前記複数のマニピュレータの関節トルクを計算し、前記関節トルクによって前記複数のマニピュレータの協調作業を制御する制御部とを含む。
【００２１】
前記相対位置計算部は、前記各エンドエフェクタの絶対座標位置から相対エンドエフェクタの相対的な座標位置を計算することを特徴とする。
【００２２】
前記相対的な座標位置は、前記複数のエンドエフェクタのうち何れか一つのエンドエフェクタの絶対座標位置から相対エンドエフェクタの絶対座標位置を引き算した値であることを特徴とする。
【００２３】
また、本発明のロボットは、前記複数のエンドエフェクタのうち何れか一つのエンドエフェクタに対する相対エンドエフェクタの相対ヤコビアンを求める相対ヤコビアン生成部をさらに含み、前記相対ヤコビアン生成部は、前記相対座標位置によってヤコビアン行列を用いてインピーダンス制御入力を求めることを特徴とする。
【００２４】
前記インピーダンス制御は、前記相対ヤコビアンと前記相対座標位置を用いて前記複数のマニピュレータの関節トルクを計算することであることを特徴とする。
【発明の効果】
【００２５】
本発明によるロボット及びその協調作業制御方法は、一つのエンドエフェクタに対する他のエンドエフェクタの相対ヤコビアンを用いるインピーダンス制御を通して複数のロボットマニピュレータが協調作業を遂行することで、複数のロボットマニピュレータを一つのロボットマニピュレータのように制御する余裕自由度の確保によって作業空間の制約を克服することができる。
【図面の簡単な説明】
【００２６】
【図１】ロボットマニピュレータを用いて協調作業を遂行する従来の例を示した図である。
【図２】本発明の実施例によるロボットの外観構成図である。
【図３】本発明の実施例によるロボットのマニピュレータの制御ブロック図である。
【図４】本発明の実施例によるロボットの協調作業制御方法を示した動作フローチャートである。
【発明を実施するための形態】
【００２７】
以下、本発明の実施例を添付された図面に基づいて詳細に説明する。
【００２８】
図２は、本発明の実施例によるロボットの外観構成図である。図２において、本発明のロボット１０は、人間と同様に、二つの脚部１１，１２によって直立移動する２足歩行ロボットとして、上体部１３、二つの腕部１４，１５（以下、第１及び第２マニピュレータという。）及び頭部１８を備えて自律的に走行する。
【００２９】
第１及び第２マニピュレータ１４，１５には、人間の腕の関節と同様に、ロボット１０の肩、ひじ及び手首に該当する部分を回転させるための多数個のリンク１４Ａ，１５Ａが互いに連結されている。各リンク１４Ａ，１５Ａの連結部位を関節といい、自由度は、各関節の動きによってある程度に動ける軸が備わることを示し、１自由度は、一方向の軸のみが備わることを示す。
【００３０】
第１及び第２マニピュレータ１４，１５の先端には、作業対象物（物体）を把持するグリッパー、対象物を塗装するために使用するスプレーガン、スポット溶接の電極接点、溶接の溶接トーチ、ドリル、グラインダー、切断用ウォータージェットなどのように、人間の手に該当する作業を遂行するエンドエフェクタ１６，１７がそれぞれ設けられている。
【００３１】
図３は、本発明の実施例によるロボットのマニピュレータの制御ブロック図で、本発明のロボットは、目標位置入力部２０、第１及び第２現在位置計算部２２，２４、位置比較部２６、相対位置計算部２８、相対ヤコビアン生成部３０、インピーダンス制御部３２、第１及び第２関節トルク制御部３４，３６を含む。
【００３２】
目標位置入力部２０は、第１及び第２マニピュレータ１４，１５を用いて協調作業を遂行するために、第１エンドエフェクタ（または第２エンドエフェクタ）１６，１７の座標系に対する第２エンドエフェクタ（または第１エンドエフェクタ）１７，１６の軌跡で第１及び第２マニピュレータ１４，１５が移動すべき目標相対位置Ｘｄｒｅｌ（以下、目標位置という。）を入力する。
【００３３】
第１及び第２現在位置計算部２２，２４は、第１及び第２マニピュレータ１４，１５の各関節角を測定する位置センサー（またはタコメータなどの速度センサー）などを用いて第１及び第２マニピュレータ１４，１５の関節角θＡ，θＢを読み込み、読み込んだ関節角θＡ，θＢから第１及び第２エンドエフェクタ１６，１７の現在の絶対座標位置ＸＡ，ＸＢ（以下、現在位置という。）をそれぞれ計算する。
【００３４】
位置比較部２６は、第１及び第２現在位置計算部２２，２４で計算された第１及び第２エンドエフェクタ１６，１７の現在位置ＸＡ，ＸＢを入力された目標位置Ｘｄｒｅｌと比較し、第１及び第２エンドエフェクタ１６，１７の現在位置ＸＡ，ＸＢが目標位置Ｘｄｒｅｌに到達したかを判断する。
【００３５】
相対位置計算部２８は、位置比較部２６の判断結果、第１及び第２エンドエフェクタ１６，１７の現在位置ＸＡ，ＸＢが目標位置Ｘｄｒｅｌに到達していない場合、第１及び第２エンドエフェクタ１６，１７の現在位置ＸＡ，ＸＢから相対座標位置Ｘｒｅｌ（以下、相対位置という。）、すなわち、相対座標系を計算する。
【００３６】
相対ヤコビアン生成部３０は、相対位置計算部２８で計算された相対位置Ｘｒｅｌ、すなわち、相対座標系を用いて第１エンドエフェクタ（または第２エンドエフェクタ）１６，１７に対する第２エンドエフェクタ（または第１エンドエフェクタ）１７，１６の相対ヤコビアンＪｒｅｌを求める。相対座標系を用いて相対ヤコビアンを求める本発明の協調作業方法によると、絶対座標系を用いる従来の方法に比べて直観的にモーションを具現することができ、第１及び第２マニピュレータ１４，１５を連結された一つのマニピュレータのように制御可能なインピーダンス制御を通して充分な余裕自由度を活用することができる。
【００３７】
インピーダンス制御部３２は、相対ヤコビアン生成部３０で求めた相対ヤコビアンＪｒｅｌ、目標位置入力部２０で入力された目標位置Ｘｄｒｅｌ、相対位置計算部２８で計算された相対位置Ｘｒｅｌ及び相対座標系に対して予め入力された剛性係数Ｋｒｅｌを用いて第１及び第２マニピュレータ１４，１５の関節トルクτＡ，τＢを計算するインピーダンス制御を遂行する。Ｋｒｅｌは、インピーダンス制御の剛性係数で、予め入力されている値である。
【００３８】
第１及び第２関節トルク制御部３４，３６は、インピーダンス制御部３２で計算された第１及び第２マニピュレータ１４，１５の関節トルクτＡ，τＢ命令によって第１及び第２マニピュレータ１４，１５を目標位置Ｘｄｒｅｌに移動させ、第１及び第２マニピュレータ１４，１５の協調作業を遂行する。
【００３９】
以下、上記のように構成されたロボット及びその協調作業制御方法の動作過程及び作用効果を説明する。
【００４０】
本発明の動作原理を説明するために、まず、第１及び第２マニピュレータ１４，１５のヤコビアンとインピーダンス制御に対して説明する。
【００４１】
第１及び第２エンドエフェクタ１６，１７の現在位置は、下記の式（１）のように関節角θの関数で表すことができる。
【００４２】
【数１】
0005733882.tif 000002
式（１）を微分して得た式（２）のＪをヤコビアンといい、デカルト（Ｃａｒｔｅｓｉａｎ）空間と関節角θの関数空間のマッピングを意味する。
【００４３】
【数２】
0005733882.tif 000003
ここで、Ｊは、絶対座標系に対するヤコビアンを示す。
【００４４】
インピーダンス制御は、第１及び第２マニピュレータ１４，１５の相互作用時に適切な力を加えられるように、大きな剛性Ｋ（インピーダンス特性における剛性係数）を有する位置制御の限界を克服し、適切に剛性を調節するための制御方法で、デカルト空間上でのインピーダンス制御式は、下記の式（３）のように表すことができる。
【００４５】
τ＝ＪＴＫ（Ｘｄ−Ｘ）        （３）
ここで、Ｘは、第１及び第２エンドエフェクタ１６，１７の現在位置を示し、Ｘｄは、第１及び第２エンドエフェクタ１６，１７の目標位置を示し、τは、第１及び第２マニピュレータ１４，１５の関節トルクを示す。なお、ＪＴは、絶対座標系に対するヤコビアンの転置行列を示す。
【００４６】
第１及び第２マニピュレータ１４，１５がインピーダンス制御を用いた協調作業を遂行する場合、まず、絶対座標系上の第１及び第２エンドエフェクタ１６，１７の軌跡から位置を計算し、式（３）を用いて関節トルクを計算する。しかしながら、このような過程を経て関節トルク命令を与えるためには、使用者が一々第１及び第２エンドエフェクタ１６，１７の位置を考慮すべきであるので、計算上の不便さをもたらす。これを克服するために提案された本発明は、第１エンドエフェクタ（または第２エンドエフェクタ）１６，１７に対して第２エンドエフェクタ（または第１エンドエフェクタ）１７，１６の相対的な座標を直観的に与えられる相対ヤコビアンの概念を考案した。本発明における相対ヤコビアンの概念は、参考文献［１］、［２］に開示された方法を使用したもので、これを図４に基づいて詳細に説明する。
【００４７】
参考文献［１］：Ａｋｉｒａ  Ｍｏｈｒｉ，Ｃｏｏｐｅｒａｔｉｖｅ  Ｐａｔｈ  Ｐｌａｎｎｉｎｇ  ｆｏｒ  Ｔｗｏ  Ｍａｎｉｐｕｌａｔｏｒｓ"，Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｃｏｎｆ．ｏｎ  Ｒｏｂｏｔｉｃｓ  ａｎｄ  Ａｕｔｏｍａｔｉｏｎ  ｐｐ．２８５３−２８５８、１９９６
参考文献［２］：Ｃｈｒｉｓｔｏｐｈｅｒ  Ｌ．Ｌｅｗｉｓ，"Ｔｒａｊｅｃｔｏｒｙ  Ｇｅｎｅｒａｔｉｏｎ  Ｆｏｒ  Ｔｗｏ  Ｒｏｂｏｔｓ  Ｃｏｏｐｅｒａｔｉｎｇ  Ｔｏ  Ｐｅｒｆｏｒｍ  Ａ  Ｔａｓｋ"，Ｉｎｔｅｒ．Ｃｏｎｆ．ｏｎ  Ｒｏｂｏｔｉｃｓ  ａｎｄ  Ａｕｔｏｍａｔｉｏｎ  ｐｐ．１６２６−１６３１，１９９６
図４は、本発明の実施例によるロボットの協調作業制御方法を示した動作フローチャートである。図４において、作業対象物に対する作業を遂行するために少なくとも２台のマニピュレータ１４，１５、すなわち、第１及び第２マニピュレータ１４，１５が必要である場合、第１及び第２マニピュレータ１４，１５が互いに協調作業を遂行すべきである。第１及び第２マニピュレータ１４，１５が協調作業を遂行するためには、まず、第１エンドエフェクタ（または第２エンドエフェクタ）１６，１７の座標系に対する第２エンドエフェクタ（または第１エンドエフェクタ）１７，１６の軌跡で第１及び第２マニピュレータ１４，１５が移動すべき目標位置Ｘｄｒｅｌを目標位置入力部２０で入力する（１００）。このとき、目標位置Ｘｄｒｅｌは、使用者（またはプログラム開発設計者）が入力する。
【００４８】
目標位置Ｘｄｒｅｌが入力されると、第１及び第２現在位置計算部２２，２４で第１及び第２マニピュレータ１４，１５の各関節角θＡ，θＢを測定する位置センサー（またはタコメータなどの速度センサー）などを用いて第１及び第２マニピュレータ１４，１５の関節角θＡ，θＢを読み込み（１０２）、読み込んだ関節角θＡ，θＢから第１及び第２エンドエフェクタ１６，１７の現在位置ＸＡ，ＸＢをそれぞれ計算する（１０４）。
【００４９】
その後、第１及び第２現在位置計算部２２，２４で計算された第１及び第２エンドエフェクタ１６，１７の現在位置ＸＡ，ＸＢを入力された目標位置Ｘｄｒｅｌと比較し、第１及び第２エンドエフェクタ１６，１７の現在位置ＸＡ，ＸＢが目標位置Ｘｄｒｅｌに到達したかを位置比較部２６で判断する（１０６）。
【００５０】
段階１０６の判断結果、第１及び第２エンドエフェクタ１６，１７の現在位置ＸＡ，ＸＢが目標位置Ｘｄｒｅｌに到達した場合、第１及び第２マニピュレータ１４，１５を用いた協調作業を終了し、第１及び第２エンドエフェクタ１６，１７の現在位置ＸＡ，ＸＢが目標位置Ｘｄｒｅｌに到達していない場合、第１及び第２エンドエフェクタ１６，１７の現在位置ＸＡ，ＸＢから相対的な座標Ｘｒｅｌ（ここで、Ｘｒｅｌ＝ＸＡ−ＸＢ）、すなわち、相対位置（相対座標系）を相対位置計算部２８で計算する（１０８）。
【００５１】
その後、相対位置計算部２８で計算された相対的な座標Ｘｒｅｌ、すなわち、相対座標系を用いて第１エンドエフェクタ（または第２エンドエフェクタ）１６，１７に対する第２エンドエフェクタ（または第１エンドエフェクタ）１７，１６の相対ヤコビアンＪｒｅｌを相対ヤコビアン生成部３０で求める（１１０）。相対座標系を用いて相対ヤコビアンを求める本発明の協調作業インピーダンス制御方法は、絶対座標系を用いる従来の方法に比べて直観的にモーションを具現することができ、第１及び第２マニピュレータ１４，１５を連結された一つのマニピュレータのように制御可能なインピーダンス制御を通して充分な余裕自由度を活用することができる。
【００５２】
第１エンドエフェクタ（または第２エンドエフェクタ）１６，１７に対する第２エンドエフェクタ（または第１エンドエフェクタ）１７，１６の相対座標系を用いて相対ヤコビアンＪｒｅｌを求める方法としては、上記に提示した参考文献［１］、［２］に開示された方法を使用した。
【００５３】
参考文献［１］、［２］に開示された方法を使用して一つのエンドエフェクタ（例えば、第１エンドエフェクタ１６）に対する他のエンドエフェクタ（例えば、第２エンドエフェクタ１７）の相対ヤコビアンＪｒｅｌが求められると、インピーダンス制御部３２は、相対ヤコビアン生成部３０から相対ヤコビアンＪｒｅｌの入力を受け、目標位置入力部２０から目標位置Ｘｄｒｅｌの入力を受け、相対位置計算部２８で計算された相対位置Ｘｒｅｌの入力を受けてインピーダンス制御を遂行することで、第１及び第２マニピュレータ１４，１５の関節トルクτＡ，τＢを計算し、これを第１及び第２関節トルク制御部３４，３６に入力する（１１２）。
【００５４】
第１及び第２マニピュレータ１４，１５の関節トルクτＡ，τＢを計算するインピーダンス制御式は、下記の式（４）のように表す。
【００５５】
（τＡ，τＢ）＝ＪｒｅｌＫｒｅｌ（Ｘｄｒｅｌ−Ｘｒｅｌ）        （４）
ここで、Ｋｒｅｌは、相対座標系に対して予め入力された剛性係数である。
【００５６】
インピーダンス制御は、第１及び第２マニピュレータ１４，１５の相互作用時に適切な力を加えて、第１及び第２マニピュレータ１４，１５の動きに硬度や柔軟さを与えることで、第１及び第２マニピュレータ１４，１５が所望の位置に移動しながら協調作業を遂行できるように関節トルクτＡ，τＢ命令を計算するアルゴリズムである。
【００５７】
したがって、第１及び第２関節トルク制御部３４，３６は、インピーダンス制御部３２で計算された第１及び第２マニピュレータ１４，１５の関節トルクτＡ，τＢ命令によって第１及び第２マニピュレータ１４，１５を目標位置Ｘｄｒｅｌに移動させて第１及び第２マニピュレータ１４，１５の協調作業を遂行しながら（１１４）、第１及び第２マニピュレータ１４，１５が目標位置Ｘｄｒｅｌに到達するまで以後の動作を反復的に進行する。
【００５８】
一方、本発明の実施例では、２台のマニピュレータ１４，１５を有する人間型ロボットを例に挙げて説明したが、本発明は、これに限定されるものでなく、多数台の産業用ロボットを用いて協調作業を遂行する場合にも、相対ヤコビアンを用いるインピーダンス制御を通して多数台のロボットを一つのロボットのように制御することで、充分な余裕自由度を確保し、作業空間の制約を克服できることはもちろんである。
【符号の説明】
【００５９】
１０  ロボット
１１，１２  脚部
１４，１５  第１及び第２マニピュレータ
１４Ａ，１５Ａ  リンク
１６，１７  第１及び第２エンドエフェクタ
１８  頭部
２０  目標位置入力部
２２，２４  第１及び第２現在位置計算部
２６  位置比較部
２８  相対位置計算部
３０  相対ヤコビアン生成部
３２  インピーダンス制御部
３４，３６  第１及び第２関節トルク制御部
【図１】
0005733882.tif 000004
【図２】
0005733882.tif 000005
【図３】
0005733882.tif 000006
【図４】
0005733882.tif 000007