WO1989008878A1 - Method of controlling tool attitude of a robot - Google Patents

Description

明 細 害

ロボッ トのツール姿勢制御方法

技 術 分 野

本発明はロボッ トのツール姿勢制御方法に関する。

背 景 技 術

手先部に装着したツールのポーズ （位置および姿勢） を制御して所要の作業を行えるようにしたロボッ ト は公 知である。 例えば、 互いに相対回転自在に連結した一連 のアームと、 手先部ひいてはツールが 3次元空間内で任 意の姿勢をとれるようにアーム先端に連結した手先部と を備える多閬節ロボッ ト が知られている。 該ロボッ ト は、 作業の始点および終点の位置ならびに始点および終点で のツールの姿勢を予め教示され、 運転時にこれら教示デ 一夕に基づく補間処理を実行して始点から終点にわたり ツールのポーズを制御する。

例えば、 ツールを始点から終点に向けて直線移動させ る場合、 ロボッ ト は、 始点および終点のそれぞれにおい て教示された姿勢に基づいて第 5図に示す 2つのパラメ 一夕 a l， α 2を算出する。 第 5図中、 符号 V 1 および V 2は始点 Ρ 1 および終点 Ρ 2でのツールの向きを表す アブローチベク トルを示し、 符号 V 1 ， はベク トル V 1 をその起点が終点 Ρ 2に合致するように平行移動して得 たベク トルを示す。 ノ、'ラメータ α 1 は、 ベク トル V 1 ， およびベク トル V 2を含む平面内において両べク トルが なす角度 （始点から終点までのツールの向きの変化量〉 を表し、 パラメ一タ α 2は始点と終点間でのツールのッ 一ル軸心回りの回転位箧の変化量を表す。 そして、 ロボ ットは、 始点 Ρ 1.と終点 Ρ 2間において、 これらパラメ ータ a 1 , a 2を直線補間してツールの姿勢を制御する。 しかしながら、 この方法によってツール姿勢を意図した とおりに制御することは困難である。

また、 第 6図に示すように、 ツールを円弧 2に沿って 始点 P 1から終点 P 2まで移動させる場合、 ロボヅ トは、 始点， 終点および中間点 P 3の位置ならびに始点および 終点でのツールの姿勢を教示され、 運転時、 ツールの移 動軌跡が円弧 2に合致するようにツールの位箧を制御す ると共にツールの姿勢を第 5図を参照して説明した方法 で制御する。 このため、 円弧とく に優弧に ½、つてツール を移動させる場合、 ツールを直線移動させる場合に比べ てツールの姿勢制御はよ り困難になる。

発 明 の 開 示

本発明の目的は、 始点から終点に向けたツールの移動 途中においてもツールの姿勢を意図したものに制御でき るロボッ トのツール姿勢制御方法を提供することにある。 上述の目的を達成するため、 本発明のツール姿勢制御 方法は、 作業の始点および終点でのツールの位置ならび に始点および終点でのツールの姿勢を予めロボッ トに教 示する工程 （ a ) と、 教示結果に従い、 始点においてッ ールと基準平面とがなす第 1 の角度と、 始点において基 準平面上に投影されたツ -ルと基準平面上に設定した基 準線とがなす第 2の角度と、 始点におけるツールのツー ル軸心回りの回転位匿とを算出する工程 （ b ) と、 教示 結果に従い、 終点における第 1 の角度， 第 2の角度およ びツール軸心回りの回転位置を算出する工程 （ c ) と、 ツールの位置， 第 1 の角度， 第 2の角度およびツール軸 心回りの回転位匿を始点と終点との間で補間する工程 ( d ) と、 補間結果に従ってツールの姿勢を制御するェ 程 （ e〉 とを備える。

上述のように、 本発明は、 予め教示した作業始点， 終 点でのツールの位箧， 姿勢に従い、 ツールと基準平面と がなす第 1 の角度， 基準平面上に投影されたツールと基 準平面上に設定した基準線とがなす第 2の角度およびッ 一ルの軸心回りの回転位置の、 始点および終点における 値を算出し、 ツールの位置， 第 1 の角度， 第 2の角度お よびツール軸心回りの回転位置を始点と終点との間で補 間するようにしたので、 ツールを直線または円弧に沿つ て移動させる場合、 ツールの姿勢を始点と終点間で常に 意図したものに制御できる。

図 面 の 簡 単 な 説 明

第 1 図はツールを直線移動させる場合での本発明の一 実施例によるツール姿勢制御方法における各種制御パラ メータを示す概略斜視図、 第 2図は本発明の方法が適用 されるロボッ ト を例示する要部プロック図、 第 3図は第 2図の制御装置が実行するツ -ル '姿勢制御プログラムの フローチャー ト、 第 4図はツールを円弧に沿って移動さ せる場合での第 1 図と同様の図、 第 5図は従来のツール 姿勢制御方法での制御パラメータを示す図、 および、 第 6図はツ -ル移動軌跡が円弧である場合での従来の方法 を示す概略斜視図である。

