JPWO2020188630A1 - ロボット制御装置、方法、及びプログラム - Google Patents

Abstract

障害状態から安全な姿勢へ短時間で復帰することができるロボット制御装置、方法、及びプログラムを提供する。
経路生成部（１２）が、ロボット（３０）の動作時に障害が発生したことが検知されると、障害発生後のロボット（３０）の周辺の環境情報、ロボット仕様情報、及びロボットが復帰すべき姿勢を示す安全姿勢情報を取得し、取得した情報に基づいて、障害発生後の姿勢から安全姿勢までのロボット（３０）の経路を生成し、制御部（１４）が、経路生成部（１２）により生成された経路に基づいて、ロボット（３０）が安全姿勢となるように、ロボット（３０）の動作を制御する。

Description

本発明は、ロボット制御装置、ロボット制御方法、及びロボット制御プログラムに関する。

ロボットが障害物との衝突や自己干渉の発生により停止状態に陥った場合に、例えばロボットの原点姿勢などの安全な姿勢にロボットを復帰させることが行われる。この復帰動作は、ティーチングされた経路を逆順実行したり、ユーザがティーチングペンダント（ＴＰ）を操作したりすることにより実現されている。

例えば、異なる複数種類の移動コマンドを有し作業時に異なる経路を採り得るロボットを原点まで復帰させる制御方法が提案されている（特許文献１参照）。この方法では、既に実行された制御プログラムを順次逆実行するに際し、ある移動コマンドに対し、通常動作時にその移動コマンドで出発した位置を戻し動作時の到達位置に置き換え、各移動コマンドをその一つ前の移動コマンドの位置引数を用いて実行することにより通常動作時の経路を遡行して作業原点まで復帰する。

また、いかなるタイミングで停止した場合でも、原点へ復帰できるロボット制御方法が提案されている（特許文献２参照）。この方法では、教示データに基づく実行プログラムにより原点から移動しながら動作を行うロボットが異常停止した時に、実行プログラム及び教示データを用いて原点から異常停止した停止位置への動作経路を算出し、動作経路を逆行する逆行経路を用いて原点へ復帰させる。この際、ロボットに作用する負荷情報から、干渉物への干渉を検知し、各負荷情報に基づいて干渉物から離脱する離脱位置を算出し、離脱位置から原点への復帰経路を逆行経路を用いて算出する。

特開平７−２８５２０号公報 特開２０１８−１４４１７１号公報

しかしながら、障害物との衝突などの障害発生状態では、ロボットは衝突した障害物と複雑に絡み合ったり、ロボット自身が複雑な姿勢になっていたりするため、経路の逆順実行では復帰できない場合も多い。経路の逆順実行により自動で復帰できない場合には、再度の衝突やリミットオーバーを起こさず安全な姿勢に復帰させるために、ユーザによる熟練した慎重な操作が必要であり、障害からの復帰に要する時間が長くなるという問題があった。復帰に時間を要すると、ロボットの通常動作により行われている作業の生産性の低下を招いてしまう。

本発明は、上記の点に鑑みてなされたものであり、障害状態から安全な姿勢へ短時間で復帰することができるロボット制御装置、方法、及びプログラムを提供することを目的とする。

上記目的を達成するために、本発明に係るロボット制御装置は、ロボットの動作時に障害が発生したことが検知されると、障害発生後の前記ロボットの周辺の環境情報、前記ロボットの仕様情報、及び前記ロボットが復帰すべき姿勢の情報を取得し、取得した情報に基づいて、障害発生後の姿勢から前記復帰すべき姿勢までのロボットの経路を生成する生成部と、前記生成部により生成された経路に基づいて、前記ロボットが前記復帰すべき姿勢となるように、前記ロボットの動作を制御する制御部と、を含んで構成することができる。

本発明に係るロボット制御装置によれば、生成部が、ロボットの動作時に障害が発生したことが検知されると、障害発生後のロボットの周辺の環境情報、ロボットの仕様情報、及びロボットが復帰すべき姿勢の情報を取得し、取得した情報に基づいて、障害発生後の姿勢から復帰すべき姿勢までのロボットの経路を生成する。そして、制御部が、生成部により生成された経路に基づいて、ロボットが復帰すべき姿勢となるように、ロボットの動作を制御する。これにより、障害状態から安全な姿勢へ短時間で復帰することができる。

また、前記生成部は、前記ロボットの周辺の各点の３次元位置を示す３次元計測データを、障害発生後の前記環境情報として取得し、前記３次元計測データに基づいて、障害発生後における前記ロボットの周辺に存在する物体の形状及び位置を特定することができる。これにより、ＣＡＤデータなどで事前に形状及び位置の情報が与えられている物体について、障害の発生により変形等が生じた場合でも、障害発生後の物体の形状及び位置を特定することができる。

また、前記生成部は、障害発生前における、前記ロボットの周辺に存在する物体の形状及び位置を示す３次元情報と、障害発生後に取得された前記環境情報とを用いて、障害発生後における前記ロボットの周辺に存在する物体の形状及び位置を特定することができる。ＣＡＤデータなどで事前に与えられている物体の３次元情報を用いることにより、障害発生後の物体の形状及び位置を精度良く特定することができる。

また、前記生成部は、前記ロボットの周辺の各点の３次元位置を示す３次元計測データを、障害発生後の前記環境情報として取得し、前記３次元情報がマッチングする３次元計測データの部分については、前記３次元情報に基づいて前記ロボットの周辺に存在する物体の形状及び位置を特定し、マッチングする前記３次元情報が存在しない３次元計測データの部分については、前記３次元計測データに基づいて前記ロボットの周辺に存在する物体の形状及び位置を特定することができる。これにより、３次元情報を利用できる部分については精度良く物体の形状及び位置を特定することができ、また、全範囲について３次元計測データのみで特定する場合に比べて、処理負荷を軽減することができる。

