JPH07104817A

JPH07104817A - ロボットの動作軌跡確認装置

Info

Publication number: JPH07104817A
Application number: JP5269560A
Authority: JP
Inventors: Koji Yoshimi; 光二吉見; Akira Shimogoe; 昭下越; Seiji Ogawa; 省二小川
Original assignee: Toyoda Koki KK
Current assignee: Toyoda Koki KK
Priority date: 1993-09-30
Filing date: 1993-09-30
Publication date: 1995-04-21

Abstract

(57)【要約】【目的】円弧補間の動作経路のトレースにおいて、現在
位置と動作プログラムとの対応関係が外れても、動作軌
跡の正確なトレースを可能とする。【構成】指定された移動先教示点が円弧上の点である場
合には、動作プログラムを順方向（動作プログラムの実
行される方向）に検索することにより、移動先教示点が
存在する教示された円弧を発生させるための連続した３
つの円弧教示点が決定される。トレース開始指令が付与
され、現在位置が特定円弧上に存在する場合には、移動
方向に沿って特定円弧上を移動先教示点まで移動させ
る。この状態で、現在位置と動作プログラムとは完全に
対応がとれていることになるので、その後に、移動先教
示点の指定が変化しても、動作点をトレース開始指令に
基づいて動作プログラムによって決定される動作軌跡上
を移動させることができる。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、ロボットのプログラム
された動作軌跡の確認を容易に行うための装置に関す
る。

【０００２】

【従来の技術】従来、ロボットの位置決めの対象となる
動作点（例えば、トーチの先端点）を所望の経路に沿っ
て、順次、移動させて教示を行い、それらの教示点に対
して直線で移動させたり、円弧で移動させたりする等の
動作プログラムを作成している。このようにして出来た
動作プログラムによる動作点の移動の様子を予めチェッ
クして、所望の経路に沿った移動が行われるように教示
されているかを確認することが行われる。

【０００３】この確認操作においては、ジョグ運転等に
より動作点を動作軌跡上から外したり、動作軌跡上に戻
したり、あるいは、現在位置から離れて、動作プログラ
ム上の任意の教示点まで移動させたりして教示点の修正
や動作軌跡の確認を行う必要がある。この場合、教示点
間の移動が直線で与えられている場合には、直線上に沿
って戻したり、指定の移動先教示点まで直線で移動させ
て、その後、直線に沿って前進、後退させることは容易
である。何故ならば、直線補間による移動命令（ＭＯＶ
ＥＳ）は、目標教示点を１つ指定して、現在位置からそ
の目標教示点まで直線で移動させる命令語であるので、
教示点と直線との関係が一対一で決定されるからであ
る。

【０００４】

【発明が解決しようとする課題】一方、円弧補間による
移動命令語（ＭＯＶＥＣ）は、１つの目標教示点を指定
するだけであり、現在位置からその目標教示点まで円弧
に沿って移動させる命令語である。しかし、円弧を決定
するには、円弧の開始点、中間点、終了点と３点必要で
あり、現在位置から円弧補間の移動命令語が２つ連続し
て、１つの円弧を与えることになる。よって、円弧補間
の場合には、円弧上の教示点がどの円弧に属するかを決
定することが必要となる。しかし、複数の円弧が連続す
る場合には、上記の理由により動作命令語を指定しただ
けでは、その動作命令語により指定された教示点がどの
円弧に属するかを判定することはできない。

【０００５】このため、指定された移動先の教示点へ戻
る場合には、現在位置が円弧上にあってもなくても、移
動先教示点へ直線で戻すようにしていた。又、その動作
点が移動先教示点に至った後の移動も、教示点と円弧と
の対応関係が把握できないため、動作点が円弧と直線と
の接点となっている円弧の端点の教示点まで移動するま
では、やはり、教示点間を直線で移動させるか、あるい
は強制的に前後の教示点との間で円弧を発生させて移動
させるしかなかった。

【０００６】よって、直線補間では教示された円弧のト
レースができないのは勿論、円弧補間で移動させても、
その移動軌跡は教示した円弧に等しいとは限らず、正確
な動作軌跡のトレースができなかった。

【０００７】本発明は上記の課題を解決するために成さ
れたものであり、その目的は、円弧補間を含む動作プロ
グラムの示す動作経路のトレースにおいて、現在位置と
動作プログラムとの対応関係が外れても、動作軌跡の正
確なトレースを可能とすることである。

【０００８】

【課題を解決するための手段】請求項１の発明は、主と
して、トレース開始指令が付与され、且つ、移動先教示
点が円弧上の点である場合には、動作プログラムを順方
向に検索することにより、移動先教示点が存在する教示
された円弧を発生させるための連続した３つの円弧教示
点を決定する円弧教示点決定手段と、動作点を特定円弧
上を移動先教示点から移動させるトレース手段とを設け
たことである。

