JP4951783B2

JP4951783B2 - ロボット制御装置及びロボット制御方法

Info

Publication number: JP4951783B2
Application number: JP2008022734A
Authority: JP
Inventors: 孝二神谷
Original assignee: Denso Wave Inc
Current assignee: Denso Wave Inc
Priority date: 2008-02-01
Filing date: 2008-02-01
Publication date: 2012-06-13
Anticipated expiration: 2028-02-01
Also published as: JP2009178835A

Description

本発明は、ロボットの動作に何らかの異常が発生した結果、将来的に侵入禁止領域内に位置することが予測された場合に対応を行うロボット制御装置及びロボット制御方法，並びに複数台のロボットが互いに近接した状態で作業する場合の干渉を回避するロボット制御装置及びロボット制御方法に関する。

ロボットの動作領域内に障害物が存在する場合、ロボットがその障害物に接触することを回避するように制御する技術は、従来より提案されている。例えば、特許文献１には、ロボットの動作空間中に定めた特定空間（禁止空間）の位置情報を記憶して、各ロボット要素に取り付けたセンサが出力する位置データに基づきロボットの現在位置を求め、その現在位置が特定空間内であるか否かを判定する技術が開示されている。
また、特許文献２には、２本のアームを備える双腕ロボットにおいて、事前にアーム同士の衝突の可能性を予測し、衝突の可能性がある場合は、衝突を回避するように動作位置を割り付けるようにした方法が開示されている。
特開昭６１−５１２０４号公報特開平１１−２８６８６号公報

また、上記のような衝突回避に関連する制御として、ロボットが障害物に接近し過ぎる事態が予測されると、現在の予定軌道を維持したまま停止させる瞬間停止制御や、予定軌道の維持を考慮せずに最短距離若しくは最短時間で停止させる異常停止制御等がある。これらの停止制御では、ロボットが障害物に接近し過ぎた位置で制御を開始すると衝突が回避できないため（図９（ｂ）参照）、マージンを確保するには侵入を禁止するエリア（領域）を予め大きめに設定する必要がある（図９（ｃ）参照）。
しかしながら、侵入禁止エリアを大きく設定することは一方でロボットの動作範囲を狭めることに繋がるため、作業を効率的に行うのに支障を来たし、ロボットを使用して行う生産のサイクルタイムが伸びてしまうという問題がある。

特許文献１，２に開示されている技術は、ロボットが動作を開始する前に障害物との衝突について事前検討し、予め衝突を回避するように制御するものである。したがって、ロボットが動作を開始した以降に、障害物近傍の侵入禁止エリアに接近するような異常が発生した場合は、瞬間停止制御や異常停止制御を従来と全く同様に行うことになり、状況によっては、ロボットが侵入禁止エリアに侵入するのみならず、障害物に接触してしまう可能性も否定できない。そのため、上記のような事態が発生した場合に障害物との衝突を回避するには、結局侵入禁止エリアを広くとるしかなく、安全性と作業効率とのトレードオフを解決することはできない。

本発明は上記事情に鑑みてなされたものであり、その目的は、ロボットについて瞬間停止制御や異常停止制御を行う場合の障害物に対する衝突回避をより効率的に行うことができる、また、複数台のロボットが互いに近接した状態で作業する場合の干渉回避を効率的に行い得るロボット制御装置及びロボット制御方法を提供することにある。

請求項１記載のロボット制御装置によれば、ロボットが動作を開始すると、先行指令値計算手段が、軌道指令値計算手段により計算された軌道指令値Ｒ（ｎ）に対し更にｍサンプル先行する軌道指令値Ｒ（ｎ＋ｍ）を計算すると、その先行指令値Ｒ（ｎ＋ｍ）を基準として、瞬時停止位置計算手段は、ロボットを軌道上において最短時間で停止させた場合の瞬時停止位置Ｉｓ（ｎ＋ｍ）を計算し、異常停止位置計算手段は、ロボットにおける１つ以上の可動部の動作を全て最短時間で停止させた場合の仮想的な異常停止位置Ｅｓ（ｎ＋ｍ）を計算する。そして、禁止領域侵入回避手段は、瞬時停止位置Ｉｓ（ｎ＋ｍ）が侵入禁止領域内に位置すると判定した場合はロボットをその時点で停止させ、異常停止位置Ｅｓ（ｎ＋ｍ）が侵入禁止領域内に位置すると判定した場合は、ロボットの動作速度を低下させると共に先行指令値Ｒ（ｎ＋ｍ）を修正した修正指令値Ｒｒ（ｎ＋ｍ）を計算する。この場合、距離Ｌ２を式：Ｌ２＝Ｇ×（Ｄ２／Ｄ１）×Ｌ１：により計算し、軌道上の位置Ｒ（ｎ＋ｍ−１）から距離Ｌ２だけ隔たるように修正指令値Ｒｒ（ｎ＋ｍ）を計算する。そして、異常停止位置Ｅｒｓ（ｎ＋ｍ）が侵入禁止エリア内になると、Ｅｓ（ｎ＋ｍ）をＥｒｓ（ｎ＋ｍ）に、Ｒ（ｎ＋ｍ）をＲｒ（ｎ＋ｍ）にそれぞれ置き換えてから、異常停止位置Ｅｒｓ（ｎ＋ｍ）が侵入禁止領域外となるまで上記の処理を繰り返し実行する。

