特許 7522381 ロボットシステム

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

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

(11)【特許番号】

(24)【登録日】2024-07-17

(45)【発行日】2024-07-25

(54)【発明の名称】ロボットシステム

(51)【国際特許分類】

   B25J 19/06 20060101AFI20240718BHJP

   B25J 13/08 20060101ALI20240718BHJP

【ＦＩ】

B25J19/06

B25J13/08 A

【請求項の数】 5

(21)【出願番号】P 2024011631

(22)【出願日】2024-01-30

【審査請求日】2024-02-08

【早期審査対象出願】

(73)【特許権者】

【識別番号】000005197

【氏名又は名称】株式会社不二越

(74)【代理人】

【識別番号】100096091

【弁理士】

【氏名又は名称】井上 誠一

(74)【復代理人】

【識別番号】100156410

【弁理士】

【氏名又は名称】山内 輝和

(74)【復代理人】

【識別番号】100155882

【弁理士】

【氏名又は名称】齋藤 昭彦

(72)【発明者】

【氏名】今岡 誠

(72)【発明者】

【氏名】林 哲也

(72)【発明者】

【氏名】大井 嘉敬

【審査官】尾形 元

(56)【参考文献】

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

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

【文献】特開２０１５－１７４１８４（ＪＰ，Ａ）

【文献】特開２０１８－１２１５９（ＪＰ，Ａ）

【文献】特開平８－１６２３４（ＪＰ，Ａ）

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

Ｂ２５Ｊ １９／０６

Ｂ２５Ｊ １３／０８

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

【請求項1】

ロボットアームを有する１又は複数のロボットと、前記ロボットの動作可能範囲を含む空間を撮像する１又は複数の３次元センサと、前記ロボット及び前記３次元センサの動作を制御する１又は複数の制御装置と、を備えるロボットシステムであって、
前記制御装置は、
前記ロボットのリンクパラメータの値を記憶するリンクパラメータ記憶部と、
前記３次元センサから、前記空間に置かれた物体の表面の位置を示す点の集まりである点群データを取得する点群データ取得部と、
前記ロボットアームの各関節の現在の関節角度を取得する関節角度取得部と、
前記リンクパラメータ及び前記関節角度に基づいて、前記ロボットの現在の姿勢と同様な姿勢を示すロボットモデルを生成するロボットモデル生成部と、
前記ロボットモデルを用いて前記点群データから少なくとも前記ロボットを示す点を除去し、残りの前記点群データを干渉物の位置を示す点の集まりである干渉物データとして設定する干渉物データ設定部と、
前記干渉物データ及び前記ロボットモデルを用いて前記干渉物と前記ロボットとの距離を算出し、前記干渉物と前記ロボットとの距離に基づいて前記干渉物と前記ロボットとが接近しているか否かを判定する接近判定部と、
を備えることを特徴とするロボットシステム。

【請求項2】

前記ロボットモデル生成部は、前記リンクパラメータ及び前記関節角度に基づいて前記ロボットの各部位の位置を示す点を基準点として算出し、前記基準点に基づいて前記ロボットモデルを生成する
ことを特徴とする請求項１に記載のロボットシステム。

【請求項3】

前記ロボットモデル生成部は、前記基準点に従って定義される空間図形をロボットオブジェクトとし、
前記干渉物データ設定部は、前記点群データから少なくとも前記ロボットオブジェクトの内部に含まれる点を除去し、前記干渉物データを生成する
ことを特徴とする請求項２に記載のロボットシステム。

【請求項4】

前記接近判定部は、前記干渉物データをグループ分けし、グループごとに前記干渉物と前記ロボットとの距離を算出する
ことを特徴とする請求項１に記載のロボットシステム。

【請求項5】

前記接近判定部は、前記干渉物データに含まれる点又は前記干渉物データに従って定義される空間図形と、前記ロボットモデルに含まれる点又は前記ロボットモデルに従って定義される空間図形との距離を、前記干渉物と前記ロボットとの距離として算出する
ことを特徴とする請求項１から請求項４のいずれかに記載のロボットシステム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、人等の干渉物の接近を検知するロボットシステムに関する。

【背景技術】

【0002】

近年、安全柵なしで人と協働して作業を行う協働ロボットが注目されている。協働ロボットの安全対策は、人等の干渉物との接触を検知すると、ロボットに危険を回避させる手法と、干渉物の接近を検知すると、ロボットに危険を回避させる手法とがある。後者の手法は、干渉物との接触前に危険回避動作を命令するので、前者の手法よりも安全性が高いと言える。後者の一例として、特許文献１に、物体との間に生じる静電容量の変化によって物体が近接したことを検出する近接センサを用いたシステムが記載されている。

