JPS62194513A - ロボツトの円弧補間姿勢制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔産業上の利用分野〕 この発明は多関節ロボットの姿勢制御装置に関し、特に
少なくとも３つのティーチング点にもとづく円弧補間制
御において円弧平面に対するツールの姿勢角を一定また
は連続性をもって制御できるようにしたロボットの円弧
補間姿勢制御装置に関する。

〔従来の技術〕

ロボットの軌跡制御の方法の１つに円弧補間といわれる
ものがある。この円弧補間は少なくとも３つのティーチ
ング点におけるティーチングデー夕にもとづき円弧軌跡
上の各点の制御データをもとめ、ロボットの各種制御要
素を制御するもので、かかる制御において、例えば溶接
やガス切断等の作業に通用する場合には上記円弧軌跡に
よって形成される円弧平面に対するロボットのツールの
姿勢角を一定または連続性をもって制御する必要がある
。

ところで、従来のロボットの姿勢制御装置としては、 １）ロボットの各軸の角度変化量をそれぞれ分割するこ
とによりツールの姿勢制御データを得る。

２）ツールの姿勢をロボットの原点に固定したロボット
座標系におけるオイラー角で表わし、このオイラー角の
変化量をそれぞれ分割することによりツールの姿勢制御
データを得る。

３）ツールの姿勢をロボット座標系におけるベクトル（
姿勢ベクトル）で表わし、この姿勢ベクトルの変化量を
分割することによりツールの姿勢制御データを得る。

等の装置がある。

しかしながら１）の装置においては円弧軌跡上の各点に
おいてツールの姿勢を一定に保つことはできない。また
２）の装置においては円弧軌跡がロボット座標系に関し
て２次元平面で表わされる場合にはツールの姿勢一定制
御が可能であるか、上記円弧軌跡が３次元の任意平面に
形成されたものであるとツールの姿勢一定制御は不可能
となる。

更にまた３）の装置は直線補間の姿勢制御には適してい
るが、円弧補間の場合は１）の装置と同ＦＭにツールの
好しい姿勢補償はできない。

〔発明が解決しようとする問題点〕

上述したように、従来のロボットの姿勢制御装置におい
ては直線補間を行なう場合または円弧捕間を行なう円弧
軌跡がロボット座標系に関して２次元平面で表わされる
場合においては満足すべき結果が得られるが、円弧捕間
を行なう円弧軌跡かロボット座標系に関して任意の３次
元平面で表わされるような場合にはツールの姿勢を一定
または連続性をもって制御することはできなかった。

そこで、この発明は任意の円弧平面に対して・リールの
姿勢を一定または連続性をもって制御することがきるロ
ボットの円弧補間姿勢制御装置を提供することを目的と
する。

ｃ問題点を解決するための手段〕 この発明によれば、ティーチング手段によってティーチ
ングされた少なくとも３つのティーチング点によって規
定される円弧軌跡上の各ティーチング点において姿勢制
御用座標系をそれぞれ設定し、ロボットの原点に固定さ
れたロボット座標系で表わした前記各ティーチング点に
おけるロボットのツールの姿勢を表わす姿勢ベクトルを
前記姿勢制御用座標系における姿勢ベクトルに変換し、
該変換された各ティーチング点の前記姿勢制御用座標系
における姿勢ベクトルにもとづき前記円弧軌跡上の各点
における前記ツールの姿勢を表わす姿勢ベクトルを計算
した後、該計算された姿勢ベクトルを前記ロボット座標
系における姿勢ベクトルに更に変換し、該変換された姿
勢ベクトルにもとづき前記ロボットのツールの姿勢を制
御するようにしている。

〔作用〕

したが１て、この発明によれば円弧捕間の演算は円弧軌
跡上の姿勢制御用座標系によって行なわれ、これにより
ロボットのツールの姿勢は円弧軌跡によって規定される
円弧平面に対して一定または連続性をもって制御される
。

〔実施例〕

以下、図面を参照して本発明の実施例を詳細に説明する
。

第１図は本発明のロボットの円弧補間姿勢１．制御装置
の一実施例の概要を示したものであり、第２図はこの実
施例の制御対象となるロボットの概要を示したものであ
る。第２図において、この実施例の制御対象となるロボ
ットはロボット本体１に下腕２が取付けられ、この下腕
２は上腕３が取付けられ、この上腕３に更に手首４が取
付けられ、手肖゛４の先端にツール５が取付けられてｔ
ｆｌｌｉ成されている。下腕２はロボット本体１に対し
て旋回自在かつ回動自在（ψ１方向およびψ２方向）に
され、」二腕３は下腕２に対して回動自在かつ回転自在
（ψ３方向およびψ４方向）にされ、手首４は上腕３に
対して回動自在かつ回転自在（ψ５方向およびψ６方向
）にされている。すなわち、このロボットは腕に関して
ψ］力方向旋回、ψ２方向の回動、ψ３方向の回動、ψ
４方向の回転の４軸、手首に関してψ５方向の回動、ψ
６方向の回転の２軸の計６軸の自由度をもって構成され
ている。