発明を実施するための最良の彤態

第 2図を参照すると、 本発明の一実施例によるツール 姿勢ポ一ズ制御方法が適用されかつ基本的には従来公知 のように構成されたロボッ トは、 作業対象 4 0に対して 所要の作業を行うためのロボッ ト機構部 3 0とこれを制 御するための制御装箧 2 0とを備えている。

- 図示を省略するが、 口ボッ ト機構部 3 0は、 例えば、 鬨節 （回転軸， 旋回軸， 直動軸) により互いに相対回転 自在または栢対直線移動自在に連結ざれた一連のァーム と、 その先端に設けた手先部と、 アームおょぴ手先部の それぞれを 1つ以上の関連する軸に関して駆動するため の所要数のァクチユエータ例えばサーボモータと、 各軸 の作動状態 （各軸のサーボモータの回転位置） を検出す るための各種センサとを有する。 本実施例において、 手 先部は、 後述の基準平面内で回転自在かつ当該基準平面 に対して回転自在 つ手先部軸心回りに回転自在にされ ている。 従って、 ，先部ひいては該手先部に装着された ツール （図示省略） は、 夫々のアームの閧節の作動状態 と上記 3つの回転位匿 （手先角） とを制御することによ り 3次元空間内の '；意の位置において任意の姿勢をとる ようにされている。 ¹ 制御装置 2 0の中央処理装置 （以下、 C P Uという） 2 1 には、 R O Mからなり制御プログラムを格納したメ モリ 2 2 と、 教示プログラム （教示データ） を記憶する ための R A Mからなるメモリ 2 3 と、 ロボッ トへの教示 を行うための教示操作盤 2. 4と、 各種指令等を入力する ための操作盤 2 5 とがバス 2 9を介レて接続されている。 ざらに、 C P U 2 ,1 には、 サーボ回路 2 7を介して各軸 のサーボモータを駆動するための軸制御器 2 6と、 C P U 2 1 と作業対象 4 0の各種作動部， 各種センサ間での 信号授受を行うためのインターフェイス 2 8とが接続さ れている。

以下、 第 1 図ないし第 4図を参照して、 ロボッ トのッ ール姿勢制御動作を説明する。

ロボッ トの運転に先立ち、 オペレータは、 教示操作盤 2 4を操作し、 ツールを作業の始点 P 1 に位箧決めする と共にツールに所定の姿勢をとらせた状態で、 始点にお けるツールのポーズ （ツールの位置および姿勢） を教示 すると共に所要の命令語を入力する。 この結果、 従来公 知のように、 始点 P 1 でのツールのポーズに対応する口 ボッ ト の各軸の作動状態を表す教示データがメモリ 2 3 に格納される。 又、 作業の終点 P 2についても同様の教 示を行う。 さらに、 ツールを円弧 2 (第 4図〉 に ½、つて 移動させる場合には始点と終点間の中間点 P 3の位置を 教示する。 この結果、 従来公知のようにして教示ブログ ' ラムが作成ざれメモリ 2 3に格納される。 なお、 第 4図 中、 符号 V 3は中間点 Ρ 3におけるツールの向きを表す アブ Π—チべク トルを示す。

ロボッ ト の運転時、 C P U 2 1 は、 教示プログラムか ら 1プロックを読出し、 当該ブロックに閬達する始点 Ρ 1 および終点 Ρ 2でのツールの教示座標位置 ( X s ,

Y s , Z s ) , ( X e， Y e , Z e ) に基づいて両座標 位置を通る第 1 図に示す基準平面 1 と、 この基準平面 1 内に含まれ所定の座標軸たとえば X軸と平行に延びる基 準線 3とを決定する （第 3図のスチッブ S 1 )。 なお、 読出したブロックにおいて指定ざれたッ一ル移動軌跡が 円弧である場合、 C P U 2 1 は、 円弧 2の中心 O (第 4 図） と始点 P 1 とを結ぶ線を基準線 3として决定する。

次に、 C P U 2 1 は、 始点 P 1での教示ツール姿勢 ならびにステップ S 1 において決定した基準平面 1およ び基準線 3に基づいて、 従来公知の座標変換処理手法に より、 始点 P 1 itおける上記 3つの手先角の値、 すなわ ち、 ツールの向きを表すアブローチベク トル V 1 と基準 平面 1 とがなす角度 （第 1 の手先角） 1 の値 e 1 s と、 基準平面 1上に投影されたァプロ一チべク トルと基準線 3とがなす角度 （第 2の手先角） 2の値 2 s と、 ッ ールのツール軸心回りの所定回転角度位置卜らの回転角 度 （第 3の手先角） 3の値 3 s とを箅 φする （ステ ップ S 2 ) 。 同様に、 終点での教示ツール姿勢等に基づ いて終点における手先角の値 0 1 e， Θ 2 0 3 eを 算出する （スチップ S 3 )。 次に、 C P U 2 1 は補間処理を実行する。 先ず、 ステ ッブ S 4において、 始点 P 1 と終点 P 2とを結ぶツール 移動軌跡上での最初の途中目標点の座標位置 （ X s + Δ X, Y s + Δ Υ, Z s +厶 Z ) と、 この算出目標点で の第 1 〜第 3の手先角のそれぞれの値 0 1 s +厶 Θ 1， ^ 2 s + Δ 6> 2, S 3 s +厶 3 とを算出する （厶 Xな いし△ Zおよび厶 0 1 ないし厶 0 3は図示省略） 。 次い で、 始点 P 1 での教示座標位置 ( X s , Y s， Z s ) か らスチップ S 4で算出した目標点の座標位置 ( X s + Δ X, Y s + A Y， Z s + A Z ) までツール位置を変化 ざせかつそれぞれの手先角 0 1， Θ 2, 5 3をステツブ S 2において算出した値 6> l s， 5 2 s , 3 sからス テツプ S 4での箅出値 0 1 s +厶 ^ 1， Θ 2 s + Δ Θ 2_f 0 3 s +厶 0 3まで変化させるための、 これに閬達する 閬節の作動状態の夫々の変化量を従来公知の座標変換処 理手法によ り算出する （ステップ S 5 )。 そして、 夫々 の変化量に応じた数のパルスを、 対応するサーボ回路に 分配し、 対応するサーボモータを駆動する （ステップ S 6 ) o

