JP4091124B2 - 動作経路シミュレーション機能を有するロボット制御装置 - Google Patents

Description

技術分野
本発明は、産業用ロボットを用いて自動化システムを構築するに際して生じ得るロボットの誤教示による障害を回避し、安全な作業環境を確保するための動作経路シミュレーション技術に関する。
背景技術
ロボットに対して必要な動作の教示を行なった場合、教示が正しく行なわれたか否か、就ずく意図せざる経路移動を伴うような教示がされていないか否かを確認することは、ロボット作業自体の適正化のみならず、人身事故や周辺機器との干渉事故を防止するという観点から見ても極めて重要な事柄である。このような観点で教示動作の確認を行なう手法としては、従来より次の２つの手法が知られている。
（Ｉ）低いオーバライド値の指定などにより非常に低い速度で動作する条件を付けた上で、教示されたプログラムの再生運転を行い、目視によって動作の安全を確認する。
（II）オフラインで教示と動作確認を行なうシステムを利用し、動作の安全をオフラインシステム上で十分確認してから、実際に使用するロボットコントローラに動作プログラムをダウンロードする。
しかし、これらの手法はそれぞれ問題点を含んでいる。先ず上記の（Ｉ）の方法は、不測の事故を確実に避けるために極端な低速動作とオペレータの細心の注意が要求されるため、作業効率が非常に悪くオペレータにかかる負担も大きい。一般に、希望通りの動作の教示を完了するまでには何回かの修正作業を要するのが常であるから、その度毎に低速運転による動作確認を行なえば、教示作業全体を通しての消費時間は膨大なものとなる。また、一旦教示を完了した後に、使用するハンドや周辺機器の機種変更や配置変更などによって動作プログラムが修正されることも多く、それに伴う動作確認作業にも多くの時間を費やすことになる。
一方、（II）のオフラインシステム上で動作確認を行なう方法は、ロボットの実際の動作を要しないので確認作業自体の安全性は確保されるが、動作確認の信頼性を高めようとすると、オフラインシステム上でロボット動作を忠実に再現する必要があり、そのためのデータ入力作業などの負担が非常に大きい。また、オフラインで動作確認を終えた動作プログラムを実機にダウンロードした後にハンドや周辺機器の機種や配置の変更があれば、やはり動作プログラムが修正されることになり、結局（Ｉ）の低速動作による確認が必要になる。
発明の開示
本発明の１つの目的は、オフラインのシミュレーションシステムを使用せず、且つ、動作経路教示修正の度に実機の低速再生運転を繰り返さなくとも、修正後の動作経路をシミュレートし、経路教示の誤りを発見するために有用な情報の提供を可能にすることである。また、もう一つの目的は、動作経路教示修正後に実機の再生運転を開始した場合あるいは開始しようとした場合てあっても、該修正内容に誤りの可能性を発見した時にはロボットの動作を停止させて事故を未然に回避出来るようにすることである。これにより、教示作業の安全性と効率が高められる。
本発明は、動作プログラム格納手段、動作プログラム再生手段、動作プログラム再生時にロボットに実動作を行なわせる実動作手段などによる通常機能の他に、動作経路シミュレーション機能をロボット制御装置に持たせた。
本発明のロボット制御装置の動作経路シミュレーション機能は、動作プログラム格納手段に格納された動作プログラムの再生時にその動作プログラムが記述する動作経路を記憶する経路記憶手段と、新旧動作プログラムの記述する動作経路を比較する経路比較手段を備える。これら経路記憶手段、経路比較手段、実動作手段の作動／非作動は、実施の形態に応じて選択的に指定され得る。また、動作プログラムの再生に際して、ロボットを非動作のまま行なえるようにしても良く、また、ロボットを動作させて行なえるようにしても良い。更に、それらの作動／非作動を選択的に組合せて設定出来るようにしても良い。
本発明で採用される動作経路比較手段は、経路記憶手段に記憶された安全を確認済みの第１の動作経路を参照動作経路とし、参照動作経路を記述する動作プログラムとは別の第２の動作プログラムの再生時に、前記第２の動作プログラムが記述する動作経路を前記参照動作経路と比較する。そして、両動作経路間に存在する差異をロボット（典型的にはツール先端点）の位置あるいは位置と姿勢の双方について評価し、評価結果を出力する。
経路記憶手段による動作経路の記憶を補間点データを含む形態で実行し、経路比較手段が再生動作経路と参照動作経路の差異を比較する際に、それら両経路の補間点に関するデータを用いるようにすれば、再生動作経路と参照動作経路の差異評価の精度を高めることが出来る。
更に、好ましい実施形態においては、経路比較手段により出力された評価結果を教示操作盤のディスプレイなどに自動表示したり、評価結果が再生動作経路と参照動作経路の間に基準を越える差異があることを示すものである場合に所定のメッセージを出力（例えば、基準オーバのウォーニング表示出力、音声アラーム作動信号出力など）したりする機能が追加される。
動作プログラムの再生によりロボットを実際に動作させた場合に、経路比較手段から再生動作経路と参照動作経路の間に基準を越える差異があることを記述する評価結果が出力されたときには、ロボットの実動作状態を無効化して以後のロボットの動作を禁止出来るようにしても良い。このようにすれば、再生動作経路と参照動作経路の間の差異をチェックしながらロボットの実際の動作を進行させ、実動作の経路が参照経路から大きくははずれるような場合には、ロボットの実動作を止めさせて危険の未然の回避と誤教示個所の発見に資することが出来る。
【図面の簡単な説明】
図１は、ロボット制御装置のハードウェア構成を示す要部ブロック図、
図２は、本発明の第１実施例における処理の概要を示すフローチャート、
図３は、本発明の第１実施例あるいは第３実施例で実行される比較処理の概要を示すフローチャート、
図４は、本発明の第２実施例における処理の概要を示すフローチャート、
図５は、本発明の第２実施例で実行される比較処理の概要を示すフローチャート、
図６は、本発明の第３実施例における処理の概要を示すフローチャートである。
発明を実施するための最良の形態
本発明のロボット制御装置は、通常のハードウェア構成を有するロボット制御装置に、後述するような処理を実行するソフトウェアを装備させることによって具体化することが出来る。図１は、そのようなロボット制御装置の一般的なハードウェア構成を要部ブロック図で示したものである。バス１０７に、メインＣＰＵ１０１、ＲＡＭ、ＲＯＭ、不揮発性メモリ（ＥＥＰＲＯＭなど）からなるメモリ１０２、教示操作盤用インターフェイス１０３、外部装置用の入出力インターフェイス１０６及びサーボ制御部１０５が並列に接続されている。
教示操作盤用インターフェイス１０３に接続される教示操作盤１０４は通常のディスプレイ機能付のもので、オペレータは、この教示操作盤１０４のマニュアル操作を通して、ロボットの動作プログラムの作成、修正、登録、あるいは各種パラメータの設定の他、教示された動作プログラムの再生運転、ジョグ送り等を実行する。又、ディスプレイはシミュレーション結果の表示（後述）にも使用される。
ロボット及びロボット制御装置の基本機能を支えるシステムプログラムは、メモリ１０２のＲＯＭに格納される。また、アプリケーションに応じて教示されるロボットの動作プログラム並びに関連設定データは、メモリ１０２の不揮発性メモリ１０２に格納される。そして、メモリ１０２のＲＡＭは、ＣＰＵ１０１が行なう各種演算処理におけるデータの一時記憶の記憶領域に使用される。
サーボ制御部１０５はサーボ制御器＃１〜＃ｎ（ｎ：ロボットの総軸数）を備えており、ロボット制御のための演算処理（軌道計画作成とそれに基づく補間、逆変換など）を経て作成された移動指令を受け、ロボット各軸機構部のアクチュエータを構成するサーボモータを各サーボアンプを介して制御する。但し、次に述べるように、本発明の方式に従って「動作軌跡記憶−実機非動作モード」を実行する場合には、サーボ制御部１０５に移動指令が渡されることはなく、ロボット各軸のサーボモータは動作しない。
以下、３つの実施例（第１〜第３実施例）についてロボット制御装置で実行される処理を説明する。
第１あるいは第２の実施例で実行される処理に関連して、経路記憶手段、経路比較手段及びロボット実機の作動／非作動を指定する手段として、下記のモードフラグＦ１〜Ｆ３がメモリ１０２内に設定される。各フラグＦ１〜Ｆ３は、モード選択スイッチとして設定される２値レジスタで、いずれも“０”または“１”の値をとる。オペレータは、教示操作盤１０４の操作によってフラグの値“０”または“１”を予め設定することで、希望するオペレーションモードを選択することが出来る。
Ｆ１：本発明に特徴的なシミュレーション機能のオン（Ｆ１＝１）／オフ（Ｆ１＝０）を選択するスイッチとして機能するフラグ
Ｆ２：実動作（ロボット実機動作）のオン（Ｆ２＝１）／オフ（Ｆ２＝０）を選択するスイッチとして機能するフラグ
Ｆ３：比較モード（今回経路と参照経路とを比較するモード）のオン（Ｆ３＝１）／オフ（Ｆ３＝０）を選択するスイッチとして機能するフラグ
図２は、本発明の第１実施例における処理の要点を説明するフローチャートである。上記フラグＦ１〜Ｆ３の設定（モード選択）を行い、オペレータが教示操作盤１０４から処理開始指令を入力することで処理が開始される。ＣＰＵ１０１はそれまでの教示作業で作成された動作プログラムの１ブロック分の動作命令（ここでは動作終了命令のケースも含むものとする）を読み込み、解釈（デコード）する（ステップＳ１）。
一般に、動作命令（動作終了命令は除く）は、絶対位置移動命令、相対位置移動命令、速度命令、目標位置における位置決め方法（位置決め割合）、外部入出力命令などを包含している。読み込まれたブロックが動作終了を指示するものでない限り（ステップＳ２でノー）、ステップＳ３へ進み、与えられた移動命令から移動目標点（位置と姿勢）を算出する。移動目標点のデータは、準拠座標系として指定されている座標系上のデータで計算される。例えば、あるワーク座標系が指定されていれば、ワーク座標系上で移動目標点を表現するデータが得られる。また、各軸移動命令であれば、各軸値で移動目標位置を表現するデータが得られる。次いで、ステップＳ４でフラグＦ１の値をチェックし、シミュレーションを実行するモードか否か判断される。
Ｆ１＝０（シミュレーション非実行）であれば、ステップＳ５へ進み、実動作の要否をフラグＦ２の値で判別する。Ｆ２＝０であれば実動作不要のモードを意味するから、ステップＳ９へ進み、そのブロックで指定された移動目標点への移動完了を擬制する処理（次ブロックの処理へ進むための内部処理）を実行する。次いで、ステップＳ１へ戻り、次の１ブロックの動作命令文に対する処理を開始する。
ステップＳ５でもしＦ２＝１であれば実動作のモードを意味するから、ステップＳ６〜Ｓ８で通常の再生運転のための処理を実行する。即ち、ステップＳ１で読み込まれ、解釈された動作条件（移動目標位置、指令速度、移動形式［直線移動／円弧移動／各軸移動など］、加減速条件等）に応じて軌道計画を立て（ステップＳ６）、各軸上での補間点を求め（ステップＳ７）、それに基づく移動指令を作成して各軸のサーボ制御部１０５へ渡す（ステップＳ８）。
なお、図２のフローチャートでは省略したが、ステップＳ７、Ｓ８は、補間周期毎に繰り返され、１ブロック分の移動処理を完了してからステップＳ１へ戻り、次の１ブロックの動作命令文に対する処理を開始する。
シミュレーションを実行するためにフラグＦ１がＦ１＝１に設定されている場合には、ステップＳ４からステップＳ１０へ進む。ステップＳ１０では、ステップＳ３で算出された移動目標点その他の動作条件データ（指令速度、移動形式、加減速条件等）を複製し、メモリ１０２の空き領域に一時的に記憶する。
更に、ステップＳ３で算出された移動目標点が直交座標系上で表現されたデータで記述されているか否かを判断し（ステップＳ１１）、もしイエスであれば直接ステップＳ１３へ進み、もしノーであれば順変換演算によって直交座標系上で表現されたデータに変換した上で（ステップＳ１２）、ステップＳ１３へ進む。
ステップＳ１３では、移動目標点を表わすデータが、経路比較を行なう基準とされる特定のロボット座標系（例えば、予めロボットベース座標系を設定）上でツール座標系の位置及び姿勢を表現しているか否かを判断し、イエスであれば直接ステップＳ１５へ進み、ノーであれば座標系間の変換演算によってロボット座標系上で表現されたデータに変換した後に（ステップＳ１４）、ステップＳ１５へ進む。
ステップＳ１５では、シミュレーションを比較モードで実行するモードが設定されているか否かを判断する。もしＦ３＝０（非比較モード）であれば、ステップＳ１６へ進み、動作経路、即ち移動目標点と運動形式（直線移動／円弧移動／各軸移動の別）をシミュレーション実行機会を表わす番号を付してメモリ１０２に記憶する。例えば、今回のシミュレーションがプログラム名「Ａ Ａ」を持つ動作プログラムに対する３回目のシミュレーションであれば、「ＳＩＭ０３ＡＡ」などとする。
これに対して、もしＦ３＝１（比較モード）であればステップＳ１７へ進み、比較処理を実行する。比較処理は、今回の移動経路を参照経路と比較し、必要に応じて比較結果を記録するための処理で、利用されるアルゴリズムの例などについては後述する。
比較処理が完了したら、ステップＳ１８へ進み、ステップＳ１０で一時記憶しておいたデータ（移動目標点その他の動作条件データ）を読み出し、ステップＳ５へ進む。
既に述べたように、ステップＳ５以下では、フラグＦ２の値（“０”または“１”）に応じて、ロボット実機の移動の処理（ステップＳ６〜Ｓ８）または目標点到達の擬制の処理（ステップＳ９）を実行してステップＳ１へ戻る。ロボット実機の移動は、言うまでもなく、ステップＳ１８で読み出されたデータに基づいて行なわれる。以上の処理サイクルを、ステップＳ２でイエスの判断出力が得られるまで繰り返し、比較処理１７（内容詳述）の結果を教示操作盤１０４付属のディスプレイに表示して処理を終了する（ステップＳ１９）。なお、比較処理１７の結果が基準以上の経路変更を意味している場合には、ブザーの作動等のためのアラーム信号を出力するなど所定のメッセージを発するための処理を行なうことが好ましい。
次に、ステップＳ１７で行なわれる比較処理について説明する。比較処理は、今回シミュレートしている移動経路を参照経路と比較し、経路に所定量を越えるような「隔たり」があるか否かを判定し、必要に応じて比較結果を記録するための処理である。そのために、両経路間の「隔たり」を表わす１個または２個以上の指標を用いる。隔たりを表わす指標を算出し、比較結果を評価するアルゴリズムについては種々のものが採用可能であるが、第１実施例では図３のフローチャートに示した処理によるものとする。
なお、参照経路については、別途、例えば低速運転で安全を確認した経路を前述したシミュレーション実行機会番号を用いて指定出来るようにする。
図３のフローチャートに示した処理は、経路間距離を位置及び姿勢について評価するものである。経路間の隔たりは、ツール先端点の位置の観点から評価する指標ｄｐと姿勢の観点から評価する指標ｄｃを用いて評価される。各ステップの要点を記せば下記の通りである。
先ず経路比較のために、当該動作の運動形式が円弧移動であるか否かに応じて（ステップＣ１）、参照経路についてメモリ１０２に記憶されている１点または２点の移動目標点のデータを読み出す。運動形式が円弧移動でなく、直線移動あるいは各軸移動である場合には、移動目標点はそのブロックの直線移動の終点のみである。記録されているのは、この直線移動の終点におけるツール先端点の位置及び姿勢を基準とされるロボット座標系上で表わすデータであるから、これを同次変換行列Ｔrefで表わすことにする（ステップＣ２）。
運動形式が円弧移動である場合には、移動目標点はそのブロックの円弧移動の中間点と終点であるから、これら２点におけるツール先端点の位置及び姿勢を表わす同次変換行列が読み出される。これをＴref1，Ｔref2で表わすことにする（ステップＣ７）。同様に、今回のシミュレート経路についてステップＳ１４で求めたデータをＴ（直線移動または各軸移動の場合）あるいはＴ1，Ｔ2（円弧移動の場合）で表わす。
当分野で良く知られているように、各行列は回転を表わす部分行列Ｒ（３行×３列）と並進を表わすベクトルｐ（１行×３列）で構成され、更に、部分行列Ｒはノーマルベクトルｎ（１行×３列）、オリエンテーションベクトルｏ（１行×３列）、アプローチベクトルａ（１行×３列）を用いて下記式（１）〜（６）のように表記出来る。以下の説明ではこの表記を引用する。

