JP2013059815A - ロボットの位置姿勢補間方法及びロボットの制御装置 - Google Patents

Abstract

【課題】ロボットの手先の姿勢が一定の条件を満たすように補間できるロボットの位置姿勢補間方法を提供する。
【解決手段】各教示点を通過する移動軌跡がスプライン曲線となるように補間点を生成し（Ｓ３）、各教示点毎に、教示点と同じ座標系における撮像対象物の座標を求め、各教示点に対応する撮像対象物の座標間を、スプライン曲線により補間して補間点を生成する（Ｓ４）。教示点側の補間点を始点,撮像対象物側の補間点を終点とするアプローチベクトルを全ての補間点に対して求め（Ｓ５）、２つの教示点間の各補間点におけるノーマルベクトルの回転角を補間すると（Ｓ６）各補間点に立つアプローチ，ノーマルベクトルの外積よりオリエントベクトルを求め（Ｓ７）各補間点と３つの姿勢ベクトルの座標とから各補間点における手先の姿勢を決定する。
【選択図】図１

Description

本発明は、垂直多関節型のロボットの手先から対象物までの距離を変化させるように、前記手先を移動させる位置及び前記手先の姿勢を教示した後、前記位置及び前記姿勢を補間する方法及びロボットの制御装置に関する。

ロボットをティーチングする場合には、一般に、ロボットの手先を移動させる軌跡に沿った位置を教示すると共に、その教示点においてロボットの手先が取るべき姿勢も教示するようになっている。例えば、特許文献１には、ティーチング及びその後に行われる補間に関連する技術が開示されている。

特開２００５−２７５４８４号公報

ところで、 ロボットの手先にカメラを取り付け、そのカメラを移動させて固定されているワークの画像を撮像する、という作業を行う場合、ティーチングを行う際のロボットの姿勢は、当然にカメラの視線が固定されているワークを捉える方向となるように決定されることが望まれる。

カメラを移動させると共に、カメラがワークを捉える焦点距離を変化させながら撮像を行う場合、ロボットの手先の位置をティーチングした後は、移動軌跡が滑らかに繋がるようにティーチングした位置の間を例えばスプライン曲線等の自由曲線を用いて補間する。各教示点については、カメラの視線はワークの方向を向くようにティーチングされているが、補間した位置においても、カメラの視線はワークの方向を向くことが望ましい。しかしながら、従来、手先の姿勢について一定の条件を満たすように補間する技術は存在しなかった。

本発明は上記事情に鑑みてなされたものであり、その目的は、ロボットの手先の姿勢が一定の条件を満たすように補間できるロボットの位置姿勢補間方法及びロボットの制御装置を提供することにある。

請求項１記載の位置姿勢補間方法によれば、各教示点を通過する移動軌跡が自由曲線となるように教示点間の位置を移動順番を付けて補間して補間点を生成し（第１ステップ）、各教示点毎に、教示点と同じ座標系における前記目標点の座標を求め（第２ステップ）、各教示点に対応する各目標点の座標間を、第１ステップと同じアルゴリズムの自由曲線により補間して第１ステップと同じ移動順番を付した補間点を生成する（第３ステップ）。尚、第１〜第３ステップについては、実行順序を適宜入れ替えても問題は無い。例えば、先に各教示点に対応する各目標点の座標間を補間した場合は、その後、教示点間を補間した点に同じ移動順番を付せば良い。

ここで、教示点における手先の姿勢角を３方向に分解した正規化姿勢ベクトルについて、手先から対象物の目標点に向かう方向を第１姿勢とし、その他を第２，第３姿勢とする。第１姿勢正規化ベクトルはアプローチベクトルに対応し、第２，第３正規化姿勢ベクトルは、ノーマル，オリエントベクトルの何れかに対応する。
そして、第１ステップで生成した補間点を始点とし、第３ステップにおいて生成した補間点を終点とする第１姿勢ベクトルを、第１ステップで生成した全ての補間点に対して求める（第４ステップ）。すなわち、第４ステップでは、第１姿勢ベクトルについてはその長さ（ノルム）を正規化された一定の長さ（単位長）にせず、対象物との距離に応じて変化させることになる。

それから、２つの教示点間の各補間点における第２姿勢正規化ベクトルの回転角を補間すると（第５ステップ）、各補間点に立つ前記第１姿勢ベクトル及び前記第２正規化姿勢ベクトルとの外積より第３姿勢ベクトルを求め（第６ステップ）、第１ステップで生成した補間点と、第４〜第６ステップで得られた各姿勢ベクトルの座標とから、各補間点における手先の姿勢を決定する（第７ステップ）。尚、手先の姿勢を決定することが目的であるから、第３姿勢ベクトルを求めるに際してその長さが正規化されているか否かは問題とならない。
すなわち、第４ステップでは、第１姿勢ベクトルの長さを対象物の距離に応じて変化させるので、第２，第３姿勢ベクトルも併せて決定される手先の姿勢は、第１姿勢ベクトルが常に対象物の方を向くように補間処理される。したがって、手先の位置を補間する際に、各補間点における手先の姿勢が、少なくとも対象物の方向を向くように補間することができる。

