JP2017177321A - 制御装置及びロボットシステム - Google Patents

Abstract

【課題】生体又は物体の少なくとも一方が挟まれても、衝突による衝撃を安全なレベルに低減できる制御装置及びロボットシステムを提供する。【解決手段】制御装置は、第１マニピュレーター２０を制御する制御装置４であって、生体又は物体の少なくとも一方が第１範囲内に位置することを検出する第１検出部８からの情報を取得する検出取得部７８と、第１マニピュレーター２０とは異なる第２マニピュレーター２０ａの速度を取得する速度取得部７４と、第１マニピュレーター２０の速度を第１速度以下に制御する制御部８４と、を有し、制御部８４は、検出取得部７８が情報を取得した場合に、第１マニピュレーター２０と第２マニピュレーター２０ａとの相対速度が第２速度以下になるように、第１マニピュレーター２０の速度を制御する。【選択図】図２

Description

本発明は、制御装置及びロボットシステムに関するものである。

従来、人と共存することを目的として、可動部（手先とは限らない）の速度を安全速度（例えば２５０ｍｍ／ｓ）以下に制御する機能安全ロボットが提案されている（例えば、特許文献１参照）。

特開２０１０−２０８００２号公報

しかしながら、特許文献１では、複数の機能安全ロボットの可動部（以下、マニピュレーター）が、接触し得るほど又は僅かな隙間を残して、近接して設置されている場合に、各マニピュレーターが安全速度以下であっても、マニピュレーター間の相対速度は２倍になり得る。これら複数本のマニピュレーターの間に生体及び物体が挟まれるおそれがある。

また、１本のマニピュレーターと衝突する場合よりも、複数本のマニピュレーターに挟まれる場合の方が逃げ場も少なくなり、一層危険になる。さらに、複数本のマニピュレーターに同時に衝突した場合は特に危険である。

本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態又は適用例として実現することが可能である。

［適用例１］本適用例に係る制御装置は、第１マニピュレーターを制御する制御装置であって、生体又は物体の少なくとも一方が第１範囲内に位置することを検出する第１検出部からの情報を取得する検出取得部と、前記第１マニピュレーターとは異なる第２マニピュレーターの速度を取得する速度取得部と、前記第１マニピュレーターの速度を第１速度以下に制御する制御部と、を有し、前記制御部は、前記検出取得部が前記情報を取得した場合に、前記第１マニピュレーターと前記第２マニピュレーターとの相対速度が第２速度以下になるように、前記第１マニピュレーターの速度を制御することを特徴とする。

本適用例によれば、第１マニピュレーターの速度を制御することで、第１マニピュレーターと第２マニピュレーターとの相対速度を第２速度以下にできる。これにより、第１マニピュレーターと第２マニピュレーターとの間に生体又は物体の少なくとも一方が挟まれても、衝突による衝撃を安全なレベルに低減できる制御装置を提供できる。

ここで、生体とは、例えば、人と、人以外の犬、猫等の動物とが含まれる。

［適用例２］上記適用例に記載の制御装置において、前記第１マニピュレーター及び前記第２マニピュレーターの各速度は、前記第１マニピュレーターと前記第２マニピュレーターとが互いに接近する方向に移動する速度であることが好ましい。

本適用例によれば、各マニピュレーター同士が接近する方向に移動する相対速度を第２速度以下にできるので、複数本のマニピュレーターに挟まれた場合にも衝突による衝撃を安全なレベルに低減できる。

［適用例３］上記適用例に記載の制御装置において、前記制御部は、前記検出取得部が前記情報を取得した場合に、前記第１マニピュレーターの先端と前記第２マニピュレーターの先端との相対速度が前記第２速度以下になるように、前記第１マニピュレーターの先端の速度を制御することが好ましい。

本適用例によれば、第１マニピュレーターの速度を制御することで、第１マニピュレーターの先端と第２マニピュレーターの先端との相対速度を第２速度以下にできる。

ここで、マニピュレーターにエンドエフェクターが設けられている場合、マニピュレーターの先端とは、エンドエフェクターの先端を指すこととする。

［適用例４］上記適用例に記載の制御装置において、前記第２マニピュレーターの位置を取得する位置取得部を有し、前記制御部は、前記検出取得部が前記情報を取得した場合、かつ前記第１マニピュレーターの先端と前記第２マニピュレーターの先端との間の距離が第１距離以下の場合に、前記第１マニピュレーターの先端と前記第２マニピュレーターの先端との相対速度が前記第２速度以下になるように、前記第１マニピュレーターの先端の速度を制御することが好ましい。

本適用例によれば、第１マニピュレーターの先端と第２マニピュレーターの先端との間の距離が第１距離以下の場合に、第１マニピュレーターの速度を制御することで、第１マニピュレーターの先端と第２マニピュレーターの先端との相対速度を第２速度以下にできる。