ステップＣ３またはＣ８では、両経路の移動目標点に関してツール先端点間の距離を表わす指標ｄ，ｄ1，ｄ2を次の各式（７）〜（９）で算出する。
直線移動あるいは各軸移動の場合（ステップＣ３）；
ｄ＝｜ｐ−ｐref｜ ・・・（７）
円弧移動の場合（ステップＣ８）；
ｄ1＝｜ｐ1−ｐref1｜ ・・・（８）
ｄ2＝｜ｐ2−ｐref2｜ ・・・（９）
更にステップＣ４またはＣ９では、ツール先端点間距離指標ｄ，ｄ1，ｄ2を基準値ｄstと比較し、距離判定指標Δｄを次式（１０）あるいは（１１）で求めて、その符号並びに絶対値を記録する。符号が＋であればツール先端点間距離が基準を越えていることを意味し、−または０であればツール先端点間距離基準以下であることを意味している。
直線移動あるいは各軸移動の場合（ステップＣ４）；
Δｄ＝ｄ−ｄst ・・・（１０）
円弧移動の場合（ステップＣ９）；
Δｄ＝ＭＡＸ［ｄ1，ｄ2］−ｄst ・・・（１１）
次に、姿勢変化の評価を行なう。先ずステップＣ５またはＣ１０では、両経路の移動目標点に関してツール先端点の姿勢差を表わす指標ｆ，ｇ，ｈ，ｆ1・・・・ｇ2，ｈ2を次の各式（１２）〜（２０）で算出する。即ち、ツール先端点の姿勢差をノーマルベクトル、オリエンテーションベクトル、アプローチベクトルに分解して比較及び評価する。なお、各式における記号‖‖はベクトルのノルムを表わしている。
直線移動あるいは各軸移動の場合（ステップＣ５）；
ｆ＝‖ｎ−ｎref‖ ・・・（１２）
ｇ＝‖ｏ−ｏreｆ‖ ・・・（１３）
ｈ＝‖ａ−ａref‖ ・・・（１４）
円弧移動の場合（ステップＣ１０）；
ｆ1＝‖ｎ1−ｎref1‖ ・・・（１５）
ｆ2＝‖ｎ2−ｎref2‖ ・・・（１６）
ｇ1＝‖ｏ1−ｏref1‖ ・・・（１７）
ｇ2＝‖ｏ2−ｏref2‖ ・・・（１８）
ｈ1＝‖ａ1−ａref1‖ ・・・（１９）
ｈ2＝‖ａ2−ａref2‖ ・・・（２０）
更にステップＣ６またはＣ１１ではこれらツール先端点姿勢差指標を各基準値ｆst，ｇst，ｈstと比較し、姿勢差判定指標Δｆ，Δｇ，Δｈを次式（２１）〜（２３）あるいは（２４）〜（２６）で求め、その符号並びに絶対値を記録する。
Δｆ＝ｆ−ｆst ・・・（２１）
Δｇ＝ｇ−ｇst ・・・（２２）
Δｈ＝ｈ−ｈst ・・・（２３）
Δｆ＝ＭＡＸ［ｆ1，ｆ2］−ｆst ・・・（２４）
Δｇ＝ＭＡＸ［ｇ1，ｇ2］−ｇst ・・・（２５）
Δｈ＝ＭＡＸ［ｈ1，ｈ2］−ｈst ・・・（２６）
距離判定指標Δｄの場合と同様に、姿勢差判定指標Δｆ，ΔｇあるいはΔｈの符号が＋であれば、その姿勢成分について差異が基準を越えていることを意味し、−または０であれば基準以下であることを意味している。
以上がステップＳ１７で実行される比較処理の概要であり、ステップＣ４，Ｃ９あるいはステップＣ６，Ｃ１１で記録されたデータは、上述した通り、ステップＳ１９で表示される。また、距離判定指標Δｄ、姿勢差判定指標Δｆ，Δｇ，Δｈの内の少なくとも１つの符号が＋である時には、アラーム信号を出力して音声、点滅表示を行なうなど所定のメッセージを発してオペレータの注意を喚起することが好ましい。
以上が第１実施例における処理の概要であるが、この第１実施例では動作経路の記録とそれに基づく経路比較が、動作プログラムの動作命令ブロック毎に指定されている移動目標点のみに基づいて行なわれている。そのため、円弧移動あるいは各軸移動の運動形式が指定されている経路区間について、動作経路記録とそれに基づく経路比較の精度が低くなるおそれがある。そこで、次に説明する第２実施例では、動作経路記録と経路比較に際して移動目標点間の経路途上の点（補間点）の位置及び姿勢を考慮した処理を実行し、円弧移動あるいは各軸移動の経路区間についての精度向上を図っている。
図４は、本発明の第２実施例における処理の要点を説明するフローチャートで、第１実施例における図２のフローチャートに代わるものである。第１実施例の場合と同様に、フラグＦ１〜Ｆ３の設定（モード選択）を行い、オペレータが教示操作盤１０４から処理開始指令を入力することで処理が開始される。ＣＰＵ１０１はそれまでの教示作業で作成された動作プログラムの１ブロック分の動作命令（動作終了命令のケースも含む。）を読み込み、解釈（デコード）する（ステップＨ１）。
一般に、動作命令（動作終了命令は除く）は絶対位置移動命令、相対位置移動命令、速度命令、目標位置における位置決め方法（位置決め割合）、外部入出力命令などを包含している。読み込まれたブロックが動作終了を指示するものでない限り（ステップＨ２でノー）、ステップＨ３へ進み、与えられた移動命令から移動目標点（位置と姿勢）を算出する。第１実施例の場合と同じく、移動目標点のデータは準拠座標系として指定されている座標系上のデータで計算される。
次いで、ステップＨ１で読み込まれ、解釈された動作条件（移動目標位置、指令速度、移動形式［直線移動／円弧移動／各軸移動など］、加減速条件等）に応じて軌道計画を立て（ステップＨ４）、各軸上での補間点を求める（ステップＨ５）。
続くステップＨ６でフラグＦ１の値をチェックし、シミュレーションを実行するモードか否かを判断する。
Ｆ１＝０（シミュレーション非実行）であれば、ステップＨ７へ進み、実動作の要否をフラグＦ２の値で判別する。Ｆ２＝０であれば実動作不要のモードを意味するから、ステップＨ８へ進み、一つの補間点への移動完了を擬制する処理を実行する。
なお、このステップＨ８で処理対象とされる補間点は、移動完了の擬制処理が完了しておらず、且つ、動作経路上最も手前側の補間点である。また、そのブロックで指定されている移動目標点（但し、円弧移動の場合の中点は対象外）は、処理上最終補間点として扱うものとする。
そして、次のステップＨ１０では、ステップＨ８で処理対象とされた補間点が最終補間点、即ち、移動目標点であるか否かを判断する。もしイエスであれば、ステップＨ１へ戻り、次の１ブロックの動作命令文に対する処理を開始する。ノーであれば、ステップＨ５へ戻り、次の補間点を求める。以下、ステップＨ１０でイエスが出力されるまで、ステップＨ５〜Ｈ８及びＨ１０の処理が繰り返される。
Ｆ１＝０，Ｆ２＝１の場合には、ステップＨ８の擬制移動処理に代えて実動作のための処理Ｈ９（移動指令の作成／サーボへ出力）が行なわれる。この処理は通常の再生運転時のための処理と特に変わりはない。実動作の場合も、移動目標点に到達したら（ステップＨ１０でイエス）、ステップＨ１へ戻り次の１ブロックの動作命令文に対する処理を開始する。
シミュレーションを実行するためにフラグＦ１がＦ１＝１に設定されている場合には、ステップＨ６からステップＨ１１へ進む。ステップＨ１１では、ステップＨ３，Ｈ５で算出された移動目標点及び補間点並びに他の動作条件データ（指令速度、移動形式、加減速条件等）を複製し、メモリ１０２の空き領域に一時的に記憶する。
更に、ステップＨ３，Ｈ５で算出された移動目標点及び補間点が直交座標系上で表現されたデータで記述されているか否かを判断し（ステップＨ１２）、イエスであれば直接ステップＨ１４へ進み、ノーであれば順変換演算によって直交座標系上で表現されたデータに変換した上で（ステップＨ１３）、ステップＨ１４へ進む。
ステップＨ１４では、移動目標点及び補間点を表わすデータが、経路比較を行なう基準とされる特定のロボット座標系上で位置及び姿勢を表現しているか否かを判断し、イエスであれば直接ステップＨ１６へ進み、ノーであれば座標系間の変換演算によってロボット座標系上で表現されたデータに変換した上で（ステップＨ１５）、ステップＨ１６へ進む。
ステップＨ１６では、シミュレーションを比較モードで実行するモードが設定されているか否かを判断する。もしＦ３＝０（非比較モード）であれば、ステップＨ１７へ進み、動作経路（移動目標点、補間点、運動形式）をシミュレーション実行機会を表わす番号を付してメモリ１０２に記憶する。例えば、今回のシミュレーションがプログラム名「ＢＢ」を持つ動作プログラムに対する２回目のシミュレーションであれば、「ＳＩＭ０２Ｂ Ｂ」などとする。
これに対して、もしＦ３＝１（比較モード）であればステップＨ１８へ進み、較処理を行なう。比較処理は、第１実施例の場合と同様、今回の移動経路を参照経路と比較し、必要に応じて比較結果を記録するための処理であるが、経路比較を補間点を考慮して行なう点で若干の違いがある。アルゴリズムの例などについては後述する。
比較処理が完了したら、ステップＨ１９へ進み、ステップＨ１１で一時記憶しておいたデータ（移動目標点。補間点その他の動作条件データ）を読み出し、ステップＨ７へ進む。既に述べたように、ステップＨ７以下では、フラグＦ２の値（“０”または“１”）に応じて、目標点到達の擬制の処理（ステップＨ８→Ｈ１０→Ｈ５→Ｈ６→Ｈ７）、あるいはロボット実機の移動の処理（ステップＨ９→Ｈ１０→Ｈ５→Ｈ６→Ｈ７）を経て、ステップＨ１１へ戻る。但し、ステップＨ１０でイエスが出力（１ブロック分処理完了）された場合には、ステップＨ１へ戻る。
以上の処理サイクルを、ステップＨ２でイエスの判断出力が得られるまで繰り返し、比較処理１８（内容詳述）の結果を教示操作盤１０４付属のディスプレイに表示して処理を終了する（ステップＨ２０）。なお、第１実施例の場合と同様、比較処理（ステップＨ１８）の結果が基準以上の経路変更を意味している場合には、ブザーの作動のためにアラーム信号を出力するなど、メッセージを発するための処理を行なうことが好ましい。
次に、第２実施例で行なわれる比較処理（ステップＨ１８）について説明する。第２実施例で行なわれる比較処理は、ステップＨ１８の実行機会が１回到来する毎に、今回シミュレートしている移動経路の補間点の１つと、それに対応する参照経路の補間点を比較し、両者間に所定量を越えるような「隔たり」があるか否かを判定し、必要に応じて比較結果を記録するための処理である。ここでは、第１実施例に準じた「隔たり」の指標を用い、図５のフローチャートに示したアルゴリズムを利用するものとする。なお、移動目標点（円弧移動の場合は中点は除外）は最終補間点として扱うものとする。
また、第１実施例の場合と同様、参照経路については、例えば低速運転で安全を確認した経路を前述したシミュレーション実行機会番号を用いて指定出来るようにする。
図５のフローチャートに示した処理は、経路間距離を対応し合う１組の補間点の位置及び姿勢について評価するものである。隔たりは、第１実施例の場合と同様、ツール先端点の位置の観点から評価する指標ｄｐと姿勢の観点から評価する指標ｄｃを用いて評価される。各ステップの要点を記せば下記の通りである。
先ず、ステップＨ１８の実行機会が到来する毎に補間点順序指標ｉを１アップする（ステップＫ１）。なお、補間点順序指標ｉの初期値（図４のフローチャートの処理開始時の値）は０とし、ステップＨ１０でイエス出力あれば、ｉ＝０にクリアされるものとする（図４のフローチャートで図示省略）。
続くステップＫ２で、参照経路のｉ番目の補間点におけるツール先端点の位置及び姿勢を基準ロボット座標系上で表わすデータ（同次変換行列）Ｔref(i)を読み出す。前述したと同様に、行列Ｔref(i)並びに今回経路のｉ番目の補間点におけるツール先端点の位置及び姿勢を表わす行列Ｔ(i)は、下記式（２７），（２８）で表される。以下の説明ではこの表記を引用する。

