JP2019123022A - ロボット制御装置および自動組立システム - Google Patents

Abstract

【課題】作業者とロボットが協働して、組立作業をおこなう場合に、作業者の安全性を向上させ、ラインの稼働率を向上させる。【解決手段】ロボット制御装置は、センサより作業者の動作状態を入力する。そして、ロボット制御装置は、ロボットの動作状態と作業者の動作状態より、ロボットと作業者のそれぞれの位置と速度ベクトルを算出し、ロボットと作業者のそれぞれの周辺に危険性判定領域（ロボットを停止させる領域、退避させる領域、減速させる領域）を生成し、生成したロボットの危険性判定領域と作業者の危険性判定領域の重なりから危険性を判定して、その判定の結果からロボットと作業者の衝突を回避する衝突回避軌道を生成して、生成した衝突回避軌道に基づいて、ロボット制御を行う。【選択図】 図６

Description

本発明は、ロボット制御装置および自動組立システムに係り、特に、作業者とロボットが協働して、組立作業を行う場合に、作業者の安全性を向上させ、ラインの稼働率を向上させるのに好適なロボット制御装置および自動組立システムに関する。

労働人口比率の低下や労働賃金の上昇に伴い、製造ラインへの自動組立機械（組立ロボット）の導入が進んでいる。組立ロボットの導入においては、動作中に作業者との接触が起きないよう、安全柵内に組立ロボットを設置する必要があった。

しかしながら、ロボットと安全柵の間に作業者がはさまれないよう、安全柵を十分広く設置する必要があり、組立ロボット設備の設置面積が大きくなり、工場内のスペースの有効活用ができない課題があった。また、生産形態が多品種変量生産にかわってきており、製品ごとの専用自動化設備ではなく、作業者と組立ロボットが協調して作業するフレキシブルラインが求められている。

そこで、作業者が安全に組立ロボットと同一空間で作業するための安全技術が、例えば、特許文献１と特許文献２に開示されている。

特許文献１には、組立ロボットが周辺障害物と衝突するか否かを動作シミュレーションで予測し、衝突すると判定した場合に、ハンドを障害物から遠ざかる方向に動作させる方法が開示されている。

また、特許文献２には、ロボットアームが障害物や人に近づく方向に移動する場合に、ロボットと障害物および人との距離がある値より接近した時にロボットアームを緊急停止させる方法が開示されている。

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

上記従来技術の特許文献１および特許文献２は、ロボットと障害物あるいは人との距離に基づいて安全性を判定するものであり、ロボットや障害物、人の移動速度や方向を考慮していない。したがって、障害物や人がロボットから遠ざかっている場合など、本来安全である場合にも、ロボットを緊急停止させることになるため、必要以上にロボットを停止させることになり、その結果、組立ラインの稼働率が低下することになる。

本発明の目的は、作業者とロボットが協働して、組立作業を行う場合に、作業者の安全性を向上させ、ラインの稼働率を向上させることのできるロボット制御装置および自動組立システムを提供することにある。

上記課題を解決するために、本発明のロボット制御装置の構成は、好ましくは、ロボットと作業者が作業空間を共有する自動組立システムのロボット制御装置であって、ロボットに動作指令を出力するロボット制御指令部と、衝突回避部とを備え、衝突回避部は、ロボットの動作状態を入力するロボット動作状態入力部と、作業者の動作状態をセンシングした結果を入力する作業者動作状態入力部と、ロボットと作業者の動作状態よりロボットと作業者のそれぞれの位置と速度ベクトルを算出する位置・速度ベクトル算出部と、ロボットと作業者のそれぞれの周辺に危険性判定領域を生成する危険性判定領域生成部と、抽出したロボットの危険性判定領域と作業者の危険性判定領域の重なりから危険性を判定する危険性判定部と、ロボットと作業者の位置と速度ベクトルから衝突回避可否を判定する衝突回避可否判定部と、危険性判定部で判定した結果から、衝突を回避するロボットの衝突回避軌道を生成する衝突回避軌道生成部と、衝突回避軌道を、ロボット制御指令部に出力するロボット制御データ出力部とを有するようにしたものである。

本発明によれば、作業者とロボットが協働して、組立作業を行う場合に、作業者の安全性を向上させ、ラインの稼働率を向上させることのできるロボット制御装置および自動組立システムを提供することができる。

自動組立システムの概要構成図である。 ロボット制御装置の機能構成図である。 モデル情報テーブルの一例を示す図である。 衝突回避情報テーブルの一例を示す図である。 軌道生成方式情報テーブルの一例を示す図である。 ロボット制御装置において衝突回避軌道を生成する処理を説明するゼネラルチャートである。 危険性判定領域の設定を説明するための図である。 危険性判定処理の詳細を示すフローチャートである。 危険性判定処理の仕方を説明する図である。 衝突回避軌道生成処理の詳細を示すフローチャートである。 衝突回避可否判定の詳細を示すフローチャートである（その一）。 ロボットが衝突回避動作を開始する直前の状態を示す図である（その一）。 衝突回避可否判定の詳細を示すフローチャートである（その二）。 ロボットが衝突回避動作を開始する直前の状態を示す図である（その二）。

