特許 5939505 ロボットハンド装置と制御方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】5939505

(24)【登録日】2016年5月27日

(45)【発行日】2016年6月22日

(54)【発明の名称】ロボットハンド装置と制御方法

(51)【国際特許分類】

   B25J 13/08 20060101AFI20160609BHJP

【ＦＩ】

   B25J13/08 A

【請求項の数】5

【全頁数】16

(21)【出願番号】特願2012-114552(P2012-114552)

(22)【出願日】2012年5月18日

(65)【公開番号】特開2013-240847(P2013-240847A)

(43)【公開日】2013年12月5日

【審査請求日】2015年2月24日

(73)【特許権者】

【識別番号】000000099

【氏名又は名称】株式会社ＩＨＩ

(74)【代理人】

【識別番号】100097515

【弁理士】

【氏名又は名称】堀田 実

(74)【代理人】

【識別番号】100136700

【弁理士】

【氏名又は名称】野村 俊博

(72)【発明者】

【氏名】藤井 正和

(72)【発明者】

【氏名】江本 周平

(72)【発明者】

【氏名】曽根原 光治

【審査官】 中田 善邦

(56)【参考文献】

【文献】 特開昭６２−０２８１８７（ＪＰ，Ａ）

【文献】 特開２０１１−１４３４９４（ＪＰ，Ａ）

【文献】 特開２００３−２１１３８１（ＪＰ，Ａ）

【文献】 特開昭６３−２４１６０３（ＪＰ，Ａ）

【文献】 特開平０７−０１９８１８（ＪＰ，Ａ）

【文献】 特開平１１−３２７６２０（ＪＰ，Ａ）

【文献】 特開２０１２−１３０９７７（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｂ２５Ｊ１／００−２１／０２

(57)【特許請求の範囲】

【請求項1】

対象物に対して作業を行うロボットハンドと、
対象物の位置と姿勢を計測する対象物計測装置と、
ロボットハンドの位置と姿勢を計測するハンド計測装置と、
対象物計測装置とハンド計測装置の計測結果に基づいてロボットハンドを動作させる制御装置と、を備え、
（Ａ）現在の制御周期において、対象物計測装置は、対象物の位置と姿勢を計測し、制御装置は、当該計測した位置と姿勢に基づいて、次の制御周期におけるロボットハンドの目標位置と目標姿勢を探索し、
（Ｂ）次の制御周期において、ハンド計測装置は、ロボットハンドの位置と姿勢を計測し、制御装置は、当該計測した位置と姿勢、および、前記（Ａ）で探索した目標位置と目標姿勢に基づいてロボットハンドの位置と姿勢を制御し、
制御装置は、ロボットハンドの設定軌道を記憶する記憶部を有し、
設定軌道における複数の経由位置の各々には、該経由位置と組をなすロボットハンドの姿勢が設定されており、
前記（Ａ）では、制御装置は、
現在の制御周期で計測した対象物の位置と姿勢に基づいて、設定軌道の平行移動と回転の一方または両方を行うことにより設定軌道を移動させ、
ロボットハンドの動作制約を満たす１組の経由位置と設定姿勢を、次の制御周期における目標位置と目標姿勢として移動後の設定軌道から選択する、ロボットハンド装置。

【請求項2】

対象物に対して作業を行うロボットハンドと、
対象物の位置と姿勢を計測する対象物計測装置と、
ロボットハンドの位置と姿勢を計測するハンド計測装置と、
対象物計測装置とハンド計測装置の計測結果に基づいてロボットハンドを動作させる制御装置と、を備え、
（Ａ）現在の制御周期において、対象物計測装置は、対象物の位置と姿勢を計測し、制御装置は、当該計測した位置と姿勢に基づいて、次の制御周期におけるロボットハンドの目標位置と目標姿勢を探索し、
（Ｂ）次の制御周期において、ハンド計測装置は、ロボットハンドの位置と姿勢を計測し、制御装置は、当該計測した位置と姿勢、および、前記（Ａ）で探索した目標位置と目標姿勢に基づいてロボットハンドの位置と姿勢を制御し、
制御装置は、ロボットハンドの設定軌道を記憶する記憶部を有し、
設定軌道における複数の経由位置の各々には、該経由位置に対応付けられたロボットハンドの姿勢が設定されており、
互いに対応付けられた経由位置と設定姿勢を１組として、制御装置は、前記（Ａ）において、
（Ａ１）現在の制御周期で計測した対象物の位置と姿勢に向けて、次の制御周期に対応する１組の経由位置と設定姿勢をそれぞれ移動させるように、設定軌道の平行移動と回転の一方または両方を行うことにより設定軌道を移動させ、
（Ａ２）現在の制御周期で計測した対象物の位置と姿勢に向けて移動させた１組の前記経由位置と前記設定姿勢が、ロボットハンドの動作制約を満たすかどうかを判断し、
（Ａ３）動作制約を満たさないと判断した場合には、移動後の設定軌道における別の１組の経由位置と設定姿勢を選択し、
（Ａ４）前記（Ａ３）で選択した別の１組の経由位置と設定姿勢が、ロボットハンドの動作制約を満たすかどうかを判断し、
ロボットハンドの動作制約を満たす組の経由位置と設定姿勢が見つかるまで、前記（Ａ３）（Ａ４）を繰り返し、
前記（Ａ２）または（Ａ４）において動作制約を満たす１組の経由位置と設定姿勢が見つかった場合には、当該１組の経由位置と設定姿勢を、次の制御周期における目標位置と目標姿勢として選択する、ロボットハンド装置。