【０００９】又、請求項２の発明は、主として、トレー
ス開始指令が付与された場合で、且つ、移動先教示点が
円弧上の点である場合には、動作プログラムを順方向に
検索することにより、移動先教示点が存在する教示され
た円弧を発生させるための連続した３つの円弧教示点を
決定する円弧教示点決定手段と、トレース開始指令が付
与された場合には、動作点の現在位置が３円弧教示点で
特定される特定円弧上に存在するか否かを判定する判定
手段と、現在位置が特定円弧上に存在する場合には、ト
レース開始指令の指令する移動方向に沿って、動作点を
特定円弧上を移動先教示点まで移動させる第１トレース
手段と、現在位置が特定円弧上に存在しない場合には、
動作点をトレース開始指令の指令する移動方向に応じ
て、所定の経路で移動先教示点まで移動させる第２トレ
ース手段とを設けたことである。

【００１０】請求項３〜５の発明は、第２トレース手
段、即ち、現在位置が特定円弧上に存在しない場合にお
いて、指定された移動先教示点までの移動経路に特徴が
ある。即ち、請求項２の発明は、第２トレース手段を、
動作点を現在位置から移動先教示点まで直線で移動させ
る手段としたことである。

【００１１】又、請求項４の発明は、第２トレース手段
を、動作点を現在位置から最も近い特定円弧上の点に直
線で移動させた後、特定円弧上に沿って移動先教示点ま
で特定円弧上を移動させる手段としたことである。

【００１２】さらに、請求項５の発明は、第２トレース
手段を、動作点を現在位置から近い方の特定円弧の端点
まで直線で移動させた後、特定円弧上に沿って移動先教
示点まで特定円弧上を移動させる手段としたことであ
る。

【００１３】

【作用】請求項１の発明では、トレース開始指令が付与
され、指定された移動先教示点が円弧上の点である場合
には、動作プログラムを順方向（動作プログラムの実行
される方向）に検索することにより、移動先教示点が存
在する教示された円弧を発生させるための連続した３つ
の円弧教示点が決定される。即ち、動作プログラムの先
頭に戻るか、又は、一旦、移動先教示点を指定している
移動命令語から逆方向に、円弧の区切りを与える命令
語、例えば、直線補間による移動命令語まで逆上る。そ
して、その位置から、順方向に、移動先教示点を指定し
ている命令語まで、同一円弧となる円弧補間の命令語を
組分けて行く。この操作により、移動先教示点の属する
特定円弧及びその特定円弧を形成する３教示点が判明す
る。

【００１４】上記の操作により移動先教示点と動作プロ
グラムとの対応がとれたことになるので、動作点を移動
先教示点から動作プログラムによって指定された動作軌
跡に沿って移動させることができる。

【００１５】又、請求項２の発明では、同様に特定円弧
を形成する３教示点が判明する。次に、トレース開始指
令が付与された場合には、まず、動作点の現在位置が特
定円弧上に存在するか否かが判定される。現在位置が特
定円弧上に存在する場合には、トレース開始指令の指令
する移動方向に応じて、移動方向に沿った最初の教示点
へ動作点を特定円弧上を移動させ、その最初の教示点が
移動先教示点と異なる場合には更に特定円弧上を移動先
教示点まで移動させる。このようにして、円弧に沿った
移動先教示点までの移動が完了する。この状態で、現在
位置と動作プログラムとは完全に対応がとれていること
になるので、その後に、移動先教示点の指定が変化して
も、動作点をトレース開始指令に基づいて動作プログラ
ムによって決定される動作軌跡上を移動させることがで
きる。

【００１６】又、請求項３の発明では、動作点は現在位
置から移動先教示点まで直線で移動し、請求項４の発明
では、動作点は現在位置から最も近い特定円弧上の点に
直線で移動した後、特定円弧上に沿って移動先教示点ま
で特定円弧上を移動し、請求項５の発明では、動作点は
現在位置から近い方の特定円弧の端点まで直線で移動し
た後、特定円弧上に沿って移動先教示点まで特定円弧上
を移動する。

【００１７】

【発明の効果】請求項１の発明では、移動先教示点が存
在する特定円弧が動作プログラムの順方向の検索により
決定される。よって、動作点が移動先教示点に位置決め
された後は、現在位置と動作プログラムとが正確に対応
がとれるため、移動先教示点から動作点の動作プログラ
ムで指令される円弧を順方向、逆方向にトレースするこ
とが可能となる。