また、前記生成部は、障害発生後に前記環境情報を計測できない領域を、障害物候補領域として特定することができる。これにより、障害状態から復帰するための、より安全な経路を生成することができる。

また、前記生成部は、前記ロボットの周辺の領域毎に、障害物が存在する可能性に応じたコストを設定しておき、前記障害物候補領域のコストに応じて、前記障害物候補領域に障害物が存在するか否かを特定することができる。これにより、環境情報を計測できない領域について、障害物が存在するか否かを柔軟に判断して、経路を生成することができる。

また、前記生成部は、障害発生後における前記ロボットの周辺に存在する物体の形状及び位置と、生成した経路を示す情報とを表示装置に表示することができる。これにより、ユーザは、ロボットを動作させる前に、経路やロボット周辺の物体の情報を確認することができる。

また、前記生成部は、前記表示装置に表示した、障害発生後における前記ロボットの周辺に存在する物体の形状及び位置と、前記経路との少なくとも一方に対する修正を受け付けることができる。これにより、修正が必要な場合には修正を行った上で、経路の再生成を指示することができる。

また、前記生成部は、前記経路を生成できない場合に、前記経路を生成できない原因に関する情報を表示装置に表示することができる。これにより、ユーザに対して、経路を生成できない原因を除去するように促すことができる。

また、本発明に係るロボット制御方法は、生成部が、ロボットの動作時に障害が発生したことが検知されると、障害発生後の前記ロボットの周辺の環境情報、前記ロボットの仕様情報、及び前記ロボットが復帰すべき姿勢の情報を取得し、取得した情報に基づいて、障害発生後の姿勢から前記復帰すべき姿勢までのロボットの経路を生成し、制御部が、前記生成部により生成された経路に基づいて、前記ロボットが前記復帰すべき姿勢となるように、前記ロボットの動作を制御する方法である。

また、本発明に係るロボット制御プログラムは、コンピュータを、ロボットの動作時に障害が発生したことが検知されると、障害発生後の前記ロボットの周辺の環境情報、前記ロボットの仕様情報、及び前記ロボットが復帰すべき姿勢の情報を取得し、取得した情報に基づいて、障害発生後の姿勢から前記復帰すべき姿勢までのロボットの経路を生成する生成部、及び、前記生成部により生成された経路に基づいて、前記ロボットが前記復帰すべき姿勢となるように、前記ロボットの動作を制御する制御部として機能させるためのプログラムである。

本発明に係るロボット制御装置、方法、及びプログラムによれば、障害発生後の環境情報を取得して、障害発生後の姿勢から復帰すべき姿勢までのロボットの経路を生成することにより、障害状態から安全な姿勢へ短時間で復帰することができる。

第１〜第３実施形態に係るロボット制御システムの概略構成を示す図である。 第１〜第３実施形態に係るロボット制御装置のハードウェア構成を示すブロック図である。 第１実施形態に係るロボット制御システムの機能ブロック図である。 経路の逆順実行で復帰動作を行う場合を説明するための図である。 経路の逆順実行での復帰動作の問題点を説明するための図である。 本実施形態の復帰動作を説明するための図である。 第１実施形態におけるロボット制御処理の一例を示すフローチャートである。 オクルージョンの発生を説明するための図である。 第２実施形態に係るロボット制御システムの機能ブロック図である。 コストの設定の一例を示す図である。 コストの設定の他の例を示す図である。 オクルージョンエリアのコストの計算を説明するための図である。 仮想の障害物の設定を説明するための図である。 第２実施形態における復帰動作のための経路の生成を説明するための図である。 第２実施形態におけるロボット制御処理の一例を示すフローチャートである。 第３実施形態に係るロボット制御システムの機能ブロック図である。 表示画面の一例を示す図である。 第３実施形態におけるロボット制御処理の一例を示すフローチャートである。

以下、本発明の実施形態の一例を、図面を参照しつつ説明する。なお、各図面において同一又は等価な構成要素及び部分には同一の参照符号を付与している。また、図面の寸法及び比率は、説明の都合上誇張されており、実際の比率とは異なる場合がある。

また、以下の各実施形態では、本発明に関する部分、すなわち、ロボットの障害発生時の復帰動作に関する部分ついて主に説明し、ワークのピックアンドプレース等の通常の動作に関しては、本発明に関連する部分についてのみ説明する。

＜第１実施形態＞
図１に示すように、第１実施形態に係るロボット制御システム１００は、ロボット制御装置１０と、操作端末２０と、ロボット３０と、３Ｄセンサ３５とを含んで構成される。

ロボット３０は、例えば、３次元空間における動作に必要な６自由度の構成を備えた垂直多関節型のロボットである。なお、ロボット３０の自由度は、６自由度に冗長自由度を加えた７自由度としてもよい。ロボット３０は、ロボット制御装置１０により生成された経路に従って制御されて動作する。

また、ロボット３０には、障害物との衝突等の障害の発生を検知するセンサ（図示省略）が設けられており、障害の発生が検知されると、ロボット３０は動作を停止し、自身の現在の姿勢を示す現在姿勢情報と共に、障害が発生したことをロボット制御装置１０へ通知する。