【請求項3】

前記（Ａ３）では、移動後の設定軌道において、現在の制御周期で計測した対象物の位置に向けて移動させた前記経由位置よりも手前にある経由位置と、当該経由位置の設定姿勢とを選択する、請求項２に記載のロボットハンド装置。

【請求項4】

前記（Ａ３）では、移動後の設定軌道において、現在の制御周期で計測した対象物の位置に向けて移動させた前記経由位置から、設定範囲内に存在する経由位置と、当該経由位置の設定姿勢とを選択する、請求項２に記載のロボットハンド装置。

【請求項5】

対象物に対して作業を行うロボットハンドを制御する方法であって、
（Ａ）現在の制御周期において、対象物計測装置により、対象物の位置と姿勢を計測し、制御装置により、当該計測した位置と姿勢に基づいて、次の制御周期におけるロボットハンドの目標位置と目標姿勢を探索し、
（Ｂ）次の制御周期において、ハンド計測装置により、ロボットハンドの位置と姿勢を計測し、制御装置により、当該計測した位置と姿勢、および、前記（Ａ）で探索した目標位置と目標姿勢に基づいてロボットハンドの位置と姿勢を制御し、
制御装置は、ロボットハンドの設定軌道を記憶する記憶部を有するものであり、設定軌道における複数の経由位置の各々には、該経由位置と組をなすロボットハンドの姿勢が設定されており、
前記（Ａ）では、制御装置により、
現在の制御周期で計測した対象物の位置と姿勢に基づいて、設定軌道の平行移動と回転の一方または両方を行うことにより設定軌道を移動させ、
ロボットハンドの動作制約を満たす１組の経由位置と設定姿勢を、次の制御周期における目標位置と目標姿勢として移動後の設定軌道から選択する、ロボットハンド制御方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、位置と姿勢が変化する対象物に対して作業を行うロボットハンド装置に関する。また、本発明は、ロボットハンド制御方法に関する。

【背景技術】

【0002】

対象物に対する作業として、対象物の把持、対象物への物体の取り付け、または、対象物からの物体の取り外しなどがある。

【0003】

このような作業をロボットハンドで行うことにおいて、対象物の位置や姿勢が変化する場合に、ロボットハンドを対象物に追従させる。その状態で、ロボットハンドが、対象物に対して作業を行う。例えば、対象物が、一定の移動方向に一定の速度でコンベアにより搬送されている場合に、対象物と同じ運動をロボットハンドにさせる。この状態で、ロボットハンドが、搬送中の対象物を把持する。

【0004】

このような制御は、例えば、下記の特許文献１に記載されている。特許文献１では、対象物をカメラで撮像し、撮像画像から対象物の特徴（例えば穴）を抽出し、この特徴に基づいて、ロボットハンドを対象物に追従させる制御を行っている。

【先行技術文献】

【特許文献】

【0005】

【特許文献1】特許第４２６５０８８号公報

【発明の概要】

【発明が解決しようとする課題】

【0006】

ロボットハンドの軌道を、対象物の運動に合わせて予め設定しておくことがある。例えば、対象物と同じ運動をするように、ロボットハンドが設定軌道上を移動する制御を行う。これにより、対象物とロボットハンドの相対速度をゼロにした状態で、ロボットハンドが対象物に対して作業をする。

【0007】

このように設定軌道を用いる場合においても、対象物の位置と姿勢を逐次計測し、その計測結果に応じて設定軌道を変更することができる。これにより、設定軌道を用いながら、対象物の位置や姿勢と設定軌道との間に誤差があるときや、対象物の位置や姿勢の変化を予測できないとき等にも、対象物の位置と姿勢の変化に対応できる。

【0008】

しかし、対象物の位置や姿勢の予期しない急激な変化により、対象物の位置や姿勢が、ロボットハンドの設定軌道や姿勢から大きく外れたときには、ロボットハンドの設定軌道や姿勢も急激に変更する必要がある。その結果、ロボットハンドの動作能力を超えた動作指令が、ロボットハンドに与えられる。すなわち、ロボットハンドが動作不能となってしまう。このように対象物の位置と姿勢が急激に変化することによる問題は、ロボットハンドの設定軌道を用いない制御においても同様である。

【0009】

そこで、本発明の目的は、対象物の位置と姿勢が急激に変化しても、この変化に対応するようにロボットハンドの動作を継続させる装置と方法を提供することにある。

【課題を解決するための手段】

【0010】

上述の目的を達成するため、本発明によると、対象物に対して作業を行うロボットハンドと、
対象物の位置と姿勢を計測する対象物計測装置と、
ロボットハンドの位置と姿勢を計測するハンド計測装置と、
対象物計測装置とハンド計測装置の計測結果に基づいてロボットハンドを動作させる制御装置と、を備え、
（Ａ）現在の制御周期において、対象物計測装置は、対象物の位置と姿勢を計測し、制御装置は、当該計測した位置と姿勢に基づいて、次の制御周期におけるロボットハンドの目標位置と目標姿勢を探索し、
（Ｂ）次の制御周期において、ハンド計測装置は、ロボットハンドの位置と姿勢を計測し、制御装置は、当該計測した位置と姿勢、および、前記（Ａ）で探索した目標位置と目標姿勢に基づいてロボットハンドの位置と姿勢を制御する、ことを特徴とするロボットハンド装置が提供される。