【００１８】又、請求項２の発明では、移動先教示点が
存在する特定円弧が動作プログラムの順方向の検索によ
り決定される。よって、動作点が移動先教示点に位置決
めされた後は、現在位置と動作プログラムとが正確に対
応がとれるため、現在位置の存在する円弧、更に、変更
した移動先教示点の存在する円弧を正確に発生すること
ができる。よって、動作軌跡が円弧である場合のトレー
スを正確に行うことができる。特に、現在位置が移動先
教示点の存在する特定円弧上に存在する場合には、現在
位置から正確に円弧に沿って、順方向、逆方向にトレー
スさせることができる。

【００１９】又、請求項３の発明では、動作点は現在位
置から移動先教示点まで直線で移動するので、作業者が
正確なトレースを要求している円弧部分を単時間で割り
出すことができる。又、請求項４の発明では、動作点は
現在位置から最も近い特定円弧上の点に直線で移動した
後、特定円弧上に沿って移動先教示点まで特定円弧上を
移動するので、経路の中間でその特定円弧のトレースが
できるので、使用性が向上する。更に、請求項５の発明
では、動作点は現在位置から近い方の特定円弧の端点ま
で直線で移動した後、特定円弧上に沿って移動先教示点
まで特定円弧上を移動するので、同様に、早期の段階で
特定円弧のトレースが可能となるので、使用性が向上す
る。

【００２０】

【実施例】以下本発明を具体的な実施例に基づいて説明
する。第１実施例図１は６軸多関節ロボットの機構を示した機構図であ
る。１０がロボット本体であり、フロアに本体１０を固
定するベース１３が配設され、ベース１３上にはコラム
１２が固設されており、コラム１２はボディ１４を回転
自在に配設している。ボディ１４はアッパーアーム１５
を回動自在に軸支し、アッパーアーム１５は、フォアア
ーム１６を回動自在に軸支している。ボディ１４、アッ
パーアーム１５、フォアアーム１６は、それぞれ、サー
ボモータSm１，Sm２，Sm３（図２参照）によって、軸
ａ，ｂ，ｃの回りに回転駆動される。この回転角はエン
コーダＥ１，Ｅ２，Ｅ３によって検出される。フォアア
ーム１６の先端部にはツイストリスト１７がｄ軸の周り
に回転可能に軸支され、ツイストリスト１７にはベンド
リスト９がｅ軸の周りに回動自在に軸支されている。ベ
ンドリスト９には先端にフランジ１８ａを有するスイベ
ルリスト１８がｆ軸の回りに回転可能に軸支されてい
る。また、フランジ１８ａにはハンド１９が取り付けら
れている。ｄ軸、ｅ軸、ｆ軸はサーボモータSm４、Sm
５、Sm６によって駆動され、その回転角はエンコーダＥ
４，Ｅ５，Ｅ６によって検出される。ハンド１９の開閉
動作は工具駆動回路２３により制御される。

【００２１】図２は、本発明の動作軌跡確認装置の電気
的構成を示したブロックダイヤグラムである。ＣＰＵ２
０には、メモリ２５、サーボモータを駆動するためのサ
ーボＣＰＵ２２ａ〜２２ｆ、動作開始指令、ジョグ運転
の指令、教示点の指示等を行う操作盤２６が接続されて
いる。操作盤２６は、動作プログラムの各命令語を上下
スクロールさせながら、同時に、５行程表示できる表示
画面２６５、動作軌跡に沿って動作点を順方向、逆方向
に、それぞれ、移動させることを指令する順方向トレー
ス開始キー２６１、逆方向トレース開始キー２６２、表
示画面２６５上のカーソルを上、下に、それぞれ、移動
させて動作プログラムの命令語を指定できる移動先教示
点指定キー２６３、２６４、ジョグ運転の指令を与える
ジョグキー２６６とを有している。尚、順方向とは、動
作プログラムの解読順序、即ち、教示された順序に動作
軌跡をトレースする方向を言い、逆方向とは、順方向と
は逆の方向にトレースする方向を言う。又、ロボットに
取付けられた各軸ａ〜ｆ駆動用のサーボモータSm１〜Sm
６は、それぞれサーボＣＰＵ２２ａ〜２２ｆによって駆
動される。

【００２２】サーボＣＰＵ２２ａ〜２２ｆのそれぞれ
は、ＣＰＵ２０から出力される各軸の角度指令値θ₁〜
θ₆、慣性モーメントＤ_i、重力トルクＴ_iに基づい
て、サーボモータSm１〜Sm６の出力トルクを制御する。
各駆動軸に連結されたエンコーダＥ１〜Ｅ６の出力する
検出角度α₁〜α₆はＣＰＵ２０及びサーボＣＰＵ２２
ａ〜２２ｆに入力しており、ＣＰＵ２０による各軸の慣
性モーメント及び重力トルクの演算及びサーボＣＰＵ２
２ａ〜２２ｆによる位置フィードバック制御、速度フィ
ードバック制御、電流フィードバック制御に用いられ
る。

