JP3666928B2 - ロボット制御方法及びその装置 - Google Patents

Description

【０００１】
【産業上の利用分野】
本発明はＮＣロボットの制御方法に関するものである。
【０００２】
【従来の技術】
従来のＮＣロボットの制御方法における、復帰，ティーチングは次のように行っていた。
【０００３】
まず復帰動作において、図１のロボット構成図及び図２の制御部の内部構造に示すような構造を持つＸ，Ｙ，Ｚの直交型３軸ＮＣロボットにおいて図３（挿入時の座標）に示すように、空間（Ｘ，Ｙ，Ｚ）で、任意の位置Ｓ点から出発してＡ点へ到着した時点から考えてみる。
【０００４】
ここで、図６〜図８はワーク挿入の様子を示す図であり、直線補間移動におけるＹ−Ｚ平面の断面構造図として示した図である。すなわち、図６は、挿入の始点にいる状態を示し、図７は挿入の途中で、非常停止により停止した状態を示しており、図８は、図６，図７のＸ−ＹとＹ−Ｚの平面座標図である。
【０００５】
次に、Ａ点とＢ点間の移動は直線補間を行い、図６〜図８に示す様にワークを斜めに挿入するものとする。
【０００６】
ここで、直線補間の始点Ａの座標を（ＸA ，ＹA ，ＺA ）とし、終点Ｂの座標を（ＸB ，ＹB ，ＺB ）とする。
【０００７】
いま、現象としてワークの挿入中に何らかの原因でロボットの制御部に対し非常停止がかかった場合、図７に示すように、ワークがフィンガーに把握されたまま挿入の途中の状態で停止してしまうことがある。
【０００８】
従来、このような状態から復帰しようとして制御部をリセットすると、ロボットの各軸は原点出しを行なおうとして、各々単一の軸が動作をはじめる。このような事が起ると、フィンガーに無理な力が加わって、機械的損傷に至ることは容易に想像できる。
【０００９】
従って、従来その復帰作業は人手によりワークの把握を解除し、ロボットの操作は周辺機器との機械的接触の危険性と排除した後に行っていた。
【００１０】
一方ティーチングにおいて、直線補間が図６の状態にある時のＡ点，Ｂ点のティーチングは従来、次の例の様に行っていた。
【００１１】
すなわち、Ｂ点はワーク単体をＢ点まで人手で挿入し、これをロボットのフィンガーで把握した点をティーチングしていた。
【００１２】
Ａ点は図８に示すように、ワークの機械図面からＸ−Ｙ平面を読み取り、Ｂ点に対するＡ点の関係を読み取れば、ＸA ＝ＸB ＋ｘAB，ＹA ＝ＹB ＋ｙABとなる。
【００１３】
Ｚ座標については、図６に示すように、Ａ点とＢ点の差であるベクトルＶをワークの機械図面から読み取り、ベクトルＶの大きさをγとし、ベクトルＶとＹ座標との角度をθとすれば、ＺAB＝γsin θとなり、ＺA ＝ＺB −ｚABとなる（図８参照）。
【００１４】
また、図９に示す様にワークがフィンガーとともに被挿入部に入り込んでいくこともある。この場合、ワークに取り付けられたフィンガーの挿入終了点（以下、Ｂ1 点）のティーチングは前記の方法では困難であり、Ｘ，Ｙ，Ｚの各軸を微妙にインチングしながらＢ１点へ到達していた。
【００１５】
また、ワークに取り付けられたフィンガーの挿入開始点（以下、Ａ1 点）のティーチングは前記Ａ点と同様にワークの機械図面とＢ1 点の関係から読み取っていた。
【００１６】
【発明が解決しようとする課題】
発明が解決しようとしている課題は、復帰の場合に関するものである。
【００１７】
つまり、復帰動作において、従来例では復帰作業に人手が介在しているため、人身への安全性低下の問題やロボットを組み込んだシステムの自動化の実現を妨げ、稼働率を下げていた。
【００２０】
【課題を解決するための手段】
本発明は、２つ以上の自由度を持つ機構のロボットに、ワークの軌跡が直線となる移動を行う動作をさせるロボット制御方法であって、前記直線の始点と終点間のベクトルを求め、前記ロボットの動作が途中で停止したとき、前記ワークが挿入中であるか否かを判断し、前記ワークが挿入中ではないと判断した場合、前記ロボットの原点出しの処理を行い、前記ワークが挿入中であると判断した場合、前記ワークを前記ベクトルに従って前記ワークの抜き取りができる方向へ移動した後に前記原点出しの処理を行うことを特徴とする。
また、本発明は、２つ以上の自由度を持つ機構のロボットに、ワークの軌跡が直線となる移動を行う動作をさせるロボット制御装置であって、前記直線の始点と終点間のベクトル算出手段と、前記ロボットの動作が途中で停止したときに、前記ワークが挿入中であるか否かを判断する判断手段と、を備え、前記ワークが挿入中ではないと判断した場合、前記ロボットの原点出しの処理を行い、前記ワークが挿入中であると判断した場合、前記ワークを前記ベクトルに従って前記ワークの抜き取りができる方向へ移動した後に前記原点出しの処理を行うことを特徴とする。
【００２４】
【実施例】
以下添付図面を参照して、本発明を適用した一実施例を用いて説明する。
【００２５】
図１にＸ，Ｙ，Ｚの直交型３軸ＮＣロボットの構成図を示す。図１において、３００は可動部で直交するＸ，Ｙ，Ｚの３軸よりなる、Ｚ軸にはフィンガー２０が備えられている。１００は前記Ｘ，Ｙ，Ｚの３軸のモータの制御を行う制御部である。２００はティーチング時の入力や、位置の表示を行うティーチング・ペンダント（Ｔ．Ｐ．）である。４００は非常停止スイッチや、リセットスイッチや、その他のスイッチ入力及び各種ランプ出力である。
【００２６】
図１の制御部１００は、更に図２のような構成になる。図２の中で、１は全ての制御処理を行う中央処理装置（ＣＰＵ）、２はランダム・アクセス・メモリ（ＲＡＭ）であり、ティーチング等で設定されるパラメータ２−１や、ＣＰＵ１が処理の途中で使用するワーキング・エリア２−２やバッファとして用いる部分を含む。３はリード・オンリ・メモリ（ＲＯＭ）であり、処理プログラム３−１や、固定パラメータ３−２を含む。４は必要に応じて備わっている外部記憶装置であり、フロッピー・ディスク（ＦＤ）や、ハードディスク（ＨＤ）や、ＩＣカード等である。５はＣＰＵ１がモータ９−１，９−２，９−３の回転位置を知るためのエンコーダ１０−１，１０−２，１０−３のエンコーダインタフェース（ＥＣＩ）である。６はモータ・ドライブ（ＭＤ）８−１，８−２，８−３とＣＰＵ１のインタフェースとなるモータ・ドライブ・インタフェース（ＭＤＩ）である。従って、各モータは、ＣＰＵ１の命令により、ＭＤＩ６に所定の値がセットされ、ＭＤＩ６にセットされた値により、Ｘ，Ｙ，Ｚのモータ９−１，９−２，９−３をドライブする。７はティーチング・ペンダント（Ｔ．Ｐ．）２００とＣＰＵ１とのインタフェースであるティーチング・ペンダント・インタフェース（ＴＰＩＦ）であり、ＴＰ２００にＣＰＵ１の命令を伝えたり、ＴＰ２００よりの入力をＣＰＵ１に伝える。１２はスイッチ・ランプ・インタフェース（ＳＷ／ＬＰＩＦ）で、ＣＰＵ１と、スイッチやランプ（ＳＷ／ＬＰ）４００のスイッチ・ランプ・インタフェース（ＳＷ／ＬＰＩＦ）である。
【００２７】
さて、図６の様に、挿入時の始点をＡ、終点をＢとし、それぞれの座標をワークの機械図面から人が読み取り、近似データとして（ＸA ，ＹA ，ＺA ）と、（ＸB ，ＹB ，ＺB ）を制御部へ入力する。
【００２８】
終点Ｂと始点Ａとの座標の差を制御部で予め計算しておき、これをベクトルデータＶとし、この座標を（ＸV ，ＹV ，ＺV ）とする。
【００２９】
まず復帰動作において、前記の条件より、以下復帰時について、本発明を説明する。
【００３０】
いま、図７の様に、挿入の途中で非常停止に陥った状態からの復帰を考える。復帰動作が必要な状態では、次の（１），（２）の処理を行う。
【００３１】
（１）現在位置Ｎの座標を（ＸN ，ＹN ，ＺN ）とし、これとベクトルＶとを加えたデータを目的地ＥとすればＥの座標は（ＸN ＋ＸV ，ＹN ＋ＹV ，ＺN ＋ＺV ）となる。
【００３２】
（２）直線補間により、現在位置Ｎから目的地Ｅへと向かって移動する。ただし、挿入の深度が浅い位置からの移動では、ロボットの残りストロークよりもベクトルＶの方が大きくてオーバーランエラーが発生してしまう場合もあるため、予め指定された軸の原点リミットスイッチがオンした場合はその場でロボットの移動を停止する。
【００３３】
以上の処理を組み込んだ復帰動作制御のフローチャートを図４に示す。
【００３４】
ステップＳ１：通常、処理をスタートすると、前記のように、ワークの機械図面より得た近似データ（ＸA ，ＹA ，ＺA ）と（ＸB ，ＹB ，ＺB ）より、終点と始点との座標の差を、ステップＳ１で計算し、ベクトルデータＶとする（ＸV ，ＹV ，ＺV ）。
【００３５】
ステップＳ２：ステップＳ２では、ＮＣロボットの原点出しを行う。
【００３６】
ステップＳ３：ステップＳ３では、メイン処理を行う。
【００３７】
ステップＳ４：メイン処理の途中で、非常停止の割り込みがあると、割り込みにより、ステップＳ４へ進み、ワークが挿入中であるか否かを判断する。ワークが挿入中でなければ、ステップＳ２へ進み原点出しを行う。
【００３８】
ステップＳ５：ワークが挿入中の状態ならば、ステップＳ５へ進み、前記（１），（２）の処理を行う。すなわち、ステップＳ５は、従来人が介在し、人手で行った部分であり、本発明の場合では、前述のように前記の処理で得られたベクトルデータＶより、図７に示す現在位置Ｎから、目的地Ｅの座標を求め、直線補間により目的地Ｅへ移動する。
【００３９】
この時、オーバーランエラー処理等も考慮する。
【００４０】
以上の処理により、復帰処理が終了したならば、ステップＳ２へ進み、原点出しを行い、ステップＳ３のメイン処理へと移る。
【００４１】
以上、ステップＳ５の処理は、整理すると次の様になる。
【００４２】
非常停止状態からの復帰は、前記（１），（２）を実行する。その後、通常のの原点出し動作が可能となる。
【００４３】
一方ティーチングにおいて、ティーチングの処理フローチャートは、図５のようになる。ステップＳ１とステップＳ２は前記説明と同様であるので、ここでは省略する。フローチャートの中で、ステップＳ１００はＢ点のティーチングであり、ステップＳ２００はＡ点のティーチングである。
【００４４】
ステップＳ１００：＜Ｂ点のティーチング＞
ステップＳ１０１：ワークの機械図面から読み取った挿入の開始点Ａ（ＸＡ，ＹＡ，ＺＡ）へワークの位置を移動する。この時、ワークの現在位置Ｎ（ＸＮ，ＹＮ，ＺＮ）は開始点Ａ（ＸＡ，ＹＡ，ＺＡ）と等しくなり、図６の形となる。
【００４５】
ステップＳ１０２：現在位置Ｎ（ＸＮ，ＹＮ，ＺＮ）とベクトルＶ（ＸＶ，ＹＶ，ＺＶ）の座標との差を取り、これを目的地Ｅ’とすれば、Ｅ’の座標は計算上（ＸN −ＸV ，ＹN −ＹV ，ＺN −ＺV ）となる。すなわち、第１回目のＥ’の座標は、機械図面から読み取った挿入の終了点Ｂ（ＸＢ，ＹＢ，ＺＢ）と等しい。
【００４６】
ステップＳ１０３：Ｔ．Ｐ．のインチングキーを押すことにより、現在位置Ｎ（ＸＮ，ＹＮ，ＺＮ）から目的地Ｅ’すなわちＢ（ＸＢ，ＹＢ，ＺＢ）へとベクトルＶと逆方向に直線補間移動させる。
【００４７】
ステップＳ１０４：ステップＳ１０３でワークと挿入穴との方向ずれがある場合はインチングキーを離して目的地Ｅ’へ到達する途中でロボットの移動を停止させる。この場合、Ｘ，Ｙ，Ｚいずれかの単軸のインチングキーで移動方向を補正し、その後、ステップＳ１０２から繰り返す。
【００４８】
ステップＳ１０５：挿入完了点へ到達したらここをＢ点（ＸＢ，ＹＢ，ＺＢ）としてティーチングする。従って、機械図面から読み取ったＢ点の座標データとは若干異なった値となることもある。
【００４９】
この時、現在位置Ｎ（ＸＮ，ＹＮ，ＺＮ）は終了点Ｂ（ＸＢ，ＹＢ，ＺＢ）と等しい。
【００５０】
ステップＳ２００：＜Ａのティーチング＞
ステップＳ２０２：現在位置Ｎ（ＸＮ，ＹＮ，ＢＮ）すなわち、（ＸＢ，ＹＢ，ＺＢ）とベクトルＶ（ＸＶ，ＹＶ，ＺＶ）の座標の和を目的地Ｅ”とすれば、Ｅ”の座標は（ＸN ＋ＸV ，ＹN ＋ＹV ，ＺN ＋ＺV ）となる。第１回目のＥ”の座標は挿入開始点Ａとほぼ等しい。
【００５１】
ステップＳ２０３：Ｔ．Ｐ．のインチングキーを押して、現在位置Ｎから目的地Ｅ”へとベクトルＶの方向へ直線補間移動させる。
【００５２】
ステップＳ２０４：ステップＳ２０３でワークと挿入穴の方向ずれがある場合はインチングキーを離して目的地Ｅ”へ到着する途中でロボットの移動を停止させる。この場合、Ｘ，Ｙ，Ｚいずれかの単軸のインチングキーで移動方向を補正し、ステップＳ２０２から繰り返す。
【００５３】
ステップＳ２０５：挿入開始点へ到着したらここをＡ点としてティーチングする。従って機械図面から読み取ったＡ点の座標データとは若干異なった値となることもある。
【００５４】
尚、直線補間の制御方法は種々のものが知られているが、次に一例を挙げておく。
【００５５】
Ｘ，Ｙ，Ｚの３軸ＮＣロボットにおいて、３軸の合成軌跡が直線となる位置司令を与えるものとする。各軸の現在位置をＰＸＳ，ＰＹＳ，ＰＺＳとする。総移動距離をＰＸ，ＰＹ，ＰＺとし、その中でもっとも大きな値のものをＫP とする。ＫＰとなる軸の移動中の位置司令をＫＰＣＭとする。各軸の位置司令をＰＸＣＭ，ＰＹＣＭ，ＰＺＣＭとする。
【００５６】
例として、ＰＸ＝ＫP であるならば、ＰＸＣＭは本来の位置司令を与え、Ｙ，Ｚ軸の位置司令ＰＹＣＭ，ＰＺＣＭはそれぞれ次式で与えられる。
【００５７】
【数１】