以下、本発明に係る一実施形態を、図１ないし図１４を用いて説明する。

先ず、図１および図２を用いて自動組立システムの構成について説明する。
自動組立システム１００は、図１に示されるように、ロボット１０１、センサ１０３、ロボット制御装置１０４、ロボット作業指示装置１０６から構成される。

ロボット１０１は、ロボット制御装置１０４からの指令により、ワーク１２０に対して、移動、加工、部品の取付けなどのなんらかの動作を行う装置である。ロボット制御装置１０４は、ロボット作業指示装置１０６からの指示に従って、ロボット１０１に動作の指令をする装置である。ロボット作業指示装置１０６は、ロボットの移動軌道や移動タイミングなどの指示をロボット制御装置１０４に与える装置である。センサ１０３は、作業者１０２の動きをとらえるセンシング装置であり、２次元カメラや３次元カメラ、レーザ距離計を用いることができ、さらに、作業者１０２に取り付けられた加速度センサなどの一般的なセンシングデバイスを用いることができる。

本実施形態の自動組立システム１００は、無人ではなく、有人作業であって、作業者１０２とロボットが協働して組立てを行うことを前提とする。自動組立システム１００は、ロボット作業指示装置１０６からの作業指示に従って、ロボット制御装置１０４の動作指令によりロボット１０１を動作させる。作業スペースには、ワーク１２０が設置され、ロボット１０１と作業者１０２は、ワーク１２０に対して部品の組み立てを行う。また、ロボット制御装置１０４は、センサ１０３で取得した作業者１０２の動作データと、ロボット制御装置１０４から取得したロボットの関節角や角速度、角加速度などの現在のロボット動作状態を表すデータを用いて、衝突危険性を判断するとともに、衝突すると判定した場合には、衝突回避軌道を生成し、その衝突回避に基づいた動作指令により回避動作を指示する。

なお、本実施形態では、ロボットが２軸制御のロボットである場合を例に採り説明するが、制御軸数や装置構成はこれに限られるものではない。

次に、図２を用いてロボット制御装置の機能構成について説明する。
ロボット制御装置１０４は、ロボット制御指令部１０５、衝突回避部１０９、入力部１１８、記憶部１１９、表示部１２１、通信部１２２の各機能部により構成される。また、衝突回避部１０９は、ロボット動作状態入力部１１１、作業者動作状態入力部１１２、位置・速度ベクトル算出部１１３、危険性判定領域生成部１１４、危険性判定部１１５、衝突回避軌道生成部１１６、衝突回避可否判定部１２４，ロボット制御データ変換部１１７、ロボット制御データ出力部１２３のサブコンポーネントより構成される。記憶部１１９は、モデル情報テーブル３００、衝突回避情報テーブル４００、軌道生成方式情報テーブル５００、ロボット制御指令履歴６００を格納している。

ロボット制御指令部１０５は、ロボット１０１に対して動作指令を与える部分である。衝突回避部１０９は、作業者１０２とロボット１０１との衝突を回避するための衝突回避軌道を生成して、ロボット制御指令部１０５に対して出力する部分である。

衝突回避部１０９のロボット動作状態入力部１１１は、ロボット制御指令履歴６００からロボット１０１の関節角や角速度、角加速度などの現在のロボット動作状態を表すデータを取得する部分である。作業者動作状態入力部１１２は、センサ１０３がセンシングした作業者１０２の動きを表す位置や速度、加速度などの作業者の動作状態を表すデータを取得する部分である。位置・速度ベクトル算出部１１３は、ロボット動作状態入力部１１１と作業者動作状態入力部１１２で取得したロボット動作と作業者動作データから、それぞれの代表点の位置と速度ベクトルを抽出する部分である。危険性判定領域生成部１１４は、モデル情報テーブル３００に格納したロボット１０１や作業者１０２のモデルデータと位置・速度ベクトル算出部１１３で算出したロボット１０１と作業者１０２の位置と速度ベクトルを用いて、ロボット１０１と作業者１０２の危険性判定領域に関する情報を生成する部分である。危険性判定部１１５は、危険性判定領域生成部１１４で生成したロボットの危険性判定領域に関する情報と作業者の危険性判定領域の情報から衝突の危険性を判定する部分である。衝突回避軌道生成部１１６は、作業者１０２からロボット１０１を退避させる軌道を生成する部分である。衝突回避可否判定部１２４は，衝突回避軌道生成処理で用いられ，生成された衝突回避軌道で衝突回避可能かを判定する部分である。ロボット制御データ変換部１１７は、衝突回避軌道生成部１１６で生成したロボットの回避軌道をロボットの制御信号に変換する部分である。ロボット制御データ出力部１２３は、ロボット制御データ変換部１１７で生成したロボット制御データを、ロボット制御指令部１０５に出力する部分である。