［適用例５］上記適用例に記載の制御装置において、前記第１マニピュレーターは、関節を有し、前記第２マニピュレーターは、関節を有し、前記制御部は、前記検出取得部が前記情報を取得した場合に、前記第１マニピュレーターの関節と前記第２マニピュレーターの関節との相対速度が前記第２速度以下になるように、前記第１マニピュレーターの関節の速度を制御することが好ましい。

本適用例によれば、第１マニピュレーターの速度を制御することで、第１マニピュレーターの関節と第２マニピュレーターの関節との相対速度を第２速度以下にできる。

［適用例６］上記適用例に記載の制御装置において、前記第１マニピュレーターは、複数の関節を有し、前記第２マニピュレーターは、複数の関節を有し、前記制御部は、前記第１マニピュレーターの複数の関節のうちの一つの第１関節と、前記第２マニピュレーターの複数の関節のうちの一つの第２関節との距離が第１距離以下の場合に、前記第１関節と前記第２関節との相対速度が前記第２速度以下になるように、前記第１関節の速度を制御することが好ましい。

本適用例によれば、第１マニピュレーターの速度を制御することで、第１マニピュレーターの複数の関節のうちの一つの第１関節と第２マニピュレーターの複数の関節のうちの一つの第２関節との相対速度を第２速度以下にできる。

［適用例７］上記適用例に記載の制御装置において、前記制御部は、前記第１関節と前記第２関節との距離が前記第１距離以下の場合に、前記第１マニピュレーターの複数の関節のうちの一つの前記第１関節とは異なる関節の速度を低下することが好ましい。

本適用例によれば、第１マニピュレーターの複数の関節のうちの一つの第１関節と第２マニピュレーターの複数の関節のうちの一つの第２関節との間の距離が第１距離以下の場合に、第１マニピュレーターの複数の関節のうちの一つの第１関節とは異なる関節の速度を低下できる。

［適用例８］上記適用例に記載の制御装置において、前記第１範囲は、前記第１マニピュレーターの可動範囲を含むことが好ましい。

本適用例によれば、第１マニピュレーターの可動範囲を第１範囲が含むことにより、衝突による衝撃を安全なレベルに低減できる。

［適用例９］上記適用例に記載の制御装置において、前記第１範囲は、前記第１マニピュレーターの可動範囲及び前記第２マニピュレーターの可動範囲を含むことが好ましい。

本適用例によれば、第１マニピュレーターの可動範囲及び第２マニピュレーターの可動範囲を第１範囲が含むことにより、衝突による衝撃を安全なレベルに低減できる。

［適用例１０］上記適用例に記載の制御装置において、前記第１速度は、前記第２速度と等しいことが好ましい。

本適用例によれば、安定した動作を行える。

［適用例１１］上記適用例に記載の制御装置において、前記第１速度及び前記第２速度は、２５０ｍｍ／ｓであることが好ましい。

本適用例によれば、近接したマニピュレーターの相対速度を安全速度（２５０ｍｍ／ｓ）以下に制御することができる。

安全速度とは、例えば、ＩＳＯ１０２１８−１に規定されているように、産業用ロボットと人とが共存可能な速度の上限である２５０ｍｍ／ｓｅｃ以内の速度で、事前のリスクアセスメントなどによって決定される。

［適用例１２］上記適用例に記載の制御装置において、前記制御部は、前記第１マニピュレーターと前記第２マニピュレーターとを制御することが好ましい。

本適用例によれば、２つのマニピュレーターの両方で同時に把持できる把持パターンを選択することで、次の動作の障害等にならない把持手法を決定すること等が可能になる。なお、マニピュレーターは３本以上あってもかまわない。

［適用例１３］本適用例に係るロボットシステムは、上記のいずれか一項に記載の制御装置と、前記制御装置により制御されるロボットと、を有することを特徴とする。

本適用例によれば、上記のいずれか一項に記載の制御装置と、制御装置により制御されるロボットと、を有するので、第１マニピュレーターの速度を制御することで、第１マニピュレーターと第２マニピュレーターとの相対速度を第２速度以下にできる。これにより、第１マニピュレーターと第２マニピュレーターとの間に生体又は物体の少なくとも一方が挟まれても、衝突による衝撃を安全なレベルに低減できるロボットシステムを提供できる。

第１実施形態に係るロボットシステムの構成を示す構成図。 第１実施形態に係るロボットシステムを示すブロック図。 第１実施形態に係る複数本のマニピュレーターと衝突する場合の安全速度を説明する図。 第２実施形態に係るロボットシステムを示すブロック図。 第３実施形態に係るロボットシステムの構成を示す構成図。 第３実施形態に係るロボットシステムを示すブロック図。

以下、本発明を具体化した実施形態について図面に従って説明する。なお、使用する図面は、説明する部分が認識可能な状態となるように、適宜拡大又は縮小して表示している。