【先行技術文献】

【特許文献】

【0003】

【文献】特許第７１０９５６２号公報

【発明の概要】

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

【0004】

しかしながら、静電容量方式の近接センサでは、ロボットの外回しの配線や据付ベース等を干渉物として検出してしまうという課題がある。ロボットの動作を制御するロボット制御装置は、予めロボットの附属物や各部位の位置を記憶し、これらとロボットアームが接触しないように制御するため、干渉物として検出する必要はない。仮にロボットの附属物や各部位を干渉物として検出してしまうと、本来の目的の動作を実行できないか、又は動作範囲が制限され、著しく生産効率が低下する恐れがある。

【0005】

本発明は、前述した事情に鑑みてなされたものであり、主要な目的は、ロボットと干渉物とを識別し、干渉物の接近を検知するロボットシステムを提供することである。

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

【0006】

前述した目的を達成するための本発明は、ロボットアームを有する１又は複数のロボットと、前記ロボットの動作可能範囲を含む空間を撮像する１又は複数の３次元センサと、前記ロボット及び前記３次元センサの動作を制御する１又は複数の制御装置と、を備えるロボットシステムであって、前記制御装置は、前記ロボットのリンクパラメータの値を記憶するリンクパラメータ記憶部と、前記３次元センサから、前記空間に置かれた物体の表面の位置を示す点の集まりである点群データを取得する点群データ取得部と、前記ロボットアームの各関節の現在の関節角度を取得する関節角度取得部と、前記リンクパラメータ及び前記関節角度に基づいて、前記ロボットの現在の姿勢と同様な姿勢を示すロボットモデルを生成するロボットモデル生成部と、前記ロボットモデルを用いて前記点群データから少なくとも前記ロボットを示す点を除去し、残りの前記点群データを干渉物の位置を示す点の集まりである干渉物データとして設定する干渉物データ設定部と、前記干渉物データ及び前記ロボットモデルを用いて前記干渉物と前記ロボットとの距離を算出し、前記干渉物と前記ロボットとの距離に基づいて前記干渉物と前記ロボットとが接近しているか否かを判定する接近判定部と、
を備えることを特徴とするロボットシステムである。

【0007】

前記ロボットモデル生成部は、前記リンクパラメータ及び前記関節角度に基づいて前記ロボットの各部位の位置を示す点を基準点として算出し、前記基準点に基づいて前記ロボットモデルを生成するようにしても良い。

【0008】

また、前記ロボットモデル生成部は、前記基準点に従って定義される空間図形をロボットオブジェクトとし、前記干渉物データ設定部は、前記点群データから少なくとも前記ロボットオブジェクトの内部に含まれる点を除去し、前記干渉物データを生成するようにしても良い。

【0009】

また、前記接近判定部は、前記干渉物データをグループ分けし、グループごとに前記干渉物と前記ロボットとの距離を算出するようにしても良い。

【0010】

また、前記接近判定部は、前記干渉物データに含まれる点又は前記干渉物データに従って定義される空間図形と、前記ロボットモデルに含まれる点又は前記ロボットモデルに従って定義される空間図形との距離を、前記干渉物と前記ロボットとの距離として算出するようにしても良い。

【発明の効果】

【0011】

本発明により、ロボットと干渉物とを識別し、干渉物の接近を検知するロボットシステムを提供できる。

【図面の簡単な説明】

【0012】

【図1】本発明の実施形態のロボットシステムの概略を説明する図

【図2】図１の制御装置の構成を示すブロック図

【図3】図１の制御装置によって実現される処理の流れの一例を示すフローチャート

【図4】ロボットモデルの例を模式的に説明する図

【図5】干渉物データ設定部による処理を模式的に説明する図

【図6】接近判定部による処理を模式的に説明する図

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

【0013】

以下図面に基づいて、本発明の実施形態を詳細に説明する。図１は、本発明の実施形態のロボットシステムの概略を説明する図である。図１に示すように、ロボットシステム１は、ロボットアーム２１を有する１又は複数のロボット２と、ロボット２の動作可能範囲を含む空間を撮像する１又は複数の３次元センサ３と、ロボット２及び３次元センサ３の動作を制御する１又は複数の制御装置４と、を備える。３次元センサ３によって撮像される空間には、ワークＷやテーブルＴが配置されても良い。また、ロボット２は協働ロボットであり、３次元センサ３によって撮像される空間には、人Ｍが存在しても良い。

【0014】