【００５８】
尚、ＫP となる軸がＸ軸でない場合は同様に、
【００５９】
【数２】

【００６０】
となる。
【００６１】
以上、直交座標型ロボットについて述べたが、多関節型ロボットをはじめとする、他のロボットへも応用展開できる。
【００６２】
但し、直線補間を行うための位置司令の計算式はロボットの型式により異なる場合があるが割愛する。
【００６３】
尚、本発明は、複数の機器から構成されるシステムに適用しても、１つの機器から成る装置に適用しても良い。また、本発明はシステム或は装置にプログラムを供給することによって達成される場合にも適用できることはいうまでもない。
【発明の効果】
以上説明した様に、本発明によれば、ロボットの動作が途中で停止し、かつ、ワークが挿入中である場合に、停止に陥った位置からの復帰が人手を介さずに行なえる。従ってシステム自動化の阻害要因が除去され稼働率の上昇が期待できると共に、人手への安全性が高まる。
【００６５】
【図面の簡単な説明】
【図１】ロボットの構成図の例である。
【図２】制御部の内部構成を示す図である。
【図３】挿入時の座標を示す図である。
【図４】復帰の処理を示すフローチャートである。
【図５】ティーチングの処理を示すフローチャートである。
【図６】ワーク挿入前の始点にある状態を示すＸ−Ｙ平面の断面構造図である。
【図７】ワーク挿入途中で停止した状態を示す図である。
【図８】図６，図７の平面座標図を示す図である。
【図９】ワークがフィンガーと共に挿入される場合の様子を示した図である。
【符号の説明】
１ 中央処理装置（ＣＰＵ）
２ ランダム・アクセス・メモリ（ＲＡＭ）
２−１ パラメータ登録
２−２ ワーキングエリア
３ リード・オンリ・メモリ（ＲＯＭ）
３−１ 処理プログラム
３−２ 固定パラメータ
４ 外部記憶装置
５ エンコーダ・インタフェース（ＥＣＩ）
６ モータ・ドライブ・インタフェース（ＭＤＩ）
７ ティーチング・ペンダント・インタフェース（ＴＰＩ）
８−１，８−２，８−３ モータ・ドライブ（ＭＤ）
９−１，９−２，９−３ モータ
１０−１，１０−２，１０−３ エンコーダ
１１ バス
１２ スイッチ・ランプ・インタフェース（ＳＷ／ＬＰＩＦ）
２０ フィンガー
３０ ワーク
４０ 被挿入部
１００ 制御部
２００ ティーチング・ペンダント（ＴＰ）
３００ 直交型３軸ＮＣロボット可動部
４００ スイッチ・ランプ（ＳＷ／ＬＰ）