【００２３】メモリ２５にはロボットを教示点データに
従って動作させるための動作プログラムが記憶されたＰ
Ａ領域とハンド１９の位置と姿勢を表す教示点データを
記憶するＰＤＡ領域と操作盤２６により指定された移動
先教示点の座標を記憶するＳＤＡ領域、動作点の現在位
置を記憶するＩＴＡ領域、補間演算により求められた補
間点における各軸の角度指令値θ₁〜θ₆を記憶するＩ
ＮＡ領域とエンコーダＥ１〜Ｅ６から出力された検出角
度α₁〜α₆を記憶するＡＮＧ領域とが形成されてい
る。

【００２４】次に、本装置の作動について説明する。図
４はＲＡＭ２５のＰＡ領域に記憶されている動作プログ
ラムである。この動作プログラムにより、ロボットの動
作点（ハンドの先端）は図５に示すように移動する。行
番号１は、停止位置（開始位置）から目標速度へ移行す
る加速過程及び目標速度から停止位置（目標位置）へ移
行する減速過程における加速度及び減速度（機械の出力
し得る最大加速度Ａに対する割合１００ｃ％（図４の場
合には１００ｃ＝１００％））を指令する命令語であ
る。行番号２は、目標速度（機械の出力し得る最大速度
Ｖに対する割合１００ｂ％（図４の場合には１００ｂ＝
１００％））である。行番号３は動作点を作業原点に位
置決めする命令語であり、行番号４は教示点Ｐ１に直線
補間により移動させるＭＯＶＥ命令語である。行番号５
〜１０は、教示点Ｐ２〜Ｐ７に、順次、円弧補間により
移動させるＭＯＶＥＣ命令語である。行番号１１は教示
点Ｐ８に直線補間により移動させるＭＯＶＥ命令語であ
る。行番号１２は動作点を作業原点に位置決めする命令
語である。

【００２５】図４の動作プログラムにより、ロボットの
動作点は、図５に示すように、原点からＰ１点まで直線
で、その後、Ｐ２〜Ｐ７点まで円弧上を移動し、Ｐ７点
からＰ８点までは直線で移動し、原点に戻る。図５にお
いて、Ｐ１〜Ｐ８点が教示点である。

【００２６】図３は、ＣＰＵ２０による動作プログラム
を解読するための主プログラムのフローチャートであ
る。ステップ１００において、ＭＯＶＥ命令語が解読さ
れると、ステップ１０２において、現在位置から指定さ
れた教示点まで動作点を移動させるために直線補間によ
る演算が実行される。そして、補間演算により求められ
た各軸の角度指令値θ₁〜θ₆はサーボＣＰＵ２２ａ〜
２２ｆに出力される。

【００２７】ステップ１０４において、ＭＯＶＥＣ命令
語が解読されると、ステップ１０６において、円弧補間
による演算が実行される。円弧補間は、現在位置Ｐ１と
ＭＯＶＥ命令語の後の最初に現れたＭＯＶＥＣ命令後に
よる教示点Ｐ２と、次のＭＯＶＥＣ命令後による教示点
Ｐ３の３教示点が円弧を特定する教示点となる。そし
て、この３教示点で特定された円関数が生成される。そ
して、円弧の中心点、半径、円弧の中心角が演算され、
中心角を等分割して円弧上の補間点が決定される。命令
語〔ＭＯＶＥＣＰ２〕の実行により、動作点は円弧Ｑ
１上を教示点Ｐ１からＰ２まで移動する。次の命令語
〔ＭＯＶＥＣＰ３〕の解読時には、同様に、教示点Ｐ
１、Ｐ２、Ｐ３が円弧Ｑ１を特定する３教示点と判定さ
れ、同一の円弧Ｑ１が用いられる。

【００２８】次の命令語〔ＭＯＶＥＣＰ４〕の解読時
には、既に、２つの連続したＭＯＶＥＣ命令語が現れた
ので、教示点Ｐ４は次の別の円弧Ｑ２上の中間点と決定
される。この時、１つ前の命令語〔ＭＯＶＥＣＰ３〕
で特定される教示点Ｐ３が円弧Ｑ２の開始点、１つ次の
命令語〔ＭＯＶＥＣＰ５〕で特定される教示点Ｐ５が
円弧Ｑ２の終了点と決定され、教示点Ｐ３、Ｐ４、Ｐ５
が円弧Ｑ２を特定する３教示点と判定される。以下同様
に、教示点Ｐ５、Ｐ６、Ｐ７が次の円弧Ｑ３を特定する
３教示点と判定される。