なお、ロボット３０の姿勢は、ロボット３０の手先（ロボットハンドなどのツールが取り付けられる側）の基準位置がある位置（ｘ，ｙ，ｚ）及び姿勢（ｒｏｌｌ，ｐｉｔｃｈ，ｙａｗ）にある場合の、ロボット３０の第一関節から第Ｎ関節までの各関節の値（回転角度）の系列（Ｊ１，Ｊ２，・・・，ＪＮ）で表される（Ｎはロボットの関節数）。

操作端末２０は、情報を入力するための情報処理装置で、例えば、パーソナルコンピュータやティーチングペンダント（ＴＰ）で実現することができる。操作端末２０は、経路の生成及びロボットの制御に必要な情報の入力を受け付け、受け付けた情報を、ロボット制御装置１０へ入力する。経路の生成及びロボットの制御に必要な情報には、環境情報、ロボット仕様情報、及び安全姿勢情報が含まれる。各情報については後述する。

３Ｄセンサ３５は、ロボット３０及びロボット３０の動作領域を含む周辺の環境を計測し、計測結果を出力する。３Ｄセンサ３５は、例えば、３次元カメラやレーザレーダなどにより実現することができる。本実施形態では、３Ｄセンサ３５は、周辺環境の各点の３次元位置を示す点群データを計測するものとする。３Ｄセンサ３５は、計測結果である点群データを操作端末２０へ入力する。なお、点群データは、本発明の３次元計測データの一例であるが、３次元計測データは、ＤｅｐｔｈＭａｐやラインスキャンデータなどでもよく、点群データに限定されない。

ロボット制御装置１０は、ロボット３０から障害の発生が通知されると、ロボット３０を指定された安全姿勢に復帰させるための経路を生成し、生成した経路に基づいて、ロボット３０の復帰動作を制御する。なお、本実施形態における経路とは、ロボット３０の手先の基準位置を、任意の始点から終点まで動作させるときのロボット３０の姿勢を時系列に並べたものである。

ロボット制御装置１０は、パーソナルコンピュータや産業用コンピュータで実現することができる。図２は、本実施形態に係るロボット制御装置１０のハードウェア構成を示すブロック図である。図２に示すように、ロボット制御装置１０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）４２、メモリ４４、記憶装置４６、入力装置４８、出力装置５０、光ディスク駆動装置５２、及び通信インタフェース（Ｉ／Ｆ）５４を有する。各構成は、バスを介して相互に通信可能に接続されている。

記憶装置４６には、ロボット制御処理を実行するためのロボット制御プログラムが格納されている。ＣＰＵ４２は、中央演算処理ユニットであり、各種プログラムを実行したり、各構成を制御したりする。すなわち、ＣＰＵ４２は、記憶装置４６からプログラムを読み出し、メモリ４４を作業領域としてプログラムを実行する。ＣＰＵ４２は、記憶装置４６に記憶されているプログラムに従って、上記各構成の制御及び各種の演算処理を行う。

メモリ４４は、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）により構成され、作業領域として一時的にプログラム及びデータを記憶する。記憶装置４６は、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、及びＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）又はＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）により構成され、オペレーティングシステムを含む各種プログラム、及び各種データを格納する。

入力装置４８は、例えば、キーボードやマウス等の、各種の入力を行うための装置である。出力装置５０は、例えば、ディスプレイやプリンタ等の、各種の情報を出力するための装置である。出力装置５０として、タッチパネルディスプレイを採用することにより、入力装置４８として機能させてもよい。光ディスク駆動装置５２は、各種の記録媒体（ＣＤ−ＲＯＭ又はブルーレイディスクなど）に記憶されたデータの読み込みや、記録媒体に対するデータの書き込み等を行う。

通信インタフェース５４は、他の機器と通信するためのインタフェースであり、例えば、イーサネット（登録商標）、ＦＤＤＩ又はＷｉ−Ｆｉ（登録商標）等の規格が用いられる。

なお、操作端末２０のハードウェア構成も、図２に示す構成と概ね同様であるため、説明を省略する。

次に、第１実施形態に係るロボット制御装置１０及び操作端末２０の機能構成について説明する。

図３は、ロボット制御システム１００の機能構成の例を示すブロック図である。図３に示すように、操作端末２０は、機能構成として、環境情報入力部２２と、ロボット仕様入力部２４と、安全姿勢入力部２６とを含む。

環境情報入力部２２は、ロボット３０及びロボット３０の動作領域を含む周辺の環境を示す環境情報の入力を受け付け、ロボット制御装置１０へ入力する。環境情報入力部２２は、環境情報として、３Ｄセンサ３５から出力される計測結果を受け付ける。また、環境情報入力部２２は、環境情報として、周辺機器、棚、コンテナなどの障害物の形状及び位置を示す３次元情報であるＣＡＤデータの入力を受け付ける。なお、障害物は、本発明の物体の一例である。

ロボット仕様入力部２４は、ロボット３０の仕様を示すロボット仕様情報の入力を受け付け、ロボット制御装置１０へ入力する。ロボット仕様情報には、例えば、ロボットの形状情報や、手先の基準位置、関節配置等のキネマティクス情報、各関節の限界角度、回転速度等のジョイント情報が含まれる。

安全姿勢入力部２６は、障害発生時にロボット３０が復帰すべき安全姿勢を示す安全姿勢情報の入力を受け付け、ロボット制御装置１０へ入力する。安全姿勢としては、ロボット３０原点（例えば、各関節の角度が０度）の姿勢や、周辺環境に応じて、完全に障害物の存在しない位置へ復帰させた場合の姿勢などとすることができる。