ステップＫ３では、両経路のｉ番目の補間点に関してツール先端点間の距離を表わす指標ｄ(i)を次の式（２９）で算出する。
ｄ(i)＝｜ｐ(i)−ｐref(i)｜ ・・・（２９）
更にステップＫ４では、ツール先端点間距離指標ｄ(i)を基準値ｄstと比較し、距離判定指標ｄ(i)を次式（３０）求め、その符号並びに絶対値を記録する。符号が＋であればツール先端点間距離が基準を越えていることを意味し、−または０であればツール先端点間距離基準以下であることを意味している。
Δｄ(i)＝ｄ(i)−ｄst ・・・（３０）
次に、姿勢変化の評価を行なう。先ずステップＫ５では、両経路のｉ番目の補間点に関してツール先端点の姿勢差を表わす指標ｆ(i)，ｇ(i)，ｈ(i)を次の各式（３１）〜（３３）で算出する。即ち、ツール先端点の姿勢差をノーマルベクトル、オリエンテーションベクトル、アプローチベクトルに分解して比較及び評価する。
ｆ(i)＝‖ｎ(i)−ｎref(i)‖ ・・・（３１）
ｇ(i)＝‖ｏ(i)−ｏref(i)‖ ・・・（３２）
ｈ(i)＝‖ａ(i)−ａref(i)‖ ・・・（３３）
更にステップＫ６ではこれらツール先端点姿勢差指標を各基準値ｆst，ｇst，ｈstと比較し、姿勢差判定指標Δｆ(i)，Δｇ(i)，Δｈ(i)を次式（３４）〜（３６）で求め、その符号並びに絶対値を記録する。距離判定指標Δｄ(i)の場合と同様に、姿勢差判定指標Δｆ(i)，Δｇ(i)あるいはΔｈ(i)の符号が＋であれば、その姿勢成分について差異が基準を越えていることを意味し、−または０であれば基準以下であることを意味している。
Δｆ(i)＝ｆ(i)−ｆst ・・・（３４）
Δｇ(i)＝ｇ(i)−ｇst ・・・（３５）
Δｈ(i)＝ｈ(i)−ｈst ・・・（３６）
以上がステップＨ１８で実行される比較処理の概要であり、ステップＫ４あるいはＫ６で記録されたデータは、ステップＨ２０で表示される。但し、第２実施例では、全補間点のデータを表示すると表示データが膨大となるので、適当な選別、加工等を行なったデータを表示することが好ましい。例えば、距離差判定指標ｄ(i)の最大値と、姿勢差の判定指標Δｆ(i)，Δｇ(i)，Δｈ(i)の最大値などを表示することが考えられる。
また、これら距離判定指標、姿勢差判定指標の内の少なくとも１つの符号が＋である時には、アラーム信号を出力して音声、点滅表示を行うなど、所定のメッセージを発してオペレータの注意を喚起することが好ましいことは、第１実施例の場合と同様である。
以上の説明から、第１実施例、第２実施例のロボット制御装置は、モードフラグＦ１〜Ｆ３の設定に応じて次の６つのモードでの動作が可能な事が判る。
（１）Ｆ１＝０；Ｆ２＝１に設定した場合（Ｆ３は任意）；動作経路シミュレーションを行なわず、実動作を行なう。即ち、通常の再生運転のモード
（２）Ｆ１＝０；Ｆ２＝０に設定した場合（Ｆ３は任意）；動作経路シミュレーションを行なわず、実動作も行なわないモード
（３）Ｆ１＝１；Ｆ２＝０；Ｆ３＝０に設定した場合；動作経路シミュレーションを行ない、動作経路を記録するが、実動作は行なわない
（４）Ｆ１＝１；Ｆ２＝１；Ｆ３＝０に設定した場合；動作経路シミュレーションと実動作を併行実施し、動作経路を記録する。
（５）Ｆ１＝１；Ｆ２＝０；Ｆ３＝１に設定した場合；動作経路シミュレーションを行ない、動作経路を参照経路と比較するが、実動作は行なわない。
（６）Ｆ１＝１；Ｆ２＝１；Ｆ３＝１に設定した場合；動作経路シミュレーションと実動作を併行実施し、動作経路と参照経路の比較も行なう。
ここで、上記説明した第１、第２の実施例に関する限り、上記６個のモードの内、（２）と（６）は実際上の意味が薄いと考えられるので、これに対応するフラグ設定（Ｆ１＝Ｆ２＝０及びＦ１＝Ｆ２＝Ｆ３＝１）を禁則化しても良い（但し、後者は次に述べる第３の実施例で有用化されることに注意）。
残りのモードは第１、第２の実施例においても有用である。例えば、安全を確認済みの動作プログラムを（４）のモードで再生し、その後、教示経路を修正した後に、（５）のモードで再生すれば、ステップＳ１９あるいはステップＨ２０（比較結果表示／警報等のメッセージ）を利用して実動作なしに大きな経路変更の有無や内容を迅速且つ簡便に確認出来る。
次に、第３の実施例について説明する。この実施例では、上記（６）の組合せ（Ｆ１＝１；Ｆ２＝１；Ｆ３＝１）に設定して動作経路シミュレーションと実動作を併行実施する条件で処理を開始するが、動作経路と参照経路の比較結果が誤教示の可能性を示唆するようなものである場合には実動作を停止させることが出来る。これにより、誤教示の可能性のある個所の手前までロボットを移動させた上でロボット停止させることが可能になり、以後の修正教示などがやり易くなる。
第３の実施例で実行される処理においては、ロボット制御装置の運転モードを切換可能に指定する手段として、前述のモードフラグＦ１〜Ｆ３に加えて、フラグＦ４がメモリ１０２内に設定される。このフラグＦ４は、実動作を必要に応じて無効化するために設定される２値レジスタで、下記の定義に従って“０”または“１”の値をとる。
Ｆ４：Ｆ４＝１は実動作無効化のオン状態（実動作阻止する）、Ｆ４＝０は実動作無効化のオフ＝０（実動作阻止せず））を意味する。
第３実施例における処理の要点は、図６のフローチャートに示した通りである。
第１、第２実施例の場合と同様、上記フラグＦ１〜Ｆ３の設定（モード選択）を行い、オペレータが教示操作盤１０４から処理開始指令を入力することで処理が開始される。ＣＰＵ１０１は、フラグＦ４を“０”にクリアした上で（ステップＧ１）、それまでの教示作業で作成された動作プログラムの１ブロック分の動作命令（ここでは動作終了命令のケースも含むものとする）を読み込み、解釈（デコード）する（ステップＧ２）。
読み込まれたブロックが動作終了を指示するものでない限り（ステップＧ３でノー）、ステップＧ４へ進み、与えられた移動命令から移動目標点（位置と姿勢）を算出する。移動目標点のデータは、準拠座標系として指定されている座標系上のデータで計算される。次いで、ステップＧ５でフラグＦ１の値をチェックし、シミュレーションを実行するモードか否か判断される。
Ｆ１＝０（シミュレーション非実行）であれば、ステップＧ６へ進み、実動作の要否をフラグＦ２の値で判別する。Ｆ２＝０であれば実動作不要を意味するから、ステップＧ１１へ進み、そのブロックで指定された移動目標点への移動完了を擬制する処理（次ブロックの処理へ進むための内部処理）を実行する。次いで、ステップＧ２へ戻り、次の１ブロックの動作命令文に対する処理を開始する。
ステップＧ６でもしＦ２＝１であれば実動作のモードが設定されていることを意味する。しかし、第３実施例では、無条件に実動作を伴う再生運転のための処理を実行するのではなく、実動作のモードを無効化するか否かを決定するフラグＦ４の値をチェックし（ステップＧ７）、Ｆ４＝０の場合に限りステップＧ８〜Ｇ１０で実動作のための処理を行なう。即ち、ステップＧ１で読み込まれ、解釈された動作条件（移動目標位置、指令速度、移動形式［直線移動／円弧移動／各軸移動など］、加減速条件等）に応じて軌道計画を立て（ステップＧ８）、各軸上での補間点を求め（ステップＧ９）、それに基づく移動指令を作成して各軸のサーボ制御部１０５へ渡す（ステップＧ１０）。
なお、記載を省略したが、ステップＧ９、Ｇ１０は、補間周期毎に繰り返され、１ブロック分の移動処理を完了してからステップＧ１へ戻り、次の１ブロックの動作命令文に対する処理を開始する（第１実施例の場合と同様）。また、ステップＧ７でＦ４＝１であれば実動作の処理を進めることなくステップＧ２３を経て処理を終了する（詳細後述）。
シミュレーションを実行するためにフラグＦ１がＦ１＝１に設定されている場合には、ステップＧ５からステップＧ１２へ進む。ステップＧ１２では、ステップＧ４で算出された移動目標点その他の動作条件データ（指令速度、移動形式、加減速条件等）を複製し、メモリ１０２の空き領域に一時的に記憶する。
更に、ステップＧ４で算出された移動目標点が直交座標系上で表現されたデータで記述されているか否かを判断し（ステップＧ１３）、もしイエスであれば直接ステップＧ１５へ進み、もしノーであれば順変換演算によって直交座標系上で表現されたデータに変換した上で（ステップＧ１４）、ステップＧ１５へ進む。
ステップＧ１５では、移動目標点を表わすデータが、経路比較を行なう基準とされる特定のロボット座標系（例えば、予めロボットベース座標系を設定）上でツール座標系の位置及び姿勢を表現しているか否かを判断し、もしイエスであれば直接ステップＧ１７へ進み、もしノーであれば座標系間の変換演算によってロボット座標系上で表現されたデータに変換した上で（ステップＧ１６）、ステップＧ１７へ進む。
ステップＧ１７では、シミュレーションを比較モードで実行するモードが設定されているか否かを判断する。もしＦ３＝０（非比較モード）であれば、ステップＧ１８へ進み、動作経路、即ち移動目標点と運動形式（直線移動／円弧移動／各軸移動の別）をシミュレーション実行機会を表わす番号を付してメモリ１０２に記憶する。例えば、今回のシミュレーションがプログラム名「Ａ Ａ」を持つ動作プログラムに対する３回目のシミュレーションであれば、「ＧＩＭ０３ＡＡ」などとする。ステップＧ１８が完了したら、ステップＧ２２へ進む。
これに対して、もしＦ３＝１（比較モード）であればステップＧ１９へ進み、比較処理を実行する。比較処理は、今回の移動経路を参照経路と比較し、必要に応じて比較結果を記録するための処理で、ここでは第１実施例で利用したアルゴリズム（図３参照）が利用出来るので、詳細を繰り返さない。
但し、距離判定指標Δｄ、姿勢差判定指標Δｆ，Δｇ，Δｈの内の少なくとも１つの符号が＋である場合、即ち、基準を越える経路の差異があると判断された場合には、実動作のモードを無効化するためにフラグＦ４を１に反転する（ステップＧ２１）。
続くステップＧ２２では、ステップＳ１２で一時記憶しておいたデータ（移動目標点その他の動作条件データ）を読み出し、ステップＳ６へ進む。
既述の通り、フラグＦ２の値が“０”であれば、フラグＦ４の値如何に関わらず、目標点到達の擬制の処理（ステップＧ１１）を実行してステップＧ２へ戻る（実動作なし）。
これに対して、フラグＦ２の値が“１”である場合には、フラグＦ４の値が“０”である場合に限り、ステップＧ７からステップＧ８以下へ進み、ロボット実機の移動の処理を実行する。
即ち、Ｆ４＝１である場合には、その経路についての実動作を行なわず、ステップＧ２３へ進んで、比較処理（ステップＧ１９）の結果を教示操作盤１０４付属のディスプレイに表示して処理を終了する。何故ならば、フラグＦ４が“１”に反転しているということは、ステップＧ１９，Ｇ２０において、これから実動作に移ろうとする経路（再生運転経路）と参照経路との間に基準を越える経路の差異が見い出されたことを意味しているので、実動作を続行（最初の経路であれば開始）することは、誤教示に起因する危険を招く可能性があるからである。換言すれば、シミュレーションに含まれる比較処理の結果を表わす出力を利用することによって、誤教示に起因する危険が未然に回避される。
また、Ｆ４＝１が原因でロボットが停止した場合、その位置は誤教示の可能性のある経路の手前であるから、ステップＧ２３で表示される結果、特に基準オーバ内容を知らせるメッセージを参考にして、誤教示の修復など必要な措置を速やかにとる上で有利となる。
本発明によれば、オフフラインのシミュレーションシステムを使用せず、且つ、ロボット本体の実動作を伴わずに再生運転を行なうことで、安全を損ねる可能性のある誤教示経路を発見出来る。また、実動作を伴う再生運転の開始により、再生経路を参照経路と比較しながら実動作を進め、誤教示による危険発生の可能性が発見された場合に実動作のモードを無効化してロボットを停止させることが出来る。従って、教示作業の安全性と効率が高められる。