ロボット２は、ロボットアーム２１を支持する据付ベース２２を有し、ロボットアーム２１の先端にはエンドエフェクタ２３が取り付けられている。ロボットアーム２１は、複数のリンクを有するリンク機構で構成され、リンク間を連結する関節を有する。各関節には図示しない駆動モータが設けられる。ロボット２の一例として、関節の数が６の垂直多関節ロボットが挙げられる。但し、本発明は、関節の数が５以下や７以上のロボットや、水平多関節型ロボット等にも適用可能である。また、ロボット２は、単腕型ロボットに限らず、双腕型ロボットでも良い。

【0015】

３次元センサ３は、物体の３次元位置を計測可能なセンサであり、撮影対象の空間に置かれた物体表面の検出点の位置を示す点の集まりである点群（Point Cloud）データを生成する。３次元センサ３は、例えば、アクティブステレオカメラ、パッシブステレオカメラ、３次元ＬｉＤＡＲ（Light Detection And Ranging）センサ、ＴｏＦ（Time of
Flight）カメラ等が挙げられるが、これらの例に限定されない。３次元センサ３によって生成される点群データは、３次元の直交座標（ｘ，ｙ，ｚ）で表現される直交座標点群データである。３次元座標（ｘ，ｙ，ｚ）の各成分の取り得る範囲は、３次元センサ３の画角や検出距離範囲による。点群データのファイル形式は、例えば、オープンソースライブラリのＰＣＬ（Point Cloud Library）によって開発されたＰＣＤ（Point Cloud
Data）ファイル等が挙げられるが、これに限定されない。

【0016】

３次元センサ３は、天井等に設置され、ロボット２の上方から撮像しても良いし、壁や三脚等に設置され、ロボット２の水平方向から撮像しても良い。また、３次元センサ３は、ロボットアーム２１やエンドエフェクタ２３等に設置され、ロボット２の一部のみが撮像範囲に含まれるように撮像しても良い。更に、１台の３次元センサ３では撮像できない範囲（ロボット２の陰になる部分等）がある場合、複数の３次元センサ３を設置し、複数の３次元センサ３から取得される点群データを合成しても良い。

【0017】

制御装置４は、ロボット２の動作を制御するロボット制御装置４ａと、３次元センサ３の動作を制御する３次元センサ制御装置４ｂと、を含む。ロボット制御装置４ａは、ロボット２の据付ベース２２に内蔵されも良いし、ロボット２の外部に設置されても良い。後者の場合、ロボット２及びロボット制御装置４ａは、通信ケーブル又は無線を介して通信可能に接続される。同様に、３次元センサ制御装置４ｂは、３次元センサ３と一体でも良いし、別体でも良い。後者の場合、３次元センサ３及び３次元センサ制御装置４ｂは、通信ケーブル又は無線を介して通信可能に接続される。また、ロボット制御装置４ａ及び３次元センサ制御装置４ｂは、通信ケーブル又は無線を介して通信可能に接続される。

【0018】

ロボット制御装置４ａのＣＰＵ（Central Processing Unit）４１ａ、メモリ４２ａ、補助記憶装置４３ａ及び入出力インタフェース４４ａは、バス４５ａを介して接続される。ＣＰＵ４１ａは、予め補助記憶装置４３ａ等に記憶される制御プログラムをメモリ４２ａに読み出して複数の命令を順次実行する。補助記憶装置４３ａは、ハードディスクドライブやソリッドステートドライブ等であり、後述する処理に用いるデータを記憶する。入出力インタフェース４４ａは、ロボット２、３次元センサ制御装置４ｂ、その他の装置等から信号を入力したり、それらに信号を出力したりする。

【0019】

３次元センサ制御装置４ｂのＣＰＵ（Central Processing
Unit）４１ｂ、メモリ４２ｂ、補助記憶装置４３ｂ及び入出力インタフェース４４ｂは、バス４５ｂを介して接続される。ＣＰＵ４１ｂは、予め補助記憶装置４３ｂ等に記憶される制御プログラムをメモリ４２ｂに読み出して複数の命令を順次実行する。補助記憶装置４３ｂは、ハードディスクドライブやソリッドステートドライブ等であり、後述する処理に用いるデータを記憶する。入出力インタフェース４４ｂは、３次元センサ３、ロボット制御装置４ａ、その他の装置等から信号を入力したり、それらに信号を出力したりする。

【0020】