（第１実施形態）
本実施形態のロボットシステムは、複数のロボットの可動部（以下、マニピュレーター）が、接触し得るほど又は僅かな隙間を残して、近接して設置されている。
図１は、本実施形態に係るロボットシステムの構成を示す構成図である。
本実施形態に係るロボットシステムは、制御装置としての第１制御装置４と、第１制御装置４により制御されるロボットとしての第１ロボット６と、第１検出部としての人侵入センサー８と、第２制御装置４ａと、第２制御装置４ａにより制御される第２ロボット６ａと、を備える。なお、第２ロボット６ａの構造は、第１ロボット６の構造と同様なので、第１ロボット６の構造を説明して、その説明を省略する。

本実施形態に係る第１ロボット６は、図１に示すように、基台１０に固定された第１マニピュレーター２０と、第１マニピュレーター２０の先端側に設けられた第１エンドエフェクター３０と、を備える。

基台１０は、第１マニピュレーター２０を回動させるための動力を発生させるモーター４４（図２参照）及びモーター４４を制御する第１制御装置４等を収納する機能を有する。また、基台１０は、例えば、床、壁、天井、及び移動可能な台車上などに固定される。

第１マニピュレーター２０は、関節としての軸回転ジョイント２１ａ，２１ｂと関節としての平面回転ジョイント２２ａ，２２ｂ，２２ｃとにより、基台軸１０ａとアーム２３ａ，２３ｂ，２３ｃ，２３ｄと、を繋ぎ、自由度５を有している。軸回転ジョイント２１ａ，２１ｂはアーム回りに両回転し、平面回転ジョイント２２ａ，２２ｂ，２２ｃは、繋ぎあうアームを相対的に両回転させる。

第１ロボット６は、各軸のモーター４４及び各軸のエンコーダー４２を備える（図２参照）。各軸のエンコーダー４２は、各関節の角度と速度とを取得する。エンコーダーは２重化されていてもよい。

第１エンドエフェクター３０は、第１指及び第２指を備えている。第１マニピュレーター２０の駆動により第１エンドエフェクター３０が所定の動作位置まで到達した後、第１指及び第２指の離間距離を調整することにより、対象物を把持することができる。第１エンドエフェクター３０は、対象物を把持する機能を備えている。

なお、第１エンドエフェクター３０は、ここでは、ハンドであるが、本実施形態では、これに限定されるものではない。エンドエフェクターの他の例としては、例えば、部品検査用器具、部品搬送用器具、部品加工用器具、部品組立用器具、及び測定器等が挙げられる。

なお、図示の構成では、第１マニピュレーター２０は、合計４本のアームによって構成されているが、本実施形態はこれに限られない。第１マニピュレーター２０が、１本のアームに構成されている場合、２〜４本のアームによって構成されている場合、及び６本以上のアームによって構成されている場合も本実施形態の範囲内である。

また、第１ロボット６と第２ロボット６ａとの各基台１０間の距離は既知である。さらに、各第１及び第２マニピュレーター２０，２０ａの各関節間の距離も既知である。

人侵入センサー８は、人が第１ロボット６の可動範囲内及びその近傍にいることを検知する。

図２は、本実施形態に係るロボットシステムを示すブロック図である。
本実施形態の第１制御装置４は、速度演算部７２と、速度取得部７４と、相対速度演算部７６と、検出取得部７８と、相対速度制限部８０と、位置制御部８２と、制御部としての速度制御部８４と、電流制御部８６と、を備える。

速度演算部７２は、各軸のエンコーダー４２から入力された第１マニピュレーター２０の各軸回転ジョイント２１ａ，２１ｂと各平面回転ジョイント２２ａ，２２ｂ，２２ｃの角度と速度とから各軸回転ジョイント２１ａ，２１ｂと各平面回転ジョイント２２ａ，２２ｂ，２２ｃや第１エンドエフェクター３０の先端の速度と方向とを演算する。

速度取得部７４は、第２マニピュレーター２０ａの各軸回転ジョイント２１ｃ，２１ｄ及び各平面回転ジョイント２２ｄ，２２ｅ，２２ｆや第２エンドエフェクター３０ａの先端の速度と方向とを取得する。

相対速度演算部７６は、第１マニピュレーター２０の各軸回転ジョイント２１ａ，２１ｂ及び各平面回転ジョイント２２ａ，２２ｂ，２２ｃや第１エンドエフェクター３０の先端と、第２マニピュレーター２０ａの各軸回転ジョイント２１ｃ，２１ｄ及び各平面回転ジョイント２２ｄ，２２ｅ，２２ｆや第２エンドエフェクター３０ａの先端との相対速度を演算する。

検出取得部７８は、生体又は物体の少なくとも一方が第１範囲内に位置することを検出する人侵入センサー８からの情報を取得する。ここで、生体とは、例えば、人と、人以外の犬、猫等の動物とが含まれる。