【0011】

本発明の好ましい実施形態によると、制御装置は、ロボットハンドの設定軌道を記憶する記憶部を有し、
設定軌道における複数の経由位置の各々には、該経由位置に対応付けられたロボットハンドの姿勢が設定されており、
互いに対応付けられた経由位置と設定姿勢を１組として、制御装置は、前記（Ａ）において、
（Ａ１）現在の制御周期で計測した対象物の位置と姿勢に向けて、次の制御周期に対応する１組の経由位置と設定姿勢をそれぞれ移動させるように、設定軌道の平行移動と回転の一方または両方を行うことにより設定軌道を移動させ、
（Ａ２）現在の制御周期で計測した対象物の位置と姿勢に向けて移動させた１組の前記経由位置と前記設定姿勢が、ロボットハンドの動作制約を満たすかどうかを判断し、
（Ａ３）動作制約を満たさないと判断した場合には、移動後の設定軌道における別の１組の経由位置と設定姿勢を選択し、
（Ａ４）前記（Ａ３）で選択した別の１組の経由位置と設定姿勢が、ロボットハンドの動作制約を満たすかどうかを判断し、
ロボットハンドの動作制約を満たす組の経由位置と設定姿勢が見つかるまで、前記（Ａ３）（Ａ４）を繰り返し、
前記（Ａ２）または（Ａ４）において動作制約を満たす１組の経由位置と設定姿勢が見つかった場合には、当該１組の経由位置と設定姿勢を、次の制御周期における目標位置と目標姿勢として選択する。

【0012】

好ましくは、前記（Ａ３）では、移動後の設定軌道において、現在の制御周期で計測した対象物の位置に向けて移動させた前記経由位置よりも手前にある経由位置と、当該経由位置の設定姿勢とを選択する。
代わりに、前記（Ａ３）では、移動後の設定軌道において、現在の制御周期で計測した対象物の位置に向けて移動させた前記経由位置から、設定範囲内に存在する経由位置と、当該経由位置の設定姿勢とを選択してもよい。

【0013】

参考例によると、制御装置は、前記（Ａ）において、現在の制御周期で計測した対象物の位置と姿勢から設定範囲内にあり、かつ、ロボットハンドの現在の位置と姿勢から許容範囲内にある位置と姿勢を次の制御周期におけるロボットハンドの目標位置と目標姿勢とする。

【0014】

また、上述の目的を達成するため、本発明によると、対象物に対して作業を行うロボットハンドを制御する方法であって、
（Ａ）現在の制御周期において、対象物計測装置により、対象物の位置と姿勢を計測し、制御装置により、当該計測した位置と姿勢に基づいて、次の制御周期におけるロボットハンドの目標位置と目標姿勢を探索し、
（Ｂ）次の制御周期において、ハンド計測装置により、ロボットハンドの位置と姿勢を計測し、制御装置により、当該計測した位置と姿勢、および、前記（Ａ）で探索した目標位置と目標姿勢に基づいてロボットハンドの位置と姿勢を制御する、ことを特徴とするロボットハンド制御方法が提供される。

【発明の効果】

【0015】

上述した本発明によると、現在の制御周期において計測した対象物の位置と姿勢に基づいて、次の制御周期におけるロボットハンドの目標位置と目標姿勢を探索するので、対象物の位置と姿勢が急激に変化しても、この変化に対応するようにロボットハンドの動作を継続させることができる。

【図面の簡単な説明】

【0016】

【図1】本発明の実施形態によるロボットハンド装置の概略構成図である。

【図2】本発明の実施形態によるロボットハンド制御方法を示すフローチャートである。

【図3】ロボットハンドの設定軌道の説明図である。

【図4】設定軌道の移動を説明する図である。

【図5】対象物の移動軌跡を示す一例である。

【図6】図５の場合における、時間に対する対象物の位置を示す。

【図7】図５と図６の場合に対する図１のロボットハンドの設定軌道を示す。

【図8】図５〜図７の場合のシミュレーション結果を示す。

【発明を実施するための形態】

【0017】

本発明の好ましい実施形態を図面に基づいて説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。

【0018】

図１は、本発明の実施形態によるロボットハンド装置１０の概略構成図である。ロボットハンド装置１０は、ロボットハンド３と対象物計測装置５とハンド計測装置７と制御装置９を備える。

【0019】

ロボットハンド３は、対象物１に対して作業を行う。ロボットハンド３による対象物１に対する作業は、例えば、対象物１の把持、対象物１への物体の取り付け、または、対象物１からの物体の取り外しである。

【0020】