請求項２記載の位置姿勢補間方法によれば、第１〜第４ステップを、請求項１と同様に実行する。それから、第２，第３姿勢ベクトルのそれぞれについて、隣り合う２つの教示点ｉ，（ｉ＋１）にそれぞれ立つ２つのベクトルがなす角θｉを求め（第５ステップ）、第２，第３姿勢ベクトルのそれぞれについて、２つの教示点ｉ，（ｉ＋１）の間で補間された補間点ｊにつき、教示点ｉ，補間点ｊにそれぞれ立つ２つのベクトルがなす角θｊを求める（第６ステップ）。
次に、第２，第３姿勢ベクトルの何れか一方について、２つの教示点ｉ，（ｉ＋１）にそれぞれ立つ２つのベクトルの外積Ｐｉを求め、求めた外積Ｐｉを正規化して正規化ベクトルｎＰｉを求め（第７ステップ）、正規化ベクトルｎＰｉを回転軸とし、当該正規化ベクトルｎＰｉについて、角度θｊを回転角とする正規化クオータニオンＱｊを求める（第８ステップ）。

それから、正規化ベクトルｎＰｉとして選択した姿勢ベクトルを、正規化クオータニオンＱｊにより回転させた姿勢ベクトルＲｂｊを求め（第９ステップ）、補間点ｊに立つ第１姿勢ベクトルＲａｊ，及び回転させた姿勢ベクトルＲｂｊの外積を求め、補間点ｊに立つ、正規化ベクトルとして選択しなかった姿勢ベクトルＲｃｊを求める（第１０ステップ）。そして、姿勢ベクトルＲａｊとＲｃｊの外積より姿勢ベクトルＲｂｊを求め直し（第１１ステップ）、これらの３つの姿勢ベクトルＲａｊ，Ｒｂｊ，Ｒｃｊより、補間点ｊにおける手先の姿勢角を求める（第１２ステップ）。

すなわち、請求項１の方法による場合、補間点を基準として姿勢を補間する結果、手先の位置の変化に応じて手先の姿勢が変化する率が線形にならないことが想定される。これに対して請求項２の方法によれば、正規化ベクトルｎＰｉについて、角度θｊを回転角とする正規化クオータニオンＱｊを求め、その正規化クオータニオンＱｊにより回転させた姿勢ベクトルＲｂｊを求める。そして、残りの姿勢ベクトルＲｃｊを、補間点ｊに立つ第１姿勢ベクトルＲａｊと姿勢ベクトルＲｂｊとから求め、再度、姿勢ベクトルＲａｊとＲｃｊの外積より姿勢ベクトルＲｂｊを求めなおし、それら３つの姿勢ベクトルＲａｊ，Ｒｂｊ，Ｒｃｊより補間点ｊにおける手先の姿勢角を求めるので、手先位置の変化に応じた手先姿勢の変化率を線形にすることができ、姿勢をより滑らかに変化させることができる。

尚、請求項１における第５〜第７ステップと、請求項２における第５〜第１２ステップとは、第２姿勢ベクトルと第３姿勢ベクトルとを決定する手順が、請求項１では第２姿勢ベクトル，第３姿勢ベクトルの順で決定するのに対し、請求項２では第２姿勢ベクトル，第３姿勢ベクトルの何れか一方，他方の順で決定する点が相違している。この相違点を除けば、請求項２における第５〜第１２ステップは、請求項１における第５〜第７ステップの手順をより詳細且つ具体的に述べたものに等しい（例えば上記第５ステップにおける回転角の補間処理を、正規化クオータニオンＱｊを用いて行うことを限定している）。

第１実施例であり、ロボット本体の手先の位置及び姿勢について、教示点間の位置及び姿勢を補間する処理を示すフローチャート 図１の処理に対応するイメージをベクトルモデルで示す図（その１） 図１の処理に対応するイメージをベクトルモデルで示す図（その２） アプローチベクトルの長さを変化させることの意義を説明する図 垂直多関節型ロボットを含む制御システムの構成を示す図 制御系の機能ブロック図 第２実施例を示す図１相当図（その１） 図１相当図（その２） 姿勢ベクトルＡｉの方向が撮像対象物の方向に対してずれを生じている状態を説明する図 正規化クオータニオンＱｎｊを用いる作用効果を説明する図（その１） 正規化クオータニオンＱｎｊを用いる作用効果を説明する図（その２）

（第１実施例）
以下、本発明の第１実施例について図１ないし図６を参照して説明する。図５は、垂直多関節型（６軸）ロボットを含む制御システムの構成を示す。このロボット本体１は、ベース（回転軸）２上に、この場合６軸のアームを有し、そのアームの先端には、図示しないハンド等のツールや、後述するカメラなどが取り付けられる。前記ベース２上には、第１関節Ｊ１を介して第１のアーム３が回転可能に連結されている。この第１のアーム３には、第２関節Ｊ２を介して上方に延びる第２のアーム４の下端部が回転可能に連結され、さらに、この第２のアーム４の先端部には、第３関節Ｊ３を介して第３のアーム５が回転可能に連結されている。

この第３のアーム５の先端には第４関節Ｊ４を介して第４のアーム６が回転可能に連結され、この第４のアーム６の先端には第５関節Ｊ５を介して第５のアーム７が回転可能に連結され、この第５のアーム７には第６関節Ｊ６を介して第６のアーム８が回転可能に連結されている。なお、各関節Ｊ１〜Ｊ６においては、図示しないサーボモータにより各アーム３〜８を回転駆動するようになっている。

ロボット本体１と制御装置１１との間は、接続ケーブル１２によって接続されている。これにより、ロボット本体１の各軸を駆動するサーボモータは、制御装置１１により制御される。操作パッド（入力手段）１３は、例えば横長の本体１４の両端側に、ユーザが左右両手でそれぞれ把持するための把持部１５Ｌ，１５Ｒが設けられており、ユーザが把持部１５Ｌ，１５Ｒを両手で把持した状態で、親指により押圧操作可能な操作スティック１６Ｌ，１６Ｒが配置されている。また、把持部１５Ｌ，１５Ｒの図中上方には、人差し指により押圧操作可能なボタン１７Ｌ，１７Ｒが配置されている。