また、図３に示すように、ロボット制御装置１０は、機能構成として、経路生成部１２と、制御部１４とを含む。各機能構成は、ＣＰＵ４２が記憶装置４６に記憶されたロボット制御プログラムを読み出し、メモリ４４に展開して実行することにより実現される。なお、経路生成部１２は、本発明の生成部の一例であり、制御部１４は、本発明の制御部の一例である。

ここで、図４の左図に示すように、破線矢印で示す経路に従って、（１）→（２）の順にロボット３０が動作する場合において、（２）の動作中に障害が発生したとする。この場合において、従来技術のように、経路の逆順実行により復帰動作を行う場合は、図４の右図に示すように、（１）→（２）の逆順で（２’）→（１’）の経路で復帰動作が行われる。

しかし、図５に示すように、障害発生後に障害物の位置や形状が変わるなど、周辺の環境が変化した場合、逆順実行では、位置が変わってしまった障害物に再度衝突してしまう可能性があるなど、安全に復帰動作を行うことができない場合がある。

そこで、本実施形態では、例えば、図６中の（３）→（４）に示すように、障害発生後の周辺環境に応じて、新たに復帰のための経路を生成して、復帰動作を行う。

具体的には、経路生成部１２は、ロボット３０の現在姿勢情報と共に、障害が発生したことの通知を制御部１４を介してロボット３０から受け取ると、環境情報入力部２２から入力された障害発生後の環境情報、ロボット仕様入力部２４から入力されたロボット仕様情報、及び安全姿勢入力部２６から入力された安全姿勢情報を取得する。

経路生成部１２は、取得した環境情報に基づいて、障害発生後の障害物の位置及び形状（以下、「障害物情報」という）を特定する。具体的には、経路生成部１２は、障害発生後に環境情報として取得した３Ｄセンサ３５の計測結果である点群データに基づいて、障害発生後におけるロボット３０の周辺に存在する障害物情報を特定する。例えば、所定範囲毎に切出した点群データを、予め学習された識別器に入力し、その認識結果として、障害物情報を得ることができる。

また、経路生成部１２は、障害発生前における、ロボット３０の周辺に存在する障害物の形状及び位置を示す３次元情報、例えば、通常動作のための経路生成に使用されたＣＡＤデータを取得する。そして、経路生成部１２は、点群データに対して、ＣＡＤデータに含まれる各障害物の形状を回転及び移動させながらマッチングすることにより、障害発生後におけるロボット３０の周辺に存在する障害物情報を特定してもよい。この場合、既知の形状データを用いることができるため、点群データのみで障害物情報を特定する場合に比べ、精度良く障害物情報を特定することができる。

また、経路生成部１２は、ＣＡＤデータで示される各障害物のうち、点群データとマッチングする部分については、ＣＡＤデータのマッチング結果に基づいて、障害発生後の障害物情報を特定し、マッチングするＣＡＤデータが存在しない点群データの部分については、点群データに基づいて、障害発生後の障害物情報を特定してもよい。この場合、ＣＡＤデータを利用できる部分については精度良く障害物情報を特定することができ、また、全範囲について点群データのみで障害物情報を特定する場合に比べて、処理負荷を軽減することができる。

なお、障害発生前のＣＡＤデータで示される障害物のうち、障害発生後の点群データにマッチングする部分がない障害物は、障害の発生により、形状が変化したことが想定される。

経路生成部１２は、特定した障害物情報、取得したロボット仕様情報と安全姿勢情報、通知された現在姿勢情報に基づいて、現在姿勢から安全姿勢までのロボット３０の経路を生成する。経路の生成には、例えば、ＲＲＴ（Ｒａｐｉｄｌｙ− ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅ）、ＰＲＭ（Ｐｒｏｂａｂｉｌｉｓｔｉｃ ＲｏａｄＭａｐ）などのパスプランニングの手法を用いることができる。経路生成部１２は、生成した経路の情報を制御部１４へ受け渡す。

このように、障害発生後の環境情報を改めて取得して、障害発生後の障害物情報を特定した上で、安全姿勢に復帰させるための経路を生成することで、安全に復帰動作を行うことができる。

制御部１４は、ロボット３０から、現在姿勢情報と共に、障害が発生したことが通知されると、現在姿勢情報及び障害が発生したことを経路生成部１２へ通知する。また、制御部１４は、経路生成部１２により生成された経路に基づいて、ロボット３０が安全姿勢となるように復帰動作を制御する。

次に、第１実施形態に係るロボット制御システム１００の作用について説明する。

ロボット３０において、ワークのピックアンドプレース等の通常の動作が行われている際に、図７に示すロボット制御処理が実行される。

図７は、ロボット制御装置１０のＣＰＵ４２により実行されるロボット制御処理の流れを示すフローチャートである。ＣＰＵ４２が記憶装置４６からロボット制御プログラムを読み出して、メモリ４４に展開して実行することにより、ＣＰＵ４２がロボット制御装置１０の各機能構成として機能し、図７に示すロボット制御処理が実行される。なお、ロボット制御処理は、本発明のロボット制御方法の一例である。

まず、ステップＳ１２で、制御部１４が、ロボット３０から、現在姿勢情報と共に、障害が発生したことが通知されたか否かを判定することにより、障害が発生したか否かを判定する。障害が発生していない場合には、本ステップの判定を繰り返し、障害が発生した場合には、制御部１４が、現在姿勢情報及び障害が発生したことを経路生成部１２へ通知し、ステップＳ１４へ移行する。

ステップＳ１４では、経路生成部１２が、環境情報入力部２２から入力された障害発生後の環境情報を取得する。

次に、ステップＳ１６で、経路生成部１２が、取得した環境情報に基づいて、障害発生後の障害物情報（形状及び位置）を特定する。

