JP5733882B2 - ロボット及びその協調作業制御方法 - Google Patents

Description

本発明は、ロボット及びその協調作業制御方法に関するもので、より詳細には、複数のロボットマニピュレータがインピーダンス制御を通して協調作業を遂行するロボット及びその協調作業制御方法に関するものである。

一般的に、ロボットは、電気的または磁気的な作用を用いて人間の動作と類似した運動を行う機械装置である。最近、ロボットは、制御技術の発達によって多様な分野で活用されており、その例としては、家庭での家事手伝いロボット、公共場所用サービスロボット、生産現場での搬送ロボット、作業者支援ロボットなどがある。このようなロボットは、電気的・機械的なメカニズムによって腕や手の動作と類似した運動を行えるように設けられたマニピュレータを用いて作業を遂行する。

現在使用されているほとんどのロボットマニピュレータは、多数個のリンクが互いに連結されて構成される。各リンクの部位を関節というが、ロボットマニピュレータの運動特性は、このような各リンクと各関節との間の幾何学的な関係によって決定される。この幾何学的な関係を数学的に表現したものが機構学（Ｋｉｎｅｍａｔｉｃｓ）であり、ほとんどのロボットマニピュレータは、このような機構学的な特性を有し、作業を遂行するための方向にロボット先端（ｅｎｄ−ｅｆｆｅｃｔｏｒ；以下、エンドエフェクタという。）を移動させる。

このようなロボットマニピュレータを用いて作業を遂行する例を図１に示した。図１に示すように、瓶の蓋を回して開ける作業を遂行するためには、少なくとも２台のロボットマニピュレータ１，３が必要であり、この場合、２台のロボットマニピュレータ１，３が互いに協調作業を遂行しなければならない。２台のロボットマニピュレータ１，３が協調作業を遂行するために、まず、制御部５は、二つのエンドエフェクタ１−１，３−１の軌跡を計算して各ロボットマニピュレータ１，３の絶対位置を計算し、二つのエンドエフェクタ１，３の位置と方向ベクトルによって関節トルクを計算し、それぞれのロボットマニピュレータ１，３を制御する。このように２台のロボットマニピュレータ１，３を用いて協調作業を遂行する一例は、特許文献１に開示されている。
大韓民国登録特許公報第０１６０６９３号

しかしながら、従来のロボットマニピュレータを用いた協調作業方法は、ロボットの軌跡を計算するにおいて、各絶対座標系に対する二つのエンドエフェクタ１−１，３−１の位置と方向が要求されるので、複雑な協調作業を遂行する場合、使用者が一々各エンドエフェクタ１−１，３−１の位置と方向を考慮すべきであるという計算上の不便さがあり、一つのロボットマニピュレータ１が物体を把持して位置を固定した状態で他の一つのロボットマニピュレータ３が協調作業を遂行する場合、関節角の制限によって作業空間における制約を受けるという問題点がある。

本発明は、上記のような従来の問題点を解決するためのもので、その目的は、一つのエンドエフェクタに対する他のエンドエフェクタの相対ヤコビアンを用いるインピーダンス制御を通して作業空間の制約なしに協調作業を遂行することができるロボット及びその協調作業制御方法を提供することにある。

上記の目的を達成するために、本発明のロボットの協調作業制御方法は、作業を遂行する複数のマニピュレータにそれぞれ設けられたエンドエフェクタの絶対座標位置を計算し、前記各エンドエフェクタの絶対座標位置から相対座標位置を計算し、前記相対座標位置を用いて前記複数のマニピュレータの関節トルクを計算し、前記関節トルクによって前記複数のマニピュレータの協調作業を制御することを含む。

前記複数のエンドエフェクタの絶対座標位置を計算することは、前記複数のマニピュレータの関節角を測定し、前記測定された関節角の関数で前記複数のエンドエフェクタの現在の絶対座標位置を計算することであることを特徴とする。

また、本発明のロボットの協調作業制御方法は、前記複数のエンドエフェクタの絶対座標位置が予め入力された目標位置に到達したかを判断することをさらに含み、前記複数のエンドエフェクタの絶対座標位置が前記目標位置に到達した場合、前記複数のマニピュレータの協調作業を終了することを特徴とする。