図１では、２台の制御装置４、すなわちロボット制御装置４ａ及び３次元センサ制御装置４ｂが図示されているが、制御装置４の台数は１台でも良いし、３台以上でも良い。以下では、ロボットシステム１は、ロボット制御装置４ａ及び３次元センサ制御装置４ｂの２台の制御装置４を有し、これらの制御装置４が互いに連携して処理を実行するものとして説明する。

【0021】

図２は、図１の制御装置の構成を示すブロック図である。ロボット制御装置４ａ及び３次元センサ制御装置４ｂは、制御プログラム等のソフトウエアと、ＣＰＵ４１ａ、４１ｂ等のハードウエア資源とが協働することによって、図２の構成を備える。

【0022】

ロボット制御装置４ａは、ロボット動作制御部５１及び関節角度提供部５２を備える。ロボット動作制御部５１は、制御プログラムや外部の装置からの命令等に基づき、ロボットアーム２１の駆動モータの回動及び停止を制御する。関節角度提供部５２は、ロボット２の現在の状態におけるロボットアーム２１の各関節の現在の関節角度を３次元センサ制御装置４ｂに提供する。

【0023】

３次元センサ制御装置４ｂは、３次元センサ動作制御部６１、リンクパラメータ記憶部６２、点群データ取得部６３、関節角度取得部６４、ロボットモデル生成部６５、干渉物データ設定部６６、接近判定部６７及び動作命令通知部６８を備える。３次元センサ動作制御部６１は、制御プログラムや外部の装置からの命令等に基づき、３次元センサ３の動作を制御する。リンクパラメータ記憶部６２は、ロボット２のリンクパラメータの値を記憶する。

【0024】

リンクパラメータとは、ロボットアーム２１のリンクと関節との幾何学的な位置関係を定めるパラメータである。例えば、リンクパラメータは、Denavit‐Hartenbergの記法（Ｄ－Ｈ法）を用いたものが知られている。Ｄ－Ｈ法を用いたリンクパラメータは、ＤＨパラメータとも呼ばれる。Ｄ－Ｈ法を用いたリンクパラメータの概要は次の通りである。１．リンク及び関節は、ロボット２のベースに近い順に順番が付けられる。２．リンクｉには、関節軸ｉをｚ[ｉ]軸とする座標系（ｘ[ｉ]、ｙ[ｉ]、ｚ[ｉ]）が設定される。３．ｘ[ｉ]軸はｚ[ｉ]軸とｚ[ｉ＋１]軸との共通垂線とし、ｚ[ｉ＋１]軸に向かう方向を正とする。４．ｙ[ｉ]軸は右手座標系をなすように設定する。５．リンクパラメータは、（１）リンク間角度θ[ｉ]＝ｚ[ｉ]軸に対するｘ[ｉ－１]軸からｘ[ｉ]軸への角度、（２）リンク間距離ｄ[ｉ]＝ｚ[ｉ]軸に沿ったｘ[ｉ－１]軸からｘ[ｉ]軸への距離、（３）リンクねじれ角α[ｉ]＝ｘ[ｉ－１]軸に対するｚ[ｉ－１]軸からｚ[ｉ]軸への角度、（４）リンク長さａ[ｉ]＝ｘ[ｉ－１]軸に沿ったｚ[ｉ－１]軸からｚ[ｉ]軸への距離、の４つである。

【0025】

リンク間角度θ[ｉ]は、ロボット制御装置４ａの関節角度提供部５２によって提供されるロボットアーム２１の各関節の現在の関節角度である。リンクパラメータ記憶部６２は、リンク間角度θ[ｉ]を除いた、リンク間距離ｄ[ｉ]、リンクねじれ角α[ｉ]及びリンク長さａ[ｉ]の３つの設計値を予め記憶しておく。

【0026】

点群データ取得部６３は、３次元センサ３から、３次元センサ３によって撮像される空間に置かれた物体の表面の位置を示す点の集まりである点群データを取得する。関節角度取得部６４は、ロボット動作制御部５１から、ロボットアーム２１の各関節の現在の関節角度を取得する。ロボットモデル生成部６５は、リンクパラメータ及び関節角度に基づいて、ロボット２の現在の姿勢と同様な姿勢を示すロボットモデルを生成する。干渉物データ設定部６６は、ロボットモデルを用いて点群データから少なくともロボットを示す点を除去し、残りの点群データを干渉物の位置を示す点の集まりである干渉物データとして設定する。接近判定部６７は、干渉物データ及びロボットモデルを用いて干渉物とロボットとの距離を算出し、干渉物とロボットとの距離に基づいて干渉物とロボットとが接近しているか否かを判定する。動作命令通知部６８は、接近判定部６７による判定結果に基づいて、ロボット制御装置４ａに動作命令を通知する。これらの構成の詳細は、図３～図６を参照しながら後述する。