入力部１１８は、ロボット１０１や作業者１０２のモデルデータや危険性判定条件、回避軌道生成条件など、衝突回避軌道生成に必要な設定情報の入力、メニューの選択指示、あるいはその他の指示等を入力する部分であり、入力された情報は、記憶部１１９の各テーブルに格納される。表示部１２１は、評価対象モデルの表示、入力情報の表示、処理結果の表示、処理途中の経緯の表示等を行う表示部である。通信部１２２は、ロボット１０１、センサ１０３、ロボット作業指示装置１０６間でデータを送受信する部分である。

このロボット制御装置１０４の各機能構成部は、専用のハードウェアロジックで実現してもよいし。パーソナルコンピュータのような一般的な情報処理装置において、各の機能を実現するプログラムを実行させて、実現するようにしてもよい。その場合には、ＣＰＵ（Central Processing Unit：中央処理装置）が、ＨＤＤ（Hard Disk Drive）やＳＳＤ（Solid State Drive）からなる補助記憶装置にインストールされたプログラムやテーブルを、ＲＡＭ（Random Access Memory）からなる主記憶装置に、ロードして参照して実行する形態になる。

また、この場合の入力部１１８は、例えば、キーボード、マウスなどであり、その他、タッチパネル、専用のスイッチやセンサあるいは音声認識装置を用いてもよい。表示部１２１は、例えば、ディスプレイ、プロジェクタ、ヘッドマウントディスプレイなど画面やスクリーンに情報を表示する装置である。さらに、表示部１２１に表示された情報を用紙に出力するプリンタ（図示せず）を、ロボット制御装置１０４に接続してもよい。

また、ロボット制御装置１０４は、ロボット１０１やロボット作業指示装置１０６とハードウェアを共有してもよい。

本実施形態は、１本腕を装着するロボットを想定したアルゴリズムを記述しているが、１本腕のロボットに限定されるものではなく、腕の本数は２本以上、または、ＸＹＺの直動軸を持つ機械など６軸ロボット以上の駆動軸数を有するものであってもよい。

なお、記憶部１１９に格納されている各テーブルの詳細は、後に説明する。記憶部１１９に格納されているロボット制御指令履歴６００は、ロボット制御指令部１０５がロボットに与えた指令の履歴情報である。

次に、図３ないし図５を用いてロボット制御装置で参照されるデータ構造について説明する。
モデル情報テーブル３００は、ロボットに作業指令を与えるための基礎データ（モデル情報）を格納するテーブルであり、図３に示されるように、ロボットモデル情報３０１、作業者モデル情報３０２、作業環境モデル情報３０３の格納欄を有する。ロボットモデル情報３０１は、ロボット構造や付帯物の情報を格納する欄であり、リンク数３０１ａ、リンク寸法３０１ｂ、リンク接続情報３０１ｃ、ロボット代表点座標３０１ｄ、把持物寸法３０１ｅ、把持座標３０１ｆの中項目からなる情報を有する。

リンク数３０１ａは、ロボットアームを構成する部品（リンク）の数である。リンク寸法３０１ｂは、その部品の寸法である。リンク接続情報３０１ｃは、各リンクをつなぐ関節に関する情報である。ロボット代表点座標３０１ｄは、ロボット速度ベクトルを算出するロボットの代表点の座標である。把持物寸法３０１ｅは、ロボット１０１が把持している物体の寸法である。把持座標３０１ｆは、その物体の座標である。

リンク接続情報３０２ｃは、関節を構成するリンク名と、回転軸や直動軸といった関節の形態が格納されている。

作業者モデル情報３０２も、ロボットモデル情報３０１と名称が若干異なるが同様の中項目を有しており、各々の意味は、作業者の身体の動きをロボットに擬似的になぞらえている。なお、本実施形態では、作業者モデルを片腕の肘から指までとして記述しているが、胴体や両腕を含むモデルであってもよい。

作業環境モデル情報３０３は、ワーク位置座標３０３ａ、作業第情報３０３ｂ、ロボット原点位置３０３ｃ、作業者原点位置３０３ｄ、センサ取付け原点位置３０３ｅの中項目からなる情報を有する。