前記目標位置は、前記複数のエンドエフェクタのうち何れか一つのエンドエフェクタの絶対座標位置に対する他のエンドエフェクタの軌跡を追跡して入力することを特徴とする。

前記相対座標位置を計算することは、前記各エンドエフェクタの絶対座標位置から相対エンドエフェクタの相対的な座標位置を計算することであることを特徴とする。

前記相対的な座標位置は、前記複数のエンドエフェクタのうち何れか一つのエンドエフェクタの絶対座標位置から相対エンドエフェクタの絶対座標位置を引き算した値であることを特徴とする。

また、本発明のロボットの協調作業制御方法は、前記複数のエンドエフェクタのうち何れか一つのエンドエフェクタに対する相対エンドエフェクタの相対ヤコビアンを求めることをさらに含み、前記相対ヤコビアンを求めることは、前記相対座標位置によってヤコビアン行列を用いてインピーダンス制御入力を求めることであることを特徴とする。

前記インピーダンス制御は、前記相対ヤコビアンと前記相対座標位置を用いて前記複数のマニピュレータの関節トルクを計算することであることを特徴とする。

そして、本発明のロボットの協調作業制御方法は、作業を遂行する第１及び第２マニピュレータにそれぞれ設けられた第１及び第２エンドエフェクタの絶対座標位置を計算し、前記第１及び第２エンドエフェクタのうち何れか一つの絶対座標位置から他のエンドエフェクタの相対座標位置を計算し、前記相対座標位置を用いて前記第１及び第２マニピュレータの関節トルクを計算し、前記関節トルクによって前記第１及び第２マニピュレータの協調作業を制御することを含む。

前記第１及び第２エンドエフェクタの絶対座標位置を計算することは、前記第１及び第２マニピュレータの関節角（θ_Ａ，θ_Ｂ）を測定し、前記測定された関節角（θ_Ａ，θ_Ｂ）の関数［ｆ（θ_Ａ，θ_Ｂ）］で前記第１及び第２エンドエフェクタの現在の絶対座標位置（Ｘ_Ａ，Ｘ_Ｂ）を計算することであることを特徴とする。

また、本発明のロボットの協調作業制御方法は、前記第１及び第２エンドエフェクタのうち何れか一つのエンドエフェクタに対する相対エンドエフェクタの相対ヤコビアンを求めることをさらに含み、前記相対ヤコビアンを求めることは、前記第１及び第２エンドエフェクタの現在の絶対座標位置（Ｘ_Ａ，Ｘ_Ｂ）から計算された相対座標位置によってヤコビアン行列を用いてインピーダンス制御入力を求めることであることを特徴とする。

前記第１及び第２マニピュレータの関節トルクを計算する前記インピーダンス制御式は、下記のように表すことを特徴とする。

（τ_Ａ，τ_Ｂ）＝Ｊ_ｒｅｌＫ_ｒｅｌ（Ｘ_ｄ ^ｒｅｌ−Ｘ^ｒｅｌ）
ここで、Ｊ_ｒｅｌは相対ヤコビアンで、Ｘ_ｄ ^ｒｅｌは目標位置で、Ｘ^ｒｅｌは相対位置で、Ｋ_ｒｅｌは相対座標系に対して予め入力された剛性係数で、（τ_Ａ，τ_Ｂ）は前記第１及び第２マニピュレータの関節トルクである。

そして、本発明のロボットは、作業を遂行する複数のマニピュレータと、前記複数のマニピュレータにそれぞれ設けられた複数のエンドエフェクタと、前記各エンドエフェクタの絶対座標位置から相対座標位置を計算する相対位置計算部と、前記相対座標位置を用いて前記複数のマニピュレータの関節トルクを計算し、前記関節トルクによって前記複数のマニピュレータの協調作業を制御する制御部とを含む。

前記相対位置計算部は、前記各エンドエフェクタの絶対座標位置から相対エンドエフェクタの相対的な座標位置を計算することを特徴とする。