第１範囲は、第１マニピュレーター２０の可動範囲を含むことが好ましい。これによれば、第１マニピュレーター２０の可動範囲を第１範囲が含むことにより、衝突による衝撃を安全なレベルに低減できる。

第１範囲は、第１マニピュレーター２０の可動範囲及び第２マニピュレーター２０ａの可動範囲を含むことが好ましい。これによれば、第１マニピュレーターの可動範囲及び第２マニピュレーターの可動範囲を第１範囲が含むことにより、衝突による衝撃を安全なレベルに低減できる。

相対速度制限部８０は、相対速度が制限値以下になるように第１マニピュレーター２０の関節速度指令を制限する。相対速度制限部８０は、特定のマニピュレーターの優先度を高め、他の（優先度が低い）マニピュレーターと比べて、速度制限がかかりにくいようにしてもよい。

また、相対速度制限部８０は、速度制限した関節に連動して、他の関節も減速してもよい。これによれば、ある関節の速度を単純に制限することによる手先の軌道のずれを抑えることができる。したがって、手先の軌道の追従性が向上する。

さらに、相対速度制限部８０は、７軸ロボットなど、冗長自由度がある場合には、速度制御した関節があっても、他の関節の角度や速度を調整することで、手先の軌道の追従性を維持してもよい。これによれば、手先が目標位置へ到達する時間の遅れを抑えることができる。

なお、相対速度制限部８０は、相対速度を常に制限するのではなく、人侵入センサー８からの情報により、人が可動範囲内にいる場合にだけ制限してもよい。

位置制御部８２は、各軸のエンコーダー４２から入力された軸回転ジョイント２１ａ，２１ｂ及び平面回転ジョイント２２ａ，２２ｂ，２２ｃの角度と速度とに基づいて、予め定められた経路上を第１エンドエフェクター３０が移動するように、第１マニピュレーター２０を駆動させる信号を出力する。位置制御部８２は、各軸のモーター４４の回転速度である速度指令値を示す信号を関節速度指令として速度制御部８４に出力する。

速度制御部８４は、第１マニピュレーター２０の軸回転ジョイント２１ａ，２１ｂ及び平面回転ジョイント２２ａ，２２ｂ，２２ｃの速度を関節速度指令に追従させる。速度制御部８４は、第１マニピュレーター２０の速度を第１速度以下に制御する。速度制御部８４は、検出取得部７８が人侵入センサー８からの情報を取得した場合に、第１マニピュレーター２０と第２マニピュレーター２０ａとの相対速度が第２速度以下になるように、第１マニピュレーター２０の速度を制御する。

第１速度は、第２速度と等しいことが好ましい。これによれば、安定した動作を行える。

第１速度及び第２速度は、２５０ｍｍ／ｓであることが好ましい。これによれば、近接した第１及び第２マニピュレーター２０，２０ａの相対速度を安全速度（２５０ｍｍ／ｓ）以下に制御することができる。

安全速度とは、例えば、ＩＳＯ１０２１８−１に規定されているように、産業用ロボットと人とが共存可能な速度の上限である２５０ｍｍ／ｓｅｃ以内の速度で、事前のリスクアセスメントなどによって決定される。

図３は、本実施形態に係る複数本のマニピュレーターと衝突する場合の安全速度を説明する図である。
例えば、第１マニピュレーター２０或いは第２マニピュレーター２０ａと衝突する場合よりも、第１マニピュレーター２０と第２マニピュレーター２０ａとに挟まれる場合の方が逃げ場も少なくなり、一層危険になる。さらに、第１マニピュレーター２０と第２マニピュレーター２０ａとに同時に衝突した場合は特に危険である。

しかしながら、本実施形態では、第１マニピュレーター２０及び第２マニピュレーター２０ａの各速度は、第１マニピュレーター２０と第２マニピュレーター２０ａとが互いに接近する方向に移動する速度である。これによれば、各マニピュレーター２０，２０ａ同士が接近する方向に移動する相対速度を第２速度以下にできるので、第１及び第２マニピュレーター２０，２０ａに挟まれた場合にも衝突による衝撃を安全なレベルに低減できる。

第１マニピュレーター２０と第２マニピュレーター２０ａとが互いに斜め方向から接近する。例えば、図３に示すように、第１マニピュレーター２０が移動する第１方向の速度ベクトルを第１速度ベクトルＳ１とする。第１マニピュレーター２０が移動する方向と、第１マニピュレーター２０が第２マニピュレーター２０ａに接近する方向との角度を第１角度θ１とする。第１マニピュレーター２０が第２マニピュレーター２０ａに接近する方向に投影した速度ベクトルを第２速度ベクトルＳ２とする。第２速度ベクトルＳ２は、第１マニピュレーター２０が第２マニピュレーター２０ａに接近する方向に移動する速度ベクトルであり、Ｓ２＝Ｓ１・ＣＯＳθ１である。速度制御部８４は、第２速度ベクトルＳ２の速度を第１速度以下に制御する。