第１図において、ロボット制御部１０は上記６軸に対す
る駆動系、すなわち、ψ１方向の旋回を駆動する旋回駆
動系１１、ψ２方向の曲げ回動を駆動する下腕曲げ駆動
系１２、ψ３方向の曲げ回動を駆動する上腕曲げ駆動系
１３、ψ４方向の回転を駆動する上腕回転駆動系１４、
ψ５方向の曲げ回動を駆動する手首曲げ駆動系１５およ
びψ６方向への回転を駆動する手首回転駆動系１６に対
してそれぞれ所要の駆動制御信号を与え、旋回駆動系１
１による旋回角ψ１は旋回角検出器２１で検出され、下
腕曲げ駆動系１２による下腕曲げ角ψ２は下腕曲げ角検
出器２２で検出され、上腕曲げ駆動系１３による上腕曲
げ角ψ３は上腕曲げ角検出器２３で検出され、上腕回転
駆動系１４による上腕回転角ψ４は上腕回転角検出器２
４で検出され、手首曲げ駆動系１５による手首曲げ角ψ
５は手首曲げ角検出器２５て検出され、手首回転駆動系
１６による手直回転角ψ６は手首回転角検出器２６で検
出され、それぞれロボット制御装置１０に加えられる。

ロボット制御装置１０はティーチングボックス３０から
のティーチング指定に対応してティーチングされたティ
ーチング点におけるツール５の姿勢データにもとづき各
軸、すなわち各駆動系１１〜１６に対する所要の駆動信
号を形成する。

ところでこの実施例にかかわるロボットは溶接ロボット
を想定しており、手首４の先端に取付けられたツール５
は溶接トーチである。以下このロボットが各軸の制御に
より、第３図に示すようにロボット本体１に対して固定
されたロボット座漂系Ｘｏ　Ｙｏ　Ｚｏに対して任意の
傾きをもった円弧平面６上に丸棒７を溶接する動作を行
なう場合につ−いて説明する。ここで溶接を均一に行な
うためにツール５、すなわち溶接トーチは円弧平面６に
対して一定の姿勢角αを保持するかもしくは姿勢角が連
続的に変化する必要がある。そこでロボ・ソト制御部１
０ではこのツール５の姿勢角αを一定もしくは連続的に
変化させるための制御を行なう。

ツール５の姿勢角αを一定もしくは連続的に変化させる
ためにこの発明では以下に示す制御方式が採用されてい
る。

この方式によればまず第４図に示すように円弧補間のた
めの円弧軌跡上に３点Ｐ　１＊　　Ｐ　２１　　Ｐ３が
ティーチングされたとし、各ティーチング点Ｐ、、Ｐ２
．Ｐ３において、円弧軌跡の法線方向にＸｊ’　　（ｊ
＝１．２．３）軸、接線方向にＹｊ’軸、Ｘｊ’Ｙｊ’
平面と垂直方向にＺ　ｊ　Ｚ軸を定めた姿勢制御用座標
系Ｘｊ　’　Ｙｊ　’　　Ｚｊ　’を設定する。ロボッ
ト本体１に固定されたロボット座標系Ｘｏ　Ｙｏ　Ｚｏ
で表わされた各ティーチング点Ｐ、、Ｐ２．Ｐ３におけ
るツール５の姿勢ベクトルをＪＬ’１，２１２．１３と
し、この姿勢ベクトル１１．　１１２．ａ３を姿勢制御
座標系Ｘｊ′Ｙｊ’Ｚｊ’で表現すると、姿勢ベクトル
８１′。

Ｚ　２／　、　　ａ　３／　となる。次いで、ベクトル
ａ１・−ｍ２′およびＩＬ２′−８３′により定まる扇
形平面上で姿勢ベクトルＪＭ’　＋　　１１２’　、８
３’を分割し、任意の時刻ｔでのツールの姿勢ベクトル
ｍ　、　Ｌ　を求める。更にこの姿勢ベクトルａ　、　
／　を０ボツト座標系Ｘｏ　Ｙｏ　Ｚｏでの姿勢ベクト
ルＩＡＩに変換する。そしてこの姿勢ベクトルｉ、にも
とづきロボットの各軸を制御する。このようにすること
により、任意の円弧平面に対するツールの姿勢を一定ま
たは連続性をもって制御すること゛が可能となる。