前記相対的な座標位置は、前記複数のエンドエフェクタのうち何れか一つのエンドエフェクタの絶対座標位置から相対エンドエフェクタの絶対座標位置を引き算した値であることを特徴とする。

また、本発明のロボットは、前記複数のエンドエフェクタのうち何れか一つのエンドエフェクタに対する相対エンドエフェクタの相対ヤコビアンを求める相対ヤコビアン生成部をさらに含み、前記相対ヤコビアン生成部は、前記相対座標位置によってヤコビアン行列を用いてインピーダンス制御入力を求めることを特徴とする。

前記インピーダンス制御は、前記相対ヤコビアンと前記相対座標位置を用いて前記複数のマニピュレータの関節トルクを計算することであることを特徴とする。

本発明によるロボット及びその協調作業制御方法は、一つのエンドエフェクタに対する他のエンドエフェクタの相対ヤコビアンを用いるインピーダンス制御を通して複数のロボットマニピュレータが協調作業を遂行することで、複数のロボットマニピュレータを一つのロボットマニピュレータのように制御する余裕自由度の確保によって作業空間の制約を克服することができる。

ロボットマニピュレータを用いて協調作業を遂行する従来の例を示した図である。 本発明の実施例によるロボットの外観構成図である。 本発明の実施例によるロボットのマニピュレータの制御ブロック図である。 本発明の実施例によるロボットの協調作業制御方法を示した動作フローチャートである。

以下、本発明の実施例を添付された図面に基づいて詳細に説明する。

図２は、本発明の実施例によるロボットの外観構成図である。図２において、本発明のロボット１０は、人間と同様に、二つの脚部１１，１２によって直立移動する２足歩行ロボットとして、上体部１３、二つの腕部１４，１５（以下、第１及び第２マニピュレータという。）及び頭部１８を備えて自律的に走行する。

第１及び第２マニピュレータ１４，１５には、人間の腕の関節と同様に、ロボット１０の肩、ひじ及び手首に該当する部分を回転させるための多数個のリンク１４Ａ，１５Ａが互いに連結されている。各リンク１４Ａ，１５Ａの連結部位を関節といい、自由度は、各関節の動きによってある程度に動ける軸が備わることを示し、１自由度は、一方向の軸のみが備わることを示す。

第１及び第２マニピュレータ１４，１５の先端には、作業対象物（物体）を把持するグリッパー、対象物を塗装するために使用するスプレーガン、スポット溶接の電極接点、溶接の溶接トーチ、ドリル、グラインダー、切断用ウォータージェットなどのように、人間の手に該当する作業を遂行するエンドエフェクタ１６，１７がそれぞれ設けられている。

図３は、本発明の実施例によるロボットのマニピュレータの制御ブロック図で、本発明のロボットは、目標位置入力部２０、第１及び第２現在位置計算部２２，２４、位置比較部２６、相対位置計算部２８、相対ヤコビアン生成部３０、インピーダンス制御部３２、第１及び第２関節トルク制御部３４，３６を含む。

目標位置入力部２０は、第１及び第２マニピュレータ１４，１５を用いて協調作業を遂行するために、第１エンドエフェクタ（または第２エンドエフェクタ）１６，１７の座標系に対する第２エンドエフェクタ（または第１エンドエフェクタ）１７，１６の軌跡で第１及び第２マニピュレータ１４，１５が移動すべき目標相対位置Ｘ_ｄ ^ｒｅｌ（以下、目標位置という。）を入力する。

第１及び第２現在位置計算部２２，２４は、第１及び第２マニピュレータ１４，１５の各関節角を測定する位置センサー（またはタコメータなどの速度センサー）などを用いて第１及び第２マニピュレータ１４，１５の関節角θ_Ａ，θ_Ｂを読み込み、読み込んだ関節角θ_Ａ，θ_Ｂから第１及び第２エンドエフェクタ１６，１７の現在の絶対座標位置Ｘ_Ａ，Ｘ_Ｂ（以下、現在位置という。）をそれぞれ計算する。