ロボット本体１のアーム８には、ＣＣＤ（Charge Coupled Device）又はＣＭＯＳイメージセンサを用いたカメラ（撮像手段）２１が取り付けられており、そのカメラ２１により撮像対象物（ワーク）２２を画像で捉える。制御装置１１には、パーソナルコンピュータ（パソコン）２３がケーブル２４を介して接続されている。

パソコン２３には、メモリやハードディスクなどの記憶装置（記憶手段）が内蔵されている。操作パッド１３は、接続ケーブル１８を経由してパソコン２３に接続され、通信インターフェイスを介してパソコン２３との間で高速のデータ転送を実行するようになっている。操作スティック１６Ｌ，１６Ｒ等が操作されて入力された操作信号等の情報は、操作パッド１３からパソコン２３を介して制御装置１１へ送信される。カメラ２１は、ケーブル２５を介してパソコン２３に接続されており、カメラ２１が捉えた画像のデータはパソコン２３に送信されて、ディスプレイ２３Ｄに表示される。

図６に示すように、制御装置１１は、制御部としてのＣＰＵ３１、各関節のモータ３０を駆動する駆動手段としての駆動回路３２、検出回路３３などを備えている。そして、ＣＰＵ３１には、ロボット全体のシステムプログラム等を記憶したＲＯＭ３４、ロボット本体１の動作プログラム等を記憶したＲＡＭ３５および前記ティーチングペンダント３を接続するためのインターフェイス３６が接続されている。なお、図６では、ショルダ部５、下アーム６、上アーム７、手首８を可動部として一つのブロックで示し、これに応じてそれらの関節の駆動源であるモータ３０も一台だけ示した。

上記検出回路３３は、各関節の現在位置（回転角度）および現在速度（回転速度）を検出するためのもので、この検出回路３３には、各関節を駆動するモータ３０に設けられたロータリエンコーダ３７が接続されている。ロータリエンコーダ３７は位置センサおよび速度センサを兼用するもので、各モータ３０の回転角度に応じたパルス信号を出力し、そのパルス信号は検出回路３３に与えられる。検出回路３３は、各ロータリエンコーダ３７からのパルス信号に基づいて各モータ３０ひいては各関節の現在位置を検出すると共に、単位時間当たり各ロータリエンコーダ３７から入力されるパルス信号数に基づいて各モータ３０ひいては各関節の現在速度を検出し、その位置および速度の情報は、各モータ３０の駆動回路３２およびＣＰＵ３１に与えられるようになっている。

そして、各駆動回路３２は、ＣＰＵ３１から与えられる位置指令値および速度指令値と検出回路３３から与えられる現在位置および現在速度とを比較し、その偏差に応じた電流を各モータ３０に供給してそれらを駆動する。これにより、ロボットアーム先端（手先）であるフランジ（図示せず）の中心部が指令位置を順に通る経路を辿って動作することで、種々の作業を行う。

ロボットアーム先端の移動経路は、操作パッド１３（若しくは、図示しないティーチングペンダント）を用いて行う教示作業によって与えられる。この教示作業では、ロボットアーム先端が辿るべき軌跡上の複数の位置が指令位置として順に教示される。そして、従来の教示作業であれば、各指令位置でのロボットアーム先端の姿勢も教示するが、本実施例では、姿勢は後述のように自動的に決定される。そして教示された指令位置と決定された姿勢とは、ＲＡＭ３５に記憶される。制御装置１１は、実際のロボット作業に際して、与えられた複数の指令位置間を曲線補間してそれら指令位置を順に滑らかに辿る曲線を設定し、ロボットアーム先端がその曲線上を移動するように制御する。

なお、ロボットアーム先端の位置は、ツール(図５Σc)に固定された３次元座標（ツール座標）の原点がロボット座標（基底座標，ベース座標）上でどの位置にあるかによって示される。また、ロボットアーム先端の姿勢は、ツールに固定のツール座標の３軸の単位ベクトル（ノーマル，オリエント，アプローチの各正規化姿勢ベクトル）がロボット座標上で示す向きによって定義される。

次に、本実施例の作用について図１ないし図４を参照して説明する。図１は、ロボット本体１の手先（ＴＣＰ）の位置及び姿勢の教示を行った後、教示点間における手先の位置及び姿勢を補間する処理を示すフローチャートであり、図２及び図３は、図１の処理に対応するイメージをベクトルモデルで示している。尚、図１に示すフローチャートは、理解を容易にするため処理の概略を原理的に示しており、より詳細な処理手順については第２実施例で説明する。

先ず、各教示点について、手先の座標；ＴＣＰ座標Ｐｉ（Ｘｉ，Ｙｉ，Ｚｉ）と、各軸の回転角である姿勢角（Ｒｘｉ，Ｒｙｉ，Ｒｚｉ）とを記録する（Ｓ１）。この時、手先の姿勢は、カメラ２１の視線（Ｚ軸）が撮像対象物２２の方向を向くように教示されている。続いて、各教示点毎に、撮像対象物２２までの距離を求めて設定する（Ｓ２）。カメラ２１を使用する場合、上記の距離は焦点距離となる（図２（ａ）参照）。

