JPH04308903A - 産業用ロボットの補間制御方法 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、先端にツールが取り付
けられ、ツール先端の位置を決定する、２軸以上の手首
軸及び３軸以上の主軸を有する多関節型産業用ロボット
の、ツール先端の直線補間又は円弧補間制御を行う産業
用ロボットの補間制御方法に関する。

【０００２】

【従来の技術】従来の多関節型産業用ロボットの補間制
御方法としては、例えば特開昭５６−０６２７８３号公
報に開示するような、各軸の現在位置角度情報及び各軸
の指令信号であるリードアウト直交座標ＸＹＺ成分を入
力し、補間点各軸指令角度情報を得るロボットの補間制
御方法があった。さらに例えば特開昭５７−０５５４１
４号公報に開示するような、ロボット取付面を基準とす
る座標系において、手首先端に取り付けられたツールの
姿勢を一定に保つという点のみに着目した、姿勢制御方
式があった。

【０００３】

【発明が解決しようとする課題】従来の上記方法に示す
では、例えば図３で示す幾何学モデルのロボットで説明
すると、各動作軸の回転角度を、主軸１軸から３軸をθ
１　からθ３　とし、手首軸４軸から６軸をθ４　から
θ６　とし、各軸の現在位置角度情報から、教示点ｊに
おける各軸の補間点指令信号であるθ１ｏからθ６ｏを
得る場合、ツールの姿勢と、ツール空間座標リードアウ
ト直交座標ＸＹＺ成分ｐ１ｊ（ｘ１ｊ、ｙ１ｊ、ｚ１ｊ
）、を与えて、周知の連立方程式をたてて、先ず主軸１
軸から３軸のθ１ｏからθ３ｏを求め、次いで求まった
θ１ｏからθ３ｏを周知の連立方程式に代入して手首軸
４軸から６軸のθ４ｏからθ６ｏを決定していた。

【０００４】しかしながら、周知の連立方程式を使用し
て演算するときは、解が２個出て角度が求まらなかった
ためロボットが停止すること等がある。例えば図３で示
す幾何学モデルで手首軸５軸の回転角度θ５ｏが０°に
近い極めて小さい値のとき４軸の回転角度θ４ｏを決定
できないことが起こった。これは４軸の下軸である３軸
の軸心と、５軸の上軸である６軸の軸心とがほぼ一致す
ると、４軸回転角度θ４ｏと６軸回転角度θ６ｏの効果
が同一となり、動作を決定出来なくなることに相当する
。又この近傍では例え解が求まったとしても、各軸の指
令動作量が大き過ぎると手首軸の不自然な動作となった
。本発明の課題はかかる従来技術の課題を解消した、解
が求まらなかったためロボットが停止すること、又は各
軸の動作指令の変化量が大き過ぎて手首軸の不自然な動
作を招くこと等のない多関節型産業用ロボットの補間制
御方法を提供することにある。

【０００５】

【課題を解決するための手段】このため本発明は、特許
請求の範囲記載の産業用ロボットの補間制御方法を提供
することにより、上述した従来技術の課題を解決した。

【０００６】

【実施例】本発明の実施例につき図面を参照して説明す
る。図１は本発明の実施例産業用ロボットの補間制御方
法を示すブロック図である。図２は本発明の実施例産業
用ロボットの補間制御装置全体を示すブロック図で、ロ
ボットアーム５を制御するロボットの制御装置は、処理
装置であるＣＰＵ１と、システムプログラムを格納する
ＲＯＭ２と、システムプログラムの作業領域及びロボッ
トの動作プログラムを格納するＲＡＭ３と、ロボットア
ーム５の動作を制御するサーボ制御回路４と、を有する
。図３はロボットの座標軸及び各軸動作を幾何学モデル
で示した概略構成図である。

【０００７】ＲＯＭ２は、計算プログラムである、ロボ
ットの取付面を基準とする座標系でのツール６の姿勢及
びツール先端の座標を同時に制御する第１の補間演算手
段２１と、ロボットの手首動作軸の角度を予め独立に各
軸毎に決定しツール先端の座標を主軸の座標値のみで決
定する第２の補間演算手段２２と、を内蔵する。ＲＡＭ
３には、記憶部として上記第１及び第２の補間演算手段
２１、２２を切り換えるための、角度範囲を指定する補
間方法切換角度３１を有する。

【０００８】本発明の実施例産業用ロボットの補間制御
方法を示す図１のブロック図、及び図３のロボットの座
標軸及び各軸動作を示す幾何学モデルを使用して、本発
明産業用ロボットの補間制御方法を説明する。以下簡単
化するため、ツール６先端の位置を決定する、各動作軸
の回転角度を、主軸１軸から３軸をθ１　からθ３　と
し、手首軸４軸から６軸の回転角度をθ４　からθ６　
とし、第ｎ軸の各動作軸の回転角度をθｎ　とすると、
各座標軸の回りの回転のマトリックスを記号でＣｘｎ、
Ｃｙｎ、Ｃｚｎで示すと、

【０００９】

【数１】