【００２９】ステップ１０８でＨＡＮＤＯＦＦ命令語
が解読されると、ステップ１１０において、工具駆動回
路２３にハンド１９を開く指令を与える。又、ステップ
１１２でＨＡＮＤＯＮ命令語が解読されると、ステッ
プ１１４において、工具駆動回路２３にハンド１９を閉
じる指令を与える。

【００３０】ステップ１１６においてACCEL 又はSPEED
命令語であると判定された場合には、ステップ１１８に
おいて加速度の指令値ｃ、目標速度の指令値ｂがＲＡＭ
２５に設定される。

【００３１】ステップ１０２及び１０６における教示点
間の補間演算は、良く知られたように、回転主軸法等を
用いて行うことができる（例えば、特開昭６２−１５４
００６号）。

【００３２】以上のような動作プログラムの実行は、操
作盤２６の指令により、連続的に実行することもできれ
ば、１行毎に実行することもできる。又、操作盤２６の
指令により順方向にしばらく実行して、次に、逆方向に
実行することもできる。さらに、操作盤２６の操作によ
り、このような実行において任意位置で一旦停止させ
て、ジョグ運転キー２６６を操作して、プログラムされ
た動作軌跡から外れて任意の位置に動作点を移動させる
こともできる。

【００３３】さらに、動作プログラム上で移動先教示点
を指定して、動作点を現在位置からその移動先教示点ま
で所定の経路で移動させることができる。即ち、プログ
ラムされた動作軌跡をトレースすることができる。次
に、この処理手順について説明する。

【００３４】図６はＣＰＵ２０によるその処理手順を示
したフローチャートである。ステップ２００において、
操作盤２６の各種のキーの操作状態が入力され、ステッ
プ２０２において、そのキー入力に応じて、動作プログ
ラムの命令語が表示盤２６５に表示され、実行行にはカ
ーソル表示が行われる。カーソルは移動先教示点設定キ
ー２６３、２６４の操作により移動させ、又、そのキー
の操作により動作プログラムを逆方向及び順方向にスク
ロールする処理が実行される。

【００３５】次に、ステップ２０４にて、トレース開始
キー２６１、２６２が押下されたか否かが判定され、そ
のキー２６１、２６２が押下されるまで、ステップ２０
０〜２０２により、表示盤２６５の命令語の表示及びカ
ーソルの位置をキー操作に応じて変化させることかでき
る。

【００３６】ステップ２０４でトレース開始キー２６
１、２６２が押下されたと判定されると、ステップ２０
６において、指定された移動先教示点の命令語が円弧補
間命令語（ＭＯＶＥＣ）か否かを判定することで、移動
先教示点が円弧上に存在するか否かが判定される。移動
先教示点が円弧上に存在する場合には、ステップ２０８
でその移動先教示点の存在する円弧（特定円弧）及びそ
の円弧を特定する３教示点が決定される。これは、上述
したように、動作プログラムの先頭から、或いは、円弧
の切れ目、即ち、移動先教示点の命令語から直線補間命
令語まで逆方向に戻り、その位置から順方向に命令語を
順次解読して、連続する２つの円弧補間命令語の組を発
生させることで、円弧を特定することができる。

【００３７】この円弧を特定する３つの教示点を、順方
向に、第１端点Ｗ１、中間点Ｗ２、第２端点Ｗ３と言
う。移動先教示点を含む３つの教示点はＲＡＭ２５のＳ
ＤＡ領域に記憶される。

【００３８】尚、移動先教示点が連続する２つの円弧の
接続点である場合であり、且つ、逆方向トレースが指令
されたときは移動先教示点を第１端点Ｗ１とする円弧が
特定円弧と決定され、又、順方向トレースが指令された
ときは移動先教示点を第２端点Ｗ３とする円弧が特定円
弧と決定される。

【００３９】例えば、図７に示すように、教示点Ｐ４が
移動先教示点と指定された場合には、図４の動作プログ
ラムにおいて、連続したＭＯＶＥＣの先頭、即ち、第５
行目のＭＯＶＥＣに戻り、その命令語から順方向に円弧
を決定して行く。すると、Ｐ１Ｐ２Ｐ３は円弧Ｑ１、Ｐ
３Ｐ４Ｐ５が円弧Ｑ２と決定できる。よって、移動先教
示点Ｐ４は、円弧Ｑ２の中間点Ｗ２であり、教示点Ｐ３
が第１端点Ｗ１、教示点Ｐ５が第２端点Ｗ３と決定でき
る。

【００４０】又、教示点Ｐ３が移動先教示点に設定され
ると、教示点Ｐ３は円弧Ｑ１の第２端点Ｗ３でもあり、
円弧Ｑ２の第１端点Ｗ１でもある。よって、この場合に
は、移動先教示点に対して逆方向トレースが指令された
場合には移動先教示点Ｐ３は円弧Ｑ２に属すると判定さ
れ、円弧Ｑ２が特定円弧となる。一方、移動先教示点に
対して順方向トレースが指令された場合には移動先教示
点Ｐ３は円弧Ｑ１に属すると判定され、円弧Ｑ１が特定
円弧となる。