【0027】

図３は、図１の制御装置によって実現される処理の流れの一例を示すフローチャートである。図３に示すように、３次元センサ制御装置４ｂは、接触前検知判定ループを実行するか否か確認する（ステップＳ１）。接触前検知判定ループでは、ロボット２が干渉物と接触する前に干渉物との接近を検知する判定処理、すなわちステップＳ２からステップＳ１０までの処理を繰り返す。

【0028】

３次元センサ制御装置４ｂは、一方、ロボット制御装置４ａから接触前検知判定ループの停止命令を受信していない場合、接触前検知判定ループを実行する（ステップＳ１のＹｅｓ）。他方、３次元センサ制御装置４ｂは、３次元センサ制御装置４ｂから接触前検知判定ループの停止命令を受信すると、接触前検知判定ループの実行を終了する（ステップＳ１のＮｏ）。ロボット制御装置４ａは、例えばダイレクトティーチングのように、人がロボット２に直接触れる動作モードの場合、３次元センサ制御装置４ｂに対して接触前検知判定ループの停止命令を送信する。

【0029】

接触前検知判定ループに入ると、３次元センサ制御装置４ｂの点群データ取得部６３は、３次元センサ３から点群データを取得する（ステップＳ２）。前述の通り、点群データは、３次元の直交座標（ｘ，ｙ，ｚ）で表現される。点群データには、ロボット２、ワークＷ、テーブルＴ、人Ｍ等の物体の表面の位置を示す点が含まれる。

【0030】

次に、３次元センサ制御装置４ｂの関節角度取得部６４は、ロボット制御装置４ａの関節角度提供部５２からロボットアーム２１の各関節の現在の関節角度を取得する（ステップＳ３）。例えば、ロボット制御装置４ａの関節角度提供部５２は、各関節の駆動モータに搭載されるエンコーダによって検出される関節角度を３次元センサ制御装置４ｂに送信する。

【0031】

次に、３次元センサ制御装置４ｂのロボットモデル生成部６５は、リンクパラメータ記憶部６２に記憶されているリンクパラメータ、及び関節角度取得部６４によって取得される関節角度に基づいてロボットモデルを生成する（ステップＳ４）。リンクパラメータ記憶部６２に記憶されているリンクパラメータの値は、リンク間距離ｄ_ｉ、リンクねじれ角α_ｉ及びリンク長さａ_ｉの３つである。また、関節角度取得部６４によって取得される関節角度は、リンクパラメータの１つであるリンク間角度θ_ｉの値として用いる。ロボットモデル生成部６５は、これら４つのリンクパラメータの値を用いて順運動学計算を実行し、ロボット２の各部位（各関節、エンドエフェクタ２３等）の位置を示す点を解析的に算出し、基準点とする。そして、ロボットモデル生成部６５は、基準点に基づいてロボット２の現在の状態を示すロボットモデルを生成する。これによって、ロボットモデル生成部６５は、ロボット２と干渉物との接近判定に十分な精度で迅速にロボットモデルを生成できる。ひいては、ロボットシステム１は、危険回避動作が間に合うタイミングで、ロボット２に対する干渉物の接近を検知できる。

【0032】

図４は、ロボットモデルの例を模式的に説明する図である。ロボットモデル７０は、ロボット２を近似するモデルであり、３次元の直交座標空間に生成されるものである。但し、図４では、３次元の直交座標空間における特定の方向から見た２次元の直交座標平面として簡略的に図示されている。後述する図５及び図６も同様である。

【0033】

四角形で図示されている点７１ａ～７１ｅは、ロボットモデル生成部６５によって算出される各関節やエンドエフェクタ２３等の位置を示す基準点である。図４の例では、基準点の数は５個であるが、基準点の数は特に限定されない。また、三角形で図示されている点７２ａ～７２ｊは、基準点７１ａ～７１ｅの隣り合う２点を結ぶ線分７３ａ～７３ｄに従って生成される線形補間の点である。線形補間の点の数は、線分７３ａ～７３ｄごとに異なっても良い。図４の例では、線分７３ａの線形補間の点は０個、線分７３ｂの線形補間の点は５個、線分７３ｃの線形補間の点は４個、線分７３ｄの線形補間の点は１個である。線形補間の点の数は、図４の例に限定されず、ロボット２の大きさ等によって適宜変更可能である。また、ロボットモデル生成部６５は、線形補間の点を生成せず、基準点だけを用いてロボットモデル７０を生成しても良い。

【0034】