【００１０】で、表わされる。図３で、手首軸４軸から
６軸の回転角度をθ４　からθ６　とし、かつ第１アー
ム７及び第２アーム８の長さを、それぞれＬ１　、　Ｌ
２　とし、ツール６のなす姿勢を手首先端のツール取付
面に定義した座標系をＥｗｘ、Ｅｗｙ、Ｅｗｚとする。 ここで、Ｅｗｘ、Ｅｗｙ、Ｅｗｚは、ツール取付面に定
義したｘ、ｙ、ｚ軸上の各単位ベクトルを、ロボット取
付面に定義した座標系（地面基準の座標系）で表記した
ものである。この時、ツール先端は、手首先端のツール
取付面で定義したｘ、ｙ、ｚ座標系で（ｘ６　、ｙ６　
、ｚ６　）の位置にあるものとする。いま、現在位置Ｐ
１　における各軸の回転角度（θ１１からθ６１）が与
えられたとき、ツール先端の現在位置Ｐ１　の空間座標
Ｐ１　（ｘ１　、ｙ１　、ｚ１　）と、教示点２である
到達点におけるツール先端位置Ｐ２　の空間座標Ｐ２　
（ｘ２　、ｙ２　、ｚ２　）、及びツールの姿勢Ｅｗｘ
、Ｅｗｙ、Ｅｗｚとは、次の関係で与えられる。

【００１１】

【数２】

【数３】

【００１２】ツールの姿勢Ｅｗｘ、Ｅｗｙ、Ｅｗｚと空
間座標Ｐ（ｘ、ｙ、ｚ）とを与えて、ツール先端の補間
経路点の各軸の角度を決定する各軸の指令角度信号θ１
オからθ６オを決定する場合、上述した特開昭５７−０
５５４１４号公報に開示する方法では、上記（１）式を
、（２）、（３）及び（４）式を用いて、

【００１３】

【数４】

【００１４】のように変形し、ｘ、ｙ、ｚ各々の式に展
開し、未知数θ１ｏからθ３ｏの３個を含む、ｘ、ｙ、
ｚに関する３式の連立方程式として１軸から３軸の指令
角度信号θ１ｏからθ３ｏを求める。次いでこの求まっ
たθ１ｏからθ３ｏを、以下に示す数５のように、上記
（２）及び（３）式に代入して手首４、５、６軸の角度
３軸の指令角度信号θ４ｏからθ６ｏを決定するもので
あった。

【００１５】

【数５】

【００１６】図１は、上記第１及び第２の補間演算手段
２１、２２の処理内容を示し、ブロック１１では、ＲＡ
Ｍに記憶されているロボットの動作プログラム３２から
、各軸の角度で実現すべき位置が記述されている、ロボ
ットの現在位置である出発点Ｐ１　における各軸の回転
角度（θ１１からθ６１）と、直線補間で移動すべき到
達点Ｐ２　（教示点２）における各軸の回転角度（θ１
２からθ６２）とを取り出す。そして出発点Ｐ１　及び
到達点Ｐ２　のツール先端空間座標Ｐ１（ｘ１　、ｙ１
　、ｚ１　）及びＰ２　（ｘ２　、ｙ２　、ｚ２　）と
、これら各点のツールの姿勢Ｅｗｘ１　、Ｅｗｙ１　、
Ｅｗｚ１　：Ｅｗｘ２　、Ｅｗｙ２　、Ｅｗｚ２　、と
を、上述した特開昭５７−０５５４１４号公報に開示す
る上記（１）、（２）、（３）及び（４）式を用いて求
める。

【００１７】次にブロック１２で、ＲＡＭ３の移動速度
３３から取り出した移動速度と、補間計算の時間間隔と
から、例えば上述した特開昭５６−０６２７８３号公報
に開示するような周知の方法を用いて、経路点の座標Ｐ
ｏ　（ｘｏ　、ｙｏ　、ｚｏ　）とツールの姿勢Ｅｗｘ
ｏ　、Ｅｗｙｏ　、Ｅｗｚｏ　とを決定する。そしてブ
ロック１３で、経路点の座標Ｐｏ　（ｘｏ　、ｙｏ　、
ｚｏ　）とツールの姿勢Ｅｗｘｏ　、Ｅｗｙｏ　、Ｅｗ
ｚｏ　とより上記式（５）より１軸から３軸の指令角度
信号θ１ｏからθ３ｏを求め、次いでこの求まったθ１
ｏからθ３ｏを、上記式（５）より手首４、５、６軸の
角度３軸の指令角度信号θ４ｏからθ６ｏを求める。

【００１８】ブロック１４で、このときθ４ｏからθ６
ｏが求まらない場合、又はブロック１５でθ４ｏからθ
６ｏが求まってもθ４ｏからθ６ｏの値が、前の計算サ
イクルで求めた値θ４ｏｏ　からθ６ｏｏ　と大きくか
け離れている場合即ちθ４ｏからθ６ｏの、θ４ｏｏ　
からθ６ｏｏ　に対する変化量が予め設定した値より大
きい場合、のいずれの場合も自動的に以下の第２の補間
演算手段２２に切り換えて計算する。