Claims (2)

1. ２つ以上の自由度を持つ機構のロボットに、ワークの軌跡が直線となる移動を行う動作をさせるロボット制御方法であって、
   前記直線の始点と終点間のベクトルを求め、
   前記ロボットの動作が途中で停止したとき、前記ワークが挿入中であるか否かを判断し、
   前記ワークが挿入中ではないと判断した場合、前記ロボットの原点出しの処理を行い、
   前記ワークが挿入中であると判断した場合、前記ワークを前記ベクトルに従って前記ワークの抜き取りができる方向へ移動した後に前記原点出しの処理を行うことを特徴とするロボット制御方法。
2. ２つ以上の自由度を持つ機構のロボットに、ワークの軌跡が直線となる移動を行う動作をさせるロボット制御装置であって、
   前記直線の始点と終点間のベクトル算出手段と、
   前記ロボットの動作が途中で停止したときに、前記ワークが挿入中であるか否かを判断する判断手段と、を備え、
   前記ワークが挿入中ではないと判断した場合、前記ロボットの原点出しの処理を行い、
   前記ワークが挿入中であると判断した場合、前記ワークを前記ベクトルに従って前記ワークの抜き取りができる方向へ移動した後に前記原点出しの処理を行うことを特徴とするロボット制御装置。

Images