また、第２マニピュレーター２０ａが移動する第２方向の速度ベクトルを第３速度ベクトルＳ３とする。第２マニピュレーター２０ａが移動する方向と、第２マニピュレーター２０ａが第１マニピュレーター２０に接近する方向との角度を第２角度θ２とする。第２マニピュレーター２０ａが第１マニピュレーター２０に接近する方向に投影した速度ベクトルを第４速度ベクトルＳ４とする。第４速度ベクトルＳ４は、第２マニピュレーター２０ａが第１マニピュレーター２０に接近する方向に移動する速度ベクトルであり、Ｓ４＝Ｓ３・ＣＯＳθ２である。

速度制御部８４は、検出取得部７８が人侵入センサー８からの情報を取得した場合に、第２速度ベクトルＳ２と第４速度ベクトルＳ４との相対速度が第２速度以下になるように、第１マニピュレーター２０の第１速度ベクトルＳ１を制御する。これにより、第１マニピュレーター２０と第２マニピュレーター２０ａとの互いに接近する方向に移動する速度の相対速度｜Ｓ２−Ｓ４｜は、第２速度以下になり安全なレベル（安全速度以下）に低減される。

速度制御部８４は、検出取得部７８が人侵入センサー８からの情報を取得した場合に、第１エンドエフェクター３０の先端と第２エンドエフェクター３０ａの先端との相対速度が第２速度以下になるように、第１エンドエフェクター３０の先端の速度を制御することが好ましい。これによれば、第１マニピュレーター２０の速度を制御することで、第１エンドエフェクター３０の先端と第２エンドエフェクター３０ａの先端との相対速度を第２速度以下にできる。

ここで、第１及び第２マニピュレーター２０，２０ａに第１及び第２エンドエフェクター３０，３０ａが設けられている場合、第１及び第２マニピュレーターの先端とは、第１及び第２エンドエフェクター３０，３０ａの先端を指すこととする。

速度制御部８４は、検出取得部７８が人侵入センサー８からの情報を取得した場合に、第１マニピュレーター２０の軸回転ジョイント２１ａ，２１ｂ及び平面回転ジョイント２２ａ，２２ｂ，２２ｃと、第２マニピュレーター２０ａの軸回転ジョイント２１ｃ，２１ｄ及び平面回転ジョイント２２ｄ，２２ｅ，２２ｆとの相対速度が第２速度以下になるように、第１マニピュレーター２０の軸回転ジョイント２１ａ，２１ｂ及び平面回転ジョイント２２ａ，２２ｂ，２２ｃの速度を制御することが好ましい。これによれば、第１マニピュレーター２０の速度を制御することで、第１マニピュレーター２０の軸回転ジョイント２１ａ，２１ｂ及び平面回転ジョイント２２ａ，２２ｂ，２２ｃと、第２マニピュレーター２０ａの軸回転ジョイント２１ｃ，２１ｄ及び平面回転ジョイント２２ｄ，２２ｅ，２２ｆとの相対速度を第２速度以下にできる。

電流制御部８６は、電流指令を入力し各軸のモーター４４を駆動する。

本実施形態の第２制御装置４ａは、速度演算部７２ａを備えている。
速度演算部７２ａは、各軸のエンコーダー４２ａから入力された第２マニピュレーター２０ａの軸回転ジョイント２１ｃ，２１ｄ及び平面回転ジョイント２２ｄ，２２ｅ，２２ｆの角度と速度とから各軸回転ジョイント２１ｃ，２１ｄと各平面回転ジョイント２２ｄ，２２ｅ，２２ｆや第２エンドエフェクター３０ａの先端の速度と方向とを演算する。

本実施形態によれば、第１マニピュレーター２０の速度を制御することで、第１マニピュレーター２０と第２マニピュレーター２０ａとの相対速度を第２速度以下にできる。これにより、第１マニピュレーター２０と第２マニピュレーター２０ａとの間に生体又は物体の少なくとも一方が挟まれても、衝突による衝撃を安全なレベルに低減できる制御装置を提供できる。

本実施形態によれば、近接した第１及び第２マニピュレーター２０，２０ａの相対速度を安全速度（２５０ｍｍ／ｓ）以下に制御することができる。これにより、相対速度を第２速度以下にしない場合と比較して、危険性低減、物体が壊れるおそれを低減できる。その結果、第１及び第２マニピュレーター２０，２０ａの相対速度を制限することで、間に人が挟まれても、衝突による衝撃を安全なレベルに低減できる制御装置を提供することができる。