位置比較部２６は、第１及び第２現在位置計算部２２，２４で計算された第１及び第２エンドエフェクタ１６，１７の現在位置Ｘ_Ａ，Ｘ_Ｂを入力された目標位置Ｘ_ｄ ^ｒｅｌと比較し、第１及び第２エンドエフェクタ１６，１７の現在位置Ｘ_Ａ，Ｘ_Ｂが目標位置Ｘ_ｄ ^ｒｅｌに到達したかを判断する。

相対位置計算部２８は、位置比較部２６の判断結果、第１及び第２エンドエフェクタ１６，１７の現在位置Ｘ_Ａ，Ｘ_Ｂが目標位置Ｘ_ｄ ^ｒｅｌに到達していない場合、第１及び第２エンドエフェクタ１６，１７の現在位置Ｘ_Ａ，Ｘ_Ｂから相対座標位置Ｘ^ｒｅｌ（以下、相対位置という。）、すなわち、相対座標系を計算する。

相対ヤコビアン生成部３０は、相対位置計算部２８で計算された相対位置Ｘ^ｒｅｌ、すなわち、相対座標系を用いて第１エンドエフェクタ（または第２エンドエフェクタ）１６，１７に対する第２エンドエフェクタ（または第１エンドエフェクタ）１７，１６の相対ヤコビアンＪ_ｒｅｌを求める。相対座標系を用いて相対ヤコビアンを求める本発明の協調作業方法によると、絶対座標系を用いる従来の方法に比べて直観的にモーションを具現することができ、第１及び第２マニピュレータ１４，１５を連結された一つのマニピュレータのように制御可能なインピーダンス制御を通して充分な余裕自由度を活用することができる。

インピーダンス制御部３２は、相対ヤコビアン生成部３０で求めた相対ヤコビアンＪ_ｒｅｌ、目標位置入力部２０で入力された目標位置Ｘ_ｄ ^ｒｅｌ、相対位置計算部２８で計算された相対位置Ｘ^ｒｅｌ及び相対座標系に対して予め入力された剛性係数Ｋ_ｒｅｌを用いて第１及び第２マニピュレータ１４，１５の関節トルクτ_Ａ，τ_Ｂを計算するインピーダンス制御を遂行する。Ｋ_ｒｅｌは、インピーダンス制御の剛性係数で、予め入力されている値である。

第１及び第２関節トルク制御部３４，３６は、インピーダンス制御部３２で計算された第１及び第２マニピュレータ１４，１５の関節トルクτ_Ａ，τ_Ｂ命令によって第１及び第２マニピュレータ１４，１５を目標位置Ｘ_ｄ ^ｒｅｌに移動させ、第１及び第２マニピュレータ１４，１５の協調作業を遂行する。

以下、上記のように構成されたロボット及びその協調作業制御方法の動作過程及び作用効果を説明する。

本発明の動作原理を説明するために、まず、第１及び第２マニピュレータ１４，１５のヤコビアンとインピーダンス制御に対して説明する。

第１及び第２エンドエフェクタ１６，１７の現在位置は、下記の式（１）のように関節角θの関数で表すことができる。

式（１）を微分して得た式（２）のＪをヤコビアンといい、デカルト（Ｃａｒｔｅｓｉａｎ）空間と関節角θの関数空間のマッピングを意味する。

ここで、Ｊは、絶対座標系に対するヤコビアンを示す。

インピーダンス制御は、第１及び第２マニピュレータ１４，１５の相互作用時に適切な力を加えられるように、大きな剛性Ｋ（インピーダンス特性における剛性係数）を有する位置制御の限界を克服し、適切に剛性を調節するための制御方法で、デカルト空間上でのインピーダンス制御式は、下記の式（３）のように表すことができる。

τ＝Ｊ ^Ｔ Ｋ（Ｘ_ｄ−Ｘ） （３）
ここで、Ｘは、第１及び第２エンドエフェクタ１６，１７の現在位置を示し、Ｘ_ｄは、第１及び第２エンドエフェクタ１６，１７の目標位置を示し、τは、第１及び第２マニピュレータ１４，１５の関節トルクを示す。なお、Ｊ ^Ｔ は、絶対座標系に対するヤコビアンの転置行列を示す。