すなわち、瞬時停止制御並びに異常停止制御は何れも先行指令値Ｒ（ｎ＋ｍ）を基準として行われるので、先行指令値Ｒ（ｎ＋ｍ）に基づく瞬時停止位置Ｉｓ（ｎ＋ｍ）や異常停止位置Ｅｓ（ｎ＋ｍ）が侵入禁止領域内に位置すると判定した時点でそれぞれの停止制御を行っても、実際に行われる瞬時停止制御や異常停止制御は現在の軌道指令値に基づいて行われるので、ロボットが侵入禁止領域に到達することは回避される。
したがって、ロボットの動作特性や予定動作内容に合わせて、各ロボット単位で先行指令値Ｒ（ｎ＋ｍ）を、軌道指令値Ｒ（ｎ）に対して侵入禁止領域の近傍で異常が発生しても瞬時停止位置Ｉｓ（ｎ＋ｍ）や異常停止位置Ｅｓ（ｎ＋ｍ）が侵入禁止領域内に侵入しないように先行して設定することで安全対応を行うことができ、侵入禁止領域自体はロボットに関係なく共通して一律に設定できるので、各ロボットに、それぞれの動作特性や予定動作内容に合わせた効率的な作業をさせることが可能となる。

請求項２記載のロボット制御装置によれば、請求項１の技術を、複数台のロボット間の制御に適用したものとなる。すなわち、この場合の禁止領域侵入回避手段は、各ロボットが動作を開始した以降において、各ロボットの何れか１つの先行指令値Ｒ（ｎ＋ｍ）又は瞬時停止位置Ｉｓ（ｎ＋ｍ）と、その他のロボットの瞬時停止位置Ｉｓ（ｎ＋ｍ）とが２つ以上のロボットの間で干渉を生じると判定した場合は、その干渉を生じる可能性があるロボットの１つ以上をその時点で停止させ、各ロボットの何れか１つの先行指令値Ｒ（ｎ＋ｍ）又は異常停止位置Ｅｓ（ｎ＋ｍ）と、その他のロボットの異常停止位置Ｅｓ（ｎ＋ｍ）とが２つ以上のロボットの間で干渉を生じると判定した場合は、その干渉を生じる可能性があるロボットの１つ以上について動作速度を低下させると共に先行指令値Ｒ（ｎ＋ｍ）を修正する。尚、ここで「干渉」とは、２つ以上のロボットがそれぞれ所定の位置に向かって動作する軌道上において衝突が発生することを示す。

すなわち、この場合も、瞬時停止制御並びに異常停止制御は何れも先行指令値Ｒ（ｎ＋ｍ）を基準として行われるので、１つのロボットの先行指令値や瞬時停止位置Ｉｓ（ｎ＋ｍ），異常停止位置Ｅｓ（ｎ＋ｍ）が他のロボットに干渉すると判定した時点でそれぞれの停止制御を行っても、実際にはロボットが干渉することは回避される。したがって、先行指令値Ｒ（ｎ＋ｍ）を、軌道指令値Ｒ（ｎ）に対してどの程度先行させた時点で求めるかによりマージンを適切に設定できるので、干渉が発生すると判定するための領域を広めに設定する必要がなく、各ロボットに効率的な作業をさせることが可能となる。

（第１実施例）
以下、本発明の第１実施例について図１乃至図６を参照して説明する。図５および図６は、ロボット制御装置を適用したロボットシステム１０を示す。図５は、ロボットシステム１０の構成を示すブロック図，図６は、ロボットシステム１０の電気的構成を示す機能ブロック図である。ロボットシステム１０は、ロボット１１および制御装置（軌道指令値計算手段，先行指令値計算手段，瞬時停止位置計算手段，異常停止位置計算手段，禁止領域侵入回避手段）１２を備えている。ロボット１１は、例えば部品の組み立て用あるいは部品の検査用など、任意の構成のロボットである。制御装置１２は、周辺機器として操作ペンダントを構成するティーチィングペンダント１３およびプログラム入力用のパソコン１４などが接続されている。

ロボット１１は、例えば六軸の垂直多関節型のロボットとして構成されている。ロボット１１は、周知の通り、それぞれアクチュエータであるサーボモータ１５などからの駆動力で駆動されるアーム１６を有している。アーム１６は、先端にエンドエフェクタ１７を有している。例えばロボット１１で部品の運搬や組み立てなどを行う場合、エンドエフェクタ１７としてこれらの部品を保持するためのハンドが用いられる。また、例えばロボット１１で部品の検査などを行う場合、エンドエフェクタ１７として対象となる部品を撮影するカメラなどが用いられる。このように、エンドエフェクタ１７は、ロボット１１を適用する工程に応じて任意に選択することができる。サーボモータ１５からアーム１６のエンドエフェクタ１７までの間には、図示しない減速機構やリンクなどの駆動力伝達機構が設けられている。これにより、アーム１６の先端に設けられているエンドエフェクタ１７は、サーボモータ１５からの駆動力によって駆動される。ロボット１１と制御装置１２との間は、接続ケーブル１８によって接続されている。これにより、ロボット１１の各軸を駆動するサーボモータ１５、および作業を実施するエンドエフェクタ１７は、制御装置１２によって制御される。