それから、教示したＴＣＰ座標Ｐｉの間を、例えばスプライン曲線等の自由曲線を用いて補間し（Ｓ３，図２（ｂ）参照，第１ステップ，位置補間手段）、各教示点から撮像対象物２２までの距離を頂点とする座標（Ｔｉ，Ｔｉ＋１，Ｔｉ＋２）について、それらの間をステップＳ３と同様に例えばスプライン曲線等の自由曲線を用いて補間する（Ｓ４，図２（ｃ）参照，第２及び第３ステップ，目標点座標決定手段，目標点位置補間手段）。尚、上記座標（Ｔｉ，Ｔｉ＋１，Ｔｉ＋２）については、教示点と同じ座標系で表す。また、「ｉ」，「ｉ＋１」等は手先位置の移動順番を示す。

次に、ステップＳ３で求めた各教示点の補間点と、この補間点に対応してステップＳ４で求めた頂点座標の補間点の２点から、アプローチベクトル（第１姿勢ベクトル）を生成する（Ｓ５，図３（ａ）参照），第４ステップ，第１姿勢ベクトル決定手段）。このアプローチベクトルは、カメラ２１の視線方向であるＺ軸に一致している。尚、Ｓ４をＳ２もしくは、Ｓ３の前に実行順序を移動させても全く問題ないことは明らかである。

ここで、図４は、アプローチベクトルを上記のように決定することの意義を説明する図である。図４（ａ）は従来技術による補間処理を示しており、従来は教示点Ｐ１，Ｐ２間の位置Ｐ１２を補間するだけであるため、各教示点Ｐ１，Ｐ２のアプローチベクトルがターゲット（撮像対象物２２）の方向を向いていても、補間点Ｐ１２でのアプローチベクトルがターゲットの方向を向くとは限らない。この場合、各補間点におけるアプローチベクトルの長さは正規化された単位長で常に一定である。
図４（ｂ）は本実施例による補間処理を示しており、補間点Ｐ１２でのアプローチベクトルを、各教示点の補間点と、この補間点に対応する撮像対象物２２側の補間点の２点から生成することで、各補間点におけるアプローチベクトルの長さは一定ではなく変化することになるから、アプローチベクトルがターゲットの方向を向くことが保証される。

再び図１を参照する。手先の姿勢を決定する他の２軸ベクトルはオリエントベクトルとノーマルベクトルであるが、これらの何れか一方について、教示点間の回転軸を中心として回転角の補間を行い、補間軌道に対応したオリエント若しくはノーマルベクトルを生成する（Ｓ６，第５ステップ，回転角補間手段）。
ここで、図３（ｂ）〜（ｄ）は、ステップＳ６においてノーマルベクトル（第２正規化姿勢ベクトル）を生成する際に回転角を補間する処理のイメージであり、図３（ｂ）は、図３（ａ）に示すＡ側，教示点側よりアプローチベクトルに沿って撮像対象物２２の方向を見た場合のノーマル，オリエントベクトルを示している。図３（ｃ）は、各教示点におけるノーマルベクトルの回転角θを、スプライン補間した場合の角度変化の曲線を示しており、図３（ｄ）は、図３（ｃ）のように補間した結果、補間点においてノーマルベクトルが示す回転角θである。

そして、図１に示すＳ７では、Ｓ６でノーマル，オリエントベクトルの一方が決まれば、他方のベクトル（第３姿勢ベクトル）は正規直交条件によりアプローチベクトルとの外積により求めることができる（第７ステップ，第３姿勢ベクトル決定手段）。これにより、各補間点についての３つの姿勢ベクトル；ノーマル，オリエント，アプローチの各ベクトルが決まるので、これらにより姿勢が決定される（第８ステップ，姿勢決定手段）。したがって、各補間点における手先のアプローチベクトルが撮像対象物２２の方向を向くと共に、ある教示点から補間点を経て次の教示点に至るまでのノーマル，オリエントベクトルの回転角が滑らかに変化するように手先の姿勢が補間されたことになる。

以上のように本実施例によれば、各教示点を通過する移動軌跡がスプライン曲線となるように教示点間の位置を移動順番を付けて補間して補間点を生成し、各教示点毎に、教示点と同じ座標系における撮像対象物２２の座標を求め、各教示点に対応する撮像対象物２２の座標間を、スプライン曲線により補間して同じ移動順番を付した補間点を生成する。そして、教示点側の補間点を始点,撮像対象物２２側の補間点を終点とするアプローチベクトルを全ての補間点に対して求め、２つの教示点間の各補間点における例えばノーマルベクトルの回転角を補間すると、各補間点に立つアプローチベクトルとノーマルベクトルとの外積よりオリエントベクトルを求め、各補間点と３つの姿勢ベクトルの座標とから各補間点における手先の姿勢を決定するようにした。

すなわち、アプローチベクトルの長さを撮像対象物２２までの距離に応じて変化させるので、ノーマル,オリエントベクトルも併せて決定される手先の姿勢は、アプローチベクトルが常に対象物の方を向くように補間処理される。したがって、手先の位置を補間する際に、各補間点における手先の姿勢が、少なくとも撮像対象物２２の方向を向くように補間することができる。加えて、ある教示点から補間点を経て次の教示点に至るまでのノーマル，オリエントベクトルの回転角が滑らかに変化するように手先の姿勢を補間することができる。