本実施形態によれば、第１制御装置４と、第１制御装置４により制御される第１ロボット６と、を備えるので、第１マニピュレーター２０の速度を制御することで、第１マニピュレーター２０と第２マニピュレーター２０ａとの相対速度を第２速度以下にできる。これにより、第１マニピュレーター２０と第２マニピュレーター２０ａとの間に生体又は物体の少なくとも一方が挟まれても、衝突による衝撃を安全なレベルに低減できるロボットシステムを提供できる。

（第２実施形態）
図４は、本実施形態に係るロボットシステムを示すブロック図である。以下、ロボットシステムの構造を、図４を参照しながら説明する。

第１実施形態の第１制御装置４は、相対速度が制限値以下になるように第１マニピュレーター２０の関節速度指令を制限している。

本実施形態の第１制御装置４ｂは、相対速度が制限値以下になるように常に制限するのではなく、第１及び第２マニピュレーター２０，２０ａ間の距離が近い（例えば３００ｍｍ以下）場合に制限する点が、第１実施形態と異なっている。以下、第１実施形態と同じ構成部材には同一符号を付し、ここではそれらの説明を省略又は簡略化する。

本実施形態のロボットシステムは、制御装置としての第１制御装置４ｂと、第２制御装置４ｃと、を備える。
第１制御装置４ｂは、図４に示すように、位置及び速度演算部９２と、位置取得部としての位置及び速度取得部９４と、相対位置及び相対速度演算部９６と、を備える。

位置及び速度演算部９２は、第１マニピュレーター２０の軸回転ジョイント２１ａ，２１ｂ及び平面回転ジョイント２２ａ，２２ｂ，２２ｃの角度と速度とから軸回転ジョイント２１ａ，２１ｂ及び平面回転ジョイント２２ａ，２２ｂ，２２ｃや第１エンドエフェクター３０の先端の速度と方向と位置とを計算する。

位置及び速度取得部９４は、第２マニピュレーター２０ａの各軸回転ジョイント２１ｃ，２１ｄと各平面回転ジョイント２２ｄ，２２ｅ，２２ｆや第２エンドエフェクター３０ａの先端の速度と方向と位置とを取得する。

相対位置及び相対速度演算部９６は、第１マニピュレーター２０の各軸回転ジョイント２１ａ，２１ｂ及び各平面回転ジョイント２２ａ，２２ｂ，２２ｃや第１エンドエフェクター３０の先端と、第２マニピュレーター２０ａの各軸回転ジョイント２１ｃ，２１ｄ及び各平面回転ジョイント２２ｄ，２２ｅ，２２ｆや第２エンドエフェクター３０ａの先端との相対速度を演算する。

相対位置及び相対速度演算部９６は、第１マニピュレーター２０の各軸回転ジョイント２１ａ，２１ｂ及び各平面回転ジョイント２２ａ，２２ｂ，２２ｃや第１エンドエフェクター３０の先端と、第２マニピュレーター２０ａの各軸回転ジョイント２１ｃ，２１ｄ及び各平面回転ジョイント２２ｄ，２２ｅ，２２ｆや第２エンドエフェクター３０ａの先端との間の距離を演算する。

速度制御部８４は、検出取得部７８が人侵入センサー８からの情報を取得した場合、かつ第１エンドエフェクター３０の先端と第２エンドエフェクター３０ａの先端との間の距離が第１距離以下の場合に、第１エンドエフェクター３０の先端と第２エンドエフェクター３０ａの先端との相対速度が第２速度以下になるように、第１エンドエフェクター３０の先端の速度を制御することが好ましい。これによれば、第１エンドエフェクター３０の先端と第２エンドエフェクター３０ａの先端との間の距離が第１距離以下の場合に、第１マニピュレーター２０の速度を制御することで、第１エンドエフェクター３０の先端と第２エンドエフェクター３０ａの先端との相対速度を第２速度以下にできる。

速度制御部８４は、第１マニピュレーター２０の複数の軸回転ジョイント２１ａ，２１ｂ及び平面回転ジョイント２２ａ，２２ｂ，２２ｃのうちの一つの第１関節と、第２マニピュレーター２０ａの複数の軸回転ジョイント２１ｃ，２１ｄ及び平面回転ジョイント２２ｄ，２２ｅ，２２ｆのうちの一つの第２関節との距離が第１距離以下の場合に、第１関節と第２関節との相対速度が第２速度以下になるように、第１関節の速度を制御することが好ましい。これによれば、第１マニピュレーター２０の速度を制御することで、第１マニピュレーター２０の複数の軸回転ジョイント２１ａ，２１ｂ及び平面回転ジョイント２２ａ，２２ｂ，２２ｃのうちの一つの第１関節と、第２マニピュレーター２０ａの複数の軸回転ジョイント２１ｃ，２１ｄ及び平面回転ジョイント２２ｄ，２２ｅ，２２ｆのうちの一つの第２関節との相対速度を第２速度以下にできる。