第１及び第２マニピュレータ１４，１５がインピーダンス制御を用いた協調作業を遂行する場合、まず、絶対座標系上の第１及び第２エンドエフェクタ１６，１７の軌跡から位置を計算し、式（３）を用いて関節トルクを計算する。しかしながら、このような過程を経て関節トルク命令を与えるためには、使用者が一々第１及び第２エンドエフェクタ１６，１７の位置を考慮すべきであるので、計算上の不便さをもたらす。これを克服するために提案された本発明は、第１エンドエフェクタ（または第２エンドエフェクタ）１６，１７に対して第２エンドエフェクタ（または第１エンドエフェクタ）１７，１６の相対的な座標を直観的に与えられる相対ヤコビアンの概念を考案した。本発明における相対ヤコビアンの概念は、参考文献［１］、［２］に開示された方法を使用したもので、これを図４に基づいて詳細に説明する。

参考文献［１］：Ａｋｉｒａ Ｍｏｈｒｉ，Ｃｏｏｐｅｒａｔｉｖｅ Ｐａｔｈ Ｐｌａｎｎｉｎｇ ｆｏｒ Ｔｗｏ Ｍａｎｉｐｕｌａｔｏｒｓ"，Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆ．ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ ｐｐ．２８５３−２８５８、１９９６
参考文献［２］：Ｃｈｒｉｓｔｏｐｈｅｒ Ｌ．Ｌｅｗｉｓ，"Ｔｒａｊｅｃｔｏｒｙ Ｇｅｎｅｒａｔｉｏｎ Ｆｏｒ Ｔｗｏ Ｒｏｂｏｔｓ Ｃｏｏｐｅｒａｔｉｎｇ Ｔｏ Ｐｅｒｆｏｒｍ Ａ Ｔａｓｋ"，Ｉｎｔｅｒ．Ｃｏｎｆ．ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ ｐｐ．１６２６−１６３１，１９９６
図４は、本発明の実施例によるロボットの協調作業制御方法を示した動作フローチャートである。図４において、作業対象物に対する作業を遂行するために少なくとも２台のマニピュレータ１４，１５、すなわち、第１及び第２マニピュレータ１４，１５が必要である場合、第１及び第２マニピュレータ１４，１５が互いに協調作業を遂行すべきである。第１及び第２マニピュレータ１４，１５が協調作業を遂行するためには、まず、第１エンドエフェクタ（または第２エンドエフェクタ）１６，１７の座標系に対する第２エンドエフェクタ（または第１エンドエフェクタ）１７，１６の軌跡で第１及び第２マニピュレータ１４，１５が移動すべき目標位置Ｘ_ｄ ^ｒｅｌを目標位置入力部２０で入力する（１００）。このとき、目標位置Ｘ_ｄ ^ｒｅｌは、使用者（またはプログラム開発設計者）が入力する。

目標位置Ｘ_ｄ ^ｒｅｌが入力されると、第１及び第２現在位置計算部２２，２４で第１及び第２マニピュレータ１４，１５の各関節角θ_Ａ，θ_Ｂを測定する位置センサー（またはタコメータなどの速度センサー）などを用いて第１及び第２マニピュレータ１４，１５の関節角θ_Ａ，θ_Ｂを読み込み（１０２）、読み込んだ関節角θ_Ａ，θ_Ｂから第１及び第２エンドエフェクタ１６，１７の現在位置Ｘ_Ａ，Ｘ_Ｂをそれぞれ計算する（１０４）。

その後、第１及び第２現在位置計算部２２，２４で計算された第１及び第２エンドエフェクタ１６，１７の現在位置Ｘ_Ａ，Ｘ_Ｂを入力された目標位置Ｘ_ｄ ^ｒｅｌと比較し、第１及び第２エンドエフェクタ１６，１７の現在位置Ｘ_Ａ，Ｘ_Ｂが目標位置Ｘ_ｄ ^ｒｅｌに到達したかを位置比較部２６で判断する（１０６）。