【００４１】このようにして決定された３教示点の座標
を用いて、円弧を表す円の方程式が決定され、その円弧
の中心座標、半径、円弧の存在する平面の法線ベクト
ル、中心角が演算され、それらの値はＲＡＭ２５のＳＤ
Ａ領域に記憶される。

【００４２】ステップ２１０において、ＲＡＭ２５のＡ
ＮＧ領域に記憶されている各軸の角度α₁〜α₆から動
作点の座標変換によりワールド座標系における現在位置
が演算され、その値はＲＡＭ２５のＩＴＡ領域に記憶さ
れる。

【００４３】次に、ステップ２１２において、現在位置
が上記で決定された特定円弧上に存在するか否かが判定
される。この判定は、現在位置が先ず特定円弧を一部と
する円上に存在するか否かで判定される。この判定条件
は、第１に特定円弧の中心と現在位置との距離が特定円
弧の半径に等しいか否か、第２に特定円弧の中心から現
在位置へのベクトルが特定円弧の法線ベクトルに垂直か
否かである。

【００４４】次に、現在位置が円上に存在する場合に
は、円弧上に存在するか否かを判定することが必要であ
る。この判定は次のようにして行われる。図８に示すよ
うに、特定円弧Ｑにおいて、円弧中心Ｏを原点とする第
１端点Ｗ１の位置ベクトルＶ１に対する中間点Ｗ２の位
置ベクトルＶ２、第２端点Ｗ３の位置ベクトルＶ３の成
す角φ_1,φ₂が、それぞれ、演算される。これはベクト
ルの内積演算により容易に求めることができる。そし
て、φ_1,φ₂は、常に、０＜φ₁＜φ₂＜２πを満たす
値と定義することで、一意的に決定される。次に、同様
に第１端点Ｗ１の位置ベクトルＶ１に対する現在位置Ｓ
の位置ベクトルＶの成す角β₁と、第２端点Ｗ３の位置
ベクトルＶ３に対する現在位置Ｓの位置ベクトルＶの成
す角β₂が演算される。この角は、常に、正値として定
義する。この時、β₁＋β₂＝φ₂の等式が成立すれ
ば、現在値は特定円弧Ｑ上に存在すると判定することが
できる。

【００４５】このようにして、現在位置が特定円弧上に
存在すると判定されると、ステップ２１４において、現
在位置と移動先教示点との間に中間点Ｗ２が存在するか
否かが判定される。例えば、図８では、逆方向トレース
が指令された場合には第１端点Ｗ１が移動先教示点とな
るが、この場合には現在位置Ｓと移動先教示点との間に
中間点Ｗ２が存在する。一方、順方向トレースが指令さ
れた場合には第２端点が移動先教示点となるが、この場
合には現在位置Ｓと移動先教示点との間には中間点Ｗ２
は存在しない。この判定のためには、中間点Ｗ２と現在
位置Ｓの角度関係を判定すれば良い。即ち、φ₁＜β₁
の場合には現在位置Ｓは中間点Ｗ２と第２端点Ｗ３との
間に存在し、β₁≦φ₁の場合には現在位置Ｓは第１端
点Ｗ１と中間点Ｗ２との間に存在する。後は、移動先教
示点が第１端点Ｗ１か、中間点Ｗ２か、第２端点Ｗ３か
という条件とトレースの方向とによって、上記のことを
決定することができる。

【００４６】次に、ステップ２１４の判定がYes の場合
には、ステップ２１６、２１８で、動作点を現在位置Ｓ
から中間点Ｗ２まで円弧補間で移動させ、さらに、中間
点Ｗ２から移動先教示点まで円弧補間で移動させる。こ
の円弧補間は、良く知られたように、特定円弧Ｑの存在
する平面を求め、その平面上における回転による座標変
換により容易に補間点のワールド座標を求めることがで
きる。

【００４７】又、ステップ２１４の判定結果がNoの場合
には、ステップ２２０において、動作点を、現在位置Ｓ
から移動先教示点へ円弧補間により移動させる。

【００４８】一方、ステップ２１２で、現在位置が特定
円弧上に存在しないと判定された場合には、ステップ２
２２へ移行して、２つの端点のうち、現在位置に近い方
の端点が検出される。そして、ステップ２２４、２２６
において、動作点は、特定円弧上に存在しない現在位置
から、その現在位置に近い方の端点Ｗ１又はＷ３に直線
で移動される。そして、ステップ２２８で現在位置の座
標を移動後の値とした後、ステップ２１４以下の現在位
置が円弧上に存在する場合の処理が実行される。以上の
処理により、現在位置が移動先教示点の存在する特定円
弧上に存在する場合には、動作点はその円弧をトレース
しながら、移動先教示点まで、移動する。一方、現在位
置が特定円弧上に存在しない場合には、動作点はその円
弧の近い方の端点に直線で移動した後、その特定円弧を
トレースして移動先教示点まで移動する。