速度制御部８４は、第１関節と第２関節との距離が第１距離以下の場合に、第１マニピュレーター２０の複数の軸回転ジョイント２１ａ，２１ｂ及び平面回転ジョイント２２ａ，２２ｂ，２２ｃのうちの一つの第１関節とは異なる関節の速度を低下することが好ましい。これによれば、第１マニピュレーター２０の複数の軸回転ジョイント２１ａ，２１ｂ及び平面回転ジョイント２２ａ，２２ｂ，２２ｃのうちの一つの第１関節と、第２マニピュレーター２０ａの複数の軸回転ジョイント２１ｃ，２１ｄ及び平面回転ジョイント２２ｄ，２２ｅ，２２ｆのうちの一つの第２関節との間の距離が第１距離以下の場合に、第１マニピュレーター２０の複数の軸回転ジョイント２１ａ，２１ｂ及び平面回転ジョイント２２ａ，２２ｂ，２２ｃのうちの一つの第１関節とは異なる関節の速度を低下できる。

第２制御装置４ｃは、位置及び速度演算部９２ａを備えている。
位置及び速度演算部９２ａは、第２マニピュレーター２０ａの軸回転ジョイント２１ｃ，２１ｄ及び平面回転ジョイント２２ｄ，２２ｅ，２２ｆの角度と速度とから軸回転ジョイント２１ｃ，２１ｄ及び平面回転ジョイント２２ｄ，２２ｅ，２２ｆや第２エンドエフェクター３０ａの先端の速度と方向と位置とを計算する。

（第３実施形態）
図５は、本実施形態に係るロボットシステムの構成を示す構成図である。図６は、本実施形態に係るロボットシステムを示すブロック図である。以下、ロボットシステムの構造を、図５及び図６を参照しながら説明する。

本実施形態の第２マニピュレーター２０ａは、第１マニピュレーター２０と互いに基台１０や制御装置４を共用する点が、第１実施形態と異なっている。以下、第１実施形態と同じ構成部材には同一符号を付し、ここではそれらの説明を省略又は簡略化する。

本実施形態のロボット６は、いわゆる双腕ロボットである。
ロボット６は、図５及び図６に示すように、基台１０と、第１マニピュレーター２０と、第２マニピュレーター２０ａと、第１マニピュレーター２０の先端側に設けられた第１エンドエフェクター３０と、第２マニピュレーター２０ａの先端側に設けられた第２エンドエフェクター３０ａと、を有する。

速度制御部８４は、第１マニピュレーター２０と第２マニピュレーター２０ａとを制御する。これによれば、第１及び第２マニピュレーター２０，２０ａの両方で同時に把持できる把持パターンを選択することで、次の動作の障害等にならない把持手法を決定すること等が可能になる。

なお、図示の構成では、アームは合計２本であるが、本実施形態はこれに限られない。ロボットが３本以上のアームを有している場合も、本実施形態の範囲内である。

なお、本実施形態では、生体又は物体の少なくとも一方として人を例示したがこれに限られることなく、生体（人を含む）又は物体に広く適用できる。

本実施形態は、人と共存することを目的とした機能安全ロボットや双腕ロボットに好適である。

以上、本発明のロボットシステムを、図示の実施形態に基づいて説明したが、本発明はこれに限定されるものではなく、各部の構成は、同様の機能を有する任意の構成のものに置換することができる。また、他の任意の構成物が付加されていてもよい。

また、前記実施形態では、ロボット（基台）が固定される平面（面）は、水平面と平行な平面（面）であるが、本発明では、これに限定されず、例えば、水平面や鉛直面に対して傾斜した平面（面）でもよく、また、鉛直面と平行な平面（面）であってもよい。すなわち、第１回動軸は、鉛直方向や水平方向に対して傾斜していてもよく、また、水平方向と平行であってもよい。

また、本発明のロボットは、垂直多関節ロボットに限らず、水平多関節ロボットやパラレルリンクロボット、双腕ロボットなどでも同様の効果が得られる。また、本発明のロボットは、６軸ロボットに限らず、７軸以上のロボットや５軸以下のロボットでも同様の効果が得られる。また、本発明のロボットは、マニピュレーターを有していれば、マニピュレーター型ロボット（ロボットマニピュレーター）に限定されず、他の形式のロボット、例えば、脚式歩行（走行）ロボット等であってもよい。