図１の例では、ロボットハンド３は、第１〜第５のアーム３ａ〜３ｅと作業実行部３ｆを有する。
第１のアーム３ａの一端部は、ハンド支持部１１に第１の軸Ｃ１回りに回転可能に連結されている。第１のアーム３ａの他端部には、第２のアーム３ｂの一端部が第２の軸Ｃ２回りに回転可能に連結されている。第２のアーム３ｂの他端部には、第３のアーム３ｃの一端部が第３の軸Ｃ３回りに回転可能に連結されている。第３のアーム３ｃの他端部には、第４のアーム３ｄの一端部が第４の軸Ｃ４回りに回転可能に連結されている。第４のアーム３ｄの他端部には、第５のアーム３ｅの一端部が第５の軸Ｃ５回りに回転可能に連結されている。第５のアーム３ｅの他端部には、作業実行部３ｆが第６の軸Ｃ６回りに回転可能に連結されている。
作業実行部３ｆは、対象物１に対して作業を行う。作業実行部３ｆは、対象物１を吸引することにより対象物１を保持し、この吸引を停止することにより、保持していた対象物１を解放する吸引部を有するものであってよい。代わりに、作業実行部３ｆは、複数の爪を有し、これらの爪を閉じることにより対象物１を挟んで把持し、これらの爪を開くことにより、把持していた対象物１を解放するものであってもよい。また、作業実行部３ｆは、他の構成を有していてもよい。

【0021】

対象物計測装置５は、対象物１の位置と姿勢を計測する。対象物計測装置５は、例えば、対象物１を含む領域を撮像するカメラを用いたものであってよい。この場合、撮像した画像内における対象物１の位置と大きさに基づいて、対象物１の位置を求め、画像内における対象物１の向きに基づいて対象物１の姿勢を求める。なお、対象物計測装置５は、他の構成を有していてもよい。

【0022】

ハンド計測装置７は、ロボットハンド３の位置と姿勢を計測する。図１の例では、ハンド計測装置７は、第１〜第５の軸Ｃ１〜Ｃ５回りに関する第１〜第５のアーム３ａ〜３ｅの回転角と、第６の軸Ｃ６回りに関する作業実行部３ｆの回転角とを計測し、これらの回転角に基づいて、ロボットハンド３（作業実行部３ｆ）の位置と姿勢を求める。
なお、ハンド計測装置７は、第１〜第５のアーム３ａ〜３ｅと作業実行部３ｆの回転角をそれぞれ計測する複数の角度センサからなるが、図１では、簡単のため、第１のアーム３ａの回転角を計測する角度センサ７のみを図示している。

【0023】

制御装置９は、対象物計測装置５とハンド計測装置７の計測結果、および、ロボットハンド３の設定軌道に基づいてロボットハンド３を動作させる。これにより、ロボットハンド３に前記作業を行わせる。

【0024】

本実施形態では、設定軌道における複数の経由位置の各々には、該経由位置に対応付けられたロボットハンド３の姿勢が設定されている。互いに対応付けられた経由位置と設定姿勢を１組として、各組の経由位置と設定姿勢は、制御装置９の記憶部９ａに記憶されている。

【0025】

図１の例では、制御装置９は、第１〜第５のアーム３ａ〜３ｅを、それぞれ、第１〜第５の軸Ｃ５回りに回転させ、作業実行部３ｆを第６の軸Ｃ６回りに回転させる。これらの回転は、第１〜第５のアーム３ａ〜３ｅと作業実行部３ｆを回転駆動するサーボモータのドライバ（図示せず）を制御装置９が制御することにより行われてよい。

【0026】

対象物計測装置５とハンド計測装置７と制御装置９は、以下の処理（Ａ）（Ｂ）をこの順で繰り返し行う。

【0027】

（Ａ）現在の制御周期において、対象物計測装置５は、対象物１の位置と姿勢を計測し、制御装置９は、当該計測した位置と姿勢に基づいて、次の制御周期におけるロボットハンド３の目標位置と目標姿勢を探索する。

【0028】

（Ｂ）次の制御周期において、ハンド計測装置７は、ロボットハンド３の位置と姿勢を計測し、制御装置９は、当該計測した位置と姿勢、および、前記（Ａ）で探索した目標位置と目標姿勢に基づいてロボットハンド３の位置と姿勢を制御する。例えば、制御装置９は、当該次の制御周期において計測されたロボットハンド３の位置と目標位置との差を減らすように（好ましくは、この差を無くすように）ロボットハンド３の位置を制御し、当該次の制御周期において計測されたロボットハンド３の姿勢と目標姿勢との差を減らすように（好ましくは、この差を無くすように）ロボットハンド３の姿勢を制御する。

【0029】

制御周期を多数回繰り返すことにより、ロボットハンド３が待機位置から対象物１に接近して、対象物１に対して作業を行う。例えば、１つの制御周期は、１０ミリ秒であり、ロボットハンド３が作業に要する時間は、４秒である。

【0030】

図２は、本発明の実施形態によるロボットハンド制御方法を示すフローチャートである。

【0031】

この制御方法は、対象物１からの距離が設定値以下となる位置までロボットハンド３を移動させてから行う。この場合、ロボットハンド３を、この位置へ移動させるまで、制御装置９は、ロボットハンド３が設定軌道上を移動するようにロボットハンド３を制御してよい。
ただし、ロボットハンド３の待機位置と対象物１との距離が設定値以下である場合には、ロボットハンド３を待機位置から動作させる時から、本実施形態の制御方法を行ってよい。

【0032】