（第２実施例）
図７ないし図１１は第２実施例であり、第１実施例と同一部分には同一符号を付して説明を省略し、以下異なる部分について説明する。前述したように、第２実施例は第１実施例で示した処理手順をより詳細且つ具体的に示すものである。図７（ａ）に示すフローチャートにおいて、先ず、教示点毎にＴＣＰ座標Ｘｃｉと、姿勢角（Ｒｘ，Ｒｙ，Ｒｚ）とを教示すると（Ｓ１１，第１実施例では前提）、各ＴＣＰ座標間（例えば、Ｘｃｉ, Ｘｃｉ＋１との間）について例えばスプライン曲線等の自由曲線を用いて補間を行い、補間点を含む軌道を生成する（Ｓ１２，第１ステップ，位置補間手段）。尚、添え字「ｉ」，「ｉ＋１」は教示点について順次付す番号であり、以下では「キーフレーム番号」と称す。

次に、姿勢角（Ｒｘｉ，Ｒｙｉ，Ｒｚｉ）より、各教示点に立つ正規化姿勢ベクトル（Ｎｉ，Ｏｉ，Ａｉ）を求める（Ｓ１３）。そして、座標（Ｘｔ，Ｙｔ，Ｚｔ）を姿勢ベクトル（Ｎｉ，Ｏｉ，Ａｉ）を用いて記述した場合の、ＴＣＰ座標を始点とする姿勢ベクトルの長さ（ｎｉ，ｏｉ，ａｉ）を求め（Ｓ１４，第２ステップ）、姿勢ベクトル（Ｎｉ，Ｏｉ，Ａｉ）の長さを（ｎｉ，ｏｉ，ａｉ）とした時の頂点の座標Ｘｔｉを求める（Ｓ１５，目標点座標決定手段，図７（ｂ）参照）。すなわち、座標Ｘｔｉは、ＴＣＰ座標を基準として撮像対象物２２の位置を示す座標である。

ここで、姿勢ベクトルＡｉの方向が撮像対象物２２の方向に完全に一致している場合、座標Ｘｔｉは姿勢ベクトルＡｉの長さを示し、他の姿勢ベクトルの長さｎｉ，ｏｉはゼロとなるが、姿勢ベクトルＡｉの方向が撮像対象物２２の方向とずれを生じていれば他の姿勢ベクトルの長さｎｉ，ｏｉも所定の値を示すことになる（図９参照）。

次に、アプローチベクトルＡｉの長さをａｉとして、当該ベクトルＡｉの先端座標の間を、Ｓ１２と同様にスプライン曲線により補間して補間点を含む軌道を生成する（Ｓ１６，第３及び第４ステップ，目標点位置補間手段）。それから、教示点ＴＣＰの座標と、長さａｉのアプローチベクトルＡｉ先端のスプライン補間軌道について、各フィールド座標（補間した座標）Ｘｃｊ，Ｘｔｊとを結ぶベクトル（フィールド毎の正規化アプローチベクトルＡｊ）を求める（Ｓ１７，第１姿勢ベクトル決定手段，図７（ｃ）参照）。尚、添え字「ｊ」は補間点について順次付す番号であり、以下では「フィールド番号」と称す。

そして、教示点Ｘｉ，Ｘｉ＋１におけるノーマルベクトルＮｉ，Ｎｉ＋１の内積を演算し、両者がなす角度Θｎｉ（ｉ＝０，…，Ｎ）を求め（Ｓ１８，教示点ベクトル角度算出手段）、その角度Θｎｉについてもスプライン補間することで各補間点におけるノーマルベクトルが示す角度Θｎｊを求める（Ｓ１９，補間点ベクトル角度算出手段）。また、教示点Ｘｉ，Ｘｉ＋１におけるオリエントベクトルＯｉ，Ｏｉ＋１についても同様に内積を演算し、両者がなす角度Θｏｉ（ｉ＝０，…，Ｎ）を求め（Ｓ２０）、その角度Θｏｉについてもスプライン補間することで各補間点におけるオリエントベクトルが示す角度Θｏｊを求める（Ｓ２１）。尚、Ｓ１８〜Ｓ２１が第５ステップに対応する。

引き続き図８（ａ）に示す処理において、教示点のノーマルベクトルＮｉ，Ｎｉ＋１の外積Ｎｐｉを演算し、その結果をノーマルベクトルの仮の回転中心として用いることにする（Ｓ２２，図８（ｂ）参照）。外積Ｎｐｉの値がゼロでなければ（Ｓ２３：ＹＥＳ）、外積Ｎｐｉを正規化し（Ｓ２４，第７ステップ，正規化ベクトル決定手段）、正規化ベクトルＮｐｉを回転軸として回転角θｎｊによる正規化クオータニオンＱｎｊを求める（Ｓ２５，第８ステップ，正規化クオータニオン算出手段）。尚、回転角θｎｊは、
θｎｊ＝Θｎｊ−Θｎｉ …（１）
で表され、フィールド番号ｊがキーフレーム番号ｉ，ｉ＋１間に有る場合に、ノーマルベクトルの回転角が教示点Ｘｉから補間点Ｘｊに至る間に変化する角度の差分である（第６ステップ）。この回転角θｎｊについては、Ｓ２５で正規化クオータニオンＱｎｊを求めるまでに求めておけば良い。また、正規化クオータニオンＱｎｊは、（２）式で求められる。

尚、Ｎｐｊは、教示点Ｘｉ，補間点Ｘｃｊにおけるノーマルベクトルの外積である。

次に、正規化したノーマルベクトルＮｉを、正規化クオータニオンＱｎｊにより回転させたノーマルベクトルＮｊを（３）式により求める（Ｓ２６，図８（ｃ）参照，第９ステップ，回転姿勢ベクトル算出手段）。