４，４ｂ…第１制御装置（制御装置） ４ａ，４ｃ…第２制御装置 ６…第１ロボット（ロボット） ６ａ…第２ロボット ８…人侵入センサー（第１検出部） １０…基台 １０ａ…基台軸 ２０…第１マニピュレーター ２０ａ…第２マニピュレーター ２１ａ，２１ｂ，２１ｃ，２１ｄ…軸回転ジョイント（関節） ２２ａ，２２ｂ，２２ｃ，２２ｄ，２２ｅ，２２ｆ…平面回転ジョイント（関節） ２３ａ，２３ｂ，２３ｃ，２３ｄ…アーム ３０…第１エンドエフェクター ３０ａ…第２エンドエフェクター ４２，４２ａ…各軸のエンコーダー ４４…各軸のモーター ７２，７２ａ…速度演算部 ７４…速度取得部 ７６…相対速度演算部 ７８…検出取得部 ８０…相対速度制限部 ８２…位置制御部 ８４…速度制御部（制御部） ８６…電流制御部 ９２，９２ａ…位置及び速度演算部 ９４…位置及び速度取得部（位置取得部） ９６…相対位置及び相対速度演算部。

Claims (13)

1. 第１マニピュレーターを制御する制御装置であって、
   生体又は物体の少なくとも一方が第１範囲内に位置することを検出する第１検出部からの情報を取得する検出取得部と、
   前記第１マニピュレーターとは異なる第２マニピュレーターの速度を取得する速度取得部と、
   前記第１マニピュレーターの速度を第１速度以下に制御する制御部と、
   を有し、
   前記制御部は、前記検出取得部が前記情報を取得した場合に、前記第１マニピュレーターと前記第２マニピュレーターとの相対速度が第２速度以下になるように、前記第１マニピュレーターの速度を制御することを特徴とする制御装置。
2. 請求項１に記載の制御装置において、
   前記第１マニピュレーター及び前記第２マニピュレーターの各速度は、
   前記第１マニピュレーターと前記第２マニピュレーターとが互いに接近する方向に移動する速度であることを特徴とする制御装置。
3. 請求項１又は２に記載の制御装置において、
   前記制御部は、前記検出取得部が前記情報を取得した場合に、前記第１マニピュレーターの先端と前記第２マニピュレーターの先端との相対速度が前記第２速度以下になるように、前記第１マニピュレーターの先端の速度を制御することを特徴とする制御装置。
4. 請求項１〜３のいずれか一項に記載の制御装置において、
   前記第２マニピュレーターの位置を取得する位置取得部を有し、
   前記制御部は、前記検出取得部が前記情報を取得した場合、かつ前記第１マニピュレーターの先端と前記第２マニピュレーターの先端との間の距離が第１距離以下の場合に、前記第１マニピュレーターの先端と前記第２マニピュレーターの先端との相対速度が前記第２速度以下になるように、前記第１マニピュレーターの先端の速度を制御することを特徴とする制御装置。
5. 請求項１〜４のいずれか一項に記載の制御装置において、
   前記第１マニピュレーターは、関節を有し、
   前記第２マニピュレーターは、関節を有し、
   前記制御部は、前記検出取得部が前記情報を取得した場合に、前記第１マニピュレーターの関節と前記第２マニピュレーターの関節との相対速度が前記第２速度以下になるように、前記第１マニピュレーターの関節の速度を制御することを特徴とする制御装置。
6. 請求項１〜５のいずれか一項に記載の制御装置において、
   前記第１マニピュレーターは、複数の関節を有し、
   前記第２マニピュレーターは、複数の関節を有し、
   前記制御部は、前記第１マニピュレーターの複数の関節のうちの一つの第１関節と、前記第２マニピュレーターの複数の関節のうちの一つの第２関節との距離が第１距離以下の場合に、前記第１関節と前記第２関節との相対速度が前記第２速度以下になるように、前記第１関節の速度を制御することを特徴とする制御装置。
7. 請求項６に記載の制御装置において、
   前記制御部は、前記第１関節と前記第２関節との距離が前記第１距離以下の場合に、前記第１マニピュレーターの複数の関節のうちの一つの前記第１関節とは異なる関節の速度を低下することを特徴とする制御装置。
8. 請求項１〜７のいずれか一項に記載の制御装置において、
   前記第１範囲は、前記第１マニピュレーターの可動範囲を含むことを特徴とする制御装置。
9. 請求項１〜８のいずれか一項に記載の制御装置において、
   前記第１範囲は、前記第１マニピュレーターの可動範囲及び前記第２マニピュレーターの可動範囲を含むことを特徴とする制御装置。
10. 請求項１〜９のいずれか一項に記載の制御装置において、
    前記第１速度は、前記第２速度と等しいことを特徴とする制御装置。
11. 請求項１０に記載の制御装置において、
    前記第１速度及び前記第２速度は、２５０ｍｍ／ｓであることを特徴とする制御装置。
12. 請求項１〜１１のいずれか一項に記載の制御装置において、
    前記制御部は、前記第１マニピュレーターと前記第２マニピュレーターとを制御することを特徴とする制御装置。
13. 請求項１〜１２のいずれか一項に記載の制御装置と、
    前記制御装置により制御されるロボットと、
    を有することを特徴とするロボットシステム。

Images