ステップＳ１において、現在の制御周期において、対象物計測装置５は、対象物１の位置と姿勢を計測し、制御装置９は、当該計測した位置と姿勢に基づいて、次の制御周期におけるロボットハンド３の目標位置と目標姿勢を探索する。

【0033】

次いで、制御周期ｉが次の制御周期（ｉ＋１）へ移行した後、ステップＳ２へ進む。なお、ｉは、正の整数であり制御周期の番号を示す（以下、同様）。

【0034】

ステップＳ２において、ハンド計測装置７は、ロボットハンド３の位置と姿勢を計測し、制御装置９は、当該計測した位置と姿勢、および、ステップＳ１で探索した目標位置と目標姿勢に基づいてロボットハンド３の位置と姿勢を制御する。例えば、制御装置９は、計測されたロボットハンド３の位置と、ステップＳ１で得た目標位置との差を減らすように（好ましくは、この差を無くすように）ロボットハンド３の位置を制御する。同様に、ステップＳ２において、制御装置９は、計測されたロボットハンド３の姿勢と、ステップＳ１で得た目標姿勢との差を減らすように（好ましくは、この差を無くすように）ロボットハンド３の姿勢を制御する。

【0035】

ステップＳ１は、ステップＳ１１〜Ｓ１６を有する。

【0036】

ステップＳ１１において、対象物計測装置５により、対象物１の位置と姿勢を計測する。

【0037】

ステップＳ１２において、次の制御周期でロボットハンド３（作業実行部３ｆ）が通過する予定の設定軌道の部分における１組の経由位置と設定姿勢を特定する。この１組の経由位置と設定姿勢は、好ましくは、それぞれ、予め定められた初期の設定軌道において、次の制御周期に対して予め定められた初期目標位置と初期目標姿勢である。設定軌道は、ロボットハンド３の動作制御開始時点からの各経過時間（各時点）に対して定められた経由位置からなる。従って、「次の制御周期でロボットハンド３が通過する予定の設定軌道の部分」とは、次の制御周期に対応する複数時点の経由位置を意味する。これについて、図３の上側部分は、図１の場合における作業実行部３ｆの基準点が通過する設定軌道を示し、図３の下側部分は、図３の上側部分の破線で囲んだ部分の拡大図である。図３の下側部分の例では、ｍを正の整数として、各時点ｔ_ｍ＋１〜ｔ_ｍ＋１０に対して経由位置（図３の黒丸）が定められている。図３において、例えば、現在の制御周期ｉにおける初期目標位置は、ｔ_ｍ＋５に対する経由位置であり、次の制御周期（ｉ＋１）における初期目標位置は、ｔ_ｍ＋１０に対する経由位置である。
なお、図１と図３において、水平方向を向くＸ軸と、紙面と垂直な水平方向を向くＹ軸と、鉛直方向を向くＺ軸を有する直交座標系を示している。

【0038】

ステップＳ１３において、ステップＳ１１（すなわち、現在の制御周期）で計測した対象物１の位置と姿勢に向けて（本実施形態では、ステップＳ１１で計測した対象物１の位置と姿勢へ）、ステップＳ１２で特定した１組の経由位置と設定姿勢をそれぞれ移動させるように、設定軌道を平行移動させることと、設定軌道を回転させることとの一方または両方を行うことにより、設定軌道を移動させる。ここで、計測した対象物１の当該位置は、対象物１に対して作業できるロボットハンド３（作業実行部３ｆ）の位置でもあり、計測した対象物１の当該姿勢は、対象物１に対して作業できるロボットハンド３（作業実行部３ｆ）の姿勢でもある。
なお、ステップＳ１２で特定した組の経由位置と、ステップＳ１１で計測した位置とが一致する場合には、ステップＳ１３で経由位置を移動させる量はゼロである。同様に、ステップＳ１２で特定した組の設定姿勢と、ステップＳ１１で計測した姿勢とが一致する場合には、ステップＳ１４で設定姿勢を移動させる量はゼロである。

【0039】

ステップＳ１３については、後で詳しく説明する。

【0040】

ステップＳ１４において、ステップＳ１３で移動させた設定軌道における１組の経由位置と設定軌道を選択する。現在の制御周期での最初のステップＳ１４では、移動後の設定軌道における、ステップＳ１２で特定した経由位置と設定姿勢を選択する。

【0041】

ステップＳ１５において、ステップＳ１４で選択した１組の経由位置と設定姿勢が、ロボットハンド３の動作制約を満たすかどうかを判断する。ここで、ロボットハンド３の動作制約を満たすと判断した場合には、ステップＳ２へ進み、そうでない場合には、ステップＳ１６へ進む。

【0042】

ステップＳ１５については、後で詳しく説明する。

【0043】

ステップＳ１６において、現在の制御周期で、ステップＳ１４を設定回数（２以上の整数）だけ行ったかを判断する。ここで、この判断が肯定である場合には、エラーであるとしてロボットハンド３の動作を終了させ、そうでない場合には、ステップＳ１４へ戻り、戻ったステップＳ１４において、ステップＳ１３で移動させた設定軌道（移動後の設定軌道）における別の１組の経由位置と設定姿勢を選択し、次のステップＳ１５において、当該選択した組の経由位置と設定姿勢がロボットハンドの動作制約を満たすかどうかを判断する。

【0044】