ティーチィングペンダント１３は、例えばユーザが携帯あるいは手に所持して操作可能な程度の大きさで、例えば薄型の略矩形箱状に形成されている。ティーチィングペンダント１３は、表面部の中央部に例えば液晶ディスプレイからなる表示部２１を有している。表示部２１には、各種の画面が表示される。表示部２１は、タッチパネルで構成されている。また、ティーチィングペンダント１３は、表示部２１の周囲に各種のキースイッチ２２が設けられており、ユーザは、キースイッチ２２やタッチパネルによって制御装置１２へ種々の指示を入力する。

ティーチィングペンダント１３は、ケーブル２５を経由して制御装置１２に接続され、インターフェイスを経由して制御装置１２との間で高速のデータ転送を実行するようになっており、キースイッチ２２等より入力された操作信号等はティーチィングペンダント１３から制御装置１２へ送信される。また、制御装置１２は、ティーチィングペンダント１３へ制御信号や表示用の信号などと共に駆動用の電力を供給する。

ユーザは、上記のティーチィングペンダント１３を用いてロボット１１の運転や設定などの各種の機能を実行可能であり、例えばキースイッチ２２等を操作することで、予め記憶されている制御プログラムを呼び出して、ロボット１１の起動や各種のパラメータの設定などを実行できる。また、ロボット１１をマニュアル操作で動作させて各種の教示作業も実行可能であり、表示部２１には、例えばメニュー画面、設定入力画面、状況表示画面など必要に応じて所望の画面が表示される。

パソコン１４は例えば汎用のノートパソコンなどであり、ユーザは、プログラミングソフトを実行させることでアプリケーションに応じてロボット１１の動作手順などを記述した動作プログラムを作成できる。この場合、ユーザは、アプリケーションに応じてパッケージ命令を組み合わせたり修正を加えたりすることで、比較的簡単に動作プログラムを作成できる。このパソコン１４は、ケーブル２６を経由して制御装置１２に接続されており、作成されたロボット１１の動作プログラムは、パソコン１４から制御装置１２へ転送される。

制御装置１２は、箱状のフレームの内部に制御部３１が組み込まれている。制御部３１は、ＣＰＵ、ＲＯＭおよびＲＡＭなどからなるマイクロコンピュータを主体として構成され、予め入力・記憶されたロボット１１の動作プログラムや、各種データおよびパラメータ、ティーチィングペンダント１３からの操作信号などに基づいて、サーボ制御部３２を経由してロボット１１の各軸のサーボモータ１５を駆動する。これにより、制御装置１２は、ロボット１１の動作を制御する。

次に、本実施例の作用について図１乃至図４も参照して説明する。図１は、ロボット１１におけるアーム１６先端部の移動軌跡をモデル的に示すものであり、図２は、制御装置１２の主として制御部３１による制御内容を、本発明の要旨に係る部分について示すフローチャートである。

図１（ａ）に示すように、制御装置１２は、所定間隔ごとにロボット１１の指令値Ｒ（ｎ）を、現在位置よりも先立って計算し、その指令値Ｒ（ｎ）に移動軌跡を一致させるように制御する。したがって、ロボット１１の現在位置は、具体的には図示しないが、図１（ａ）では１周期（１サンプル）前の指令値Ｒ（ｎ−１）よりも更に手前（例えばｘサンプル前）となっている。また、ロボット１１の動作領域については、図９と同様に、障害物を回避するための侵入禁止エリア（領域）が予め設定されている。

図２において、制御装置１２は、先ず現指令値Ｒ（ｎ）を計算すると（ステップＳ１：「軌道計算タイミング待ち」，軌道指令値計算手段）、その現指令値Ｒ（ｎ）を基準としてｍサンプル先となる指令値Ｒ（ｎ＋ｍ）を計算する（ステップＳ２，先行指令値計算手段）。続いて、その指令値Ｒ（ｎ＋ｍ）を基準として、ロボット１１の動作を、現在の軌道を外れないことを条件にして最短時間で停止させた場合の瞬時停止位置Ｉｓ（ｎ＋ｍ）を計算する（ステップＳ３，瞬時停止位置計算手段）。この場合、アーム１６のその時点における動作速度Ｖ（ｎ＋ｍ）を考慮する。

次に、制御装置１２は、ステップＳ３で求めた瞬時停止位置Ｉｓ（ｎ＋ｍ）が侵入禁止エリア内に入るか否かを判断し（ステップＳ４）、侵入禁止エリア内に入る場合は（ＹＥＳ）その時点で直ちに瞬時停止を実行する（ステップＳ８）。この時点で、ロボット１１の現在位置は（ｎ−ｘ）であるから、実際の瞬時停止位置Ｉｓ（ｎ−ｘ）は軌道上のより手前側となり、アーム１６が侵入禁止エリア内に入ることは回避される。