第５図は上述した制御方式を実行するための制御フロー
を示したものである。第５図において、まずステップ５
１においてロボット座標系Ｘ。

ｙｏ　ｚｏでのＸｊ′軸上の単位ベクトルｅ３、Ｘ Ｙｊ′軸上の単位ベクトルｅ、、Ｚｊ’軸上の単ｙ 位ベクトルｅ　を計算する。第６図に示すように０ボツ
ト座標系Ｘｏ　Ｙｏ　Ｚｏにおけるティーチング点ＰＪ
　（ｊ−１，２，３）の座標を（Ｘ、。

ｙｊ、ｚ、）とし、円弧軌跡の中心の座標をＪ （ｘｏ、Ｙｏ、ｚ□）とすると、Ｘｊ′軸上の単位ベク
トルｅ　　（ｊ−１，２，３）はＪＸ ・・・（１） となり、Ｚｊ’軸上の単位ベクトルｅ　はｅ　　−１／
ｓｉｎθ（”　ｌｘｘ”　２Ｘ）（ｔこだし、θはや由
Ｘ１′とＸ２’　とのなす角）となる。したがって ・・・（２） となる。また、ＹＪ′軸上の単位ベクトルｅ、　（ｊ＝
１．２．３）は Ｊｙ ｅ、　　　−ｅ　　　　ｘｅ。

Ｊｙ　　　　　Ｚ　　　　　　　ＪＸ となり、したがって となる。

よって、ステップ５１においては上述した（１）式、（
２）式、（３）式の演算を実行する。

次にステップ５２において姿勢ベクトルａ　、　／（ｊ
−１，２，３）の計算を行なう。ここで姿勢ベクトルａ
、′の計算は以下のようにして行なわれる。

すなわち、ロボット座標系Ｘｏ　Ｙｏ　Ｚｏで表わした
各ティーチング点Ｐｊ　　（ｊ−１，２，３）での姿勢
ベクトルをａ、（ｊ＝１．２．３）とすると姿勢制御用
座標系Ｘｊ’Ｙｊ’Ｚｊ’での姿勢ベクトル８．、は、
その成分ａ　、　７　　ａ　、　Ｊ　　ａ　、　／Ｊ　
　　　　　　　　　　　　　　　　　　　　　ｊＸ　　
　　　　、ＨＪＺａ　　、　′　Ｎｃｏｓ　　θ　・ ｊＸ　　　　　　　　　　　ＪＸ ＝　（ｅ　、　・　ａ　、　　）　　／　　（ｌ　　ｅ
　、　ｌｌａ、１）ＪＸ　　　　　Ｊ　　　　　　　　
　　　Ｊｘ　　　　　　　３ＪＸ　　　　　　Ｊ ａ、’　　−ｃｏｓ　　θ 、Ｈ’　　　　　　　　　　　Ｊｙ ＝　（ｅ、　・　ａ＋、）／（ｌｅ、ｌｌ　　晟、　１
）Ｊｙ　　　　ｊ　　　　　　　　　　　ｊ）’　　　
　　　ｊＪＹ　　　　　　Ｊ ａ、’　　−ｃｏｓ　　θ　・ ｊＺ　　　　　　　　　　　ＪＺ ＝　　（ｅ、　　・　ａ、）／　　（ｌｅ　、　ｌ　　
ｌａ、　　Ｉ）ＪＺ　　　　Ｊ　　　　　　　　　　　
ＪＺ　　　　　　ＪＪＺ　　　　　Ｊ と表わされる。ここでθ、は第７図に示すようＪＺ にベクトルａ、と軸ｅ、とのなす角、θ、はベクＪ　　
　　　　　ＪＸ　　　　　　　　　　　　　　Ｊｙトル
ａｊと輔ｅ３．とのなす角、θｊ２はベク　トルａ、と
輔ｅ、７とのなす角である。

したがって、ベクトルａ　、　／　は ・・・　（４） となる。

よって、ステップ５２では（４）式の演算が行なわれる
。ステップ５’１．５２の処理は円弧補間実行前に行な
われる初期処理である。

次に、ステップ５３において姿勢ベクトルａ、、、ｊＸ
　の計算が実行される。