尚、Ｑｎｊ^-1は、正規化クオータニオンＱｎｊの逆行列である。すると、補間点ＸｃｊにおけるアプローチベクトルＡｊと、ノーマルベクトルＮｊとが求められたので、これらの外積よりオリエントベクトルＯｊを求めることができる（Ｓ２７，図８（ｄ）参照，第１０ステップ，最終姿勢ベクトル算出手段）。またここで、アプローチベクトルＡｊとオリエントベクトルＯｊの外積からノーマルベクトルＮｊを求め直す（Ｓ２７ａ，第１１ステップ）。

以上より、フィールド番号ｊについて、アプローチベクトルＡｊ，ノーマルベクトルＮｊ，オリエントベクトルＯｊが求められたので、補間点Ｘｃｊにおける姿勢角（Ｒｘｊ，Ｒｙｊ，Ｒｚｊ）が得られる（Ｓ２８，図８（ｅ）参照，第１２ステップ）。尚、図８（ｅ）に示すＮｊ（ｏｌｄ）は、第１１ステップにおいて、アプローチベクトルＡｊとオリエントベクトルＯｊの外積からノーマルベクトルＮｊを求め直す前に求められているノーマルベクトルＮｊを示す。

一方、Ｓ２３において外積Ｎｐｉの値がゼロである場合は（ＮＯ）、Ｓ２２で用いた教示点のノーマルベクトルＮｉ，Ｎｉ＋１に替えてオリエントベクトルＯｉ，Ｏｉ＋１を用いてそれらの外積Ｏｐｉを演算する（Ｓ２９）。そして、外積Ｏｐｉの値がゼロでなければ（Ｓ３０：ＹＥＳ），Ｓ２４〜Ｓ２７におけるノーマルベクトルＮをオリエントベクトルＯに置き換えて同様の処理を行う（Ｓ３１）。また、Ｓ３０において外積Ｏｐｉの値がゼロである場合は（ＮＯ）手先の姿勢が全く変化していないことを意味するので（Ｓ３２）、そのまま処理を終了する。

尚、図７，図８に示すフローチャートは、１つのキーフレーム番号ｉについて、次のキーフレーム番号（ｉ＋１）との間を補間した点を示すフィールド番号の全てについて処理を実行し、次に、キーフレーム番号（ｉ＋１）と次のキーフレーム番号（ｉ＋２）との間を補間した点を示すフィールド番号の全てについて処理を実行する、というように順次フィールド番号とキーフレーム番号とをインクリメントして行う。

次に、上述したように正規化クオータニオンＱｎｊを用いることによる作用効果について図１０及び図１１を参照して説明する。図１０は、位置Ａから位置Ｂまで円弧に沿って回転させる間の回転角を補間する場合をモデル的に示している。補間のやり方には色々あるが、例えば図１０（ｂ）に示すように位置Ａ，Ｂ間に直線を引いて、その直線を４等分するように３点ｐ１，ｐ２，ｐ３を決定する（図１０（ｃ）参照）。これらの３点を用いて回転角を補間するのであれば、円の中心から各点ｐ１，ｐ２，ｐ３を通り、円周上に至る位置ＡＢ１，ＡＢ２，ＡＢ３が各補間点における回転角を与える。

位置ＡＢ２は、位置Ａ，Ｂ間の回転角の丁度１／２となるが、位置ＡＢ１については、位置Ａ，ＡＢ２間の１／２よりも小さくなり、位置ＡＢ３については、位置ＡＢ２，Ｂ間の１／２よりも大きくなる。その結果、図１１に破線で示すように回転角の変化率が線形とならず、滑らかに変化しない。第１実施例では、回転角の補間方式を任意としているため、回転角の変化がこの破線のようになる場合も想定される。これに対して正規化クオータニオンＱｎｊを用いて回転させることで各補間点のノーマル若しくはオリエントベクトルを決定すると、図１１に実線で示すように回転角の変化率が線形となるので滑らかに変化するようになる。

以上のように第２実施例によれば、教示点である各ＴＣＰ座標間についてスプライン曲線を用いて補間を行い、補間点を含む軌道を生成すると、各教示点に立つ正規化姿勢ベクトルを求め、撮像対象物２２の座標（Ｘｔ，Ｙｔ，Ｚｔ）を姿勢ベクトル（Ｎｉ，Ｏｉ，Ａｉ）を用いて記述した場合の、ＴＣＰ座標を始点とする姿勢ベクトルの長さ（ｎｉ，ｏｉ，ａｉ）を求め、姿勢ベクトル（Ｎｉ，Ｏｉ，Ａｉ）の長さを（ｎｉ，ｏｉ，ａｉ）とした時の頂点の座標Ｘｔｉを求める。次に、アプローチベクトルＡｉの長さをａｉとして、当該ベクトルＡｉの先端座標の間をスプライン曲線により補間して補間点を含む軌道を生成すると、教示点ＴＣＰの座標と、長さａｉのアプローチベクトルＡｉ先端のスプライン補間軌道において各フィールド座標とを結ぶ正規化アプローチベクトルＡｊを求める。

そして、教示点Ｘｉ，Ｘｉ＋１におけるノーマルベクトルＮｉ，Ｎｉ＋１の内積を演算し、得られた角度Θｎｉをスプライン補間して各補間点におけるノーマルベクトルが示す角度Θｎｊを求め、オリエントベクトルＯｉ，Ｏｉ＋１についても同様に内積を演算し、得られた角度Θｏｉをスプライン補間して各補間点におけるオリエントベクトルが示す角度Θｏｊを求める。