次に、ステップＳ１８で、経路生成部１２が、ロボット仕様入力部２４から入力されたロボット仕様情報を取得する。なお、ロボット仕様情報は、障害の発生前後で変化しないため、通常の経路生成時に取得済みのロボット仕様情報がロボット制御装置１０の所定領域に記憶されている場合には、そのロボット仕様情報を読み出してもよい。

次に、ステップＳ２０で、経路生成部１２が、安全姿勢入力部２６から入力された安全姿勢情報を取得する。なお、障害の状況によらず、予め定めた安全姿勢に復帰させる場合には、安全姿勢情報をロボット制御装置１０の所定領域に予め記憶しておいて、その安全姿勢情報を読み出してもよい。

なお、ステップＳ１４及びＳ１６と、ステップＳ１８と、ステップＳ２０との処理順は、図７に示す例に限定されず、いずれを先に実行してもよいし、並行して実行してもよい。

次に、ステップＳ２２で、経路生成部１２が、上記ステップＳ１６で特定した障害物情報、上記ステップＳ１８で取得したロボット仕様情報、上記ステップＳ２０で取得した安全姿勢情報、及び上記ステップＳ１２で通知された現在姿勢情報に基づいて、現在姿勢から安全姿勢までのロボット３０の経路を、例えばパスプランニングの手法を用いて生成する。

次に、ステップＳ２４で、制御部１４が、上記ステップＳ２２で経路生成部１２により生成された経路に基づいて、ロボット３０が安全姿勢となるように復帰動作を制御し、ロボット制御処理は終了する。

復帰動作の完了後は、通常動作を再開させたり、通常動作を一旦停止したりするなど、アプリケーションに応じた処理を実行すればよい。

以上説明したように、第１実施形態に係るロボット制御システム１００によれば、ロボット制御装置１０が、障害発生後の環境情報を改めて取得して、障害発生後の障害物の形状及び位置を特定した上で、安全姿勢に復帰させるための経路を生成する。これにより、再度の衝突やリミットオーバーを起こすことなく、また、ユーザによる熟練した慎重な操作を必要とすることなく、障害状態から安全な姿勢へ短時間で復帰することができる。

＜第２実施形態＞
次に、第２実施形態について説明する。なお、第２実施形態に係るロボット制御システムにおいて、第１実施形態に係るロボット制御システム１００と同一又は等価な構成要素及び部分には同一の参照符号を付与し、詳細な説明を省略する。

図１に示すように、第２実施形態に係るロボット制御システム２００は、ロボット制御装置２１０と、操作端末２０と、ロボット３０と、３Ｄセンサ３５とを含んで構成される。

第１実施形態において、障害発生後の環境情報を改めて取得することについて説明したが、障害発生後において、３Ｄセンサ３５において点群データを計測できないオクルージョンエリアが生じる場合がある。例えば、図８の左図に示すように、障害発生前には作業空間全体が計測結果として得られていたが、障害発生により箱が横転し、図８の右図に示すように、オクルージョン（図８中の網掛け部分）により、箱の内側の計測結果が得られない状態となる場合がある。

第２実施形態では、このようなオクルージョンエリアの発生も考慮して、障害発生後の障害物情報を特定した上で、安全姿勢へ復帰させるための経路を生成する。

第２実施形態に係るロボット制御装置２１０の機能構成について説明する。なお、第２実施形態に係るロボット制御装置２１０のハードウェア構成は、図２に示す第１実施形態に係るロボット制御装置１０のハードウェア構成と同様であるため、説明を省略する。

図９は、ロボット制御システム２００の機能構成の例を示すブロック図である。図９に示すように、ロボット制御装置２１０は、機能構成として、経路生成部２１２と、制御部１４と、コスト評価部１６とを含む。なお、経路生成部２１２及びコスト評価部１６は、本発明の生成部の一例である。

コスト評価部１６は、環境情報入力部２２から環境情報を取得し、取得した環境情報に基づいて、ロボット３０周辺の障害物情報を特定する。障害物情報の特定方法は、第１実施形態の経路生成部１２における特定方法と同様である。

また、コスト評価部１６は、特定した障害物情報に基づいて、オクルージョンエリアを特定する。なお、オクルージョンエリアは、本発明の障害物候補領域の一例である。例えば、点群データにＣＡＤデータをマッチングさせて障害物情報を特定した場合、障害物の移動や回転等が把握できるため、オクルージョンエリアも特定することができる。例えば、図８の場合、箱のＣＡＤデータと点群データとのマッチングにより、箱が横転した状態になっていることが把握され、ＣＡＤデータに基づく箱の形状から、箱の内部がオクルージョンエリアになっていることを特定することができる。

コスト評価部１６は、特定したオクルージョンエリアに障害物が存在するか否かを評価するためのコストを計算する。具体的には、コスト評価部１６は、ロボット３０の周辺の領域毎に、障害物が存在する可能性に応じたコストを設定しておく。

例えば、床に近い方が、障害物が存在する可能性が高く、一方で、空中に障害物が存在する可能性は低いという前提の下、図１０に示すように、床面に近い領域ほどコストが高くなるように設定することができる。また、例えば、ロボット３０の中心に近い位置は障害物が存在する可能性が低いという前提の下、図１１に示すように、ロボット３０に近い領域ほどコストが低くなるように設定することができる。なお、本実施形態では、コストが高いほど、障害物が存在する可能性が高いことを表すものとする。各領域のコストは１パターンのみ設定してもよいし、複数設定してもよい。本実施形態では、図１０に示すパターンと図１１に示すパターンでコストが設定されているものとする。