第８′図に示すようにティーチング点Ｐ１におけ１゛ る姿勢制御用座標系Ｘ１ｔ　、ｙ１′、Ｚ、　′とティ
ーチング点Ｐ２における姿勢制御用座標系ｘ２’　、Ｙ
２’、ｚ２’　とティーチング点Ｐ３における姿勢制御
用座標系ｘ３’　、ｙ３’　、ｚ３’を一致させると、
ティーチング点Ｐ１．Ｐ２゜Ｐ３ての姿勢ベクトル”Ｉ
＋　　”２＋　　Ｊ１３は同−座標系でおのおのＪ１１
’　、１２’　、１３′と表わされる。またベクトルＪ
１１′　とベクトル８２′　とのなす角をθａ１、ベク
トル８２′　とペクトルミ３′　とのなす角をθａ２と
する。

ここで、ベクトル８１′とベクトル８２′で定まる扇形
平面上での姿勢ベクトルの分割を行ない、更にベクトル
８２′　とベクトル８３′で定まる扇形平面上での姿勢
ベクトルの分割を行なう。

まず、ベクトル８１′　とベクトルＪＬ２′で定まる扇
形平面上での分割（ａ＋１’−ａ２’）において、姿勢
制御用座標系での任意時刻ｔの姿勢ベクトルａ　′は ａ、’−（ｃｏｓ　θ１　（ｔ） （ただしθ１　（ｔ）はベクトル８１′とベクトルａ、
′のなす角） で表わされる。

また、ベクトル８２′とベクトルａ３′で定まる扇形平
面での分割（ａ２’−ａ３’）において、姿勢制御用座
標系での任意時刻ｔの姿勢ベクトルａ、′は ａ、　’　　−（ｃｏｓ　θ２　（ｔ）（ただしθ２　
（ｔ）はベクトル８２′とベクトルａ　　／のなす角） で表わされる。

したがって、ステップ５３では −（ＣＯＳ　　θｋ　　（ｔ） ・・・　（５） （ただしａ１’−ａ２’（１）とき’ｋ　−１，８２′
→ａ３′のときに＝２） の演算処理を実行する。

続いて、第９図に示すようにステップ５４において円弧
軌跡上での任意の点Ｐ　（ｔ）における姿勢制御用座標
系ＸＩ’Ｙｊ’Ｚｉ’の各軸のロボット座標系における
単位ベクトル、すなわち法線方向単位ベクトルｅ１、接
線方向の単位ベクトルＸ ｅ　１ベクトルｅ、Ｘｅ、方向の単位ベクトルｌｙ　　
　　　　ＬＸ　　　ｌｙ ｅ　を求める。

すなわち、点ｐ　（Ｂでの法線方向単位ベクトルｅｌｘ
は となる。ここでｘ　（ｔ）、　　ｙ　（ｔ）、　　ｚ　
（ｔ）は点Ｐ　（ｔ）のロボット座標系における座標で
ある。

また、点Ｐ　（ｔ）での接線方向の単位ベクトルｅ１．
は ＩＪ ｅ　・・−ｅ、ｘｅ。

ＩＪ　　　　　　Ｚ　　　　　　　ＬＸで与えられる。

なお単位ベクトルｅ　は前述した式（２）によって求め
ることができる。

したがってステップ５４では ・・・（６） ・・・　（７） の演算処理を実行する。

続いて、ステップ５５においてはステップ５４で算出し
た単位ベクトルｅ、、ｅ　　にもとづき姿ＩＸ　　　　
　　Ｚ）’ 勢ベクトルａ、′をロボット座標系で表わした姿】 勢ベクトルａ、に変換する演算を行なう。

すなわち姿勢制御用座標系で求めた姿勢ベクトル　、Ｊ
　はロボット座標系で ａ■ａ、’　ｅ、　＋ａ、’　ｅ、　＋ａ　’ＩＸ　　
　ＩＸ　　　ＩｙＩｙｉＺ　　　Ｚで表わされる。

したがって、ステップ５５においては ＼へ ・・　（８） の演算処理を実行する。

これにより、任意時刻ｔにおける円弧軌跡上のロボット
座標系でのツールの姿勢ベクトルエ、を求めることがで
きる。

以上のステップ５３から５５までの演算処理は円弧捕間
を実行している間繰り返される。

第１図に示したロボット制御装置１０では１−述した演
算処理を実行し、任意時刻ｔにおける円弧軌跡上のロボ
ット座標系でのツールの姿勢ベクトル２．を求めるとの
姿勢ベクトルａ、を実現すべく各軸、すなわち旋回駆動
系１１、下腕曲げ駆動系１２、上腕曲げ駆動系１３、上
腕回転駆動系１４、手首曲げ駆動系１５および手首回転
駆動系１６に対して所望の駆動信号を与える。これによ
り円弧補間時円弧平面とツール姿勢との傾き角を一定ま
たは連続性をもって制御することが可能となる。