更に、教示点のノーマルベクトルＮｉ，Ｎｉ＋１の外積Ｎｐｉを演算して正規化し、正規化ベクトルＮｐｉを回転軸として回転角θｎｊによる正規化クオータニオンＱｎｊを求めると、正規化したノーマルベクトルＮｉを正規化クオータニオンＱｎｊにより回転させたノーマルベクトルＮｊを求める。そして、補間点ＸｃｊにおけるアプローチベクトルＡｊと、ノーマルベクトルＮｊとの外積よりオリエントベクトルＯｊを求め、再度、アプローチベクトルＡｊとオリエントベクトルＯｊとの外積よりノーマルベクトルＮｊを求め直し、補間点Ｘｃｊにおける姿勢角（Ｒｘｊ，Ｒｙｊ，Ｒｚｊ）を得るようにした。すなわち、補間点におけるノーマル，オリエントベクトルの何れか一方を求めるに当たり、正規化クオータニオンＱｎｊを用いて回転角をえることにより、手先位置の変化に応じた手先姿勢の変化率を線形にすることができ、姿勢をより滑らかに、自然に変化させることができる。

本発明は上記し又は図面に記載した実施例にのみ限定されるものではなく、以下のような変形又は拡張が可能である。
補間に用いる曲線はスプライン曲線に限ることなく、その他ベジェ曲線等の自由曲線を用いても良い。

図面中、１はロボット本体、１１は制御装置、２１はカメラ（撮像手段）、２２は撮像対象物（ワーク）を示す。

Claims (4)

1. 垂直多関節型のロボットの手先から対象物までの距離を変化させるように、前記手先を移動させる位置及び前記手先の姿勢を教示した後に、前記位置及び前記姿勢を補間する方法において、
   教示点を通過する移動軌跡が自由曲線となるように教示点の間の位置を移動順番を付けて補間し、補間点を生成する第１ステップと、
   各教示点毎に、教示点と同じ座標系における前記目標点の座標を求める第２ステップと、
   前記各教示点に対応する各目標点の座標間を、前記第１ステップと同じアルゴリズムの自由曲線により補間して前記第１ステップと同じ移動順番を付した補間点を生成する第３ステップと、
   教示点における手先の姿勢角を３方向に分解した正規化姿勢ベクトルについて、手先から対象物の目標点に向かう方向を第１姿勢とし、その他を第２，第３姿勢として、前記第１ステップで生成した補間点を始点とし、前記第３ステップにおいて生成した補間点を終点とする第１姿勢ベクトルを、前記第１ステップで生成した全ての補間点に対して求める第４ステップと、
   ２つの教示点間の各補間点について、前記第２正規化姿勢ベクトルの回転角を補間する第５ステップと、
   各補間点に立つ前記第１姿勢ベクトル及び前記第２正規化姿勢ベクトルとの外積より、第３姿勢ベクトルを求める第６ステップと、
   前記第１ステップで生成した補間点と、前記第４ステップから第６ステップで得られた各姿勢ベクトルの座標とから、各補間点における前記手先の姿勢を決定する第７ステップとからなることを特徴とするロボットの位置姿勢補間方法。
2. 垂直多関節型のロボットの手先から対象物までの距離を変化させるように、前記手先を移動させる位置及び前記手先の姿勢を教示した後に、前記位置及び前記姿勢を補間する方法において、
   教示点を通過する移動軌跡が自由曲線となるように教示点の間の位置を移動順番を付けて補間し、補間点を生成する第１ステップと、
   各教示点毎に、教示点と同じ座標系における前記目標点の座標を求める第２ステップと、
   前記各教示点に対応する各目標点の座標間を、前記第１ステップと同じアルゴリズムの自由曲線により補間して前記第１ステップと同じ移動順番を付した補間点を生成する第３ステップと、
   教示点における手先の姿勢角を３方向に分解した正規化姿勢ベクトルについて、手先から対象物の目標点に向かう方向を第１姿勢とし、その他を第２，第３姿勢として、前記第１ステップで生成した補間点を始点とし、前記第３ステップにおいて生成した補間点を終点とする第１姿勢ベクトルを、前記第１ステップで生成した全ての補間点に対して求める第４ステップと、
   前記第２，第３姿勢ベクトルのそれぞれについて、隣り合う２つの教示点ｉ，（ｉ＋１）にそれぞれ立つ２つのベクトルがなす角θｉを求める第５ステップと、
   前記第２，第３姿勢ベクトルのそれぞれについて、前記２つの教示点ｉ，（ｉ＋１）の間で補間された補間点ｊについて、前記教示点ｉ，前記補間点ｊにそれぞれ立つ２つのベクトルがなす角θｊを求める第６ステップと、
   前記第２，第３姿勢ベクトルの何れか一方について、前記２つの教示点ｉ，（ｉ＋１）にそれぞれ立つ２つのベクトルの外積Ｐｉを求め、求めた外積Ｐｉを正規化して正規化ベクトルｎＰｉを求める第７ステップと、
   前記正規化ベクトルｎＰｉを回転軸とし、当該正規化ベクトルｎＰｉについて、前記角度θｊを回転角として、正規化クオータニオンＱｊを求める第８ステップと、
   前記正規化ベクトルｎＰｉとして選択した姿勢ベクトルを、前記正規化クオータニオンＱｊにより回転させた姿勢ベクトルＲｂｊを求める第９ステップと、
   前記補間点ｊに立つ前記第１姿勢ベクトルＲａｊ，及び前記回転させた姿勢ベクトルＲｂｊの外積を求め、前記補間点ｊに立つ、前記正規化ベクトルとして選択しなかった姿勢ベクトルＲｃｊを求める第１０ステップと、
   再度、姿勢ベクトルＲａｊとＲｃｊの外積より姿勢ベクトルＲｂｊを求め直す第１１ステップと、
   前記３つの姿勢ベクトルＲａｊ，Ｒｂｊ，Ｒｃｊより、前記補間点ｊにおける前記手先の姿勢角を求める第１２ステップとからなることを特徴とするロボットの位置姿勢補間方法。