コスト評価部１６は、例えば、図１２に示すように、設定されている各領域のコストを足し合わせ、オクルージョンエリアのコストを計算する。また、コスト評価部１６は、コストが予め定めた閾値以下のオクルージョンエリアには障害物は存在しないと評価する。一方、コスト評価部１６は、コストが閾値を超えるオクルージョンエリアには障害物が存在すると評価し、例えば図１３に示すように、その領域に相当する形状（例えば直方体などの形状）の仮想の障害物を設定する。

コスト評価部１６は、設定した仮想の障害物についての障害物情報を、実際の障害物についての障害物情報に追加し、障害発生後のロボット３０周辺の障害物情報を経路生成部２１２へ受け渡す。

経路生成部２１２は、コスト評価部１６で特定された障害物情報、ロボット仕様入力部２４から入力されたロボット仕様情報、安全姿勢入力部２６から入力された安全姿勢情報、及び制御部１４から通知されたロボット３０の現在姿勢情報を取得し、取得した情報に基づいて、現在姿勢から安全姿勢に復帰するための経路を生成する。

第２実施形態では、上記の仮想の障害物も含む障害物情報が特定されているため、例えば図１４に示すように、実際の障害物だけでなく、オクルージョンエリアのうち、障害物が存在する可能性の高い領域も回避して、復帰動作のための経路が生成される。

次に、第２実施形態に係るロボット制御システム２００の作用について説明する。第２実施形態では、ロボット制御装置２１０において、図１５に示すロボット制御処理が実行される。なお、第２実施形態におけるロボット制御処理において、第１実施形態におけるロボット制御処理（図７）と同様の処理については、同一のステップ番号を付して詳細な説明を省略する。

ステップＳ１２〜Ｓ１６を経て、ステップＳ２１２へ移行すると、コスト評価部１６が、上記ステップＳ１４で特定した実際の障害物についての障害物情報に基づいて、オクルージョンエリアを特定し、特定したオクルージョンエリアに障害物が存在するか否かを評価するためのコストを計算する。そして、コスト評価部１６は、コストが予め定めた閾値以下のオクルージョンエリアには障害物は存在しないと評価し、コストが閾値を超えるオクルージョンエリアには障害物が存在すると評価する。

次に、ステップＳ２１４で、コスト評価部１６が、障害物が存在すると評価したオクルージョンエリアの領域に相当する形状（例えば直方体などの形状）の仮想の障害物を設定し、設定した仮想の障害物についての障害物情報を、上記ステップＳ１６で特定した実際の障害物についての障害物情報に追加し、障害発生後のロボット３０周辺の障害物情報として経路生成部２１２へ受け渡す。

以下、第１実施形態と同様にステップＳ１８〜Ｓ２４が実行されて、ロボット制御処理は終了する。

以上説明したように、第２実施形態に係るロボット制御システム２００によれば、ロボット制御装置２１０が、オクルージョンエリアの発生も考慮して、障害発生後の環境情報を改めて取得して、障害発生後の障害物の形状及び位置を特定した上で、安全姿勢に復帰させるための経路を生成する。これにより、障害物が崩れてきてロボットの動作中に新たに障害物が発生しそうなエリア（コストが閾値を超える領域）や、机の下などの３Ｄセンサで計測不可だが明らかに障害物が存在しないエリア（コストが閾値以下の領域）などに柔軟に対応した安全な経路を効率よく生成することができる。

＜第３実施形態＞
次に、第３実施形態について説明する。なお、第３実施形態に係るロボット制御システムにおいて、第１実施形態に係るロボット制御システム１００と同一又は等価な構成要素及び部分には同一の参照符号を付与し、詳細な説明を省略する。

図１に示すように、第３実施形態に係るロボット制御システム３００は、ロボット制御装置３１０と、表示部２８を備えた操作端末２０と、ロボット３０と、３Ｄセンサ３５とを含んで構成される。

第３実施形態に係るロボット制御装置３１０の機能構成について説明する。なお、第３実施形態に係るロボット制御装置３１０のハードウェア構成は、図２に示す第１実施形態に係るロボット制御装置１０のハードウェア構成と同様であるため、説明を省略する。

図１６は、ロボット制御システム３００の機能構成の例を示すブロック図である。図１６に示すように、ロボット制御装置３１０は、機能構成として、経路生成部３１２と、制御部１４とを含む。なお、経路生成部３１２は、本発明の生成部の一例である。

経路生成部３１２は、第１実施形態における経路生成部１２と同様に、現在姿勢から安全姿勢に復帰するための経路を生成する。障害物を回避可能な経路が存在しない、障害発生時にロボット３０がロックされた等の原因により、経路を生成することができない場合、経路生成部３１２は、経路を生成できない原因に関するメッセージを生成する。例えば、障害物を回避可能な経路が存在しないことにより経路を生成できなかった場合には、「障害物を回避可能な経路を生成できませんでした。障害物を除去してください。」等のメッセージを生成する。

経路生成部３１２は、生成した経路又はメッセージと、経路を生成する際に特定した、障害発生後におけるロボット３０の周辺に存在する障害物についての障害物情報とを、表示部２８に表示する。

図１７に、表示部２８に表示する表示画面６０の一例を示す。表示画面６０には、障害物情報と、生成した経路に基づいてロボット３０の動作をシミュレーションした結果とを３次元のアニメーションで表示するための表示領域６２が含まれる。また、表示画面６０には、表示領域６２に表示された経路及び障害物情報の少なくとも一方に対する修正を受け付けるための、各種パラメータの入力領域６４が含まれる。