【００４９】例えば、図８で示すように、移動先教示点
が第１端点Ｗ１であり、現在位置Ｓが中間点Ｗ２と第２
端点Ｗ３との間に存在する場合には、動作点は特定円弧
上を中間点Ｗ２を経由して第１端点Ｗ１へと移動する。
又、現在位置Ｓ’が特定円弧上に存在しない場合には、
動作点は近い方の第２端点Ｗ３に直線Ｌで移動した後、
特定円弧上を中間点Ｗ２を経由して第１端点Ｗ１へと移
動する。

【００５０】このようにして、指定された移動先教示点
への位置決めが完了すると、ステップ２００に戻り、次
の移動先教示点の指定を待つ。尚、ステップ２０６で移
動先教示点が円弧上に存在しないと判定された場合、即
ち、直線上に存在する場合には、ステップ２３０で、動
作点は現在位置から移動先教示点まで直線で移動され
る。

【００５１】尚、第１実施例において、現在位置が特定
円弧上に存在しない場合には、現在位置と、第１端点、
中間点、第２端点のうち、最も近い点に直線により復帰
させるようにしても良い。さらに、移動先教示点へ直線
により復帰させるようにしても良い。

【００５２】さらに、現在位置が特定円弧上に存在しな
い場合には、操作盤２６の移動先教示点指定キー２６
３、２６４を用いて、作業者がカーソル移動により指定
した実行行に書かれている位置まで直線移動し、続いて
円弧動作を行うようにしても良い。

【００５３】第２実施例次に、第２実施例について説明する。ハードウエアの構
成は第１実施例と同一である。本実施例では、第１実施
例の図６にステップ２１２で現在位置が特定円弧上に存
在しない場合に、別の方法で特定円弧に復帰させること
を特徴としている。図９は、図６のステップ２１２にお
いて、Noと判定された時に実行される処理手順を示して
いる。ステップ３００において、現在位置に最近接の特
定円弧上の点が演算される。

【００５４】この演算は次のように行われる。図１０に
示すように、現在位置Ｓの円弧中心Ｏを基準とする位置
ベクトルＶが求められる。特定円弧Ｑの存在する平面Ｇ
の方程式は第１端点Ｗ１、中間点Ｗ２、第２端点Ｗ３の
座標を用いて求めることができる。次に、ベクトルＶを
平面Ｇへ射影したベクトルＶ’を求める。このベクトル
Ｖ’と円弧曲線との交点Ｈが現在位置Ｓに最も近い特定
円弧上の点となる。但し、交点Ｈ’が特定円弧上に存在
しない場合には、第１端点Ｗ１と第２端点Ｗ３のうち、
現在位置Ｓ’に近い方の点が最近接点として決定され
る。

【００５５】次に、ステップ３０２において、現在位置
と最近接点との距離が所定値以下か否かが判定され、判
定結果がYes の場合には、ステップ３０４において動作
点を最近接点まで直線移動させる。その後の処理は、図
６のステップ２２８へと続く。一方、ステップ３０２の
判定結果がNoの場合には、ステップ３０６で、動作点を
円弧上の最近接点に復帰させるか、移動先教示点へ直線
で復帰させるかを作業者に選択させる。現在位置が特定
円弧から遠く離れ過ぎている場合には、動作点に予想外
の動きをさせないために、作業者の判断が求められる。
最近接点が選択された場合には、ステップ３０４以下が
実行され、移動先教示点が選択された場合には、ステッ
プ３０８で、動作点を移動先教示点まで移動させる。

【００５６】以上述べた実施例においては、移動先教示
点まで各種の移動方法を設定しているが、本願発明の要
旨は移動先教示点の存在する円弧を特定することであ
り、その移動先教示点からその特定円弧に沿って動作点
を移動させることにある。従って、作業者が動作軌跡上
の教示点まで動作点を移動させた後、その点から正しい
円弧に沿ってトレースを行う構成としても良い。

【図面の簡単な説明】

【図１】本発明の具体的な実施例装置で使用されるロボ
ットを示した構成図

【図２】ロボットの制御装置の構成を示したブロック
図。

【図３】ＣＰＵによる命令語解読の主処理手順を示した
フローチャート。

【図４】動作プログラムを示した説明図。

【図５】動作経路を示した説明図。

【図６】ＣＰＵによる動作軌跡のトレース手順を示した
フローチャート。