段階１０６の判断結果、第１及び第２エンドエフェクタ１６，１７の現在位置Ｘ_Ａ，Ｘ_Ｂが目標位置Ｘ_ｄ ^ｒｅｌに到達した場合、第１及び第２マニピュレータ１４，１５を用いた協調作業を終了し、第１及び第２エンドエフェクタ１６，１７の現在位置Ｘ_Ａ，Ｘ_Ｂが目標位置Ｘ_ｄ ^ｒｅｌに到達していない場合、第１及び第２エンドエフェクタ１６，１７の現在位置Ｘ_Ａ，Ｘ_Ｂから相対的な座標Ｘ^ｒｅｌ（ここで、Ｘ^ｒｅｌ＝Ｘ_Ａ−Ｘ_Ｂ）、すなわち、相対位置（相対座標系）を相対位置計算部２８で計算する（１０８）。

その後、相対位置計算部２８で計算された相対的な座標Ｘ^ｒｅｌ、すなわち、相対座標系を用いて第１エンドエフェクタ（または第２エンドエフェクタ）１６，１７に対する第２エンドエフェクタ（または第１エンドエフェクタ）１７，１６の相対ヤコビアンＪ_ｒｅｌを相対ヤコビアン生成部３０で求める（１１０）。相対座標系を用いて相対ヤコビアンを求める本発明の協調作業インピーダンス制御方法は、絶対座標系を用いる従来の方法に比べて直観的にモーションを具現することができ、第１及び第２マニピュレータ１４，１５を連結された一つのマニピュレータのように制御可能なインピーダンス制御を通して充分な余裕自由度を活用することができる。

第１エンドエフェクタ（または第２エンドエフェクタ）１６，１７に対する第２エンドエフェクタ（または第１エンドエフェクタ）１７，１６の相対座標系を用いて相対ヤコビアンＪ_ｒｅｌを求める方法としては、上記に提示した参考文献［１］、［２］に開示された方法を使用した。

参考文献［１］、［２］に開示された方法を使用して一つのエンドエフェクタ（例えば、第１エンドエフェクタ１６）に対する他のエンドエフェクタ（例えば、第２エンドエフェクタ１７）の相対ヤコビアンＪ_ｒｅｌが求められると、インピーダンス制御部３２は、相対ヤコビアン生成部３０から相対ヤコビアンＪ_ｒｅｌの入力を受け、目標位置入力部２０から目標位置Ｘ_ｄ ^ｒｅｌの入力を受け、相対位置計算部２８で計算された相対位置Ｘ^ｒｅｌの入力を受けてインピーダンス制御を遂行することで、第１及び第２マニピュレータ１４，１５の関節トルクτ_Ａ，τ_Ｂを計算し、これを第１及び第２関節トルク制御部３４，３６に入力する（１１２）。

第１及び第２マニピュレータ１４，１５の関節トルクτ_Ａ，τ_Ｂを計算するインピーダンス制御式は、下記の式（４）のように表す。

（τ_Ａ，τ_Ｂ）＝Ｊ_ｒｅｌＫ_ｒｅｌ（Ｘ_ｄ ^ｒｅｌ−Ｘ^ｒｅｌ） （４）
ここで、Ｋ_ｒｅｌは、相対座標系に対して予め入力された剛性係数である。

インピーダンス制御は、第１及び第２マニピュレータ１４，１５の相互作用時に適切な力を加えて、第１及び第２マニピュレータ１４，１５の動きに硬度や柔軟さを与えることで、第１及び第２マニピュレータ１４，１５が所望の位置に移動しながら協調作業を遂行できるように関節トルクτ_Ａ，τ_Ｂ命令を計算するアルゴリズムである。

したがって、第１及び第２関節トルク制御部３４，３６は、インピーダンス制御部３２で計算された第１及び第２マニピュレータ１４，１５の関節トルクτ_Ａ，τ_Ｂ命令によって第１及び第２マニピュレータ１４，１５を目標位置Ｘ_ｄ ^ｒｅｌに移動させて第１及び第２マニピュレータ１４，１５の協調作業を遂行しながら（１１４）、第１及び第２マニピュレータ１４，１５が目標位置Ｘ_ｄ ^ｒｅｌに到達するまで以後の動作を反復的に進行する。