入力領域６４へは、環境情報入力部２２、ロボット仕様入力部２４、及び安全姿勢入力部２６を介してユーザにより修正された値等が入力される。例えば、障害物が存在する領域を生成された経路が通過するような場合には、経路を修正する情報が入力される。また、例えば、３Ｄセンサ３５の計測エラーや、点群データから障害物を特定する際の認識エラー等により、特定された障害物情報に誤りがある場合には、正しい障害物情報が入力される。なお、表示領域６２に対するドラッグ操作等による修正を受け付け可能にしてもよい。

さらに、表示画面６０には、経路生成部３１２で生成されたメッセージが表示されるメッセージ領域６６と、生成された経路でのロボット３０の動作を許可する場合に選択される実行ボタン６８とが含まれる。

入力領域６４を介して入力された経路及び障害物情報の少なくとも一方に対する修正は、環境情報入力部２２、ロボット仕様入力部２４、及び安全姿勢入力部２６を介して経路生成部３１２へ入力される。したがって、経路生成部３１２は、修正された情報に基づいて、経路を再生成する。

また、メッセージ領域６６に表示されたメッセージにしたがって、ユーザが障害物の除去やロボット３０のロック解除等の対処を行い、対処完了を示すコマンドが操作端末２０から入力されると、３Ｄセンサ３５により、対処完了後の環境情報が改めて計測され、環境情報入力部２２を介して経路生成部３１２へ入力される。したがって、経路生成部３１２は、対処完了後の環境情報に基づいて、経路を再生成する。

また、実行ボタン６８が選択されると、表示部２８から制御部１４へ、生成された経路でロボット３０を動作させる許可が通知され、制御部１４により、ロボット３０の動作が制御される。

次に、第３実施形態に係るロボット制御システム３００の作用について説明する。第３実施形態では、ロボット制御装置３１０において、図１８に示すロボット制御処理が実行される。なお、第３実施形態におけるロボット制御処理において、第１実施形態におけるロボット制御処理（図７）と同様の処理については、同一のステップ番号を付して詳細な説明を省略する。

ステップＳ１２〜Ｓ２２を経て、経路生成部３１２が経路の生成を実行し、次のステップＳ３１２で、経路が生成できたか否かを判定する。経路が生成できた場合には、ステップＳ３１４へ移行し、経路が生成できなかった場合には、ステップＳ３１８へ移行する。

ステップＳ３１４では、経路生成部３１２が、上記ステップＳ２２で生成した経路、及び上記ステップＳ１６で特定した障害物情報に関する内容を含む表示画面６０を表示部２８に表示する。

次に、ステップＳ３１６で、操作端末２０が、表示画面６０に表示された内容に対して修正を受け付けたか否かを判定する。修正を受け付けた場合には、ステップＳ２２に戻り、経路生成部３１２が、修正された情報に基づいて、経路を再生成する。一方、修正がない場合、又は修正が終了した場合には、表示画面６０に含まれる実行ボタン６８をユーザが選択する。

これにより、ステップＳ２４へ移行し、表示部２８から制御部１４へ、生成された経路でロボット３０を動作させる許可が通知され、制御部１４により、ロボット３０の動作が制御される。

また、ステップＳ３１８では、経路生成部３１２が、経路を生成できない原因に関するメッセージを生成し、メッセージ領域６６に生成したメッセージを含む表示画面６０を表示部２８に表示する。

次に、ステップＳ３２０で、メッセージ領域６６に表示されたメッセージにしたがった原因の除去などの対処が完了したことを示すコマンドが操作端末２０から入力されると、ステップＳ１４に戻る。

以上説明したように、第３実施形態に係るロボット制御システム２００によれば、ロボット制御装置３１０が、障害発生後の環境情報に基づいて生成した経路及び障害物情報を表示して、ユーザからの修正を受け付ける。また、経路が生成できない場合には、その原因をユーザに提示して、原因の除去などの対処を促す。これにより、ロボットの復帰動作が実行される前に、ユーザがロボットの動作を確認して、修正が必要な場合には修正を行った上で、経路の再生成を指示したり、原因を除去したりすることができる。

なお、第３実施形態では、表示部への各種表示や修正の受け付け等を第１実施形態に加えた構成として説明したが、第２実施形態に適用してもよい。

また、第３実施形態では、操作端末２０が備える表示部２８に経路等を表示する場合について説明したが、表示部２８は操作端末２０とは独立した表示装置として構成してもよい。

また、上記各実施形態では、パスプランニングにより経路を生成する場合について説明したが、これに限定されず、事前に学習した結果に基づいて経路を生成するなどしてもよい。

また、上記各実施形態では、垂直多関節型のロボットの制御について説明したが、本発明は、垂直多関節型のロボットに限定されず、任意の開ループ型のロボットに適用できる。また、デルタロボットなどの閉ループ型のロボットに対しても適用できる。さらに、台座が固定されていない浮遊リンク系であるモバイルロボットやモバイルマニピュレータ等、広範なロボットに対しても適用することができる。

また、上記各実施形態では、ロボット制御装置と操作端末とを別々の装置とする場合について説明したが、ロボット制御装置と操作端末とを１つの情報処理装置で構成するようにしてもよい。また、ロボット制御装置の各機能構成を、それぞれ異なる装置で実現し、上記のロボット制御処理を分散処理により実現してもよい。