【図７】動作軌跡のトレース手順を説明する説明図。

【図８】動作軌跡のトレース手順を説明する説明図。

【図９】他の実施例に係るＣＰＵによる動作軌跡のトレ
ース手順を示したフローチャート。

【図１０】動作軌跡のトレース手順を説明する説明図。

【符号の説明】

１０…ロボット１８…スイベルリスト１８ａ…フランジ１９…ハンド２０…ＣＰＵ（動作点演算手段、移動先教示点指定手
段、トレース指令手段、円弧教示点決定手段、判定手
段、第１トレース手段、第２トレース手段、第３トレー
ス手段）２６…操作盤２６１…順方向トレース開始キー（トレース指令手段）２６２…逆方向トレース開始キー（トレース指令手段）２６３、２６４…移動先教示点指定キー（移動先教示点
指定手段）２２ａ〜２２ｆ…サーボＣＰＵ２５…ＲＡＭ（動作プログラム記憶手段、教示点記憶手
段）ステップ２１０、２２８…動作点演算手段ステップ２００〜２０２…移動先教示点指定手段ステップ２０４…トレース指令手段ステップ２０８…円弧教示点決定手段ステップ２１２…判定手段ステップ２１４〜２２０…第１トレース手段ステップ２２２〜２２６…第２トレース手段ステップ３００〜３０８…第２トレース手段

Claims

【特許請求の範囲】

【請求項１】直線補間、円弧補間等を指令した動作プ
ログラムに沿ってロボットの動作軌跡を確認する装置に
おいて、前記動作プログラムを記憶した動作プログラム記憶手段
と、前記動作軌跡上の教示点の座標データを記憶した教示点
記憶手段と、動作点の現在位置を検出する動作点演算手段と、移動先教示点を指定する移動先教示点指定手段と、前記移動先教示点から動作軌跡に沿って、教示された順
序である順方向、又は、その逆順序である逆方向に移動
させるトレース開始指令を付与するトレース指令手段
と、前記トレース開始指令が付与された場合であり、且つ、
前記移動先教示点が円弧上の点である場合には、前記動
作プログラムを順方向に検索することにより、前記移動
先教示点が存在する教示された円弧を発生させるための
連続した３つの円弧教示点を決定する円弧教示点決定手
段と、前記動作点を前記特定円弧上を前記移動先教示点から移
動させるトレース手段とを有することを特徴とするロボ
ットの動作軌跡確認装置。
【請求項２】直線補間、円弧補間等を指令した動作プ
ログラムに沿ってロボットの動作軌跡を確認する装置に
おいて、前記動作プログラムを記憶した動作プログラム記憶手段
と、前記動作軌跡上の教示点の座標データを記憶した教示点
記憶手段と、動作点の現在位置を検出する動作点演算手段と、移動先教示点を指定する移動先教示点指定手段と、前記移動先教示点から動作軌跡に沿って、教示された順
序である順方向、又は、その逆順序である逆方向に移動
させるトレース開始指令を付与するトレース指令手段
と、前記トレース開始指令が付与された場合であり、且つ、
前記移動先教示点が円弧上の点である場合には、前記動
作プログラムを順方向に検索することにより、前記移動
先教示点が存在する教示された円弧を発生させるための
連続した３つの円弧教示点を決定する円弧教示点決定手
段と、前記トレース開始指令が付与された場合には、前記動作
点の現在位置が前記３円弧教示点で特定される特定円弧
上に存在するか否かを判定する判定手段と、前記現在位置が前記特定円弧上に存在する場合には、前
記トレース開始指令の指令する移動方向に沿って、前記
動作点を前記特定円弧上を前記移動先教示点まで移動さ
せる第１トレース手段と、前記現在位置が前記特定円弧上に存在しない場合には、
前記動作点を前記トレース開始指令の指令する移動方向
に応じて、所定の経路で前記移動先教示点まで移動させ
る第２トレース手段とを有することを特徴とするロボッ
トの動作軌跡確認装置。
【請求項３】請求項２に記載の動作軌跡確認装置にお
いて、前記第２トレース手段は、前記動作点を前記現在位置か
ら前記移動先教示点まで直線で移動させる手段である。
【請求項４】請求項２に記載の動作軌跡確認装置にお
いて、前記第２トレース手段は、前記動作点を、前記現在位置
から最も近い前記特定円弧上の点に直線で移動させた
後、前記特定円弧上に沿って前記移動先教示点まで前記
特定円弧上を移動させる手段である。
【請求項５】請求項２に記載の動作軌跡確認装置にお
いて、前記第２トレース手段は、前記動作点を、前記現在位置
から近い方の前記特定円弧の端点まで直線で移動させた
後、前記特定円弧上に沿って前記移動先教示点まで前記
特定円弧上を移動させる手段である。