【００１９】又、ブロック１６でθ４ｏｏ　からθ６ｏ
ｏ　に対する変化量が予め設定した値より小さい場合で
も、特定の手首軸例えばθ５ｏの値がＲＡＭ３に記憶さ
れた補間方法切換指定角度３１内に入ると、自動的に第
２の補間演算手段２２に切り換えて解を求めるようにさ
れている。 これは手首軸の動きの連続性を重視し、解が求まらなく
近傍で予め補間方法を切り換えてしまうものである。

【００２０】次にブロック１７以下で示す第２の補間演
算手段２２の具体的計算プログラムを示す。ブロック１
７はで出発点Ｐ１　における各軸の回転角度（θ１１か
らθ６１）及び到達点Ｐ２　における各軸の回転角度（
θ１２からθ６２）から、出発点Ｐ１　及び到達点Ｐ２
　のツール先端空間座標Ｐ１　（ｘ１　、ｙ１　、ｚ１
　）及びＰ２　（ｘ２　、ｙ２　、ｚ２　）を，求める
。そしてブロック１８で、第１の補間演算手段２１と同
様に経路点の座標Ｐｏ　（ｘｏ　、ｙｏ　、ｚｏ　）を
求めるが、この場合第２の補間演算手段２２ではツール
の姿勢を決定する代わりに、分割数をｍとして例えば：
　　θ４ｏ＝θ４１＋（θ４２−θ４１）／ｍ　　のよ
うに、θ４１からθ６１と、　θ４２からθ６２の角度
から按分して独立にθ４０からθ６０を決定するように
されている。 今、

【００２１】

【数６】

【００２２】となる。この（８）式は（５）式と同様、
ｘ、ｙ、ｚ各々の３つの式にθ１ｏからθ３ｏの３つの
未知数であり、θ１０からθ３０として解が求まる。以
上によりθ１０からθ６０が決定される。こうして第１
の補間演算手段２１で解が求まらない場合でも、又は求
まった解の通りに動かすとロボットアームの手首軸が不
自然な動きをしてしまう場合でも、第２の補間演算手段
２２に切り換えることにより、ツール先端が直線軌跡上
を移動しながら、かつ自然な動きを実現することが可能
となった。

【００２３】

【発明の効果】以上説明したように、本発明によると、
ツールの姿勢を含めて制御する第１の補間演算手段にお
いてツール先端の補間経路点の各軸の角度を決定する解
が求まらない場合でも、又は求まった解の通りに動かす
とロボットアームの手首軸が不自然な動きをしてしまう
場合でも、第２の補間演算手段に自動的に切り換えるこ
とにより、ツール先端が直線軌跡上を移動しながら、か
つ手首軸の自然な動きを実現することが可能となる産業
用ロボットの補間制御方法を提供するものとなった。

【図面の簡単な説明】

【図１】本発明の実施例産業用ロボットの補間制御方法
を示すブロック図である。

【図２】本発明の実施例産業用ロボットの補間制御装置
全体を示すブロック図である。

【図３】ロボットの座標軸及び各軸動作を幾何学モデル
で示した概略構成図である。

【符号の説明】

２　　ＲＯＭ ３　　ＲＡＭ ２１　　第１の補間演算手段 ２２　　第２の補間演算手段 ３１　　補間方法切換角度

Claims (2)

【特許請求の範囲】
1. 【請求項１】先端にツールが取り付けられ、ツール先端
   の位置を決定する、２軸以上の手首軸及び３軸以上の主
   軸を有する多関節型産業用ロボットの、ツール先端の補
   間制御において、前記ロボットの制御装置はシステムプ
   ログラムを格納するＲＯＭと、システムプグラムの作業
   領域及びロボットの動作プログラムを格納するＲＡＭと
   、を有し、前記ＲＯＭは、前記ロボットの取付面を基準
   とする座標系でのツールの姿勢及びツール先端の座標を
   同時に制御する第１の補間演算手段と、前記ロボットの
   手首動作軸の角度を予め独立に各軸毎に決定しツール先
   端の座標を前記主軸の座標値のみで決定する第２の補間
   演算手段と、を同時に持ち、先ず前記第１の補間演算手
   段によりツール先端の補間経路点の各軸の角度を決定す
   る解を求め、前記第１の補間演算手段により解が求まら
   ない場合又は第１の補間演算手段により求まった補間経
   路点の各軸の角度を決定する各解と各その前の位置の値
   との差が予め設定した値より大きい場合に、自動的に前
   記第２の補間演算手段に切り換えて各解を求めて前記ロ
   ボットの制御を行うことを特徴とする産業用ロボットの
   補間制御方法。
2. 【請求項２】前記ＲＡＭは予め設定した補間方法切換指
   定角度を有し、特定の手首軸の前記補間経路点の角度が
   前記補間方法切換指定角度内に入ると、自動的に前記第
   ２の補間演算手段に切り換えて各解を求めて前記ロボッ
   トの制御を行うことを特徴とする請求項１項記載の産業
   用ロボットの補間制御方法。