3. 垂直多関節型のロボットの手先から対象物までの距離を変化させるように、前記手先を移動させる位置及び前記手先の姿勢を教示した後に、前記位置及び前記姿勢を補間するロボットの制御装置において、
   教示点を通過する移動軌跡が自由曲線となるように教示点の間の位置を移動順番を付けて補間し、補間点を生成する位置補間手段と、
   各教示点毎に、教示点と同じ座標系における前記目標点の座標を求める目標点座標決定手段と、
   前記各教示点に対応する各目標点の座標間を、前記位置補間手段と同じアルゴリズムの自由曲線により補間して前記位置補間手段により付された移動順番と同じ移動順番を付した補間点を生成する目標点位置補間手段と、
   教示点における手先の姿勢角を３方向に分解した正規化姿勢ベクトルについて、手先から対象物の目標点に向かう方向を第１姿勢とし、その他を第２，第３姿勢として、前記位置補間手段により生成された補間点を始点とし、前記目標点位置補間手段により生成された補間点を終点とする第１姿勢ベクトルを、前記位置補間手段により生成された全ての補間点に対して求める第１姿勢ベクトル決定手段と、
   ２つの教示点間の各補間点について、前記第２正規化姿勢ベクトルの回転角を補間する回転角補間手段と、
   各補間点に立つ前記第１姿勢ベクトル及び前記第２正規化姿勢ベクトルとの外積より、第３姿勢ベクトルを求める第３姿勢ベクトル決定手段と、
   前記位置補間手段により生成された補間点と、前記各姿勢ベクトルの座標とから、各補間点における前記手先の姿勢を決定する姿勢決定手段とを備えることを特徴とするロボットの制御装置。
4. 垂直多関節型のロボットの手先から対象物までの距離を変化させるように、前記手先を移動させる位置及び前記手先の姿勢を教示した後に、前記位置及び前記姿勢を補間するロボットの制御装置において、
   教示点を通過する移動軌跡が自由曲線となるように教示点の間の位置を移動順番を付けて補間し、補間点を生成する位置補間手段と、
   各教示点毎に、教示点と同じ座標系における前記目標点の座標を求める目標点座標決定手段と、
   前記各教示点に対応する各目標点の座標間を、前記位置補間手段と同じアルゴリズムの自由曲線により補間して前記位置補間手段により付された移動順番と同じ移動順番を付した補間点を生成する目標点位置補間手段と、
   教示点における手先の姿勢角を３方向に分解した正規化姿勢ベクトルについて、手先から対象物の目標点に向かう方向を第１姿勢とし、その他を第２，第３姿勢として、前記位置補間手段により生成された補間点を始点とし、前記目標点位置補間手段により生成された補間点を終点とする第１姿勢ベクトルを、前記位置補間手段により生成された全ての補間点に対して求める第１姿勢ベクトル決定手段と、
   前記第２，第３姿勢ベクトルのそれぞれについて、隣り合う２つの教示点ｉ，（ｉ＋１）にそれぞれ立つ２つのベクトルがなす角θｉを求める教示点ベクトル角度算出手段と、
   前記第２，第３姿勢ベクトルのそれぞれについて、前記２つの教示点ｉ，（ｉ＋１）の間で補間された補間点ｊについて、前記教示点ｉ，前記補間点ｊにそれぞれ立つ２つのベクトルがなす角θｊを求める補間点ベクトル角度算出手段と、
   前記第２，第３姿勢ベクトルの何れか一方について、前記２つの教示点ｉ，（ｉ＋１）にそれぞれ立つ２つのベクトルの外積Ｐｉを求め、求めた外積Ｐｉを正規化して正規化ベクトルｎＰｉを求める正規化ベクトル決定手段と、
   前記正規化ベクトルｎＰｉを回転軸とし、当該正規化ベクトルｎＰｉについて、前記角度θｊを回転角として、正規化クオータニオンＱｊを求める正規化クオータニオン算出手段と、
   前記正規化ベクトルｎＰｉとして選択した姿勢ベクトルを、前記正規化クオータニオンＱｊにより回転させた姿勢ベクトルＲｂｊを求める回転姿勢ベクトル算出手段と、
   前記補間点ｊに立つ前記第１姿勢ベクトルＲａｊ，及び前記回転させた姿勢ベクトルＲｂｊの外積を求め、前記補間点ｊに立つ、前記正規化ベクトルとして選択しなかった姿勢ベクトルＲｃｊを求める最終姿勢ベクトル算出手段と、
   再度、姿勢ベクトルＲａｊとＲｃｊの外積より姿勢ベクトルＲｂｊを求め直し、前記３つの姿勢ベクトルＲａｊ，Ｒｂｊ，Ｒｃｊより、前記補間点ｊにおける前記手先の姿勢角を求める姿勢角決定手段とを備えることを特徴とするロボットの制御装置。

Images