好ましくは、戻ったステップＳ１４では、移動後の設定軌道において、ステップＳ１３で対象物１の位置に向けて移動させた経由位置（すなわち、ステップＳ１２で特定して経由位置）よりも手前にある経由位置と、当該手前にある経由位置の設定姿勢とを選択する。ここで、「手前」とは、設定軌道において、上述の動作制御開始時点に対して定められた経由位置の側を意味する。より好ましくは、３回目以降のステップＳ１４では、前回のステップＳ１４で選択した経由位置よりも手前の経由位置と、当該手前にある経由位置の設定姿勢とを選択する。
代わりに、戻ったステップＳ１４では、移動後の設定軌道において、ステップＳ１３で対象物１の位置に向けて移動させた経由位置から、設定範囲内に存在する経由位置と、当該経由位置の設定姿勢とを選択してもよい。ここで、「設定範囲内」とは、ステップＳ１３で対象物１の位置に向けて移動させた経由位置からの距離が、設定値より小さいことを意味する。また、この「設定範囲」は、ステップＳ１３で対象物１の位置に向けて移動させた経由位置よりも手前の範囲に限定されない。

【0045】

その後の処理は、上述と同じであり、上述のステップＳ１４〜Ｓ１６を繰り返す。

【0046】

なお、ステップＳ１５からステップＳ２へ進む場合には、現在の制御周期ｉから次の制御周期（ｉ＋１）へ移行してから、次のステップＳ２を行う。また、次のステップＳ２では、直前のステップＳ１４で選択した１組の経由位置と設定姿勢を目標位置と目標姿勢として用いる。

【0047】

ステップＳ２は、ステップＳ２１とステップＳ２２とからなる。

【0048】

ステップＳ２１では、ハンド計測装置７により、ロボットハンド３の位置と姿勢を計測する。
ステップＳ２２では、制御装置９により、ステップＳ２１で計測したロボットハンド３の位置と姿勢、および、直前のステップＳ１４で選択した１組の経由位置と設定姿勢を目標位置と目標姿勢とし、この目標位置と目標姿勢に基づいてロボットハンド３の位置と姿勢を制御する。
ステップＳ２２を終えたら、ステップＳ１へ戻る。

【0049】

次に、上述のステップＳ１３の具体例を詳しく説明する。

【0050】

ステップＳ１３で移動させた設定軌道における、ステップＳ１４で選択した１組の経由位置と設定姿勢は、次のＱｔにより表すことができる。

Ｑｔ＝Ｓ［ｉ］Ｑ［ｃ］

Ｓ［ｉ］：ステップＳ１１で計測した位置と姿勢を表わす同次変換行列である。このＳ［ｉ］は、ロボットハンド３の座標系で表される。
Ｑ［ｃ］：ステップＳ１３で移動させた設定軌道における、ステップＳ１４で選択した１組の経由位置と設定姿勢を表わす同次変換行列である。このＱ［ｃ］は、対象物１の座標系で表される。
Ｑｔ：ロボットハンド３の位置と姿勢を表わす同次変換行列である。このＱｔは、ロボットハンド３の座標系で表される。

【0051】

図４において、破線は、ステップＳ１３で移動させる前の設定軌道の一部を示し、破線上の黒丸は経由位置を示し、破線上の黒丸と同心で書かれた破線の丸は、ステップＳ１２で特定した経由位置を示し、当該破線の丸から延びている矢印Ｄは、ステップＳ１２で特定した経由位置の設定姿勢（ロボットハンド３の向き）を示す。
図４において、実線は、ステップＳ１３で破線の設定軌道の一部から移動させた設定軌道の一部を示し、実線上の黒丸は、この移動後の設定軌道における経由位置を示し、実線上の黒丸と同心で書かれた実線の丸は、移動後の設定軌道における、ステップＳ１２で特定した経由位置を示し、当該破線の丸から延びている矢印Ｄは、移動後の設定軌道における、ステップＳ１２で特定した経由位置の設定姿勢（ロボットハンド３の向き）を示す。

【0052】

また、図４に示すように、各時点ｔ_ｍ＋３〜ｔ_ｍ＋１０に対して経由位置（図４の黒丸）が定められている。図４において、ステップＳ１２で特定した経由位置は、ｔ_ｍ＋１０に対する経由位置である。

【0053】

Ｓ［ｉ］は、次の［数１］で表される。

【数1】

ここで、ロボットハンド３の座標系が、ｘ軸とｙ軸とｚ軸を有するとする。
この［数１］において、ｘ、ｙ、ｚは、対象物１の位置を示し、それぞれ、ｘ軸座標、ｙ軸座標、ｚ軸座標であり、α、β、γは、対象物１の姿勢を示し、それぞれ、ｘ軸回りの回転角度、ｙ軸回りの回転角度、ｚ軸回りの回転角度である。

【0054】

［数１］について、次の［数２］のようにＡｘ、Ａｙ、Ａｚ、Ｐを定義すると、Ｓ［ｉ］は、下記の［数３］で表すことができる。

【数2】

【数3】

【0055】

Ｑ［ｃ］が、ステップＳ１２で特定した経由位置と設定姿勢（すなわち、図４において時点ｔ_ｍ＋１０に対する経由位置と設定姿勢）である場合には、Ｑ［ｃ］（以下、Ｑ［１０］と表記する）は、次の［数４］の同次変換行列で表すことができる。