一方、本発明の実施例では、２台のマニピュレータ１４，１５を有する人間型ロボットを例に挙げて説明したが、本発明は、これに限定されるものでなく、多数台の産業用ロボットを用いて協調作業を遂行する場合にも、相対ヤコビアンを用いるインピーダンス制御を通して多数台のロボットを一つのロボットのように制御することで、充分な余裕自由度を確保し、作業空間の制約を克服できることはもちろんである。

１０ ロボット
１１，１２ 脚部
１４，１５ 第１及び第２マニピュレータ
１４Ａ，１５Ａ リンク
１６，１７ 第１及び第２エンドエフェクタ
１８ 頭部
２０ 目標位置入力部
２２，２４ 第１及び第２現在位置計算部
２６ 位置比較部
２８ 相対位置計算部
３０ 相対ヤコビアン生成部
３２ インピーダンス制御部
３４，３６ 第１及び第２関節トルク制御部

Claims (16)

1. 目標相対位置を入力し、
   作業を遂行する複数のマニピュレータにそれぞれ設けられたエンドエフェクタの絶対座標位置を計算し、
   前記各エンドエフェクタの絶対座標位置から相対座標位置を計算し、
   計算された前記相対座標位置と入力された前記目標相対位置とを比較することによって、前記エンドエフェクタが、前記目標相対位置に到達したか否かを判断し、
   前記目標相対位置に到達していないと判断された場合に、一つのエンドエフェクタに対向する別のエンドエフェクタの相対ヤコビアンを計算し、
   前記目標相対位置、前記相対座標位置、および、前記相対ヤコビアンを用いて前記複数のマニピュレータの関節トルクを計算し、
   前記相対座標位置が、前記目標相対位置に近づくように、
   前記関節トルクによって前記複数のマニピュレータの協調作業を制御する、
   ことを含むロボットの協調作業制御方法。
2. 前記複数のエンドエフェクタの絶対座標位置を計算することは、
   前記複数のマニピュレータの関節角を測定し、前記測定された関節角の関数で前記複数のエンドエフェクタの現在の絶対座標位置を計算することである請求項１に記載のロボットの協調作業制御方法。
3. 前記複数のエンドエフェクタが予め入力された目標相対位置に到達したかを判断することをさらに含み、
   前記複数のエンドエフェクタが前記目標相対位置に到達した場合、前記複数のマニピュレータの協調作業を終了する請求項１に記載のロボットの協調作業制御方法。
4. 前記目標相対位置は、前記複数のエンドエフェクタのうち何れか一つのエンドエフェクタの絶対座標位置に対する他のエンドエフェクタの軌跡を追跡して入力する請求項３に記載のロボットの協調作業制御方法。
5. 前記相対座標位置は、前記複数のエンドエフェクタのうち何れか一つのエンドエフェクタの絶対座標位置から相対エンドエフェクタの絶対座標位置を引き算した値である請求項１に記載のロボットの協調作業制御方法。
6. インピーダンス制御が実現されるように、前記相対ヤコビアンと前記相対座標位置を使用して前記複数のマニピュレータの関節トルクを計算すること、をさらに含む請求項１に記載のロボットの協調作業制御方法。
7. 前記エンドエフェクタの一つに対向するエンドエフェクタの相対ヤコビアンを計算することにおいて、１個の相対ヤコビアンが計算される請求項１に記載のロボットの協調作業制御方法。
8. 目標相対位置を入力し、
   作業を遂行する第１及び第２マニピュレータにそれぞれ設けられた第１及び第２エンドエフェクタの絶対座標位置を計算し、
   前記第１及び第２エンドエフェクタのうち何れか一つの絶対座標位置から他のエンドエフェクタの相対座標位置を計算し、
   計算された前記相対座標位置と入力された前記目標相対位置とを比較することによって、前記第１及び第２エンドエフェクタが、前記目標相対位置に到達したか否かを判断し、
   前記目標相対位置に到達していないと判断された場合に、
   一方のエンドエフェクタに対向する他方のエンドエフェクタの相対ヤコビアンを計算し、
   前記目標相対位置、前記相対座標位置および、前記相対ヤコビアンを用いて前記第１及び第２マニピュレータの関節トルクを計算し、
   前記相対座標位置が、前記目標相対位置に近づくように、
   前記関節トルクによって前記第１及び第２マニピュレータの協調作業を制御する、
   ことを含むロボットの協調作業制御方法。