一方、ステップＳ４において、瞬時停止位置Ｉｓ（ｎ＋ｍ）が侵入禁止エリア内に入らない場合は（ＮＯ）、指令値Ｒ（ｎ＋ｍ）を基準として、ロボット１１の動作を、現在の軌道を外れても良いことを条件に最短時間で停止させた場合の異常停止位置Ｅｓ（ｎ＋ｍ）を計算する（ステップＳ５，異常停止位置計算手段，図１（ｂ）参照）。この場合、ロボット１１の各可動部を、それぞれ最短時間で停止させることになる。そして、その異常停止位置Ｅｓ（ｎ＋ｍ）が、侵入禁止エリア内に入るか否かを判断し（ステップＳ６）、侵入禁止エリア内に入らなければ（ＮＯ）ステップＳ７に移行する。全ての軌道生成処理が完了していれば（ＹＥＳ）「エンド」となり、完了していなければ（ＮＯ）ステップＳ１に戻り、上記の処理を繰り返す。

一方、ステップＳ６において、異常停止位置Ｅｓ（ｎ＋ｍ）が侵入禁止エリア内に入る場合は（ＹＥＳ）、その位置Ｅｓ（ｎ＋ｍ）が侵入禁止エリア外となるように修正した指令値Ｒｒ（ｎ＋ｍ）を計算する（ステップＳ９，禁止領域侵入回避手段）。またこの時、修正指令値Ｒｒ（ｎ＋ｍ）を計算して軌道を修正する時間を確保するため、ロボット１１の動作速度を若干低下させる。

図３は、ステップＳ９の詳細な処理内容を示すもので、図４は、図３のフローチャートに応じて図１（ｂ）をより詳細に示したものである。制御装置１２は、距離Ｄ１，Ｄ２，Ｌ１を算出する（ステップＳ１１）。ここで、
Ｄ１：Ｅｓ（ｎ＋ｍ），Ｒ（ｎ＋ｍ）間の直線距離
Ｄ２：Ａ，Ｒ（ｎ＋ｍ）間の直線距離
Ｌ１：Ｒ（ｎ＋ｍ），Ｒ（ｎ＋ｍ−１）間の軌道上の距離
Ａ：線分Ｅｓ（ｎ＋ｍ）Ｒ（ｎ＋ｍ）と侵入禁止エリア境界との交点
である。

次に、制御装置１２は、距離Ｌ２を計算する（ステップＳ１２）。距離Ｌ２は、
Ｌ２：Ｒｒ（ｎ＋ｍ），Ｒ（ｎ＋ｍ−１）間の軌道上の距離
であり、
Ｌ２＝Ｇ×（Ｄ２／Ｄ１）×Ｌ１
で計算される。ただしＧは、修正指令値Ｒｒ（ｎ＋ｍ）を決定するための係数である。

それから、制御装置１２は、ロボット１１の軌道上の位置Ｒ（ｎ＋ｍ−１）から距離Ｌ２だけ隔たるようにして、修正指令値Ｒｒ（ｎ＋ｍ）を計算すると（ステップＳ１３）、その修正指令値Ｒｒ（ｎ＋ｍ）を基準として異常停止位置Ｅｒｓ（ｎ＋ｍ）を計算する（ステップＳ１４）。そして、異常停止位置Ｅｒｓ（ｎ＋ｍ）が侵入禁止エリア外にあれば（ステップＳ１５：ＹＥＳ）処理を終了し、侵入禁止エリア内となった場合は（ＮＯ）
Ｅｒｓ（ｎ＋ｍ）→Ｅｓ（ｎ＋ｍ）
Ｒｒ（ｎ＋ｍ）→Ｒ（ｎ＋ｍ）
に置き換えてから（ステップＳ１６）ステップＳ１１に戻り、ステップＳ１５で「ＹＥＳ」と判断するまで上記の処理を繰り返し実行する。

以上のように本実施例によれば、ロボット１１が動作を開始した以降において、制御装置１２は、軌道指令値Ｒ（ｎ）に対し更に先行する軌道指令値Ｒ（ｎ＋ｍ）を計算すると、その先行指令値Ｒ（ｎ＋ｍ）を基準として、ロボット１１を軌道上において最短時間で停止させた場合の瞬時停止位置Ｉｓ（ｎ＋ｍ）を計算し、また、ロボット１１における各可動部の動作を全て最短時間で停止させた場合の仮想的な異常停止位置Ｅｓ（ｎ＋ｍ）を計算する。そして、瞬時停止位置Ｉｓ（ｎ＋ｍ）が侵入禁止エリア内に位置すると判定した場合はロボット１１をその時点で停止させ、異常停止位置Ｅｓ（ｎ＋ｍ）が侵入禁止エリアに位置すると判定した場合は、ロボット１１の動作速度を低下させると共に軌道指令値を修正したＲｒ（ｎ＋ｍ）を求めるようにした。