【数4】

【0056】

この場合、図４において時点ｔ_ｍ＋９に対する経由位置と設定姿勢（以下、Ｑ［９］と表記する）は、設定軌道における時点ｔ_ｍ＋１０との相対位置と相対姿勢から求まる。例えば、Ｑ［９］は、次の［数５］で表されるとする。

【数5】

【0057】

ここで、Ｓ［ｉ］が、次の［数６］であるとする。この場合、Ｓ［ｉ］Ｑ［１０］は、下記の［数７］のようになり、Ｓ［ｉ］Ｑ［９］は下記の［数８］のようになる。

【数6】

【数7】

【数8】

【0058】

他の経由位置と設定姿勢についても同様である。

【0059】

次に、上述のステップＳ１５の動作制約を詳しく説明する。

【0060】

ステップＳ１４で用いる動作制約として、次の（１）〜（７）がある。好ましくは、動作制約は、（１）〜（７）のうち、少なくとも（１）を含む。ただし、動作制約は、（１）〜（７）のうちのいずれか１つ、または、（１）〜（７）から選択した複数のものであってもよい。

【0061】

（１）ロボットハンド３の動作速度の大きさが、しきい値より小さい。
ここで、動作速度は、現在の制御周期の目標位置と目標姿勢から次の制御周期の目標位置と目標姿勢への必要移動量を、１つの制御周期の時間で割った値であってよい。
図１の例では、動作速度は、第１〜第５のアーム３ａ〜３ｅと作業実行部３ｆの各々（以下、回転部という）の回転速度ωである。この場合、各回転速度ωは、次の式で表わされる。

ω＝（θｔ−θｃ）／Ｔ

この式において、θｔは、ロボットハンド３が次の制御周期の目標位置と目標姿勢にある時の、該回転速度ωに対応する回転部の回転角であり、θｃは、ロボットハンド３が現在の制御周期の目標位置と目標姿勢にある時の、該回転速度ωに対応する回転部の回転角である。Ｔは、１つの制御周期の時間である（以下、同様）。

【0062】

（２）ロボットハンド３の動作加速度の大きさが、しきい値より小さい。
ここで、動作加速度は、現在の制御周期におけるロボットハンド３の動作速度と、次の制御周期におけるロボットハンド３の動作速度との差を、１つの制御周期の時間で割った値であってよい。
図１の例では、動作加速度は、回転部３ａ〜３ｆの各々の回転加速度ａである。この場合、各回転加速度ａは、次の式で表わされる。

ａ＝（ω−ω_０）／Ｔ

この式において、回転速度ωは、上記（１）のωと同じであり、ω_０は、現在の制御周期における当該回転部の目標回転速度である。目標回転速度ω_０は、次の式で表わされる。

ω_０＝（θ_ｉ−θ_ｉ−１）／Ｔ

この式において、θ_ｉは、ロボットハンド３が現在の制御周期ｉにおける目標位置と目標姿勢にある時の回転部の回転角であり、θ_ｉ−１は、ロボットハンド３が、現在の制御周期より１つ前の制御周期（ｉ−１）における目標位置と目標姿勢にある時の回転部の回転角である。

【0063】

（３）対象物１に対するロボットハンド３の相対速度ｖ_ｒの大きさが、しきい値より小さい。このｖ_ｒは、例えば、次の式で表わされる。

ｖ_ｒ＝ｖ_ｈ−ｖ_ｔ

ｖ_ｈは、現在の制御周期における目標位置と目標姿勢を、次の制御周期における目標位置と目標姿勢にするのに必要なロボットハンド３（作業実行部３ｆ）の移動量を、制御周期の時間Ｔで割った値である。
ｖ_ｔは、現在の制御周期より１つ前の制御周期おける対象物１の計測された位置と姿勢を、現在の制御周期おける対象物１の計測された位置と姿勢にするのに必要な対象物１の移動量を、制御周期の時間Ｔで割った値である。

【0064】

（４）対象物１に対するロボットハンド３の相対加速度の大きさが、しきい値より小さい。
（５）ステップＳ１４で選択した１組の目標位置と目標姿勢が特異点とならない。
ここで、特異点とは、ロボットハンド３の各回転部の回転速度を作業実行部３ｆの並進移動速度に変換する行列（ヤコビ行列）に逆行列が存在しない、すなわち、作業実行部３ｆの並進移動速度から各回転部の回転速度を求めることができないことを意味する。前記ヤコビ行列の固有値の絶対値や行列式の絶対値がしきい値よりも小さいとき、前記ヤコビ行列は逆行列を持たないと判定できる。
（６）ロボットハンド３が、設定範囲外へ移動する。
（７）ロボットハンド３の軸Ｃ１〜Ｃ６が、設定範囲外へ移動する。

【0065】

上述した実施形態によると、現在の制御周期において計測した対象物１の位置と姿勢に基づいて、次の制御周期におけるロボットハンド３の目標位置と目標姿勢を探索するので、対象物１の位置や姿勢が急激に変化しても、この変化に対応したロボットハンド３の制御が可能となる。
しかも、計測した対象物１の位置と姿勢に向けて移動させた設定軌道から、次の制御周期におけるロボットハンド３の目標位置と目標姿勢を探索するので、設定軌道を反映させたロボットハンド３の動作が可能となる。
また、２回目以降のステップＳ１４では、移動後の設定軌道において、最初のステップＳ１４で選択した経由位置よりも手前にある経由位置と、当該手前にある経由位置の設定姿勢とを選択する。このように選択した経由位置と設定姿勢は、現在のロボットハンド３の位置と姿勢により近くなっている可能性が高い。その結果、例えば、上述の動作制約（１）（２）が満たされやすくなる。