ワーク位置座標３０３ａは、作業台の中の作業位置を示す座標である。作業第情報３０３ｂは、作業台の形状寸法を記述したデータである。ロボット原点位置３０３ｃは、作業台に設置されたロボット１０１の原点位置を示す座標である。作業者原点位置３０３ｄは、作業台の近くで作業する作業者１０２の立ち位置を示す座標である。センサ取付け原点位置３０３ｅは、作業者１０２の動きを検出するセンサ１０３の取付け位置を示す座標である。

衝突回避情報テーブル４００は、ロボット制御装置１０４がロボット１０１と作業者の衝突を回避するためのデータを格納するテーブルであり、図４に示されるように、衝突回避時間４０１、減速条件４０２、危険性判定領域幅４０３、接近距離情報４０４を有する。

衝突回避時間４０１は、衝突回避判定領域を速度ベクトル方向に拡大する幅を設定するために使用される衝突を回避するための時間情報である。減速条件４０２は、ロボットを減速させると判定されたときのロボット動作の条件である。危険性判定領域幅４０３は、ロボット１０１と作業者１０２の相対位置における危険性を判定する領域幅を設定するための情報である。接近距離情報４０４は、ロボット１０１と作業者１０２が近接したときに危険であると判断するための情報である。

衝突回避時間４０１は、停止領域、退避領域、減速領域毎に設定できるようにし、それぞれ停止領域用衝突回避時間４０１ａ、退避領域用衝突回避時間４０１ｂ、減速領域用衝突回避時間４０１ｃの中項目情報を有する。減速条件４０２は、減速時動作速度上限値４０２ａ、対作業者速度比率４０２ｂ、対ロボット指示値速度比率４０２ｃの中項目情報を有する。

減速時動作速度上限値４０２ａは、ロボット１０１が減速するときの速度の上限値の情報である。対作業者速度比率４０２ｂは、作業者速度に対する減速比率である。対ロボット指示値速度比率４０２ｃは、ロボット指示値に対する減速比率である。

危険性判定領域幅４０３は、停止領域、退避領域、減速領域毎に設定できるようにし、それぞれ停止領域用危険性判定領域幅４０３ａ、退避領域用危険性判定領域幅４０３ｂ、減速領域用危険性判定領域幅４０３ｃの中項目情報を有する。

接近距離情報４０４には、最小接近距離４０４ａの中項目情報を有する。最小接近距離４０４ａは、作業者１０２が安全であると判断できる最小の接近距離である。

軌道生成方式情報テーブル５００は、ロボット１０１に対して軌道を生成する情報を格納するテーブルであり、図５に示されるように、方式数５０１、軌道生成方式情報５０２を有する。

方式数５０１は、このテーブルにおいて格納されている方式の数である。軌道生成方式情報５０２は、軌道生成される方式の各種情報である。

軌道生成方式情報５０２には、方式番号５０２ａ、方式名称５０２ｂ、軌道生成アルゴリズムリンク情報５０２ｃ、優先順位５０２ｄ、衝突回避可否判定結果５０２ｅ、衝突回避軌道データ５０２ｆの中項目情報を有する。

方式番号５０２ａは、このテーブルにおける識別番号である。方式名称５０２ｂは、この方式に対する名称である。アルゴリズムリンク情報５０２ｃは、軌道生成アルゴリズムを呼びだす時のパスなどの情報である。優先順位５０２ｄは、適用するアルゴリズムの優先順位を示す情報である。衝突回避可否判定結果５０２ｅは、衝突回避可否判定の結果情報である。衝突回避軌道データ５０２ｆは、衝突回避軌道の点列データである。

次に、図６ないし図１４を用いてロボット制御装置の衝突回避部において衝突回避軌道を生成する処理について説明する。
衝突回避部１０９は、ロボット１０１と作業者１０２のセンサ１０３によるセンシングデータに基づいて衝突の危険性を判定し、衝突の危険性があると判定されたときに、衝突回避軌道を生成し、ロボット制御指令部１０５に出力する。

先ず、衝突回避部１０９のロボット動作状態入力部１１１は、ロボット状態取得処理を行う（Ｓ１１）。ロボット状態取得処理では、ロボット制御指令履歴６００からロボットの各関節の角度を取得する。短時間でロボットの状態をロボット制御にフィードバックするためには、その取得間隔は、数ミリ秒から数十ミリ秒であることが望ましい。また、ロボット関節角の取得方法は、ロボットに取り付けられた加速度センサや、外部に取り付けられた２次元カメラや３次元カメラ、レーザ距離計などの一般的なセンサによって取得してもよい。２次元カメラや３次元カメラ、レーザ距離計などから得られたデータを用い、モデル情報テーブル３００に格納されているロボットモデル情報と比較することにより、ロボットの関節角を算出することができる。