すなわち、瞬時停止制御並びに異常停止制御は何れも先行指令値Ｒ（ｎ＋ｍ）を基準として行われるので、先行指令値に基づく瞬時停止位置Ｉｓ（ｎ＋ｍ）や異常停止位置Ｅｓ（ｎ＋ｍ）が侵入禁止エリア内に位置すると判定した時点でそれぞれの停止制御を行っても、実際に行われる瞬時停止制御や異常停止制御は現在の軌道指令値Ｒ（ｎ）に基づいて行われるので、ロボットが侵入禁止エリアに到達することは回避される。
したがって、ロボットの動作特性や予定動作内容に合わせて、各ロボット単位で先行指令値Ｒ（ｎ＋ｍ）を、軌道指令値Ｒ（ｎ）に対して侵入禁止エリアの近傍で異常が発生しても瞬時停止位置Ｉｓ（ｎ＋ｍ）や異常停止位置Ｅｓ（ｎ＋ｍ）が侵入禁止エリア内に侵入しないように先行して設定することで安全対応を行うことができ、侵入禁止エリア自体はロボットに関係なく共通して一律に設定できるので、各ロボットに、それぞれの動作特性や予定動作内容に合わせた効率的な作業をさせることが可能となる。

（第２実施例）
図７及び図８は本発明の第２実施例を示すものであり、第１実施例と同一部分には同一符号を付して説明を省略し、以下異なる部分について説明する。第２実施例は、第１実施例で１台のロボット１１が動作する場合に障害物を回避するための制御内容を、複数台のロボットが互いに近接した状態で動作し、両者のアーム１６が干渉する可能性がある場合に、その干渉を回避する制御に適用する。

図７は、全体のシステム構成図であり、２台のロボット１１（１），１１（２）の構成は、第１実施例と同様であり、制御装置（禁止領域侵入回避手段）４１（１），４１（２）による制御内容が相違している。また、制御装置４１（１），４１（２）の間は例えばＬＡＮ（Local Area Network）ケーブルなどで接続されており、両者は相互に通信可能となっている。尚、パソコン１４については、図示を省略している。

図８は図２相当図であり、制御装置４１（１），４１（２）による制御内容をそれぞれ（ａ）,（ｂ）に示している。また、図８（ｂ）は、ロボット１１が３台以上存在する場合も含め一般化して示しているが、図７の場合は「ｋ＝２」となる。そして、図８に示す処理は、基本的には図２に示すステップＳ１〜Ｓ９と同様であり、対応するそれぞれのロボット１１（１），１１（ｋ）について同様の処理を行うが（夫々のステップ番号にＡ，Ｂを付している）、ステップＳ４’，Ｓ６’の処理内容が異なっている。

すなわち、ステップＳ３Ａ，Ｓ３Ｂを実行すると制御装置４１（１），４１（２）は通信処理を行い（Ｃ１）、互いの先行指令値Ｒ１（ｎ＋ｍ），Ｒ２（ｎ＋ｍ）と瞬時停止位置Ｉｓ１（ｎ＋ｍ），Ｉｓ２（ｎ＋ｍ）を取得する。そして、ステップＳ４’ Ａでは、自身が計算した瞬時停止位置Ｉｓ１（ｎ＋ｍ）が、自身の侵入禁止エリア内に入るか否かと共に、他方のロボット１１（２）の動作と干渉するか否かを判断する。ここでの「干渉」とは、一方の瞬時停止位置Ｉｓ１（ｎ＋ｍ）が、他方の先行指令値Ｒ２（ｎ＋ｍ）または瞬時停止位置Ｉｓ２（ｎ＋ｍ）の軌跡と交差するか否かであり、ステップＳ４’Ｂでは、一方の瞬時停止位置Ｉｓ２（ｎ＋ｍ）が、他方の先行指令値Ｒ１（ｎ＋ｍ）または瞬時停止位置Ｉｓ１（ｎ＋ｍ）の軌跡と交差するか否かとなる。この場合、一方のロボット１１が、他方のロボット１１の動作に応じて動的に変化する侵入禁止エリアに入ることに等しい。

そして、ステップＳ６’Ａ，Ｓ６’Ｂも同様であり、ステップＳ５Ａ，Ｓ５Ｂを実行すると制御装置４１（１），４１（２）は通信処理を行い（Ｃ２）、互いの異常停止位置Ｅｓ１（ｎ＋ｍ），Ｅｓ２（ｎ＋ｍ）を取得する。そして、ステップＳ６’ Ａでは、自身が計算した異常停止位置Ｅｓ１（ｎ＋ｍ）が、自身の侵入禁止エリア内に入るか否かと共に、他方のロボット１１（２）の動作と干渉するか否かを判断する。ここでの「干渉」は、の異常停止位置Ｅｓ１（ｎ＋ｍ）が、他方の先行指令値Ｒ２（ｎ＋ｍ）または異常停止位置Ｅｓ２（ｎ＋ｍ）の軌跡と交差するか否かであり、ステップＳ６’Ｂでは、異常停止位置Ｅｓ２（ｎ＋ｍ）が、他方の先行指令値Ｒ１（ｎ＋ｍ）または異常停止位置Ｅｓ１（ｎ＋ｍ）の軌跡と交差するか否かとなる。