次に、 C P U 2 1 は、 ステップ S 1 において読出した ブロックについての処理が終了したか否かを判別する (ステップ S 7 ) 。 ここではステップ S 7での判別結果 が否定になり、 ステップ S 4に移行して、 ツール移動軌 跡上での第 2の途中目標点の座標位蚩と、 この算出目標 点での第 1〜第 3の手先角のそれぞれの値とを算出する。 次いで、 最初の途中目標点から第 2の途中百標点までッ ールを移動させかつツールの姿勢を最初の途中目標点に おけるものから第 2の途中目標点におけるものまで変化 させるための夫々の ¾節の作動状態の変化量を算出し、 この算出結果に応じたパルス分配を行う （ステップ S 5， S 6 ) 。： 以上のように、 ステップ S 4〜 S 7よりなるル 一プの処理が周期 β に実行され、 ツールは各々の途中目 標点において意図した姿勢をとりつつ、 所定の移動経路 に沿って終点に向けて移動する。

その後、 ツールが終点に至ると、 ステップ S 7におい て、 1プロックが終了したと判別ざれる。 このとき、 ッ ールは '座標位置 （X e， Y e , Z e ) で表される教示終 点位置において手先角 0 1 e， Θ 2 e , 6 3 eで表され る教示姿勢をとる。 次に、 ステップ S 8に移行して教示 プログラムの次のブロックが読出される。 そして、 斯く 読出された.プロックにブログラム終了を'指定する命令が 含まれているか否かに基づいてプログラム終了の要否が 判別される （ステップ S 9 )。 プログラムを繞行すべき 場合は、 ステップ S 8で読出したブロックについてステ ッブ S 2〜 S 7よ りなる上述の処理を実行してツールの ポーズを制御する。 その後、 スチップ 9においてプログ ラムを終了すべきと判別されると、 第 3図のブログラム に従う制御を終了する。

Claims

- 請 求 の 範 囲

. 作業の始点および終点でのツールの位 gならびに前 記始点および前記終点での前記ツールの姿勢を予め口 ボッ ト に教示する工程 （ a ) と、 教示結果に従い、 前 記始点において前記ツールと基準平面とがなす第 1 の 角度と、 前記始点において前記基準平面上に投影され た前記ツールと前記基準平面上に設定した基準線とが なす第 2の角度と、 前記始点における前記ツールのッ 一ル軸心回りの回転位箧とを算出する工程 （ b〉 と、 前記教示結果に従い、 前記終点における前記第 1 の角 度， 前記第 2の角度および前記ツール軸心回りの回転 位置を箅出する工程 （ C ) と、 前記ツールの位置， 前 記第 1 の角度， 前記第 2の角度および前記ツール軸心 回りの回転位箧を前記始点と前記終点との間で補間す る工程 （ d ) と、 補間結果に従って前記ツールの姿勢 を制御する工程 （ e ) とを備えるロボッ トのツール姿 勢制御方法。

. 前記工程 （ a ) において教示した前記始点および前 記終点での前記ツ -ル位匿を通るように前記基準平面 を設定する請求の範囲第 1項記載のロボッ トのツール 姿勢制御方法。

前記工程 （ a ) におけるツール位置の教示に用い る座標系の所定の座標軸に平行に前記基準線を設定す る請求の範囲第 1項または第 2項記載のロボッ トのッ ール姿勢制御方法。

4. 前記工程 （ a ) において前記始点と前記終点間の中 間点でのツール位置を教示する請求の範囲第 1項また は第 2項記載のロボッ トのツール姿勢制御方法

5. 前記工程 （ a ) において前記始点と前記終点間の中 間点でのツール位箧を教示す 請求の範囲第 3 ¾記载 の Πボッ トのツール姿勢制御方法。