9. 前記第１及び第２エンドエフェクタの絶対座標位置を計算することは、
   前記第１及び第２マニピュレータの関節角（θ_Ａ，θ_Ｂ）を測定し、前記測定された関節角（θ_Ａ，θ_Ｂ）の関数［ｆ（θ_Ａ，θ_Ｂ）］で前記第１及び第２エンドエフェクタの現在の絶対座標位置（Ｘ_Ａ，Ｘ_Ｂ）を計算することである請求項８に記載のロボットの協調作業制御方法。
10. 前記関節トルクを計算することは、インピーダンス制御が実現されるように、前記第１及び第２エンドエフェクタの現在の絶対座標位置（Ｘ_Ａ，Ｘ_Ｂ）から計算された相対座標位置に従って相対ヤコビアン行列を用いて前記関節トルクを求めることである請求項９に記載のロボットの協調作業制御方法。
11. 前記第１及び第２マニピュレータの関節トルクを計算する前記インピーダンス制御の式は、下記のように表す請求項１０に記載のロボットの協調作業制御方法。
    （τ_Ａ，τ_Ｂ）＝Ｊ_ｒｅｌＫ_ｒｅｌ（Ｘ_ｄ ^ｒｅｌ−Ｘ^ｒｅｌ）
    ここで、Ｊ_ｒｅｌは相対ヤコビアンで、Ｘ_ｄ ^ｒｅｌは目標相対位置で、Ｘ^ｒｅｌは相対位置で、Ｋ_ｒｅｌは相対座標系に対して予め入力された剛性係数で、（τ_Ａ，τ_Ｂ）は前記第１及び第２マニピュレータの関節トルクである。
12. 作業を遂行する複数のマニピュレータと；
    前記複数のマニピュレータにそれぞれ設けられた複数のエンドエフェクタと；
    目標相対位置を入力するための目標位置入力部と；
    前記各エンドエフェクタの絶対座標位置を計算する現在位置計算部と；
    前記各エンドエフェクタの絶対座標位置から相対座標位置を計算する相対位置計算部と；
    計算された前記相対座標位置と入力された前記目標相対位置とを比較することによって、前記エンドエフェクタが、前記目標相対位置に到達したか否かを判断する位置比較部と；
    前記目標相対位置に到達していないと判断された場合に、一つのエンドエフェクタに対向する別のエンドエフェクタの相対ヤコビアンを計算する相対ヤコビアン生成部と；
    前記目標相対位置、前記相対座標位置、および、前記相対ヤコビアンを用いて前記複数のマニピュレータの関節トルクを計算し、前記相対座標位置が、前記目標相対位置に近づくように、前記関節トルクによって前記複数のマニピュレータの協調作業を制御する制御部と；
    を含むロボット。
13. 前記相対位置計算部は、一つのエンドエフェクタの相対座標位置を、複数のエンドエフェクタのうち前記エンドエフェクタに対向する別のエンドエフェクタの絶対座標位置から計算する請求項１２に記載のロボット。
14. 前記相対座標位置は、前記エンドエフェクタに対向するエンドエフェクタの絶対座標位置から前記エンドエフェクタの絶対座標位置を引き算した値である請求項１３に記載のロボット。
15. インピーダンス制御が実現されるように、前記相対ヤコビアンと前記相対座標位置を使用して前記複数のマニピュレータの関節トルクを計算する請求項１２に記載のロボット。
16. 相対ヤコビアン生成部は、前記エンドエフェクタの一つに対向するエンドエフェクタの１個の相対ヤコビアンを計算する請求項１２に記載のロボット。
    

Images