ロボットモデル生成部６５は、基準点に従って定義される空間図形をロボットオブジェクトとして設定する。ロボットオブジェクトは、ロボット２を近似する１又は複数の仮想的な図形である。図４に示すロボットオブジェクトは、基準点７１ａ～７１ｅと、基準点７１ａ～７１ｅに従って生成される線形補間の点７２ａ～７２ｊとの各点を中心とし、所定の半径を有する球７４ａ～７４ｏである。球７４ａ～７４ｏの半径は、ロボットアーム２１やエンドエフェクタ２３等の各部位の大きさに応じて、それぞれ異なっても良い。

【0035】

ロボットオブジェクトは、図４に示す球７４ａ～７４ｏに限定されるものではない。ロボットオブジェクトは、ロボット２の外回しの配線等の附属物や据付ベース２２等のロボットアーム２１ではない部位も含むようにロボット２を近似できれば、どのような図形でも良い。例えば、ロボットオブジェクトは、基準点７１ａ～７１ｅ及び線形補間の点７２ａ～７２ｊの各点を中心とする複数の立方体でも良い。また、例えば、ロボットオブジェクトは、基準点７１ａ～７１ｅに従って生成される線分７３ａ～７３ｄを中心軸とする複数の円柱又は直方体でも良い。

【0036】

図３の説明に戻る。３次元センサ制御装置４ｂの干渉物データ設定部６６は、点群データ取得部６３によって取得される点群データから、少なくともロボット２を示す点を除去する。ここで、ロボット２を示す点とは、ロボットアーム２１、据付ベース２２、エンドエフェクタ２３、ロボット２の附属物等を示す点を含む。そして、干渉物データ設定部６６は、除去されていない残りの点群データを干渉物データに設定する（ステップＳ５）。これにより、３次元センサ制御装置４ｂは、点群データに含まれる点を、ロボット２を示す点と干渉物を示す点とに分けることができる。すなわち、３次元センサ制御装置４ｂは、ロボット２と干渉物とを識別できる。

【0037】

図５は、干渉物データ設定部による処理を模式的に説明する図である。図５では、ロボットモデル７０と点群データ７５とが重ねて図示されている。干渉物データ設定部６６は、点群データ７５から、少なくともロボットオブジェクト（＝球７４ａ～７４ｏ）の内部に含まれる点を除去し、除去されていない残りの点群データ７５を干渉物データ７６に設定する。これによって、干渉物データ設定部６６は、ロボット２と干渉物との接近判定に十分な精度で迅速にロボット２と干渉物とを識別できる。ひいては、ロボットシステム１は、危険回避動作が間に合うタイミングで、ロボット２に対する干渉物の接近を検知できる。

【0038】

ここで、干渉物データ設定部６６は、点群データ７５からロボット２を示す点のみを除去するのではなく、ロボット２の他に干渉物として認識不要な物体を示す点を除去しても良い。ロボット２の他に干渉物として認識不要な物体とは、例えば、ワークＷやテーブルＴである。例えば、エンドエフェクタ２３がワークＷを把持するツールの場合、エンドエフェクタ２３とワークＷとの接触は許容されるべきであり、ワークＷは干渉物として認識不要である。また、例えば、テーブルＴが静態物であって、予め位置が判明している場合、ロボット制御装置４ａは、ロボットアーム２１がテーブルＴに接触しないように動作可能範囲を制限してロボット２の動作を制御する。この場合、テーブルＴは干渉物として認識不要である。例えば、干渉物データ設定部６６は、予めワークＷやテーブルＴの位置情報が含まれる環境地図を記憶しておき、ロボット２を示す点と同様、点群データ７５からワークＷやテーブルＴを示す点を除去しても良い。

【0039】

図３の説明に戻る。３次元センサ制御装置４ｂの接近判定部６７は、干渉物データ７６及びロボットモデル７０を用いて干渉物とロボット２との距離を算出し、干渉物とロボット２との距離に基づいて干渉物とロボット２とが接近しているか否かを判定する（ステップＳ６）。

【0040】

図６は、接近判定部による処理を模式的に説明する図である。図６では、ロボットモデル７０と干渉物データ７６とが重ねて図示されている。干渉物は単数とは限らず、様々な方向から複数の干渉物がロボット２に接近することも考えられる。そこで、接近判定部６７は、干渉物データ７６をグループ分けし、グループごとにロボット２と干渉物との距離を算出する。これによって、接近判定部６７は、迅速かつ正確に干渉物とロボット２との接近を判定できる。ひいては、ロボットシステム１は、危険回避動作が間に合うタイミングで、ロボット２に対する干渉物の接近を検知できる。