Claims (18)

1. ロボットの動作経路をシミュレーションする機能を有するロボット制御装置であって、
   安全を確認済みの第１の動作経路を参照動作経路として記述する第１の動作プログラムを格納する手段と、
   前記ロボットを実際に動作させることなく、前記第１の動作プログラムを再生する手段と、
   前記第１の動作プログラムの再生時に、前記第１の動作経路を示すデータを記憶する経路記憶手段と、
   第２の動作経路を記述する第２の動作プログラムの再生時に、前記第２の動作経路を示すデータを前記第１の動作経路を示すデータと比較する経路比較手段と、
   前記第２の動作経路と前記第１の動作経路との差異を評価する評価手段と、
   前記評価手段による評価結果を出力する出力手段とを備える、ロボット制御装置。
2. 前記経路比較手段は、前記第２の動作経路と前記第１の動作経路中のロボットの位置及び姿勢を比較し、前記評価手段は、ロボットの位置及び姿勢を考慮して前記第２の動作経路と前記第１の動作経路の差異を評価する、請求の範囲第１項に記載のロボット制御装置。
3. 前記出力手段により出力された評価結果を表示する表示手段を更に備える、請求の範囲第１項に記載のロボット制御装置。
4. 前記出力手段から、前記第２動作経路と前記第１の動作経路の間に基準を越える差異があることを示す評価結果が出力されたときに、所定のメッセージを出力する手段を更に備える、請求の範囲第１項に記載のロボット制御装置。
5. ロボットの動作経路をシミュレーションする機能を有するロボット制御装置であって、
   安全を確認済みの第１の動作経路を参照動作経路として記述する第１の動作プログラムを格納する手段と、
   前記ロボットを実際に動作させることなく、前記第１の動作プログラムを再生する手段と、
   前記第１の動作プログラムの再生時に、前記第１の動作経路を示すデータを補間点データを含めて記憶する経路記憶手段と、
   第２の動作経路を記述する第２の動作プログラムの再生時に、前記第２の動作経路を示すデータを前記第１の動作経路を示すデータと比較する経路比較手段と、
   前記第２の動作経路と前記第１の動作経路との差異を評価する評価手段と、
   前記評価手段による評価結果を出力する出力手段とを備える、ロボット制御装置。