なお、上記実施例では本発明を腕４軸手首２軸の６軸溶
接ロボツトに適用した場合を示したが、他の形式、用途
のロボットに対しても同様に適用できるのは勿論である
。

〔発明の効果〕

以上説明したように、この発明によれば円弧補間の演算
を円弧軌跡上の姿勢制御座標系によって行なうようにし
たので、円弧平面に対するツールの姿勢を一定または連
続性をもって制御することが可能となる。

【図面の簡単な説明】

第１図はこの発明のロボットの円弧補間姿勢制御装置の
一実施例を示すブロック図、第２図はこの実施例が適用
されるロボットの一例を示す略図、第３図はこの実施例
のロボットによる制御例を示す略図、第４図はこの発明
による円弧補間の原理を説明する図、第５図はこの実施
例の動作を示すフローチャート、第６図から第９図は同
実施例における演算処理を説明する図である。 １・・・本体、２・・・下腕、３・・・上腕、４・・・
手肖−１５・・・ツール、６・・・円弧平面、７・・・
丸棒、１０・・・ロボット制御装置、３０・・・テイー
チングボ・ソクス。 第２図 第３図 第５図 第６図 第７図 ｉ 第８図 手続ネｍ正ｔ（自発） 昭和６２年５月１４日 ２、発明の名称 ロボットの円弧補間姿勢制御装置 ３、補正をする者 事件との関係　　　特許出願人 （１２３）株式会社小松製作所 ４、代理人 （〒１０４）東京都中央区銀座２丁目１１昌２号銀座大
作ビル６階　電話０３−５４５〜３５０８　（代表）５
、補正の対象 明細占の発明の詳細な説明の瀾および゛図面６、補正の
内容 （１）本願の明細出、第６ページ第２行の「濃口」を「
姿勢演算」に訂正する。 （２）同、第１０ページ第１７行のｒ　ｅ　ｊｙ’を「
Φ、］に訂正する。 Ｖ （３）同、第１１ページの式（１）を下記のように訂正
する。 ・・・（１）」 （４）同、第１２ページの式（２）を下記のように訂正
づる。 （に）固　笛らペーり笛１つ行の「！財蓼１か「−例」
に訂正する。 （６）同、第１３ページ第１１行の「θ、」　をＺ 「θ、ｘ」に訂正する。 （７）同、第１１ページの式（４）を下記のように訂正
する。 ・・・（４）」 く８）同、第１４ページ下から第８行のｒ、１１．Ｉを
「凪、′」に訂正する。 ■ （９）同、第１８ページ第８行の「［Ｆ］・・」をＪ 「の、Ｊに訂正する。 ｙ （１０）同、第１８ページ第９行の「Φ、」をＪ 「１．」に訂正する。 （１１）同、第１８ページの式（６）を下記のように訂
正する。 ・・・　（６）Ｊ （１２）同、第１９ページの式（７）を下記のＪ：うに
訂正する。 （１３）同、第２０ページの式（８）を下記のように訂
正する。 （１４）同、第１９ページ下から第８行のｒ　４１４２
．Ｊを「［Ｆ］、」に訂正する。 ｙ （１５）本願の図面、第５図を別紙の通り訂正する。 （１６）同、第８図、第９図を別紙の通り訂正する。 第５図

Claims (1)

1. 【特許請求の範囲】 制御空間内の少なくとも３つのティーチング点において
   ロボットのツールの姿勢をティーチングするティーチン
   グ手段と、 前記ティーチング点によって規定される円弧軌跡上の各
   ティーチング点において姿勢制御用座標系をそれぞれ設
   定し、ロボットの原点に固定されたロボット座標系にお
   ける各ティーチング点の前記ロボットのツールの姿勢を
   表わす姿勢ベクトルを前記姿勢制御用座標系における姿
   勢ベクトルに変換する第１の変換手段と、 該変換手段によって変換された各ティーチング点の前記
   姿勢制御用座標系における姿勢ベクトルにもとづき前記
   円弧軌跡上の各点におけるツールの姿勢ベクトルを計算
   する計算手段と、 該計算手段によって計算された姿勢ベクトルを前記ロボ
   ット座標系における姿勢ベクトルに変換する第２の変換
   手段と、 該第２の変換手段によって変換された姿勢ベクトルにも
   とづき前記ロボットのツールの姿勢を制御する制御手段
   と を具えたロボットの円弧補間姿勢制御装置。