【0041】

干渉物データ７６は点群データなので、干渉物データ７６のグループ分けは、点群データのグループ分けを意味する。接近判定部６７は、例えば、オープンソースのソフトウエアライブラリであるＰＣＬ（Point Cloud Library）のクラスタリング手法（＝「EuclideanClusterExtraction」クラスを用いた手法）を干渉物データ７６に適用し、グループ分けを行う。このクラスタリング手法によれば、未整理の点群データをより小さな部分に分割できる。

【0042】

また、接近判定部６７は、グループ分けされた干渉物データ７６ごとに、ＰＣＬの形状近似手法（＝「MomentOfInertiaEstimation」クラスを用いた手法）を適用し、干渉物を包含する空間図形としてバウンディングボックス（境界ボックス）を生成する。バウンディングボックスは、与えられた点群データを含む直方体である。尚、干渉物を近似する空間図形は、バウンディングボックス（直方体）に限らず、球や円柱など他の空間図形でも良い。

【0043】

そして、接近判定部６７は、バウンディングボックスと基準点又は線形補間の点とのユークリッド距離を算出する。基準点又は線形補間の点は複数存在するので、接近判定部６７は、バウンディングボックスと各点とのユークリッド距離を算出し、最小値（最短距離の値）を干渉物とロボット２との距離とする。図６の例では、それぞれ干渉物データ７６の一部を含むバウンディングボックス７７ａ及び７７ｂが図示されている。例えば、バウンディングボックス７７ａが人の左腕、バウンディングボックス７７ｂが人の右腕である。距離７８ａ及び７８ｂは、それぞれ、バウンディングボックス７７ａ及び７７ｂと基準点７１ｅとの距離であり、複数の基準点又は線形補間の点の中で最短距離の値である。接近判定部６７は、グループ分けされた干渉物データ７６の少なくとも一つの干渉物とロボット２との距離が閾値以下の場合、干渉物とロボット２とが接近していると判定する。図４の例では、接近判定部６７は、距離７８ａ又は７８ｂの少なくとも一方が閾値以下の場合、干渉物とロボット２とが接近していると判定する。

【0044】

前述の説明では、接近判定部６７は、干渉物とロボット２との距離に基づいて干渉物とロボット２とが接近しているか否かを判定するものとしたが、更に、干渉物とロボット２との相対速度に基づいて判定しても良い。例えば、接近判定部６７は、前回の点群データ取得時から今回の点群データ取得時までの干渉物とロボット２との相対的な位置の変位を求め、前回の点群データ取得時から今回の点群データ取得時までの時間で割ることによって、干渉物とロボット２との相対速度を算出できる。そして、例えば、接近判定部６７は、干渉物とロボット２との距離の閾値について、干渉物とロボット２との相対速度に比例する値を用いる。これにより、干渉物とロボット２との近づく速度が速い場合でも、危険回避が可能なタイミングで危険回避命令を通知できる。

【0045】

また、前述の説明では、接近判定部６７は、干渉物を近似する空間図形と、ロボット２の位置を示す点（基準点又は線形補間の点）との距離を、干渉物とロボット２との距離として算出したが、他の算出方法でも良い。例えば、接近判定部６７は、干渉物を近似する空間図形と、ロボット２を近似する空間図形（ロボットオブジェクト）との距離を算出するものとしても良い。また、例えば、接近判定部６７は、干渉物データ７６に含まれる点と、ロボット２を近似する空間図形との距離を算出するものとしても良い。また、例えば、接近判定部６７は、干渉物データ７６に含まれる点と、ロボット２の位置を示す点との距離を算出するものとしても良い。これらを纏めると、接近判定部６７は、干渉物データ７６に含まれる点又は干渉物データに従って定義される空間図形と、ロボットモデル７０に含まれる点又はロボットモデル７０に従って定義される空間図形との距離を、干渉物とロボット２との距離として算出する。

【0046】

図３の説明に戻る。接近判定部６７によって干渉物とロボット２とが接近していると判定された場合（ステップＳ６のＹｅｓ）、３次元センサ制御装置４ｂの動作命令通知部６８は、ロボット２が動作中か否か確認する（ステップＳ７）。ロボット２が動作中の場合（ステップＳ７のＹｅｓ）、動作命令通知部６８は、危険回避命令としてロボット２の停止命令をロボット制御装置４ａに通知し、ステップＳ１に戻る（ステップＳ８）。ロボット２が動作中ではない場合（ステップＳ７のＮｏ）、動作命令通知部６８は、何もせずにステップＳ１に戻る。