Ｓ１１と並行して、作業者動作状態入力１１２は、センシングデータ取得処理を行う（Ｓ１２）。この処理では、作業者１０２の動作、例えば、腕の動きの状態を取得する。センシングデータ取得処理においても、ロボット状態取得処理と同様に、腕の座標を数ミリ秒から数十ミリ秒間隔で取得し、ロボット制御にフィードバックすることが望ましい。データの取得には、作業者１０２の腕に取り付けられた加速度センサや、外部に取り付けられた２次元カメラや３次元カメラ、レーザ距離計などの一般的なセンサによって取得してもよい。２次元カメラや３次元カメラ、レーザ距離計などから得られたデータを用い、モデル情報テーブル３００に格納されている作業者モデル情報と比較することにより、作業者の腕の角度や位置を算出することができる。

次に、位置・速度ベクトル算出部１１３は、位置・速度ベクトル算出処理を行う（Ｓ１３）。この処理は、Ｓ１１のロボット状態取得処理で取得したロボットの関節角情報とモデル情報テーブル３００に格納されているロボットモデル情報から、ロボット１０１の代表点の位置と速度ベクトルを算出するとともに、センシングデータ取得処理で取得した作業者１０２の関節角情報とモデル情報テーブル３００に格納されている作業者モデル情報から、作業者の代表点の位置と速度ベクトルを算出する。ここで、代表点とは、例えばハンドの先端やハンドが把持している物体の先端など、衝突の危険性が高い点のことである。衝突の危険性が高い点は、ロボット１０１と作業者１０２の相対的位置関係や作業内容により変化する。

次に、危険性判定領域生成部１１４は、危険性判定領域生成処理を行う（Ｓ１４）。この処理では、Ｓ１３の位置・速度ベクトル抽出処理で取得したロボット１０１と作業者１０２の代表点の位置と速度ベクトル、およびモデル情報テーブル３００に格納されているロボットモデル情報と作業者モデル情報を用いて、危険性判定領域を生成する。

ここで、図７を用いて危険性判定領域の設定について説明する。危険性判定領域には、停止領域２０１ａ、ｂと退避領域２０２ａ、ｂ、および減速領域２０３ａ、ｂがあり、少なくともその内の一つの領域を設定する。ロボットの停止領域２０１ｂに作業者１０２が侵入したときにはロボットを停止させ、ロボットの退避領域２０２ｂに作業者１０２が侵入したときにはロボットを退避させ、ロボットの減速領域２０３ｂに作業者１０２が侵入したときにはロボットを減速させるという判定に用いる。

危険性判定領域は、ロボット１０１と作業者１０２の各時刻における位置を基準とし、衝突回避情報テーブル４００に格納した危険性判定領域幅４０３に基づいて、３次元的に設定される。さらに、速度ベクトル２０４ａ、ｂ方向に、各領域を拡大した領域とする。拡大幅は、衝突回避情報テーブル４００に格納した衝突回避時間４０１を用い、衝突回避時間×速度ベクトルの大きさで算出される拡大幅とする。

次に、危険性判定部１１５は、危険性判定処理を行う（Ｓ１５）。

危険性判定処理で停止と判定されたときには、「停止」というステイタスを付与し（Ｓ２１）、ロボット制御指令部１０５に送信され（Ｓ２３）、ロボット１０１は直ちに停止される。危険性判定処理で減速と判定されたときには、「減速」というステイタスを付与し（Ｓ２０）、ロボット制御指令部１０５に送信され（Ｓ２３）、ロボット１０１は動作速度を低下させて作業を続行する。このときの動作軌道は、ロボット作業指示装置１０６が指示した軌道である。速度低下の割合は、衝突回避情報テーブル４００に格納した減速条件４０２を用い、例えば、減速時動作速度上限値４０２ａや作業者１０２の速度ベクトルの大きさに対する対作業者速度比率４０２ｂ、ロボット作業指示装置１０６が指示した速度の大きさに対する対ロボット指示値速度比率４０２ｃなどが用いられる。危険性判定処理で回避なしと判定されたときには、「回避なし」というステイタスを付与し（Ｓ１９）、ロボット制御指令部１０５に送信され（Ｓ２３）、ロボット１０１はロボット作業指示装置１０６が指示した動作を変更することなく実行する。退避と判定されたときには、「退避」というステイタスを付与（Ｓ２２）するとともに、衝突回避軌道生成処理（Ｓ１６）、ロボットデータ変換処理（Ｓ１７）、衝突回避軌道出力処理（Ｓ１８）をおこなって、衝突回避軌道情報をロボット制御指令部１０５に出力する。その後、判定結果「退避」は、ロボット制御指令部１０５に送信され（Ｓ２３）、Ｓ１８で出力した衝突回避軌道にロボット動作を切り替える。

なお、Ｓ１７のロボット制御データ変換処理は、ロボット制御データ変換部１１７で実行される処理である。この処理は、ロボット固有の言語や回避軌道をあらわすロボット関節角の点列等、ロボットを制御する一般的なデータに変換する処理である。