6. 前記経路比較手段は、前記第２の動作経路と前記第１の動作経路中のロボットの位置及び姿勢を比較し、前記評価手段は、ロボットの位置及び姿勢を考慮して前記第２の動作経路と前記第１の動作経路の差異を評価する、請求の範囲第５項に記載のロボット制御装置。
7. 前記出力手段により出力された評価結果を表示する表示手段を更に備える、請求の範囲第５項に記載のロボット制御装置。
8. 前記出力手段から、前記第２の動作経路と前記第１の動作経路の間に基準を越える差異があることを示す評価結果が出力されたときに、所定のメッセージを出力する手段を更に備える、請求の範囲第５項に記載のロボット制御装置。
9. ロボットの動作経路をシミュレーションする機能を有するロボット制御装置であって、
   安全を確認済みの第１の動作経路を参照動作経路として記述する第１の動作プログラムを格納する手段と、
   前記ロボットを実際に動作させて、前記第１の動作プログラムを再生する手段と、
   前記第１の動作プログラムの再生時に、前記第１の動作経路を示すデータを記憶する経路記憶手段と、
   第２の動作経路を記述する第２の動作プログラムの再生時に、前記第２の動作経路を示すデータを前記第１の動作経路を示すデータと比較する経路比較手段と、
   前記第２の動作経路と前記第１の動作経路との差異を評価する評価手段と、
   前記評価手段による評価結果を出力する出力手段とを備える、ロボット制御装置。