【0047】

一方、接近判定部６７によって干渉物とロボット２とが接近していないと判定された場合（ステップＳ６のＮｏ）、動作命令通知部６８は、ロボット２が動作中か否か確認する（ステップＳ９）。ロボット２が動作中の場合（ステップＳ９のＹｅｓ）、動作命令通知部６８は、何もせずにステップＳ１に戻る。ロボット２が動作中ではない場合（ステップＳ９のＮｏ）、動作命令通知部６８は、制限緩和命令としてロボット２の動作再開命令をロボット制御装置４ａに通知し、ステップＳ１に戻る（ステップＳ１０）。

【0048】

ステップＳ８において通知される危険回避命令は、停止命令に限らず、例えば、減速命令や接触回避経路に沿った経路変更命令等でも良い。同様に、ステップＳ１０において通知される制限緩和命令は、動作再開命令に限らず、例えば、加速命令や最短経路に沿った経路変更命令等でも良い。

【0049】

以上の通り、本発明の実施形態のロボットシステム１によれば、ロボット２と干渉物とを識別し、干渉物の接近を検知できる。本発明の実施形態のロボットシステム１では、３次元センサ３を用いるので、３次元センサ３の設置場所や、ロボット２と３次元センサ３との位置関係を適切に設定すれば、広範囲に干渉物の接近を検知できる。また、本発明の実施形態のロボットシステム１は、ロボット２の形状や大きさに関わらず適用可能なので、ロボット２の機種ごとにハードウエアの構成を変える必要がなく、システムの導入が容易である。

【0050】

また、従来技術における静電容量方式の近接センサの場合、静電容量の変化を検出できる距離が短く、干渉物の接近を検知した後に危険回避行動が間に合わない恐れがある。一方、本発明の実施形態のロボットシステム１では、静電容量方式の近接センサではなく、広範囲で干渉物の接近を検知できる３次元センサ３を用いるので、危険回避動作が間に合うタイミングで干渉物の接近を検知できる。

【0051】

また、従来技術における静電容量方式の近接センサの場合、検出面に水滴がついたり、湿度が変化したりすると、干渉物の検出が不安定になる。一方、本発明の実施形態のロボットシステム１では、湿度等の環境の変化に影響されない３次元センサ３を用いるので、環境の変化に関わらず安定的に干渉物の接近を検知できる。

【0052】

以上、添付図面を参照しながら、本発明に係るロボットシステム等の好適な実施形態について説明したが、本発明はかかる例に限定されない。当業者であれば、本願で開示した技術的思想の範疇内において、各種の変更例又は修正例に想到し得ることは明らかであり、それらについても当然に本発明の技術的範囲に属するものと了解される。

【符号の説明】

【0053】

１………ロボットシステム
２………ロボット
３………３次元センサ
４………制御装置
４ａ………ロボット制御装置
４ｂ………３次元センサ制御装置
２１………ロボットアーム
２２………据付ベース
２３………エンドエフェクタ
５１………ロボット動作制御部
５２………関節角度提供部
６１………３次元センサ動作制御部
６２………リンクパラメータ記憶部
６３………点群データ取得部
６４………関節角度取得部
６５………ロボットモデル生成部
６６………干渉物データ設定部
６７………接近判定部
６８………動作命令通知部
７０………ロボットモデル
７１ａ～７１ｅ………基準点
７２ａ～７２ｊ………線形補間の点
７３ａ～７３ｄ………線分
７４ａ～７４ｏ………球（ロボットオブジェクト）
７５………点群データ
７６………干渉物データ
７７ａ、７７ｂ………バウンディングボックス
７８ａ、７８ｂ………距離

【要約】

【課題】ロボットと干渉物とを識別し、干渉物の接近を検知する。
【解決手段】ロボットシステムの制御装置４は、ロボットのリンクパラメータの値を記憶するリンクパラメータ記憶部６２と、３次元センサから点群データを取得する点群データ取得部６３と、ロボットアームの各関節の現在の関節角度を取得する関節角度取得部６４と、リンクパラメータ及び関節角度に基づいて、ロボットの現在の姿勢と同様な姿勢を示すロボットモデルを生成するロボットモデル生成部６５と、ロボットモデルを用いて点群データから少なくともロボットを示す点を除去し、残りの点群データを干渉物データとして設定する干渉物データ設定部６６と、干渉物データ及びロボットモデルを用いて干渉物とロボットとの距離を算出し、距離に基づいて干渉物とロボットとが接近しているか否かを判定する接近判定部６７と、を備える。
【選択図】図２

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】