また、上記各実施形態でＣＰＵがソフトウェア（プログラム）を読み込んで実行したロボット制御処理を、ＣＰＵ以外の各種のプロセッサが実行してもよい。この場合のプロセッサとしては、ＦＰＧＡ（Ｆｉｅｌｄ−Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）等の製造後に回路構成を変更可能なＰＬＤ（Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｄｅｖｉｃｅ）、及びＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）等の特定の処理を実行させるために専用に設計された回路構成を有するプロセッサである専用電気回路等が例示される。また、ロボット制御処理を、これらの各種のプロセッサのうちの１つで実行してもよいし、同種又は異種の２つ以上のプロセッサの組み合わせ（例えば、複数のＦＰＧＡ、及びＣＰＵとＦＰＧＡとの組み合わせ等）で実行してもよい。また、これらの各種のプロセッサのハードウェア的な構造は、より具体的には、半導体素子等の回路素子を組み合わせた電気回路である。

また、上記各実施形態では、ロボット制御プログラムが記憶装置に予め記憶（インストール）されている態様を説明したが、これに限定されない。プログラムは、ＣＤ−ＲＯＭ（Ｃｏｍｐａｃｔ Ｄｉｓｃ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＤＶＤ−ＲＯＭ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、及びＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）メモリ等の記録媒体に記録された形態で提供されてもよい。また、プログラムは、ネットワークを介して外部装置からダウンロードされる形態としてもよい。

１０、２１０、３１０ ロボット制御装置
１２、２１２、３１２ 経路生成部
１４ 制御部
１６ コスト評価部
２０ 操作端末
２２ 環境情報入力部
２４ ロボット仕様入力部
２６ 安全姿勢入力部
２８ 表示部
３０ ロボット
３５ ３Ｄセンサ
４２ ＣＰＵ
４４ メモリ
４６ 記憶装置
４８ 入力装置
５０ 出力装置
５２ 光ディスク駆動装置
５４ 通信インタフェース
６０ 表示画面
６２ 表示領域
６４ 入力領域
６６ メッセージ領域
６８ 実行ボタン
１００、２００、３００ ロボット制御システム

Claims (11)

1. ロボットの動作時に障害が発生したことが検知されると、障害発生後の前記ロボットの周辺の環境情報、前記ロボットの仕様情報、及び前記ロボットが復帰すべき姿勢の情報を取得し、取得した情報に基づいて、障害発生後の姿勢から前記復帰すべき姿勢までのロボットの経路を生成する生成部と、
   前記生成部により生成された経路に基づいて、前記ロボットが前記復帰すべき姿勢となるように、前記ロボットの動作を制御する制御部と、
   を含むロボット制御装置。
2. 前記生成部は、前記ロボットの周辺の各点の３次元位置を示す３次元計測データを、障害発生後の前記環境情報として取得し、前記３次元計測データに基づいて、障害発生後における前記ロボットの周辺に存在する物体の形状及び位置を特定する請求項１に記載のロボット制御装置。
3. 前記生成部は、障害発生前における、前記ロボットの周辺に存在する物体の形状及び位置を示す３次元情報と、障害発生後に取得された前記環境情報とを用いて、障害発生後における前記ロボットの周辺に存在する物体の形状及び位置を特定する請求項１に記載のロボット制御装置。
4. 前記生成部は、前記ロボットの周辺の各点の３次元位置を示す３次元計測データを、障害発生後の前記環境情報として取得し、前記３次元情報がマッチングする３次元計測データの部分については、前記３次元情報に基づいて前記ロボットの周辺に存在する物体の形状及び位置を特定し、マッチングする前記３次元情報が存在しない３次元計測データの部分については、前記３次元計測データに基づいて前記ロボットの周辺に存在する物体の形状及び位置を特定する請求項３に記載のロボット制御装置。
5. 前記生成部は、障害発生後に前記環境情報を計測できない領域を、障害物候補領域として特定する請求項１〜請求項４のいずれか１項に記載のロボット制御装置。
6. 前記生成部は、前記ロボットの周辺の領域毎に、障害物が存在する可能性に応じたコストを設定しておき、前記障害物候補領域のコストに応じて、前記障害物候補領域に障害物が存在するか否かを特定する請求項５に記載のロボット制御装置。
7. 前記生成部は、障害発生後における前記ロボットの周辺に存在する物体の形状及び位置と、生成した経路を示す情報とを表示装置に表示する請求項１〜請求項５のいずれか１項に記載のロボット制御装置。
8. 前記生成部は、前記表示装置に表示した、障害発生後における前記ロボットの周辺に存在する物体の形状及び位置と、前記経路との少なくとも一方に対する修正を受け付ける請求項７に記載のロボット制御装置。
9. 前記生成部は、前記経路を生成できない場合に、前記経路を生成できない原因に関する情報を表示装置に表示する請求項１〜請求項８のいずれか１項に記載のロボット制御装置。
10. 生成部が、ロボットの動作時に障害が発生したことが検知されると、障害発生後の前記ロボットの周辺の環境情報、前記ロボットの仕様情報、及び前記ロボットが復帰すべき姿勢の情報を取得し、取得した情報に基づいて、障害発生後の姿勢から前記復帰すべき姿勢までのロボットの経路を生成し、
    制御部が、前記生成部により生成された経路に基づいて、前記ロボットが前記復帰すべき姿勢となるように、前記ロボットの動作を制御する
    ロボット制御方法。
11. コンピュータを、
    ロボットの動作時に障害が発生したことが検知されると、障害発生後の前記ロボットの周辺の環境情報、前記ロボットの仕様情報、及び前記ロボットが復帰すべき姿勢の情報を取得し、取得した情報に基づいて、障害発生後の姿勢から前記復帰すべき姿勢までのロボットの経路を生成する生成部、及び、
    前記生成部により生成された経路に基づいて、前記ロボットが前記復帰すべき姿勢となるように、前記ロボットの動作を制御する制御部
    として機能させるためのロボット制御プログラム。

Images