以上のように第２実施例によれば、制御装置４１（１），４１（２）は、各ロボット１１（１），１１（２）について、一方の先行指令値Ｒ（ｎ＋ｍ）又は瞬時停止位置Ｉｓ（ｎ＋ｍ）と、他方の瞬時停止位置Ｉｓ（ｎ＋ｍ）と干渉を生じると判定した場合は、その干渉を生じる可能性があるロボット１１をその時点で停止させ、一方の先行指令値Ｒ（ｎ＋ｍ）又は異常停止位置Ｅｓ（ｎ＋ｍ）と、他方の異常停止位置Ｅｓ（ｎ＋ｍ）とが干渉を生じると判定した場合は、その干渉を生じる可能性があるロボット１１の動作速度を低下させると共に軌道指令値Ｒ（ｎ＋ｍ）を修正する。

すなわち、この場合も、第１実施例と同様に、１つのロボット１１の先行指令値や瞬時停止位置，異常停止位置が他のロボット１１に干渉すると判定した時点でそれぞれの停止制御を行っても、実際にはロボット１１が干渉することは回避される。したがって、干渉が発生すると判定するための領域を広めに設定する必要がなく、各ロボット１１（１），１１（２）に効率的な作業をさせることが可能となる。

本発明は上記し、または図面に記載した実施例にのみ限定されるものではなく、以下のような変形または拡張が可能である。
第２実施例を、３台以上のロボット１１（ｋ≧３）が同時に動作する場合について適用しても良い。その場合、それらの何れか２台以上の間について干渉が発生すると判定した場合に、同様に制御すれば良い。
ロボットは、６軸に限ることはない。また、垂直多関節型に限ることもなく、水平多関節型や直角座標型、円筒座標型のロボットなどに適用しても良い。

本発明の第１実施例であり、ロボットアーム先端部の移動軌跡をモデル的に示す図制御装置の制御内容を、本発明の要旨に係る部分について示すフローチャートステップＳ９の詳細な処理内容を示すフローチャート図１（ｂ）をより詳細に示す図ロボットシステムの構成を示すブロック図ロボットシステムの電気的構成を示す機能ブロック図本発明の第２実施例を示す図５相当図図２相当図従来技術を説明する図

符号の説明

図面中、１０はロボットシステム、１１はロボット、１２は制御装置（軌道指令値計算手段，先行指令値計算手段，瞬時停止位置計算手段，異常停止位置計算手段，禁止領域侵入回避手段）、１６はアーム、４１は制御装置を示す。

Claims