また、Ｓ１８の回避軌道出力処理は、ロボット制御データ出力部１２３で実行される処理である。この処理は、ロボット制御データ変換部１１７で生成したロボット制御データを、ロボット制御指令部１０５に出力する処理である。

次に、図８および図９を用いてＳ１５の危険性判定処理について説明する。
危険性判定処理では、先ず、図９（ａ）に示すように、ロボット１０１と作業者１０２の停止領域２０１ａ、ｂに重なり領域が存在するか否かを判定する（Ｓ３１）。ここで、停止領域に重なり部分が存在すると判定されたときには、「停止」という判定結果を出力する（Ｓ３５）。停止領域に重なり部分が存在しないときは、退避領域２０２ａ、ｂに重なり領域が存在するか否かを判定する（Ｓ３２）。ここで、退避領域に重なり部分が存在すると判定されたときには、「退避」という判定結果を出力する（Ｓ３６）。退避領域に重なり部分が存在しないときは、図９（ｂ）に示すように、減速領域２０３ａ、ｂに重なり領域が存在するか否かを判定する（Ｓ３３）。ここで、減速領域に重なり部分が存在すると判定されたときには、「減速」という判定結果を出力する（Ｓ３７）。減速領域に重なり部分が存在しない場合は、回避の必要がないと判断し、「回避なし」という判定結果を出力する（Ｓ３４）。

以上のように、危険性が高い順番、すなわち、ロボット１０１と作業者１０２が接近する順に危険性を判定して、ロボット１０１に与える指令動作を「停止」、「退避」、「減速」と振り分けることにより、危険レベルを正しく判定した動作指令を与えることができる。危険性判定領域の重なり部分の検出は、一般的な３次元点群データの干渉チェックアルゴリズムを用いることができる。また、モデル情報テーブル３００に格納したロボット１０１と作業者１０２のモデル情報を簡素な形状、例えば、球と円柱の組み合わせで表現すれば、干渉チェック処理を短時間で実行可能である。

次に、図１０を用いてＳ１６の衝突回避軌道生成処理について説明する。
この衝突回避軌道生成処理は、衝突回避軌道生成部１１６で実行される処理である。

先ず、軌道生成方式情報テーブル５００に格納されている複数の軌道回避生成方法の一つについて、衝突回避軌道を生成し、軌道の点列データを軌道生成方式情報テーブル５００に格納する（Ｓ４０）。次に、回避可否判定を実施し、判定結果を軌道生成方式情報テーブル５００に格納する（Ｓ４１）。

次に、軌道生成方式情報テーブル５００に格納されたすべての回避軌道生成方式に対して、回避可否判定を実施したか否かについて判定し（Ｓ４２）、回避可否を判定していない回避軌道生成方式がある場合は、Ｓ４０に戻る。

次に、軌道生成方式情報テーブル５００に格納された回避可否判定結果情報から、「回避可能」と判定された生成方式の有無を判定し（Ｓ４３）、「回避可能」と判定された生成方式がない場合には、ロボット制御指令部１０５に対して停止信号を出力する（Ｓ４６）。「回避可能」と判定された回避軌道生成方式が存在する場合には、図５の軌道生成方式情報テーブル５００の中から、回避可否判定結果が「回避可能」かつ最も優先順位が高い方式を一つ選択し（Ｓ４４）、選択した回避軌道生成方式の方式番号等を出力する（Ｓ４５）。

ロボット制御装置１０４は、出力された衝突回避軌道生成方式番号に関連付けられた衝突回避軌道の点列データを取得し、ロボット１０１の退避動作を指令する。ロボット退避軌道として、例えば、今まで移動してきた経路を逆にたどる軌道や作業者速度ベクトルを基準として、作業者速度ベクトルと平行方向に移動する経路、作業者速度ベクトルとの角度が鋭角である方向に移動する経路など、様々な衝突回避軌道生成アルゴリズムを軌道生成方式情報テーブル５００に格納し、衝突回避軌道生成に利用することができる。

例えば、衝突回避開始時のロボットの速度ベクトルと作業者の速度ベクトルから算出される単位時間後のロボットと作業者間の距離が大きくなるようなロボットの速度ベクトル方向を衝突回避軌道とすることができる。

また、衝突回避開始時のロボットの速度ベクトルと作業者の速度ベクトル間の最小距離が予め設定した値より大きくなるロボットの速度ベクトル方向を衝突回避軌道とすることができる。