【0066】

また、設定軌道や設定姿勢は、対象物１の移動軌跡や姿勢に正確に一致させなくてもよい。すなわち、大まかに定めた設定軌道であっても、対象物１の計測された位置と姿勢に向けて設定軌道を移動させるので、対象物１の位置と姿勢の変化に対応できる。

【実施例】

【0067】

図５は、対象物１の移動軌跡を示す一例である。図５の場合には、図１に示すように、対象物１が、Ｘ−Ｚ平面内で、支点を中心に振り子の運動をする。この運動は、前記支点からロープで吊るされた対象物１の運動を想定している。すなわち、前記支点が水平移動して静止すると、対象物１は振り子の運動をする。
図６は、図５の場合における、時間に対する対象物１の位置を示す。この例では、対象物１の姿勢は一定である。

【0068】

図７は、図５と図６の場合に対する図１のロボットハンド３の設定軌道を示す。この例では、設定軌道は、大まかに定められている。図７において、横軸は、時間を示し、縦軸は、Ｘ軸方向において、作業実行部３ｆから、振り子運動の中心位置（図５の原点に相当）までの距離を示す。
図８は、上述した実施形態による制御方法のシミュレーション結果を示す。図８（Ａ）は、第２のアーム３ｂの回転速度（角速度）と時間の関係を示し、図８（Ｂ）は、第３のアーム３ｃの回転速度（角速度）と時間の関係を示す。この例では、他のアーム３ａ，３ｄ，３ｅと作業実行部３ｆは回転しないものとし、作業実行部３ｆをＸ−Ｚ平面内で移動させた。
図８（Ａ）（Ｂ）の実線は、上述の動作制約を用いなかった場合を示す。すなわち、図８（Ａ）（Ｂ）の実線は、ステップＳ１５、Ｓ１６を省略し、ステップＳ１４の次にステップＳ２１を行った場合を示す。図８（Ａ）（Ｂ）の破線は、上述の動作制約を用いた場合を示す。すなわち、図８（Ａ）（Ｂ）の破線は、図２のフローチャートに従った場合を示す。ここでは、動作制約は、第２および第３のアーム３ｂ、３ｃの角速度の大きさが７５度／秒以下となることである。なお、図８（Ａ）（Ｂ）において、約０．２〜０．９秒の範囲以外で、破線は実線と重なっている。

【0069】

図６と図８（Ａ）（Ｂ）によると、作業実行部３ｆは、対象物１の振り子運動に追従するように動作している。

【0070】

本発明は上述した実施の形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更を加え得ることは勿論である。

【0071】

（参考例）
ロボットハンド３の設定軌道を用いなくてもよい。この参考例では、ステップＳ１（すなわち前記（Ａ）の処理）では、現在の制御周期で、対象物計測装置５により、対象物１の位置と姿勢を計測し、制御装置９により、当該計測した位置と姿勢から設定範囲内にあり、かつ、ロボットハンド３の現在の位置と姿勢から許容範囲内にある位置と姿勢を次の制御周期におけるロボットハンド３の目標位置と目標姿勢とする。この場合、ロボットハンド３が移動する範囲を含む範囲において、間隔をおいて、多数の候補位置を設定しておき、制御装置９により、これらの候補位置から、次の制御周期におけるロボットハンド３の目標位置を選択してよい。また、ロボットハンド３の多数の候補姿勢を設定しておき、制御装置９により、これらの候補姿勢から、次の制御周期におけるロボットハンド３の目標姿勢を選択してよい。なお、ロボットハンド３の現在の位置と姿勢は、ロボットハンド３の位置と姿勢の最新の計測値であってよい。ステップＳ２は、上述と同じである。

【0072】

（変更例）
上述の実施形態では、ステップＳ１３では、ステップＳ１２で特定した経由位置を、ステップＳ１１で計測した対象物１の位置に移動させたが、これの代わりに、ステップＳ１３において、ステップＳ１２で特定した経由位置を、ステップＳ１１で計測した対象物１の位置へ向けて移動させて当該位置へ近づけてもよい。
同様に、上述のステップＳ１３では、ステップＳ１２で特定した設定姿勢を、ステップＳ１１で計測した対象物１の姿勢に移動（回転）させたが、これの代わりに、ステップＳ１３において、ステップＳ１２で特定した設定姿勢を、ステップＳ１１で計測した対象物１の姿勢へ向けて移動させて当該姿勢へ近づけてもよい。
この場合、他の点は、上述と同じであってもよいし、本発明の範囲内で上述の内容から適宜してもよい。

【符号の説明】

【0073】

１ 対象物、３ ロボットハンド、３ａ〜３ｅ アーム、３ｆ 作業実行部、５ 対象物計測装置、７ ハンド計測装置、９ 制御装置、９ａ 記憶部、１０ ロボットハンド装置、１１ ハンド支持部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】