ロボットの軌道指令値Ｒ（ｎ）を一定周期毎に順次計算する軌道指令値計算手段と、
この軌道指令値計算手段により計算された軌道指令値Ｒ（ｎ）に対して、更にｍサンプル先行する軌道指令値（先行指令値）Ｒ（ｎ＋ｍ）を計算する先行指令値計算手段と、
前記先行指令値Ｒ（ｎ＋ｍ）を基準として、前記ロボットを軌道上において最短時間で停止させた場合の瞬時停止位置Ｉｓ（ｎ＋ｍ）を計算する瞬時停止位置計算手段と、
前記先行指令値Ｒ（ｎ＋ｍ）を基準として、前記ロボットにおける１つ以上の可動部の動作を全て最短時間で停止させた場合の仮想的な異常停止位置Ｅｓ（ｎ＋ｍ）を計算する異常停止位置計算手段と、
前記瞬時停止位置Ｉｓ（ｎ＋ｍ）が予め定められている前記ロボットの侵入禁止領域内に位置すると判定した場合は前記ロボットをその時点で停止させ、前記異常停止位置Ｅｓ（ｎ＋ｍ）が前記侵入禁止領域内に位置すると判定した場合は、前記ロボットの動作速度を低下させると共に前記先行指令値Ｒ（ｎ＋ｍ）を修正した修正指令値Ｒｒ（ｎ＋ｍ）を計算する禁止領域侵入回避手段とを備え、
前記禁止領域侵入回避手段は、
Ｄ１：Ｅｓ（ｎ＋ｍ），Ｒ（ｎ＋ｍ）間の直線距離
Ｄ２：Ａ，Ｒ（ｎ＋ｍ）間の直線距離
Ｌ１：Ｒ（ｎ＋ｍ），Ｒ（ｎ＋ｍ−１）間の軌道上の距離
Ｌ２：Ｒｒ（ｎ＋ｍ），Ｒ（ｎ＋ｍ−１）間の軌道上の距離
Ａ：線分Ｅｓ（ｎ＋ｍ）Ｒ（ｎ＋ｍ）と侵入禁止領域境界との交点
Ｇ：修正指令値Ｒｒ（ｎ＋ｍ）を決定するための係数
とすると、距離Ｌ２を次式により計算し、
Ｌ２＝Ｇ×（Ｄ２／Ｄ１）×Ｌ１
軌道上の位置Ｒ（ｎ＋ｍ−１）から距離Ｌ２だけ隔たるように修正指令値Ｒｒ（ｎ＋ｍ）を計算し、前記異常停止位置Ｅｒｓ（ｎ＋ｍ）が侵入禁止領域内になると、Ｅｒｓ（ｎ＋ｍ）をＥｓ（ｎ＋ｍ）に、Ｒｒ（ｎ＋ｍ）をＲ（ｎ＋ｍ）にそれぞれ置き換えてから、前記異常停止位置Ｅｒｓ（ｎ＋ｍ）が侵入禁止領域外となるまで上記の処理を繰り返し実行することを特徴とするロボット制御装置。
互いの動作領域の一部が重複するように配置されている複数のロボットを制御する制御装置において、
各ロボットの軌道指令値Ｒ（ｎ）を一定周期毎に順次計算する軌道指令値計算手段と、
この軌道指令値計算手段により計算された軌道指令値Ｒ（ｎ）に対して、更にｍサンプル先行する軌道指令値（先行指令値）Ｒ（ｎ＋ｍ）を計算する先行指令値計算手段と、
前記先行指令値Ｒ（ｎ＋ｍ）を基準として、前記各ロボットを軌道上において最短時間で停止させた場合の瞬時停止位置Ｉｓ（ｎ＋ｍ）を計算する瞬時停止位置計算手段と、
前記先行指令値Ｒ（ｎ＋ｍ）を基準として、前記各ロボットにおける１つ以上の可動部の動作を全て最短時間で停止させた場合の仮想的な異常停止位置Ｅｓ（ｎ＋ｍ）を計算する異常停止位置計算手段と、
前記各ロボットの何れか１つの先行指令値Ｒ（ｎ＋ｍ）又は瞬時停止位置Ｉｓ（ｎ＋ｍ）と、その他のロボットの瞬時停止位置Ｉｓ（ｎ＋ｍ）とが２つ以上のロボットの間で干渉を生じると判定した場合は、前記干渉を生じる可能性があるロボットの１つ以上をその時点で停止させ、
前記各ロボットの何れか１つの先行指令値Ｒ（ｎ＋ｍ）又は異常停止位置Ｅｓ（ｎ＋ｍ）と、その他のロボットの異常停止位置Ｅｓ（ｎ＋ｍ）とが２つ以上のロボットの間で干渉を生じると判定した場合は、前記干渉を生じる可能性があるロボットの１つ以上について動作速度を低下させると共に前記先行指令値Ｒ（ｎ＋ｍ）を修正した修正指令値Ｒｒ（ｎ＋ｍ）を計算する禁止領域侵入回避手段とを備え、
前記禁止領域侵入回避手段は、
Ｄ１：Ｅｓ（ｎ＋ｍ），Ｒ（ｎ＋ｍ）間の直線距離
Ｄ２：Ａ，Ｒ（ｎ＋ｍ）間の直線距離
Ｌ１：Ｒ（ｎ＋ｍ），Ｒ（ｎ＋ｍ−１）間の軌道上の距離
Ｌ２：Ｒｒ（ｎ＋ｍ），Ｒ（ｎ＋ｍ−１）間の軌道上の距離
Ａ：線分Ｅｓ（ｎ＋ｍ）Ｒ（ｎ＋ｍ）と侵入禁止領域境界との交点
Ｇ：修正指令値Ｒｒ（ｎ＋ｍ）を決定するための係数
とすると、距離Ｌ２を次式により計算し、
Ｌ２＝Ｇ×（Ｄ２／Ｄ１）×Ｌ１
軌道上の位置Ｒ（ｎ＋ｍ−１）から距離Ｌ２だけ隔たるように修正指令値Ｒｒ（ｎ＋ｍ）を計算し、前記異常停止位置Ｅｒｓ（ｎ＋ｍ）が侵入禁止領域内になると、Ｅｒｓ（ｎ＋ｍ）をＥｓ（ｎ＋ｍ）に、Ｒｒ（ｎ＋ｍ）をＲ（ｎ＋ｍ）にそれぞれ置き換えてから、前記異常停止位置Ｅｒｓ（ｎ＋ｍ）が侵入禁止領域外となるまで上記の処理を繰り返し実行することを特徴とするロボット制御装置。
ロボットの軌道指令値を一定周期毎に順次計算するステップと、
前記軌道指令値Ｒ（ｎ）に対して、更にｍサンプル先行する軌道指令値（先行指令値）Ｒ（ｎ＋ｍ）を計算するステップと、
前記先行指令値Ｒ（ｎ＋ｍ）を基準として、前記ロボットを軌道上において最短時間で停止させた場合の瞬時停止位置Ｉｓ（ｎ＋ｍ）と、前記ロボットにおける１つ以上の可動部の動作を全て最短時間で停止させた場合の仮想的な異常停止位置Ｅｓ（ｎ＋ｍ）とを計算するステップと、
前記瞬時停止位置Ｉｓ（ｎ＋ｍ）が予め定められている前記ロボットの侵入禁止領域内に位置すると判定した場合は前記ロボットをその時点で停止させ、前記異常停止位置Ｅｓ（ｎ＋ｍ）が前記侵入禁止領域内に位置すると判定した場合は、前記ロボットの動作速度を低下させると共に前記先行指令値Ｒ（ｎ＋ｍ）を修正した修正指令値Ｒｒ（ｎ＋ｍ）を計算するステップとからなり、
Ｄ１：Ｅｓ（ｎ＋ｍ），Ｒ（ｎ＋ｍ）間の直線距離
Ｄ２：Ａ，Ｒ（ｎ＋ｍ）間の直線距離
Ｌ１：Ｒ（ｎ＋ｍ），Ｒ（ｎ＋ｍ−１）間の軌道上の距離
Ｌ２：Ｒｒ（ｎ＋ｍ），Ｒ（ｎ＋ｍ−１）間の軌道上の距離
Ａ：線分Ｅｓ（ｎ＋ｍ）Ｒ（ｎ＋ｍ）と侵入禁止領域境界との交点
Ｇ：修正指令値Ｒｒ（ｎ＋ｍ）を決定するための係数
とすると、距離Ｌ２を次式により計算し、
Ｌ２＝Ｇ×（Ｄ２／Ｄ１）×Ｌ１
軌道上の位置Ｒ（ｎ＋ｍ−１）から距離Ｌ２だけ隔たるように修正指令値Ｒｒ（ｎ＋ｍ）を計算し、
更に、前記異常停止位置Ｅｒｓ（ｎ＋ｍ）が侵入禁止領域内になると、Ｅｒｓ（ｎ＋ｍ）をＥｓ（ｎ＋ｍ）に、Ｒｒ（ｎ＋ｍ）をＲ（ｎ＋ｍ）にそれぞれ置き換えてから、前記異常停止位置Ｅｒｓ（ｎ＋ｍ）が侵入禁止領域外となるまで上記の処理を繰り返し実行するステップとからなることを特徴とするロボット制御方法。
互いの動作領域の一部が重複するように配置されている複数のロボットを制御する方法において、
各ロボットの軌道指令値Ｒ（ｎ）を一定周期毎に順次計算するステップと、
この軌道指令値計算手段により計算された軌道指令値Ｒ（ｎ）に対して、更にｍサンプル先行する軌道指令値（先行指令値）Ｒ（ｎ＋ｍ）を計算するステップと、
前記先行指令値Ｒ（ｎ＋ｍ）を基準として、前記各ロボットを軌道上において最短時間で停止させた場合の瞬時停止位置Ｉｓ（ｎ＋ｍ）と、前記各ロボットにおける１つ以上の可動部の動作を全て最短時間で停止させた場合の仮想的な異常停止位置Ｅｓ（ｎ＋ｍ）を計算するステップと、
前記各ロボットの何れか１つの先行指令値Ｒ（ｎ＋ｍ）又は瞬時停止位置Ｉｓ（ｎ＋ｍ）と、その他のロボットの瞬時停止位置Ｉｓ（ｎ＋ｍ）とが２つ以上のロボットの間で干渉を生じると判定した場合は、前記干渉を生じる可能性があるロボットの１つ以上をその時点で停止させ、前記各ロボットの何れか１つの先行指令値Ｒ（ｎ＋ｍ）又は異常停止位置Ｅｓ（ｎ＋ｍ）と、その他のロボットの異常停止位置Ｅｓ（ｎ＋ｍ）とが２つ以上のロボットの間で干渉を生じると判定した場合は、前記干渉を生じる可能性があるロボットの１つ以上について動作速度を低下させると共に前記先行指令値Ｒ（ｎ＋ｍ）を修正した修正指令値Ｒｒ（ｎ＋ｍ）を計算するステップとからなり、
Ｄ１：Ｅｓ（ｎ＋ｍ），Ｒ（ｎ＋ｍ）間の直線距離
Ｄ２：Ａ，Ｒ（ｎ＋ｍ）間の直線距離
Ｌ１：Ｒ（ｎ＋ｍ），Ｒ（ｎ＋ｍ−１）間の軌道上の距離
Ｌ２：Ｒｒ（ｎ＋ｍ），Ｒ（ｎ＋ｍ−１）間の軌道上の距離
Ａ：線分Ｅｓ（ｎ＋ｍ）Ｒ（ｎ＋ｍ）と侵入禁止領域境界との交点
Ｇ：修正指令値Ｒｒ（ｎ＋ｍ）を決定するための係数
とすると、距離Ｌ２を次式により計算し、
Ｌ２＝Ｇ×（Ｄ２／Ｄ１）×Ｌ１
軌道上の位置Ｒ（ｎ＋ｍ−１）から距離Ｌ２だけ隔たるように修正指令値Ｒｒ（ｎ＋ｍ）を計算し、
更に、前記異常停止位置Ｅｒｓ（ｎ＋ｍ）が侵入禁止領域内になると、Ｅｒｓ（ｎ＋ｍ）をＥｓ（ｎ＋ｍ）に、Ｒｒ（ｎ＋ｍ）をＲ（ｎ＋ｍ）にそれぞれ置き換えてから、前記異常停止位置Ｅｒｓ（ｎ＋ｍ）が侵入禁止エリア外となるまで上記の処理を繰り返し実行するステップとからなることを特徴とするロボット制御方法。