次に、図１１および図１２を用いて、Ｓ４１の衝突回避可否判定処理について説明する。
図１２に示されるように、ロボット１０１が衝突回避動作を開始するときのロボット速度ベクトルＶｒ（Ｒｘ、Ｒｙ、Ｒｚ）と作業者１０２の作業者速度ベクトルＶｗ（Ｗｘ、Ｗｙ、Ｗｚ）から、ロボット１０１の現在位置であるロボット速度ベクトルＶｒの始点Ｒ０（Ｘｒ０、Ｙｒ０、Ｚｒ０）と、作業者の現在位置である作業者速度ベクトルＶｗの始点Ｗ０（Ｘｗ０、Ｙｗ０、Ｚｗ０）、および単位時間経過したときのロボット１０１と作業者１０２の位置をそれぞれＲ１、Ｗ１とする。

回避可否判定処理では、先ず、以下の（式１）を用いて、ロボット速度ベクトル始点Ｒ０と作業者速度ベクトル始点Ｗ０間の距離Ｄ０を算出する（Ｓ５１）。次にロボット速度ベクトルＲ１と作業者速度ベクトルＷ１から、単位時間経過したときのロボット位置Ｒ１と作業者位置Ｗ１を（式２）、（式３）を用いて算出する（Ｓ５２）。次に、Ｒ１とＷ１間の距離Ｄ１を、（式４）を用いて算出し（Ｓ５３）、（式５）を用いて、回避可否を判定する（Ｓ５４）。ここで、回避可能と判定されたときは、軌道生成方式情報テーブル５００に「回避可能」と格納し（Ｓ５５）、回避不可能と判定されたときは、「回避不可能」と格納する（Ｓ５６）。

次に、図１３および図１４を用いて、Ｓ４１の衝突回避可否判定処理の別の一例について説明する。
図１４に示されるように、ロボット１０１が回避動作を開始するときのロボット位置をＲ０、ロボット速度ベクトルをＶｒ、作業者１０２の位置をＷ０、作業者速度ベクトルＶｗとする。Ｒ０を通りＶｒに平行な直線Ｌｒ上の点Ｒｔと、Ｗ０を通りＶｗに平行な直線Ｌｗ上の点Ｗｓは、（式６）（式７）であらわされる。ここで、ｓ、ｔは任意の実数である。

先ず、ロボット速度ベクトルＶｒと作業者速度ベクトルＶｗが平行であるか否かを判定する（Ｓ６１）。２直線が平行である場合は、２直線間の距離を最短距離Ｄ３として算出する（Ｓ６２）。２直線が平行ではなく、ねじれの位置にある場合は、Ｒ０を通りＶｒに平行な直線ＬｒとＷ０を通りＶｗに平行な直線Ｌｗの共通垂線Ｌｈを求める（Ｓ６３）。共通垂線の長さは２直線間の最短距離であることから、共通垂線の長さを最短距離Ｄ３として算出する（Ｓ６４）。

次に、衝突回避情報テーブル４００の接近距離情報に格納された最小接近距離Ｃを参照し、最短距離Ｄ３と最小接近距離Ｃの大きさを比較する（Ｓ６５）。最短距離Ｄ３が最小接近距離Ｃより大きい場合は、直線Ｌｒ、Ｌｗ上のどの点をとってもロボットと作業者は衝突しないため、軌道生成方式情報１３３に「回避可能」と格納する（Ｓ６８）。最短距離Ｄ３が最小接近距離Ｃ以下である場合は、直線Ｌｒと共通垂線Ｌｈの交点Ｒ２を示すｔ２を（式６）から求め、直線Ｌｗと共通垂線Ｌｈの交点Ｗ２を示すｓ２を（式７）から求める（Ｓ６６）。

次に、ｔ２とｓ２を判定し（Ｓ６７）、ｔ２≦０かつｓ２≦０である場合は、ロボット１０１と作業者１０２の両者とも、共通垂線との交点Ｒ２、Ｗ２から遠ざかる方向へ移動するため、双方の距離が離れていくことから、軌道生成方式情報１３３に「回避可能」と格納する（Ｓ６８）。一方、ｔ２≦０かつｓ２≦０でない場合（すなわち、ｔ２＞０またはｓ２＞０の場合）は、軌道生成方式情報１３３に「回避不可能」と格納する（Ｓ６９）。ここで、最小接近距離Ｃは、例えば、モデル情報テーブル３００に格納した把持物寸法の最大値とすることで、ロボット１０１と作業者１０２の衝突の有無を判定することができる。

以上説明した本実施形態のロボット制御装置によれば、ロボットと作業者が空間を共有して作業する場合において、作業者の安全を確保しつつ、ロボットを長い時間動作させることが可能となり、設備稼働率を向上させることができる。

１００…自動組立システム
１０１…ロボット
１０２…作業者
１０４…ロボット制御装置
１０６…ロボット作業指示装置
１２０…ワーク
２０１…停止領域
２０２…退避領域
２０３…減速領域

Claims (12)

