ロボットの制御装置及びロボットの姿勢補間方法
【課題】ロボットの手先がワークを把持して加工を行う場合の作業精度を向上させる。
【解決手段】制御装置は、手先の先端点を座標原点とするＸ方向，Ｙ方向，Ｚ方向の各軸について、手先の位置及び姿勢をティーチングする場合に、座標原点から各軸方向の所定距離に手先の姿勢を定める姿勢点を設定すると、ティーチングした姿勢に対応する姿勢点の間について、各軸の姿勢点の移動軌跡がそれぞれ滑らかに繋がるように補間する。
【解決手段】制御装置は、手先の先端点を座標原点とするＸ方向，Ｙ方向，Ｚ方向の各軸について、手先の位置及び姿勢をティーチングする場合に、座標原点から各軸方向の所定距離に手先の姿勢を定める姿勢点を設定すると、ティーチングした姿勢に対応する姿勢点の間について、各軸の姿勢点の移動軌跡がそれぞれ滑らかに繋がるように補間する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの手先がワークを把持し、固定されているツールに対して前記ワークを移動させて加工を行う際に、ロボットの姿勢を補間する制御装置及び方法に関する。
【背景技術】
【０００２】
従来、ロボットを用いてワークの加工等を行う場合には、ワークを固定し、ロボットの手先がツールを持って作業することが一般的である。しかし、例えばシール塗布やバリ取りといった一部の作業については、シーリングガンやリュータ等のツールを固定し、垂直多関節型のロボットの手先がワークを把持して作業する場合があり、このような作業形態を外部ＴＣＰ（Tool Center Point；先端点）と称することがある。
【０００３】
外部ＴＣＰでは、例えば図１２に示すようにシール塗布の場合、ワークの加工点（塗布点）を含む平面が、シーリングガンによる塗料の吐出方向に対してなす角度が常に９０度となるように維持することが望ましい。図１２（ａ）に示すように、ワークの形状が平面である場合、手先の姿勢は一定に維持して位置だけを変化させれば良いので問題とならない。しかし、図１２（ｂ）に示すようにワークの形状が曲面を含んでいる場合は、ロボットが手先の姿勢を連続して微妙に変化させることで、上述した角度を９０度にする必要がある。したがって、姿勢が当初にティーチングした通りに維持されなければ、ワークの加工精度が低下するおそれがある。
また、図１３に示すバリ取りの場合には、ワークの複雑な外形に応じて、エッジの各点がリュータに対して常に最適な角度で当たるように、ロボットの手先の姿勢をティーチングした通りに制御する必要があり、姿勢が維持されないとバリが取り切れず加工精度が低下する。
【０００４】
外部ＴＣＰへの適用を意識した技術ではないが、例えば特許文献１には、ロボットの手先が教示点Ｐ１，Ｐ２，Ｐ３を経由して連続的に動作する場合、各教示点における姿勢（回転行列）をＲ１，Ｒ２，Ｒ３とすると、姿勢Ｒ１から姿勢Ｒ２への変化量に関する等価回転角ベクトルＫ１２と、姿勢Ｒ２から姿勢Ｒ３への同等価回転角ベクトルＫ２３とを算出し、これらに基づき中間の教示点Ｐ２近傍の仮想的な内回り軌跡を算出し、この仮想的な内回り軌跡に対して指定された回転速度でサンプリング補間を行い、各補間点において移動方向に隣接する補間点への変化量に関する等価回転角ベクトルを算出し、その等価回転角ベクトルに基づいて動作軌跡を生成する技術が開示されている。また、特許文献２には、ロボットの急激な姿勢変化を抑制するため、経路変更点近傍におけるロボットの姿勢に関する補間指令値として、経路変更点近傍について得られた姿勢に関する補間指令値を、重ね合わせて得られた値を用いる技術が開示されている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００５−２７５４８４号公報
【特許文献２】特開平１１−２３７９１０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、特許文献１に開示されている技術は、特定の教示点を内回りする軌跡を生成する技術であるから、例えば図１４（特許文献１の図７相当図）に示すように教示点Ｐ２を通過しない軌跡が生成される。そして姿勢は、前記軌跡上の補間点毎に再計算された姿勢をとるので、当初に教示した姿勢が維持されない場合がある。また、特許文献２のようにロボットの姿勢を補間する制御形態を外部ＴＣＰにそのまま適用すると、外部ＴＣＰの場合は姿勢が変化するので、ワークの加工精度が低下する。したがって、これらの従来技術を、外部ＴＣＰに適用することはできない。
【０００７】
基本的に、ティーチングを微小な間隔で精密に行うようにすれば、手先の位置と姿勢とをティーチングした通りに再現することは可能ではあるが、現実問題としてティーチングをそのように行うことは非効率的であり、どうしても補間という処理を導入せざるを得ない。したがって、通常通りの間隔でティーチングを行い、教示点を補間することを前提とした上で問題を解決することが要求されている。
【０００８】
本発明は上記事情に鑑みてなされたものであり、その目的は、ロボットの手先がワークを把持して加工を行う場合の作業精度を向上させることができるロボットの制御装置及びロボットの姿勢補間方法を提供することにある。
【課題を解決するための手段】
【０００９】
請求項１記載のロボットの制御装置によれば、既に教示された手先の移動先である複数のＸ成分，Ｙ成分，Ｚ成分，Ｘ方向回転成分，Ｙ方向回転成分，Ｚ方向回転成分で構成された移動目標点について、姿勢座標変換処理手段は、各移動目標点におけるＸ，Ｙ，Ｚの方向回転成分について回転行列変換処理を行い、方向回転成分毎にＸ成分，Ｙ成分，Ｚ成分で座標化されたＸ，Ｙ，Ｚの各方向姿勢座標値を計算する。姿勢ベクトル増幅処理手段は、計算された移動目標点毎に存在するＸ，Ｙ，Ｚの各方向姿勢座標値について、各移動目標点を原点に、Ｘ，Ｙ，Ｚの各方向姿勢座標値それぞれに向けた単位ベクトルを形成すると、そのベクトルをｎ倍した先の位置をそれぞれ増幅Ｘ，増幅Ｙ，増幅Ｚ方向姿勢座標値とする。増幅姿勢座標値連結処理手段は、移動目標点毎に存在する増幅Ｘ，増幅Ｙ，増幅Ｚ方向姿勢座標値について、増幅Ｘ，増幅Ｙ，増幅Ｚ方向姿勢座標値のみを対象として、それぞれの座標値を滑らかな軌跡として連結する軌跡処理を行うと、得られた軌跡に沿って、各移動目標点間の姿勢を補完して移動する処理を行う。
【００１０】
ここでの「滑らかな」とは、数学的に連続であること（各点が微分可能であること）を意味している。このように制御すれば、ロボットの手先を移動させることで、固定されているツールに対して手先に把持したワークを移動させて加工を行う場合に、手先の姿勢は、各位置についてティーチングを行った際に設定された各軸の姿勢決定座標の移動軌跡がそれぞれ滑らかに繋がるように、つまり、ティーチングを行った際の姿勢が維持された状態で手先が移動する。したがって、固定されているツールの軸に対してワークの加工点を含む面がなす角度が、ティーチングを行った通りに維持される。これにより、ワークの加工精度を向上させるために教示点を増やす必要が無くなるので、ティーチング作業を簡単にすることができる。
【図面の簡単な説明】
【００１１】
【図１】一実施例であり、姿勢補間の手順をイメージ的に示す図
【図２】教示姿勢と各軸姿勢点との関係を示す図
【図３】姿勢補間処理のフローチャート
【図４】（ａ）は本実施例、（ｂ）は従来技術の姿勢ズレを示す図
【図５】（ａ）は位置のズレ数値、（ｂ）は姿勢のズレ数値を、本実施例と従来技術とについて示す図
【図６】手先の移動経路制御のフローチャート
【図７】接線ベクトルを求める方法を示す図
【図８】指令位置とそれら指令位置を連ねた曲線を示す図
【図９】速度パターンを示すグラフ
【図１０】ロボットのシステム構成を示す図
【図１１】制御装置を中心とする機能ブロック図
【図１２】従来技術を説明する図（その１）
【図１３】図１２相当図（その２）
【図１４】従来の位置，姿勢の補間処理を説明する図
【発明を実施するための形態】
【００１２】
以下、一実施例について図１ないし図１１を参照しながら説明する。ロボットは、図１０に示すように、ロボット本体１と、このロボット本体１を制御する制御装置（姿勢決定座標設定手段，姿勢補間手段）２と、ティーチング装置としてのティーチングペンダント３を備えている。この実施例において、制御装置２の制御対象であるロボット本体１は、例えば垂直多関節型の作業用ロボットとして構成され、ベース４と、このベース４に水平方向に旋回可能に設けられたショルダ部５と、このショルダ部５に上下方向に旋回可能に設けられた下アーム６と、この下アーム６に上下方向に旋回可能に且つ捻り回転可能に設けられた上アーム７と、この上アーム７に上下方向に旋回可能に設けられた手首８とを備えている。
【００１３】
手首８は、先端部に捻り回転可能なフランジ９を備えており、フランジ９にはハンド２１が取り付けられて、そのハンド２１がワーク２２を把持している。そして、ショルダ部５の水平方向の旋回動作、下アーム６の上下方向の旋回動作、上アーム７の上下方向の旋回動作、上アーム７の回転動作、手首８の上下方向の旋回動作、フランジ９の回転動作は、それぞれ関節が、駆動源である例えば直流サーボモータからなるモータ１０（図１１参照）により図示しない適宜の伝動機構を介して回転駆動されることにより行われる。
本実施例は、前述した外部ＴＣＰによりワーク２２を加工するもので、工具２３は固定されており、その工具２３に対してロボット本体１がワーク２２を移動させて接触させ、加工を行う。
【００１４】
図１１に示すように、制御装置２は、制御部としてのＣＰＵ１１、各関節のモータ１０を駆動する駆動手段としての駆動回路１２、検出回路１３などを備えている。そして、ＣＰＵ１１には、ロボット全体のシステムプログラム等を記憶したＲＯＭ１４、ロボット本体１の動作プログラム等を記憶したＲＡＭ１５および前記ティーチングペンダント３を接続するためのインターフェース１６が接続されている。なお、図５では、ショルダ部５、下アーム６、上アーム７、手首８およびフランジ９を可動部として一つのブロックで示し、これに応じてそれらの関節の駆動源であるモータ１０も一台だけ示した。
【００１５】
上記検出回路１３は、各関節の現在位置（回転角度）および現在速度（回転速度）を検出するためのもので、この検出回路１３には、各関節を駆動するモータ１０に設けられたロータリエンコーダ１７が接続されている。ロータリエンコーダ１７は位置センサおよび速度センサを兼用するもので、各モータ１０の回転角度に応じたパルス信号を出力し、そのパルス信号は検出回路１３に与えられる。検出回路１３は、各ロータリエンコーダ１７からのパルス信号に基づいて各モータ１０ひいては各関節の現在位置を検出すると共に、単位時間当たり各ロータリエンコーダ１７から入力されるパルス信号数に基づいて各モータ１０ひいては各関節の現在速度を検出し、その位置および速度の情報は、各モータ１０の駆動回路１２およびＣＰＵ１１に与えられるようになっている。
【００１６】
そして、各駆動回路１２は、ＣＰＵ１１から与えられる位置指令値および速度指令値と検出回路１３から与えられる現在位置および現在速度とを比較し、その偏差に応じた電流を各モータ１０に供給してそれらを駆動する。これにより、ロボットアーム先端（手先）であるフランジ９の中心部が指令位置を順に通る経路を辿って動作し、種々の作業を行う。
【００１７】
ロボットアーム先端の移動経路は、ティーチングペンダント３を用いて行う教示作業によって与えられる。この教示作業では、ロボットアーム先端が辿るべき軌跡上の複数の位置が指令位置として順に教示されると共に、各指令位置でのロボットアーム先端の姿勢が教示され、その教示された指令位置と姿勢は、ＲＡＭ１５に記憶される。そして、制御装置２は、実際のロボット作業に際して、与えられた複数の指令位置間を曲線補間してそれら指令位置を順に滑らかに辿る曲線を設定し、ロボットアーム先端がその曲線上を移動するように制御する。
【００１８】
なお、ロボットアーム先端の位置は、フランジ９に固定された３次元座標（ツール座標）の原点がロボット座標（ベース座標）上でどの位置にあるかによって示される。また、ロボットアーム先端の姿勢は、フランジ９に固定の３次元座標の３軸のうち、所定の２軸上の単位ベクトルがロボット座標上で示す向きによって定義される。
【００１９】
さて、上記の複数の指令位置の曲線補間は、スプライン補間方式によって行われる。このスプライン補間の一例を図８によって説明する。図８において、Ｒ０，Ｒ１，……Ｒｎは、（ｎ＋１）個の指令位置であり、Ｒ０が移動開始位置、Ｒｎが移動終了位置である。また、指令位置Ｒ０〜Ｒｎの相互間を結んでいる曲線Ｇ１，Ｇ２……Ｇｎは、指令位置Ｒ０〜Ｒｎの相互間を一区間として各区間について定義されるスプライン曲線セグメントである。スプライン補間では、スプライン曲線セグメントに対応するスプラインセグメント関数により、指令位置を通るスプライン曲線を定義する。
【００２０】
各スプライン曲線セグメント上の各点は、３次元のロボット座標上の点として次の（１）〜（３）によって定めることができる。なお、ｔはパラメータである。
Ｘ（ｔ）＝ａ3ｔ3＋ａ2ｔ2＋ａ1ｔ＋ａ ……（１）
Ｙ（ｔ）＝ｂ3ｔ3＋ｂ2ｔ2＋ｂ1ｔ＋ｂ ……（２）
Ｚ（ｔ）＝ｃ3ｔ3＋ｃ2ｔ2＋ｃ1ｔ＋ｃ ……（３）
この（１）〜（３）式をまとめると、スプライン曲線セグメント上の各点の位置は、次の（４）式に示す３次の一般式（スプラインセグメント関数）によって定義される。
【００２１】
【数１】
【００２２】
【数２】
よって、上記（４）式は、次の（９）式のようになる。
【００２３】
【数３】
以上のように指令位置相互間を一区画としたとき、各区画のスプライン曲線セグメントは上記（９）式の一般式によって表されるが、それら各区画のスプライン曲線セグメントを接続するための条件として、接線ベクトルＴ（ｋ−１）、Ｔｋを次の条件（ア）〜（ウ）により定める。
【００２４】
（ア）接続点が一致していること。
（イ）一区画の両端（始点側指令位置および終点側指令位置）の接線ベクトルの大きさは、両端を結ぶ直線の長さに比例する大きさに定め、且つ、両端の接線ベクトルの向きは、両端を結ぶ直線が、前の一区間についての両端を結ぶ直線および後の一区間について両端を結ぶ直線となす角を、それぞれ２分する方向に定めること。
（ウ）上記のように定めた始点側接線ベクトルの向きが前の区画の終点側接線ベクトルの向きと一致し、終点側接線ベクトルの向きが後の区画の始点側接線ベクトルの向きと一致すること。
【００２５】
この（ア）〜（ウ）の条件を具体的に図７に示す第２の区間（ｋ＝２；指令位置Ｒ１とＲ２の区間）を中心にして具体的に説明する。図７は図８の指令位置Ｒ０〜Ｒ３までを拡大して示している。この図７において、第１の区間（ｋ＝１）の両端（指令位置Ｒ０とＲ１）間を結ぶ直線をＬ１、第２の区間（ｋ＝２）の両端（指令位置Ｒ１とＲ２）間を結ぶ直線をＬ２、第３の区間（ｋ＝３）の両端（指令位置Ｒ２とＲ３）間を結ぶ直線をＬ３とする。
【００２６】
また、第１の区間の両端Ｒ０，Ｒ１の接線ベクトルをそれぞれＴ１ｓ（始点側接線ベクトル），Ｔ１ｅ（終点側接線ベクトル）、第２の区間の両端Ｒ１，Ｒ２の接線ベクトルをそれぞれＴ２ｓ（始点側接線ベクトル），Ｔ２ｅ（終点側接線ベクトル）、第３の区間の両端Ｒ２，Ｒ３の接線ベクトルをそれぞれＴ３ｓ（始点側接線ベクトル），Ｔ３ｅ（終点側接線ベクトル）とする。
【００２７】
すると、第１の区間の終点側接線ベクトルベクトルＴ１ｅ、第２の区間の始点側接線ベクトルＴ２ｓおよび終点側接線ベクトルＴ２ｅ、第３の区間の始点側接線ベクトルＴ２ｓおよび終点側接線ベクトルＴ２ｅは、次の（１０）式〜（１３）式のようになる。ここで、第１の区間は前の区間がないので、始点側接線ベクトルＴ１ｓについては、例えば大きさを直線Ｌ１の長さに比例係数ｒを乗じた値に定め、向きを直線Ｌ１と同じに定める。
【００２８】
【数４】
上記（１０）式〜（１３）式において、ｒはＴ１ｓの大きさを定める際に使用したと同じ値の比例係数であり、本実施例では１と定めている。
【００２９】
ここで、矢印付きのＰ０〜Ｐ３は、指令位置Ｒ０〜Ｒ３の位置ベクトルであるから、（１０）式および（１１）式において、小括弧内の第１式は直線Ｌ１に一致する方向の単位ベクトル、第２式は、直線Ｌ２に一致する方向の単位ベクトルをそれぞれ求めるものであり、同様に（１２）式および（１３）式において、小括弧内の第１式は直線Ｌ２に一致する方向の単位ベクトル、第２式は、直線Ｌ３に一致する方向の単位ベクトルをそれぞれ求めるものである。従って、（１０）式〜（１３）式において、小括弧内の式を演算することによって直線Ｌ１とＬ２とのなす角の差、直線Ｌ２とＬ３とのなす角の差が求められ、それに０．５を乗ずることによって直線Ｌ１とＬ２とのなす角を二分する方向、直線Ｌ２とＬ３とのなす角を二分する方向がそれぞれ求められる。
【００３０】
このようにして各区間についてそれぞれ始点側接線ベクトルおよび終点側接線ベクトルが求められる。そして、各区間について両端の接線ベクトルを求めた後、前述のようにして各区間のスプライン曲線セグメントを表す関数式の４つのスプラインパラメータを求める。すると、各区間の始点側接線ベクトルおよび終点側接線ベクトルが、それぞれ前の区間の終点側接線ベクトルおよび後の区間の始点側接線ベクトルに向きと一致する関数を求めることができ、その結果、各接続点（指令位置）において滑らかに連続するスプライン曲線（スプライン関数）を求めることができる。
【００３１】
次に、上記構成においてロボット作業を行う場合の作用を説明する。ロボット作業を行わせるに先立って、ティーチングペンダント３を用いて実際にロボット本体１を動かしてそのロボットアーム先端の移動開始位置、通過点および移動終了位置を指令位置として教示する。この指令位置および姿勢は、ＲＡＭ１５（記憶手段）に記憶される。
【００３２】
図６に示すように、ロボット作業を行わせるべく、スタート操作を行うと、ロボットの制御装置２は、まず、指令位置をＲＡＭ１５から読み出し（ステップＳ１１）、移動経路の両端、つまり移動開始位置および移動終了位置の接線ベクトルを設定する（ステップＳ１２）。この場合、移動開始位置での接線ベクトルは、移動開始位置と次の指令位置とを結ぶ直線と同じ方向を向き、次の指令位置までの直線の長さに比例する大きさとする。また、移動終点位置での接線ベクトルは、直ぐ前の指令位置と移動終点位置とを結ぶ直線と同じ方向を向き、直ぐ前の指令位置と移動終点位置とを結ぶ直線の長さに比例する大きさとする。なお、この移動開始位置および移動終了位置での接線ベクトルの向きと大きさはこれに限られるものではなく、前後の作業を考慮して定めることができ、また、大きさは０であっても良い。
【００３３】
次に、制御装置２は、移動開始位置から移動終了位置までを指令位置で複数区画に分割し、区間毎に前述の（１０）式〜（１３）式によって始点側接線ベクトルと終点側接線ベクトルとを演算する（ステップＳ１３）。そして、制御装置２は、各区間のスプライン曲線セグメントの関数式（スプラインセグメント関数）のベクトル係数を前述した（５）式〜（８）式によって求める（ステップＳ１４）。
【００３４】
このようにして各区間のスプラインセグメント関数のベクトル係数が求まると、移動始点位置から移動終点位置までの全指令位置を滑らかな曲線で結ぶスプライン曲線が求められたこととなるので、その後、制御装置２は、このスプラインセグメント関数である（９）式を用いて、具体的には（９）式をｔ（例えば０≦ｔ≦１）で微分して求めた式を０から１まで積分することにより、移動開始位置から移動終了位置までの総曲線距離を計算する（ステップＳ１５）。
【００３５】
求められた総曲線距離はロボットアーム先端の移動開始位置から移動終了位置までの移動距離であるから、次に、制御装置２は、総曲線距離から図９（ａ）に示す速度パターンを生成する（ステップＳ１６）。この速度パターンは、ロボットアーム先端の速度が台形となるように定めるもので、ロボットアーム先端が移動開始位置から予め定められた一定の加速度で予め定められた最高速度となるまで速度を上昇させながら移動し、予め定められた最高速度に達したら、その最高速度で等速度移動し、そして、予め定められた減速度で減速して移動終了位置に至るような速度パターンを生成する。
【００３６】
速度パターンを生成すると、制御装置２は、速度パターンから一定のサンプリング時間内でのロボットアーム先端の移動距離を求め、今回のサンプリング時間内での移動距離を前のサンプリング時間内での移動終了位置にプラスして、今回のサンプリング時間でのスプライン曲線上の移動終了位置を求めると共に、ロボットアーム先端の姿勢を計算する（ステップＳ１７）。
【００３７】
そして、制御装置２は、サンプリング時間内でロボットアーム先端が移動開始位置から移動終了位置まで移動するように各アームの角速度および位置を計算し（ステップＳ１８）、各アームのモータ１０に速度指令値および位置指令値を出力する（ステップＳ１９）。それから、制御装置２は、上記のステップＳ１７〜ステップＳ１９を繰り返すことによって、ロボットアーム先端を移動終了位置まで（ステップＳ２０：ＹＥＳ）動作させる。
【００３８】
ここで、本実施例では、ロボット本体１を外部ＴＣＰで動作させるに際して、上述した指令位置について行った補間を手先の姿勢についても同様に適用して補間を行う。以下、この姿勢補間処理について図１ないし図５を参照して説明する。図３は、姿勢補間処理を示すフローチャートである。上述したように、ティーチングペンダント３を用いて位置及び姿勢について教示点が設定されると（ステップＳ１）、ロボット本体１を固定工具周りで動作させるため、教示点（ベース座標から見たツール座標，図１０参照）が、ツール（手先）から見た工具２３への位置，姿勢（ツール座標から見た固定工具座標）となるように座標変換する（ステップＳ２）。
【００３９】
尚、図１０中に示す座標変換行列式は、外部ＴＣＰではあるが従来との整合を図るため、手先をツール（ｔ），固定工具２３をワーク（ｗ）と称している。そして、ベース座標からツール座標への変換を行う行列式は、ベース座標から固定工具座標への変換行列と、ツール座標から固定工具座標への変換行列の逆行列との積で表わされることを示している（右辺第２式は、固定工具座標からツール座標への変換行列に等しい）。
【００４０】
次に、各教示位置におけるそれぞれの教示姿勢を座標値として特定する。教示された姿勢は回転角度値（Ｘ方向回転成分，Ｙ方向回転成分，Ｚ方向回転成分）で与えられているので、回転行列を用いて空間座標値（Ｘ方向姿勢座標値，Ｙ方向姿勢座標値，Ｚ方向姿勢座標値）に変換する（回転行列変換処理，姿勢座標変換処理手段）。各位置における手先の姿勢に対応する回転行列が、変換された座標によって表現される。すなわち、この回転行列が、手先の姿勢を特定するノーマルベクトル（Ｘ軸），オリエントベクトル（Ｙ軸），アプローチベクトル（Ｚ軸）を、それぞれベース座標上で表現した３次元座標値となる。また、各教示位置の各方向座標値と、当該教示位置における各方向姿勢座標値とからなるものが移動目標位置に対応する。
【００４１】
そして、教示位置を原点とするＸ，Ｙ，Ｚの各軸方向に、任意の長さを伸ばして仮想した点を姿勢点（増幅Ｘ方向姿勢座標値，増幅Ｙ方向姿勢座標値，増幅Ｚ方向姿勢座標値）とする（ステップＳ３，姿勢ベクトル増幅処理手段）。すなわち、図２に示すように、教示姿勢Ｄを、教示位置（ＳＣ）を原点にして立つ方向ベクトルと考えれば、その方向ベクトルの３次元座標値（ＳＮ，ＳＯ，ＳＡ）を決定することで教示姿勢が特定される。尚、「任意の長さ」は教示姿勢を特定可能な長さであれば良いので、例えば上記座標系の単位ベクトルのｎ（＞２）倍等とする。
【００４２】
ここで、回転行列を用いて表わされた姿勢を示す座標値は、ゼロに近い値となる。補間を行うための演算は、制御装置２に内蔵されるＣＰＵで行われるが、ＣＰＵには計算限界桁があるので、最初からゼロに近い値を用いるとその計算限界桁を超えた部分に各教示点での姿勢に影響を与える値が存在することになる。しかし、ＣＰＵの計算上ではその微妙な差が計算誤差として飲み込まれてしまうので、姿勢が滑らかに変化するように補間値を演算できなくなる。そこで、そのような事態を回避するためベクトルの長さをｎ倍して、ＣＰＵの計算限界桁内で姿勢を滑らかに変化させるように演算するための値を取り込むようにする。
【００４３】
ステップＳ３の処理により、１つの教示姿勢について３つの姿勢点がそれぞれ３次元座標値として決定されるが、これらを「位置」と同様に扱うことで教示姿勢を補間することができる。すなわち、Ｘ，Ｙ，Ｚそれぞれの姿勢点を，例えば上述した教示位置のようにスプライン曲線によって、滑らかな軌跡としてそれぞれ連結するように軌跡処理を行い補間する（ステップＳ４，増幅姿勢座標値連結処理手段）。
【００４４】
ここで、図１は、教示位置並びに教示姿勢を補間する処理をイメージ的に示している。尚、図示が煩雑になるのを避けるため、姿勢点についてはＹ，Ｚの２軸分だけ示している。図１（ａ），（ｂ）は、各教示位置を曲線補間した場合を示し、図１（ｃ）は、各教示位置における教示姿勢についてＹ方向姿勢点及びＺ方向姿勢点を決定した場合であり、各矢印がオリエントベクトル，アプローチベクトルに対応する。そして、図１（ｄ）はＹ方向姿勢点を曲線補間した場合、図１（ｅ）はＺ方向姿勢点を曲線補間した場合である。また、図示しないＸ方向（ノーマルベクトル）姿勢点についても同様に曲線補間する。
【００４５】
ステップＳ４では、Ｘ，Ｙ，Ｚの各方向について姿勢点を独立に補間したが、これらの各方向の補間点については３次元座標の各軸に対応することから、互いの関係が正規直交条件を満たす必要がある。したがって、その条件を満たすように修正を行う（ステップＳ５）。以上で教示姿勢の補間処理が完了する。
【００４６】
それから、ステップＳ５で得た補間点を、ベースから見たツールの位置・姿勢となるように座標変換すると（ステップＳ６）、補間点を加えて得られた手先の軌道（Ｘ，Ｙ，Ｚ各方向姿勢点の軌道）を、各軸Ｊ１〜Ｊ６の角度に変換する（ステップＳ７）。以上で姿勢の補間処理が終了する。
【００４７】
ここで、図４は、教示点におけるアプローチベクトルの教示姿勢（ＳＡ）に対して、（ｂ）は特許文献１，２のような従来の補間技術を適用して、手先が教示点を通過した場合のアプローチベクトルを３次元的に示している。（ｂ）では教示姿勢に対してズレを生じているが、本実施例の姿勢補間技術を適用した場合の（ａ）では、姿勢のズレが略生じていないことが判る。
【００４８】
また図５は、位置，姿勢の教示点について、従来の補間技術と本実施例の補間技術とのズレの発生量を示したものである。尚、同図中に示すワーク２２及び工具２３は、図１０に対応しないモデル的なものである。（ａ）の位置ずれについては、従来と本実施例とでは何れも０．１ｍｍ未満の誤差であり有意な差はない。一方、（ｂ）の姿勢ずれについては、従来技術では誤差角度が５［ｄｅｇ］を超える場合もあるが、本実施例では、誤差角度が０．１［ｄｅｇ］以下となっており、大きく改善されている。
【００４９】
以上のように本実施例によれば、制御装置２は、手先の先端点を座標原点とするＸ方向，Ｙ方向，Ｚ方向の各軸について手先の位置及び姿勢をティーチングする場合に、既に教示された手先の移動先である複数のＸ成分，Ｙ成分，Ｚ成分，Ｘ方向回転成分，Ｙ方向回転成分，Ｚ方向回転成分で構成された移動目標点について、各移動目標点におけるＸ，Ｙ，Ｚの方向回転成分について回転行列変換処理を行い、方向回転成分毎にＸ成分，Ｙ成分，Ｚ成分で座標化されたＸ，Ｙ，Ｚの各方向姿勢座標値を計算する。それから、計算された移動目標点毎に存在するＸ，Ｙ，Ｚの各方向姿勢座標値について、各移動目標点を原点に、Ｘ，Ｙ，Ｚの各方向姿勢座標値それぞれに向けた単位ベクトルを形成すると、そのベクトルをｎ倍した先の位置をそれぞれ増幅Ｘ，増幅Ｙ，増幅Ｚ方向姿勢座標値とする。そして、移動目標点毎に存在する増幅Ｘ，増幅Ｙ，増幅Ｚ方向姿勢座標値について、増幅Ｘ，増幅Ｙ，増幅Ｚ方向姿勢座標値のみを対象として、それぞれの座標値を滑らかな軌跡として連結する軌跡処理を行うと、得られた軌跡に沿って、各移動目標点間の姿勢を補完して移動する処理を行うようにした。
【００５０】
これにより、ロボット本体１の手先を移動させることで、固定されている工具２３に対してワーク２２を移動させて加工を行う場合に、手先の姿勢は、教示された姿勢が維持されながら手先が移動する。したがって、固定されている工具２３の軸に対して、ワーク２２の加工点を含む面がなす角度がティーチングを行った通りに維持されるので、ワーク２２の加工精度を向上させるために教示点を増やす必要が無くなるため、ティーチング作業を簡単にすることができる。
【００５１】
本発明は上記し、又は図面に記載した実施例にのみ限定されるものではなく、以下のような変型又は拡張が可能である。
姿勢の補間については、スプライン曲線を用いるものに限ることなく、各軸の姿勢決定座標の移動軌跡がそれぞれ滑らかに（数学的に連続となるように）繋がるように補間すれば良い。
【符号の説明】
【００５２】
図面中、１はロボット本体、２は制御装置（姿勢座標変換処理手段，姿勢ベクトル増幅処理手段，増幅姿勢座標値連結処理手段）、３はティーチングペンダント、９はフランジ（手先）である。
【技術分野】
【０００１】
本発明は、ロボットの手先がワークを把持し、固定されているツールに対して前記ワークを移動させて加工を行う際に、ロボットの姿勢を補間する制御装置及び方法に関する。
【背景技術】
【０００２】
従来、ロボットを用いてワークの加工等を行う場合には、ワークを固定し、ロボットの手先がツールを持って作業することが一般的である。しかし、例えばシール塗布やバリ取りといった一部の作業については、シーリングガンやリュータ等のツールを固定し、垂直多関節型のロボットの手先がワークを把持して作業する場合があり、このような作業形態を外部ＴＣＰ（Tool Center Point；先端点）と称することがある。
【０００３】
外部ＴＣＰでは、例えば図１２に示すようにシール塗布の場合、ワークの加工点（塗布点）を含む平面が、シーリングガンによる塗料の吐出方向に対してなす角度が常に９０度となるように維持することが望ましい。図１２（ａ）に示すように、ワークの形状が平面である場合、手先の姿勢は一定に維持して位置だけを変化させれば良いので問題とならない。しかし、図１２（ｂ）に示すようにワークの形状が曲面を含んでいる場合は、ロボットが手先の姿勢を連続して微妙に変化させることで、上述した角度を９０度にする必要がある。したがって、姿勢が当初にティーチングした通りに維持されなければ、ワークの加工精度が低下するおそれがある。
また、図１３に示すバリ取りの場合には、ワークの複雑な外形に応じて、エッジの各点がリュータに対して常に最適な角度で当たるように、ロボットの手先の姿勢をティーチングした通りに制御する必要があり、姿勢が維持されないとバリが取り切れず加工精度が低下する。
【０００４】
外部ＴＣＰへの適用を意識した技術ではないが、例えば特許文献１には、ロボットの手先が教示点Ｐ１，Ｐ２，Ｐ３を経由して連続的に動作する場合、各教示点における姿勢（回転行列）をＲ１，Ｒ２，Ｒ３とすると、姿勢Ｒ１から姿勢Ｒ２への変化量に関する等価回転角ベクトルＫ１２と、姿勢Ｒ２から姿勢Ｒ３への同等価回転角ベクトルＫ２３とを算出し、これらに基づき中間の教示点Ｐ２近傍の仮想的な内回り軌跡を算出し、この仮想的な内回り軌跡に対して指定された回転速度でサンプリング補間を行い、各補間点において移動方向に隣接する補間点への変化量に関する等価回転角ベクトルを算出し、その等価回転角ベクトルに基づいて動作軌跡を生成する技術が開示されている。また、特許文献２には、ロボットの急激な姿勢変化を抑制するため、経路変更点近傍におけるロボットの姿勢に関する補間指令値として、経路変更点近傍について得られた姿勢に関する補間指令値を、重ね合わせて得られた値を用いる技術が開示されている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００５−２７５４８４号公報
【特許文献２】特開平１１−２３７９１０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、特許文献１に開示されている技術は、特定の教示点を内回りする軌跡を生成する技術であるから、例えば図１４（特許文献１の図７相当図）に示すように教示点Ｐ２を通過しない軌跡が生成される。そして姿勢は、前記軌跡上の補間点毎に再計算された姿勢をとるので、当初に教示した姿勢が維持されない場合がある。また、特許文献２のようにロボットの姿勢を補間する制御形態を外部ＴＣＰにそのまま適用すると、外部ＴＣＰの場合は姿勢が変化するので、ワークの加工精度が低下する。したがって、これらの従来技術を、外部ＴＣＰに適用することはできない。
【０００７】
基本的に、ティーチングを微小な間隔で精密に行うようにすれば、手先の位置と姿勢とをティーチングした通りに再現することは可能ではあるが、現実問題としてティーチングをそのように行うことは非効率的であり、どうしても補間という処理を導入せざるを得ない。したがって、通常通りの間隔でティーチングを行い、教示点を補間することを前提とした上で問題を解決することが要求されている。
【０００８】
本発明は上記事情に鑑みてなされたものであり、その目的は、ロボットの手先がワークを把持して加工を行う場合の作業精度を向上させることができるロボットの制御装置及びロボットの姿勢補間方法を提供することにある。
【課題を解決するための手段】
【０００９】
請求項１記載のロボットの制御装置によれば、既に教示された手先の移動先である複数のＸ成分，Ｙ成分，Ｚ成分，Ｘ方向回転成分，Ｙ方向回転成分，Ｚ方向回転成分で構成された移動目標点について、姿勢座標変換処理手段は、各移動目標点におけるＸ，Ｙ，Ｚの方向回転成分について回転行列変換処理を行い、方向回転成分毎にＸ成分，Ｙ成分，Ｚ成分で座標化されたＸ，Ｙ，Ｚの各方向姿勢座標値を計算する。姿勢ベクトル増幅処理手段は、計算された移動目標点毎に存在するＸ，Ｙ，Ｚの各方向姿勢座標値について、各移動目標点を原点に、Ｘ，Ｙ，Ｚの各方向姿勢座標値それぞれに向けた単位ベクトルを形成すると、そのベクトルをｎ倍した先の位置をそれぞれ増幅Ｘ，増幅Ｙ，増幅Ｚ方向姿勢座標値とする。増幅姿勢座標値連結処理手段は、移動目標点毎に存在する増幅Ｘ，増幅Ｙ，増幅Ｚ方向姿勢座標値について、増幅Ｘ，増幅Ｙ，増幅Ｚ方向姿勢座標値のみを対象として、それぞれの座標値を滑らかな軌跡として連結する軌跡処理を行うと、得られた軌跡に沿って、各移動目標点間の姿勢を補完して移動する処理を行う。
【００１０】
ここでの「滑らかな」とは、数学的に連続であること（各点が微分可能であること）を意味している。このように制御すれば、ロボットの手先を移動させることで、固定されているツールに対して手先に把持したワークを移動させて加工を行う場合に、手先の姿勢は、各位置についてティーチングを行った際に設定された各軸の姿勢決定座標の移動軌跡がそれぞれ滑らかに繋がるように、つまり、ティーチングを行った際の姿勢が維持された状態で手先が移動する。したがって、固定されているツールの軸に対してワークの加工点を含む面がなす角度が、ティーチングを行った通りに維持される。これにより、ワークの加工精度を向上させるために教示点を増やす必要が無くなるので、ティーチング作業を簡単にすることができる。
【図面の簡単な説明】
【００１１】
【図１】一実施例であり、姿勢補間の手順をイメージ的に示す図
【図２】教示姿勢と各軸姿勢点との関係を示す図
【図３】姿勢補間処理のフローチャート
【図４】（ａ）は本実施例、（ｂ）は従来技術の姿勢ズレを示す図
【図５】（ａ）は位置のズレ数値、（ｂ）は姿勢のズレ数値を、本実施例と従来技術とについて示す図
【図６】手先の移動経路制御のフローチャート
【図７】接線ベクトルを求める方法を示す図
【図８】指令位置とそれら指令位置を連ねた曲線を示す図
【図９】速度パターンを示すグラフ
【図１０】ロボットのシステム構成を示す図
【図１１】制御装置を中心とする機能ブロック図
【図１２】従来技術を説明する図（その１）
【図１３】図１２相当図（その２）
【図１４】従来の位置，姿勢の補間処理を説明する図
【発明を実施するための形態】
【００１２】
以下、一実施例について図１ないし図１１を参照しながら説明する。ロボットは、図１０に示すように、ロボット本体１と、このロボット本体１を制御する制御装置（姿勢決定座標設定手段，姿勢補間手段）２と、ティーチング装置としてのティーチングペンダント３を備えている。この実施例において、制御装置２の制御対象であるロボット本体１は、例えば垂直多関節型の作業用ロボットとして構成され、ベース４と、このベース４に水平方向に旋回可能に設けられたショルダ部５と、このショルダ部５に上下方向に旋回可能に設けられた下アーム６と、この下アーム６に上下方向に旋回可能に且つ捻り回転可能に設けられた上アーム７と、この上アーム７に上下方向に旋回可能に設けられた手首８とを備えている。
【００１３】
手首８は、先端部に捻り回転可能なフランジ９を備えており、フランジ９にはハンド２１が取り付けられて、そのハンド２１がワーク２２を把持している。そして、ショルダ部５の水平方向の旋回動作、下アーム６の上下方向の旋回動作、上アーム７の上下方向の旋回動作、上アーム７の回転動作、手首８の上下方向の旋回動作、フランジ９の回転動作は、それぞれ関節が、駆動源である例えば直流サーボモータからなるモータ１０（図１１参照）により図示しない適宜の伝動機構を介して回転駆動されることにより行われる。
本実施例は、前述した外部ＴＣＰによりワーク２２を加工するもので、工具２３は固定されており、その工具２３に対してロボット本体１がワーク２２を移動させて接触させ、加工を行う。
【００１４】
図１１に示すように、制御装置２は、制御部としてのＣＰＵ１１、各関節のモータ１０を駆動する駆動手段としての駆動回路１２、検出回路１３などを備えている。そして、ＣＰＵ１１には、ロボット全体のシステムプログラム等を記憶したＲＯＭ１４、ロボット本体１の動作プログラム等を記憶したＲＡＭ１５および前記ティーチングペンダント３を接続するためのインターフェース１６が接続されている。なお、図５では、ショルダ部５、下アーム６、上アーム７、手首８およびフランジ９を可動部として一つのブロックで示し、これに応じてそれらの関節の駆動源であるモータ１０も一台だけ示した。
【００１５】
上記検出回路１３は、各関節の現在位置（回転角度）および現在速度（回転速度）を検出するためのもので、この検出回路１３には、各関節を駆動するモータ１０に設けられたロータリエンコーダ１７が接続されている。ロータリエンコーダ１７は位置センサおよび速度センサを兼用するもので、各モータ１０の回転角度に応じたパルス信号を出力し、そのパルス信号は検出回路１３に与えられる。検出回路１３は、各ロータリエンコーダ１７からのパルス信号に基づいて各モータ１０ひいては各関節の現在位置を検出すると共に、単位時間当たり各ロータリエンコーダ１７から入力されるパルス信号数に基づいて各モータ１０ひいては各関節の現在速度を検出し、その位置および速度の情報は、各モータ１０の駆動回路１２およびＣＰＵ１１に与えられるようになっている。
【００１６】
そして、各駆動回路１２は、ＣＰＵ１１から与えられる位置指令値および速度指令値と検出回路１３から与えられる現在位置および現在速度とを比較し、その偏差に応じた電流を各モータ１０に供給してそれらを駆動する。これにより、ロボットアーム先端（手先）であるフランジ９の中心部が指令位置を順に通る経路を辿って動作し、種々の作業を行う。
【００１７】
ロボットアーム先端の移動経路は、ティーチングペンダント３を用いて行う教示作業によって与えられる。この教示作業では、ロボットアーム先端が辿るべき軌跡上の複数の位置が指令位置として順に教示されると共に、各指令位置でのロボットアーム先端の姿勢が教示され、その教示された指令位置と姿勢は、ＲＡＭ１５に記憶される。そして、制御装置２は、実際のロボット作業に際して、与えられた複数の指令位置間を曲線補間してそれら指令位置を順に滑らかに辿る曲線を設定し、ロボットアーム先端がその曲線上を移動するように制御する。
【００１８】
なお、ロボットアーム先端の位置は、フランジ９に固定された３次元座標（ツール座標）の原点がロボット座標（ベース座標）上でどの位置にあるかによって示される。また、ロボットアーム先端の姿勢は、フランジ９に固定の３次元座標の３軸のうち、所定の２軸上の単位ベクトルがロボット座標上で示す向きによって定義される。
【００１９】
さて、上記の複数の指令位置の曲線補間は、スプライン補間方式によって行われる。このスプライン補間の一例を図８によって説明する。図８において、Ｒ０，Ｒ１，……Ｒｎは、（ｎ＋１）個の指令位置であり、Ｒ０が移動開始位置、Ｒｎが移動終了位置である。また、指令位置Ｒ０〜Ｒｎの相互間を結んでいる曲線Ｇ１，Ｇ２……Ｇｎは、指令位置Ｒ０〜Ｒｎの相互間を一区間として各区間について定義されるスプライン曲線セグメントである。スプライン補間では、スプライン曲線セグメントに対応するスプラインセグメント関数により、指令位置を通るスプライン曲線を定義する。
【００２０】
各スプライン曲線セグメント上の各点は、３次元のロボット座標上の点として次の（１）〜（３）によって定めることができる。なお、ｔはパラメータである。
Ｘ（ｔ）＝ａ3ｔ3＋ａ2ｔ2＋ａ1ｔ＋ａ ……（１）
Ｙ（ｔ）＝ｂ3ｔ3＋ｂ2ｔ2＋ｂ1ｔ＋ｂ ……（２）
Ｚ（ｔ）＝ｃ3ｔ3＋ｃ2ｔ2＋ｃ1ｔ＋ｃ ……（３）
この（１）〜（３）式をまとめると、スプライン曲線セグメント上の各点の位置は、次の（４）式に示す３次の一般式（スプラインセグメント関数）によって定義される。
【００２１】
【数１】
【００２２】
【数２】
よって、上記（４）式は、次の（９）式のようになる。
【００２３】
【数３】
以上のように指令位置相互間を一区画としたとき、各区画のスプライン曲線セグメントは上記（９）式の一般式によって表されるが、それら各区画のスプライン曲線セグメントを接続するための条件として、接線ベクトルＴ（ｋ−１）、Ｔｋを次の条件（ア）〜（ウ）により定める。
【００２４】
（ア）接続点が一致していること。
（イ）一区画の両端（始点側指令位置および終点側指令位置）の接線ベクトルの大きさは、両端を結ぶ直線の長さに比例する大きさに定め、且つ、両端の接線ベクトルの向きは、両端を結ぶ直線が、前の一区間についての両端を結ぶ直線および後の一区間について両端を結ぶ直線となす角を、それぞれ２分する方向に定めること。
（ウ）上記のように定めた始点側接線ベクトルの向きが前の区画の終点側接線ベクトルの向きと一致し、終点側接線ベクトルの向きが後の区画の始点側接線ベクトルの向きと一致すること。
【００２５】
この（ア）〜（ウ）の条件を具体的に図７に示す第２の区間（ｋ＝２；指令位置Ｒ１とＲ２の区間）を中心にして具体的に説明する。図７は図８の指令位置Ｒ０〜Ｒ３までを拡大して示している。この図７において、第１の区間（ｋ＝１）の両端（指令位置Ｒ０とＲ１）間を結ぶ直線をＬ１、第２の区間（ｋ＝２）の両端（指令位置Ｒ１とＲ２）間を結ぶ直線をＬ２、第３の区間（ｋ＝３）の両端（指令位置Ｒ２とＲ３）間を結ぶ直線をＬ３とする。
【００２６】
また、第１の区間の両端Ｒ０，Ｒ１の接線ベクトルをそれぞれＴ１ｓ（始点側接線ベクトル），Ｔ１ｅ（終点側接線ベクトル）、第２の区間の両端Ｒ１，Ｒ２の接線ベクトルをそれぞれＴ２ｓ（始点側接線ベクトル），Ｔ２ｅ（終点側接線ベクトル）、第３の区間の両端Ｒ２，Ｒ３の接線ベクトルをそれぞれＴ３ｓ（始点側接線ベクトル），Ｔ３ｅ（終点側接線ベクトル）とする。
【００２７】
すると、第１の区間の終点側接線ベクトルベクトルＴ１ｅ、第２の区間の始点側接線ベクトルＴ２ｓおよび終点側接線ベクトルＴ２ｅ、第３の区間の始点側接線ベクトルＴ２ｓおよび終点側接線ベクトルＴ２ｅは、次の（１０）式〜（１３）式のようになる。ここで、第１の区間は前の区間がないので、始点側接線ベクトルＴ１ｓについては、例えば大きさを直線Ｌ１の長さに比例係数ｒを乗じた値に定め、向きを直線Ｌ１と同じに定める。
【００２８】
【数４】
上記（１０）式〜（１３）式において、ｒはＴ１ｓの大きさを定める際に使用したと同じ値の比例係数であり、本実施例では１と定めている。
【００２９】
ここで、矢印付きのＰ０〜Ｐ３は、指令位置Ｒ０〜Ｒ３の位置ベクトルであるから、（１０）式および（１１）式において、小括弧内の第１式は直線Ｌ１に一致する方向の単位ベクトル、第２式は、直線Ｌ２に一致する方向の単位ベクトルをそれぞれ求めるものであり、同様に（１２）式および（１３）式において、小括弧内の第１式は直線Ｌ２に一致する方向の単位ベクトル、第２式は、直線Ｌ３に一致する方向の単位ベクトルをそれぞれ求めるものである。従って、（１０）式〜（１３）式において、小括弧内の式を演算することによって直線Ｌ１とＬ２とのなす角の差、直線Ｌ２とＬ３とのなす角の差が求められ、それに０．５を乗ずることによって直線Ｌ１とＬ２とのなす角を二分する方向、直線Ｌ２とＬ３とのなす角を二分する方向がそれぞれ求められる。
【００３０】
このようにして各区間についてそれぞれ始点側接線ベクトルおよび終点側接線ベクトルが求められる。そして、各区間について両端の接線ベクトルを求めた後、前述のようにして各区間のスプライン曲線セグメントを表す関数式の４つのスプラインパラメータを求める。すると、各区間の始点側接線ベクトルおよび終点側接線ベクトルが、それぞれ前の区間の終点側接線ベクトルおよび後の区間の始点側接線ベクトルに向きと一致する関数を求めることができ、その結果、各接続点（指令位置）において滑らかに連続するスプライン曲線（スプライン関数）を求めることができる。
【００３１】
次に、上記構成においてロボット作業を行う場合の作用を説明する。ロボット作業を行わせるに先立って、ティーチングペンダント３を用いて実際にロボット本体１を動かしてそのロボットアーム先端の移動開始位置、通過点および移動終了位置を指令位置として教示する。この指令位置および姿勢は、ＲＡＭ１５（記憶手段）に記憶される。
【００３２】
図６に示すように、ロボット作業を行わせるべく、スタート操作を行うと、ロボットの制御装置２は、まず、指令位置をＲＡＭ１５から読み出し（ステップＳ１１）、移動経路の両端、つまり移動開始位置および移動終了位置の接線ベクトルを設定する（ステップＳ１２）。この場合、移動開始位置での接線ベクトルは、移動開始位置と次の指令位置とを結ぶ直線と同じ方向を向き、次の指令位置までの直線の長さに比例する大きさとする。また、移動終点位置での接線ベクトルは、直ぐ前の指令位置と移動終点位置とを結ぶ直線と同じ方向を向き、直ぐ前の指令位置と移動終点位置とを結ぶ直線の長さに比例する大きさとする。なお、この移動開始位置および移動終了位置での接線ベクトルの向きと大きさはこれに限られるものではなく、前後の作業を考慮して定めることができ、また、大きさは０であっても良い。
【００３３】
次に、制御装置２は、移動開始位置から移動終了位置までを指令位置で複数区画に分割し、区間毎に前述の（１０）式〜（１３）式によって始点側接線ベクトルと終点側接線ベクトルとを演算する（ステップＳ１３）。そして、制御装置２は、各区間のスプライン曲線セグメントの関数式（スプラインセグメント関数）のベクトル係数を前述した（５）式〜（８）式によって求める（ステップＳ１４）。
【００３４】
このようにして各区間のスプラインセグメント関数のベクトル係数が求まると、移動始点位置から移動終点位置までの全指令位置を滑らかな曲線で結ぶスプライン曲線が求められたこととなるので、その後、制御装置２は、このスプラインセグメント関数である（９）式を用いて、具体的には（９）式をｔ（例えば０≦ｔ≦１）で微分して求めた式を０から１まで積分することにより、移動開始位置から移動終了位置までの総曲線距離を計算する（ステップＳ１５）。
【００３５】
求められた総曲線距離はロボットアーム先端の移動開始位置から移動終了位置までの移動距離であるから、次に、制御装置２は、総曲線距離から図９（ａ）に示す速度パターンを生成する（ステップＳ１６）。この速度パターンは、ロボットアーム先端の速度が台形となるように定めるもので、ロボットアーム先端が移動開始位置から予め定められた一定の加速度で予め定められた最高速度となるまで速度を上昇させながら移動し、予め定められた最高速度に達したら、その最高速度で等速度移動し、そして、予め定められた減速度で減速して移動終了位置に至るような速度パターンを生成する。
【００３６】
速度パターンを生成すると、制御装置２は、速度パターンから一定のサンプリング時間内でのロボットアーム先端の移動距離を求め、今回のサンプリング時間内での移動距離を前のサンプリング時間内での移動終了位置にプラスして、今回のサンプリング時間でのスプライン曲線上の移動終了位置を求めると共に、ロボットアーム先端の姿勢を計算する（ステップＳ１７）。
【００３７】
そして、制御装置２は、サンプリング時間内でロボットアーム先端が移動開始位置から移動終了位置まで移動するように各アームの角速度および位置を計算し（ステップＳ１８）、各アームのモータ１０に速度指令値および位置指令値を出力する（ステップＳ１９）。それから、制御装置２は、上記のステップＳ１７〜ステップＳ１９を繰り返すことによって、ロボットアーム先端を移動終了位置まで（ステップＳ２０：ＹＥＳ）動作させる。
【００３８】
ここで、本実施例では、ロボット本体１を外部ＴＣＰで動作させるに際して、上述した指令位置について行った補間を手先の姿勢についても同様に適用して補間を行う。以下、この姿勢補間処理について図１ないし図５を参照して説明する。図３は、姿勢補間処理を示すフローチャートである。上述したように、ティーチングペンダント３を用いて位置及び姿勢について教示点が設定されると（ステップＳ１）、ロボット本体１を固定工具周りで動作させるため、教示点（ベース座標から見たツール座標，図１０参照）が、ツール（手先）から見た工具２３への位置，姿勢（ツール座標から見た固定工具座標）となるように座標変換する（ステップＳ２）。
【００３９】
尚、図１０中に示す座標変換行列式は、外部ＴＣＰではあるが従来との整合を図るため、手先をツール（ｔ），固定工具２３をワーク（ｗ）と称している。そして、ベース座標からツール座標への変換を行う行列式は、ベース座標から固定工具座標への変換行列と、ツール座標から固定工具座標への変換行列の逆行列との積で表わされることを示している（右辺第２式は、固定工具座標からツール座標への変換行列に等しい）。
【００４０】
次に、各教示位置におけるそれぞれの教示姿勢を座標値として特定する。教示された姿勢は回転角度値（Ｘ方向回転成分，Ｙ方向回転成分，Ｚ方向回転成分）で与えられているので、回転行列を用いて空間座標値（Ｘ方向姿勢座標値，Ｙ方向姿勢座標値，Ｚ方向姿勢座標値）に変換する（回転行列変換処理，姿勢座標変換処理手段）。各位置における手先の姿勢に対応する回転行列が、変換された座標によって表現される。すなわち、この回転行列が、手先の姿勢を特定するノーマルベクトル（Ｘ軸），オリエントベクトル（Ｙ軸），アプローチベクトル（Ｚ軸）を、それぞれベース座標上で表現した３次元座標値となる。また、各教示位置の各方向座標値と、当該教示位置における各方向姿勢座標値とからなるものが移動目標位置に対応する。
【００４１】
そして、教示位置を原点とするＸ，Ｙ，Ｚの各軸方向に、任意の長さを伸ばして仮想した点を姿勢点（増幅Ｘ方向姿勢座標値，増幅Ｙ方向姿勢座標値，増幅Ｚ方向姿勢座標値）とする（ステップＳ３，姿勢ベクトル増幅処理手段）。すなわち、図２に示すように、教示姿勢Ｄを、教示位置（ＳＣ）を原点にして立つ方向ベクトルと考えれば、その方向ベクトルの３次元座標値（ＳＮ，ＳＯ，ＳＡ）を決定することで教示姿勢が特定される。尚、「任意の長さ」は教示姿勢を特定可能な長さであれば良いので、例えば上記座標系の単位ベクトルのｎ（＞２）倍等とする。
【００４２】
ここで、回転行列を用いて表わされた姿勢を示す座標値は、ゼロに近い値となる。補間を行うための演算は、制御装置２に内蔵されるＣＰＵで行われるが、ＣＰＵには計算限界桁があるので、最初からゼロに近い値を用いるとその計算限界桁を超えた部分に各教示点での姿勢に影響を与える値が存在することになる。しかし、ＣＰＵの計算上ではその微妙な差が計算誤差として飲み込まれてしまうので、姿勢が滑らかに変化するように補間値を演算できなくなる。そこで、そのような事態を回避するためベクトルの長さをｎ倍して、ＣＰＵの計算限界桁内で姿勢を滑らかに変化させるように演算するための値を取り込むようにする。
【００４３】
ステップＳ３の処理により、１つの教示姿勢について３つの姿勢点がそれぞれ３次元座標値として決定されるが、これらを「位置」と同様に扱うことで教示姿勢を補間することができる。すなわち、Ｘ，Ｙ，Ｚそれぞれの姿勢点を，例えば上述した教示位置のようにスプライン曲線によって、滑らかな軌跡としてそれぞれ連結するように軌跡処理を行い補間する（ステップＳ４，増幅姿勢座標値連結処理手段）。
【００４４】
ここで、図１は、教示位置並びに教示姿勢を補間する処理をイメージ的に示している。尚、図示が煩雑になるのを避けるため、姿勢点についてはＹ，Ｚの２軸分だけ示している。図１（ａ），（ｂ）は、各教示位置を曲線補間した場合を示し、図１（ｃ）は、各教示位置における教示姿勢についてＹ方向姿勢点及びＺ方向姿勢点を決定した場合であり、各矢印がオリエントベクトル，アプローチベクトルに対応する。そして、図１（ｄ）はＹ方向姿勢点を曲線補間した場合、図１（ｅ）はＺ方向姿勢点を曲線補間した場合である。また、図示しないＸ方向（ノーマルベクトル）姿勢点についても同様に曲線補間する。
【００４５】
ステップＳ４では、Ｘ，Ｙ，Ｚの各方向について姿勢点を独立に補間したが、これらの各方向の補間点については３次元座標の各軸に対応することから、互いの関係が正規直交条件を満たす必要がある。したがって、その条件を満たすように修正を行う（ステップＳ５）。以上で教示姿勢の補間処理が完了する。
【００４６】
それから、ステップＳ５で得た補間点を、ベースから見たツールの位置・姿勢となるように座標変換すると（ステップＳ６）、補間点を加えて得られた手先の軌道（Ｘ，Ｙ，Ｚ各方向姿勢点の軌道）を、各軸Ｊ１〜Ｊ６の角度に変換する（ステップＳ７）。以上で姿勢の補間処理が終了する。
【００４７】
ここで、図４は、教示点におけるアプローチベクトルの教示姿勢（ＳＡ）に対して、（ｂ）は特許文献１，２のような従来の補間技術を適用して、手先が教示点を通過した場合のアプローチベクトルを３次元的に示している。（ｂ）では教示姿勢に対してズレを生じているが、本実施例の姿勢補間技術を適用した場合の（ａ）では、姿勢のズレが略生じていないことが判る。
【００４８】
また図５は、位置，姿勢の教示点について、従来の補間技術と本実施例の補間技術とのズレの発生量を示したものである。尚、同図中に示すワーク２２及び工具２３は、図１０に対応しないモデル的なものである。（ａ）の位置ずれについては、従来と本実施例とでは何れも０．１ｍｍ未満の誤差であり有意な差はない。一方、（ｂ）の姿勢ずれについては、従来技術では誤差角度が５［ｄｅｇ］を超える場合もあるが、本実施例では、誤差角度が０．１［ｄｅｇ］以下となっており、大きく改善されている。
【００４９】
以上のように本実施例によれば、制御装置２は、手先の先端点を座標原点とするＸ方向，Ｙ方向，Ｚ方向の各軸について手先の位置及び姿勢をティーチングする場合に、既に教示された手先の移動先である複数のＸ成分，Ｙ成分，Ｚ成分，Ｘ方向回転成分，Ｙ方向回転成分，Ｚ方向回転成分で構成された移動目標点について、各移動目標点におけるＸ，Ｙ，Ｚの方向回転成分について回転行列変換処理を行い、方向回転成分毎にＸ成分，Ｙ成分，Ｚ成分で座標化されたＸ，Ｙ，Ｚの各方向姿勢座標値を計算する。それから、計算された移動目標点毎に存在するＸ，Ｙ，Ｚの各方向姿勢座標値について、各移動目標点を原点に、Ｘ，Ｙ，Ｚの各方向姿勢座標値それぞれに向けた単位ベクトルを形成すると、そのベクトルをｎ倍した先の位置をそれぞれ増幅Ｘ，増幅Ｙ，増幅Ｚ方向姿勢座標値とする。そして、移動目標点毎に存在する増幅Ｘ，増幅Ｙ，増幅Ｚ方向姿勢座標値について、増幅Ｘ，増幅Ｙ，増幅Ｚ方向姿勢座標値のみを対象として、それぞれの座標値を滑らかな軌跡として連結する軌跡処理を行うと、得られた軌跡に沿って、各移動目標点間の姿勢を補完して移動する処理を行うようにした。
【００５０】
これにより、ロボット本体１の手先を移動させることで、固定されている工具２３に対してワーク２２を移動させて加工を行う場合に、手先の姿勢は、教示された姿勢が維持されながら手先が移動する。したがって、固定されている工具２３の軸に対して、ワーク２２の加工点を含む面がなす角度がティーチングを行った通りに維持されるので、ワーク２２の加工精度を向上させるために教示点を増やす必要が無くなるため、ティーチング作業を簡単にすることができる。
【００５１】
本発明は上記し、又は図面に記載した実施例にのみ限定されるものではなく、以下のような変型又は拡張が可能である。
姿勢の補間については、スプライン曲線を用いるものに限ることなく、各軸の姿勢決定座標の移動軌跡がそれぞれ滑らかに（数学的に連続となるように）繋がるように補間すれば良い。
【符号の説明】
【００５２】
図面中、１はロボット本体、２は制御装置（姿勢座標変換処理手段，姿勢ベクトル増幅処理手段，増幅姿勢座標値連結処理手段）、３はティーチングペンダント、９はフランジ（手先）である。
【特許請求の範囲】
【請求項１】
垂直多関節型のロボットの手先がワークを把持し、固定されているツールに対して前記ワークを移動させて加工を行う際に、
既に教示された前記手先の移動先である複数のＸ成分，Ｙ成分，Ｚ成分，Ｘ方向回転成分，Ｙ方向回転成分，Ｚ方向回転成分で構成された移動目標点について、
各移動目標点におけるＸ方向回転成分，Ｙ方向回転成分，Ｚ方向回転成分について回転行列変換処理を行い、方向回転成分毎にＸ成分，Ｙ成分，Ｚ成分で座標化されたＸ方向姿勢座標値，Ｙ方向姿勢座標値，Ｚ方向姿勢座標値を計算する姿勢座標変換処理手段と、
この姿勢座標変換処理手段により計算された移動目標点毎に存在するＸ方向姿勢座標値，Ｙ方向姿勢座標値，Ｚ方向姿勢座標値について、各移動目標点を原点に、Ｘ方向姿勢座標値，Ｙ方向姿勢座標値，Ｚ方向姿勢座標値それぞれに向けた単位ベクトルを形成し、そのベクトルをｎ倍（ｎ＞２）した先の位置を増幅Ｘ方向姿勢座標値，増幅Ｙ方向姿勢座標値，増幅Ｚ方向姿勢座標値とする姿勢ベクトル増幅処理手段と、
移動目標点毎に存在する増幅Ｘ方向姿勢座標値，増幅Ｙ方向姿勢座標値，増幅Ｚ方向姿勢座標値について、前記増幅Ｘ方向姿勢座標値のみを対象としてそれらを滑らかな軌跡として連結する軌跡処理，前記増幅Ｙ方向姿勢座標値のみを対象としてそれらを滑らかな軌跡として連結する軌跡処理，前記増幅Ｚ方向姿勢座標値のみを対象としてそれらを滑らかな軌跡として連結する軌跡処理を行う増幅姿勢座標値連結処理手段とを備え、
この増幅姿勢座標値連結処理手段により得られた軌跡に沿って、各移動目標点間の姿勢を補間して移動する処理を行うことを特徴とするロボットの制御装置。
【請求項２】
垂直多関節型のロボットの手先がワークを把持し、固定されているツールに対して前記ワークを移動させて加工を行う際に、
既に教示された前記手先の移動先である複数のＸ成分，Ｙ成分，Ｚ成分，Ｘ方向回転成分，Ｙ方向回転成分，Ｚ方向回転成分で構成された移動目標点について、
各移動目標点におけるＸ方向回転成分，Ｙ方向回転成分，Ｚ方向回転成分について回転行列変換処理を行い、方向回転成分毎にＸ成分，Ｙ成分，Ｚ成分で座標化されたＸ方向姿勢座標値，Ｙ方向姿勢座標値，Ｚ方向姿勢座標値を計算し、
前記移動目標点毎に存在する計算されたＸ方向姿勢座標値，Ｙ方向姿勢座標値，Ｚ方向姿勢座標値について、各移動目標点を原点に、Ｘ方向姿勢座標値，Ｙ方向姿勢座標値，Ｚ方向姿勢座標値それぞれに向けた単位ベクトルを形成し、そのベクトルをｎ倍（ｎ＞２）した先の位置を増幅Ｘ方向姿勢座標値，増幅Ｙ方向姿勢座標値，増幅Ｚ方向姿勢座標値とし、
移動目標点毎に存在する増幅Ｘ方向姿勢座標値，増幅Ｙ方向姿勢座標値，増幅Ｚ方向姿勢座標値について、前記増幅Ｘ方向姿勢座標値のみを対象としてそれらを滑らかな軌跡として連結する軌跡処理，前記増幅Ｙ方向姿勢座標値のみを対象としてそれらを滑らかな軌跡として連結する軌跡処理，前記増幅Ｚ方向姿勢座標値のみを対象としてそれらを滑らかな軌跡として連結する軌跡処理を行い、
前記連結された軌跡に沿って、各移動目標点間の姿勢を補完して移動する処理を行うことを特徴とするロボットの姿勢補間方法。
【請求項１】
垂直多関節型のロボットの手先がワークを把持し、固定されているツールに対して前記ワークを移動させて加工を行う際に、
既に教示された前記手先の移動先である複数のＸ成分，Ｙ成分，Ｚ成分，Ｘ方向回転成分，Ｙ方向回転成分，Ｚ方向回転成分で構成された移動目標点について、
各移動目標点におけるＸ方向回転成分，Ｙ方向回転成分，Ｚ方向回転成分について回転行列変換処理を行い、方向回転成分毎にＸ成分，Ｙ成分，Ｚ成分で座標化されたＸ方向姿勢座標値，Ｙ方向姿勢座標値，Ｚ方向姿勢座標値を計算する姿勢座標変換処理手段と、
この姿勢座標変換処理手段により計算された移動目標点毎に存在するＸ方向姿勢座標値，Ｙ方向姿勢座標値，Ｚ方向姿勢座標値について、各移動目標点を原点に、Ｘ方向姿勢座標値，Ｙ方向姿勢座標値，Ｚ方向姿勢座標値それぞれに向けた単位ベクトルを形成し、そのベクトルをｎ倍（ｎ＞２）した先の位置を増幅Ｘ方向姿勢座標値，増幅Ｙ方向姿勢座標値，増幅Ｚ方向姿勢座標値とする姿勢ベクトル増幅処理手段と、
移動目標点毎に存在する増幅Ｘ方向姿勢座標値，増幅Ｙ方向姿勢座標値，増幅Ｚ方向姿勢座標値について、前記増幅Ｘ方向姿勢座標値のみを対象としてそれらを滑らかな軌跡として連結する軌跡処理，前記増幅Ｙ方向姿勢座標値のみを対象としてそれらを滑らかな軌跡として連結する軌跡処理，前記増幅Ｚ方向姿勢座標値のみを対象としてそれらを滑らかな軌跡として連結する軌跡処理を行う増幅姿勢座標値連結処理手段とを備え、
この増幅姿勢座標値連結処理手段により得られた軌跡に沿って、各移動目標点間の姿勢を補間して移動する処理を行うことを特徴とするロボットの制御装置。
【請求項２】
垂直多関節型のロボットの手先がワークを把持し、固定されているツールに対して前記ワークを移動させて加工を行う際に、
既に教示された前記手先の移動先である複数のＸ成分，Ｙ成分，Ｚ成分，Ｘ方向回転成分，Ｙ方向回転成分，Ｚ方向回転成分で構成された移動目標点について、
各移動目標点におけるＸ方向回転成分，Ｙ方向回転成分，Ｚ方向回転成分について回転行列変換処理を行い、方向回転成分毎にＸ成分，Ｙ成分，Ｚ成分で座標化されたＸ方向姿勢座標値，Ｙ方向姿勢座標値，Ｚ方向姿勢座標値を計算し、
前記移動目標点毎に存在する計算されたＸ方向姿勢座標値，Ｙ方向姿勢座標値，Ｚ方向姿勢座標値について、各移動目標点を原点に、Ｘ方向姿勢座標値，Ｙ方向姿勢座標値，Ｚ方向姿勢座標値それぞれに向けた単位ベクトルを形成し、そのベクトルをｎ倍（ｎ＞２）した先の位置を増幅Ｘ方向姿勢座標値，増幅Ｙ方向姿勢座標値，増幅Ｚ方向姿勢座標値とし、
移動目標点毎に存在する増幅Ｘ方向姿勢座標値，増幅Ｙ方向姿勢座標値，増幅Ｚ方向姿勢座標値について、前記増幅Ｘ方向姿勢座標値のみを対象としてそれらを滑らかな軌跡として連結する軌跡処理，前記増幅Ｙ方向姿勢座標値のみを対象としてそれらを滑らかな軌跡として連結する軌跡処理，前記増幅Ｚ方向姿勢座標値のみを対象としてそれらを滑らかな軌跡として連結する軌跡処理を行い、
前記連結された軌跡に沿って、各移動目標点間の姿勢を補完して移動する処理を行うことを特徴とするロボットの姿勢補間方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【公開番号】特開２０１２−１３５８３５（Ｐ２０１２−１３５８３５Ａ）
【公開日】平成２４年７月１９日（２０１２．７．１９）
【国際特許分類】
【出願番号】特願２０１０−２９００２８（Ｐ２０１０−２９００２８）
【出願日】平成２２年１２月２７日（２０１０．１２．２７）
【出願人】（５０１４２８５４５）株式会社デンソーウェーブ (1,155)
【Ｆターム（参考）】
【公開日】平成２４年７月１９日（２０１２．７．１９）
【国際特許分類】
【出願日】平成２２年１２月２７日（２０１０．１２．２７）
【出願人】（５０１４２８５４５）株式会社デンソーウェーブ (1,155)
【Ｆターム（参考）】
[ Back to top ]