10. 前記経路比較手段は、前記第２の動作経路と前記第１の動作経路中のロボットの位置及び姿勢を比較し、
    前記評価手段は、ロボットの位置及び姿勢を考慮して前記第２の動作経路と前記第１の動作経路の差異を評価する、請求の範囲第９項に記載のロボット制御装置。
11. 前記出力手段により出力された評価結果を表示する表示手段を更に備える、請求の範囲第９項に記載のロボット制御装置。
12. 前記出力手段から、前記第２の動作経路と前記第１の動作経路の間に基準を越える差異があることを示す評価結果が出力されたときに、所定のメッセージを出力する手段を更に備える、請求の範囲第９項に記載のロボット制御装置。
13. 前記出力手段から、前記第２の動作経路と前記第１の動作経路の間に基準を越える差異があることを示す評価結果が出力されたときに、前記ロボットの実際の動作を無効化する実動作無効化手段を更に備える、請求の範囲第９項に記載のロボット制御装置。
14. ロボットの動作経路をシミュレーションする機能を有するロボット制御装置であって、
    安全を確認済みの第１の動作経路を参照動作経路として記述する第１の動作プログラムを格納する手段と、
    前記ロボットを実際に動作させて、前記第１の動作プログラムを再生する手段と、
    前記第１の動作プログラムの再生時に、前記第１の動作経路を示すデータを補間点データを含めて記憶する経路記憶手段と、
    第２の動作経路を記述する第２の動作プログラムの再生時に、前記第２の動作経路を示すデータを前記第１の動作経路を示すデータと比較する経路比較手段と、
    前記第２の動作経路と前記第１の動作経路との差異を評価する評価手段と、
    前記評価手段による評価結果を出力する出力手段とを備える、ロボット制御装置。
15. 前記経路比較手段は、前記第２の動作経路と前記第１の動作経路中のロボットの位置及び姿勢を比較し、
    前記評価手段は、ロボットの位置及び姿勢を考慮して前記第２の動作経路と前記第１の動作経路の差異を評価する、請求の範囲第１４項に記載のロボット制御装置。
16. 前記出力手段により出力された評価結果を表示する表示手段を更に備える、請求の範囲第１４項に記載のロボット制御装置。
17. 前記出力手段から、前記第２の動作経路と前記第１の動作経路の間に基準を越える差異があることを示す評価結果が出力されたときに、所定のメッセージを出力する手段を更に備える、請求の範囲第１４項に記載のロボット制御装置。
18. 前記出力手段から、前記第２の動作経路と前記第１の動作経路の間に基準を越える差異があることを示す評価結果が出力されたときに、前記ロボットの実際の動作を無効化する実動作無効化手段を更に備える、請求の範囲第１４項に記載のロボット制御装置。

Images