1. ロボットと作業者が作業空間を共有する自動組立システムのロボット制御装置であって、
   前記ロボットに動作指令を出力するロボット制御指令部と、衝突回避部とを備え、
   前記衝突回避部は、
   前記ロボットの動作状態を入力するロボット動作状態入力部と、
   前記作業者の動作状態をセンシングした結果を入力する作業者動作状態入力部と、
   前記ロボットと前記作業者の動作状態より前記ロボットと前記作業者のそれぞれの位置と速度ベクトルを算出する位置・速度ベクトル算出部と、
   前記ロボットと前記作業者のそれぞれの周辺に設定される危険性判定領域を生成する危険性判定領域生成部と、
   前記ロボットの危険性判定領域と前記作業者の危険性判定領域の重なりから危険性を判定する危険性判定部と、
   前記ロボットと前記作業者の位置と速度ベクトルから衝突回避可否を判定する衝突回避可否判定部と、
   前記危険性判定部で判定した結果から、衝突を回避するロボットの衝突回避軌道を生成する衝突回避軌道生成部と、
   前期衝突回避軌道生成部で生成した軌道をロボット制御するデータに変換するロボット制御データ変換部と，
   前記衝突回避軌道を、前記ロボット制御指令部に出力するロボット制御データ出力部とを有することを特徴とするロボット制御装置。
2. 前記危険性判定領域は、
   各々が重なっているときにロボットを停止させる衝突回避軌道を生成する停止領域、
   各々が重なっているときにロボットを退避させる衝突回避軌道を生成する退避領域、
   各々が重なっているときにロボットを減速させる衝突回避軌道を生成する減速領域のいずれか一つまたはその組合せであることを特徴とする請求項１記載のロボット制御装置。
3. 前記ロボットが今まで移動してきた軌道を逆にたどる軌道を衝突回避軌道とすることを特徴とする請求項１記載のロボット制御装置。
4. 前記作業者の速度ベクトルと平行な方向を衝突回避軌道とすること特徴とする請求項１記載のロボット制御装置。
5. 衝突回避開始時のロボットの速度ベクトルと作業者の速度ベクトルから算出される単位時間後のロボットと作業者間の距離が大きくなるロボットの速度ベクトル方向を衝突回避軌道とすることを特徴とする請求項１記載のロボット制御装置。
6. 衝突回避開始時のロボットの速度ベクトルと作業者の速度ベクトル間の最小距離が予め設定した値より大きくなるロボットの速度ベクトル方向を衝突回避軌道とすることを特徴とする請求項１記載のロボット制御装置。
7. ロボットと作業者が作業空間を共有する自動組立システムであって、
   前記ロボットを制御する動作指令を与えるロボット制御装置と、
   前記ロボット制御装置制御に動作指令を与えるロボット作業指示装置と、
   前記作業者の動作をセンシングするセンサとを備え、
   前記ロボット制御装置にロボットの動作状態を入力し、
   前記ロボット制御装置に前記センサより作業者の動作状態を入力し、
   前記ロボット制御装置は、前記ロボットの動作状態と前記作業者の動作状態より、前記ロボットと前記作業者のそれぞれの位置と速度ベクトルを算出し、
   前記ロボット制御装置は、前記ロボットと前記作業者のそれぞれの周辺に危険性判定領域を生成し、前記ロボットの危険性判定領域と前記作業者の危険性判定領域の重なりから危険性を判定して、その判定の結果から前記ロボットと前記作業者の衝突を回避する衝突回避軌道を生成して、
   前記ロボット制御装置は、生成した衝突回避軌道に基づき、前記ロボットを制御する動作指令を与えることを特徴とする自動組立システム。
8. 前記危険性判定領域は、
   各々が重なっているときにロボットを停止させる衝突回避軌道を生成する停止領域、
   各々が重なっているときにロボットを退避させる衝突回避軌道を生成する退避領域、
   各々が重なっているときにロボットを減速させる衝突回避軌道を生成する減速領域のいずれか一つまたはその組合せであることを特徴とする請求項７記載の自動組立システム。
9. 前記ロボットが今まで移動してきた軌道を逆にたどる軌道を衝突回避軌道とすることを特徴とする請求項７記載の自動組立システム。
10. 前記作業者の速度ベクトルと平行な方向を衝突回避軌道とすること特徴とする請求項７記載の自動組立システム。
11. 衝突回避開始時のロボットの速度ベクトルと作業者の速度ベクトルから算出される単位時間後のロボットと作業者間の距離が大きくなるロボットの速度ベクトル方向を衝突回避軌道とすることを特徴とする請求項７記載の自動組立システム。
12. 衝突回避開始時のロボットの速度ベクトルと作業者の速度ベクトル間の最小距離が予め設定した値より大きくなるロボットの速度ベクトル方向を衝突回避軌道とすることを特徴とする請求項７記載の自動組立システム。

Images